diff options
| author | Xiaohui Tao <xtao@nvidia.com> | 2014-01-08 14:31:34 -0800 |
|---|---|---|
| committer | Mitch Luban <mluban@nvidia.com> | 2014-01-09 14:20:20 -0800 |
| commit | c401827da87830d26426a1e05fd2a4fc7523fef5 (patch) | |
| tree | f6fd4e58d3abd86f819de490f29cbf8204f9d9ae /drivers/iio | |
| parent | 168a30189073179c1394e2ec8b19de8c6341cd20 (diff) | |
Invensense V5.2.0 code drop for K310/KK
Change-Id: I166b0dc65bb90edd582bb0121a59683a9ce2a4af
Signed-off-by: Xiaohui Tao <xtao@nvidia.com>
Reviewed-on: http://git-master/r/353453
GVS: Gerrit_Virtual_Submit
Reviewed-by: Mitch Luban <mluban@nvidia.com>
Diffstat (limited to 'drivers/iio')
55 files changed, 19910 insertions, 0 deletions
diff --git a/drivers/iio/imu/inv_mpu/Kconfig b/drivers/iio/imu/inv_mpu/Kconfig new file mode 100644 index 000000000000..1b8c95484f02 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/Kconfig @@ -0,0 +1,32 @@ +# +# inv-mpu-iio driver for Invensense MPU devices and combos +# + +config INV_MPU_IIO + tristate "Invensense MPU devices" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF && IIO_TRIGGER && !INV_MPU + default n + help + This driver supports the Invensense MPU devices. + This includes MPU6050/MPU3050/MPU9150/ITG3500/MPU6500/MPU9250. + This driver can be built as a module. The module will be called + inv-mpu-iio. + +config INV_IIO_MPU3050_ACCEL_SLAVE_BMA250 + bool "Invensense MPU3050 slave accelerometer device for bma250" + depends on INV_MPU_IIO + default n + help + This is slave device enable MPU3050 accelerometer slave device. + Right now, it is only bma250. For other acceleromter device, + it can be added to this menu if the proper interface is filled. + There are some interface function to be defined. + +config DTS_INV_MPU_IIO + bool "Invensense MPU driver using Device Tree Structure (DTS)" + depends on INV_MPU_IIO + default n + help + This enables Invensense MPU devices to use Device Tree Structure. + DTS must be enabled in the system. + diff --git a/drivers/iio/imu/inv_mpu/Makefile b/drivers/iio/imu/inv_mpu/Makefile new file mode 100644 index 000000000000..1563c35489ea --- /dev/null +++ b/drivers/iio/imu/inv_mpu/Makefile @@ -0,0 +1,39 @@ +# +# Makefile for Invensense inv-mpu-iio device. +# + +obj-$(CONFIG_INV_MPU_IIO) += inv-mpu-iio.o + +inv-mpu-iio-objs := inv_mpu_core.o +inv-mpu-iio-objs += inv_mpu_ring.o +inv-mpu-iio-objs += inv_mpu_trigger.o +inv-mpu-iio-objs += inv_mpu_misc.o +inv-mpu-iio-objs += inv_mpu3050_iio.o +inv-mpu-iio-objs += dmpDefaultMPU6050.o +inv-mpu-iio-objs += inv_slave_compass.o +inv-mpu-iio-objs += inv_slave_pressure.o + +CFLAGS_inv_mpu_core.o += -Idrivers/staging/iio +CFLAGS_inv_mpu_ring.o += -Idrivers/staging/iio +CFLAGS_inv_mpu_trigger.o += -Idrivers/staging/iio +CFLAGS_inv_mpu_misc.o += -Idrivers/staging/iio +CFLAGS_inv_mpu3050_iio.o += -Idrivers/staging/iio +CFLAGS_dmpDefaultMPU6050.o += -Idrivers/staging/iio +CFLAGS_inv_slave_compass.o += -Idrivers/staging/iio +CFLAGS_inv_slave_pressure.o += -Idrivers/staging/iio + +# the Bosch BMA250 driver is added to the inv-mpu device driver because it +# must be connected to an MPU3050 device on the secondary slave bus. +ifeq ($(CONFIG_INV_IIO_MPU3050_ACCEL_SLAVE_BMA250), y) +inv-mpu-iio-objs += inv_slave_bma250.o +CFLAGS_inv_slave_bma250.o += -Idrivers/staging/iio +endif + +# compile Invensense MPU IIO driver as DTS +ifeq ($(CONFIG_DTS_INV_MPU_IIO), y) +inv-mpu-iio-objs += inv_mpu_dts.o +CFLAGS_inv_mpu_dts.o += -Idrivers/staging/iio +endif + +obj-y += inv_compass/ +obj-y += inv_test/ diff --git a/drivers/iio/imu/inv_mpu/README b/drivers/iio/imu/inv_mpu/README new file mode 100644 index 000000000000..0963954a8441 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/README @@ -0,0 +1,659 @@ +Kernel driver inv-mpu-iio +Author: Invensense <http://invensense.com> + +Table of Contents: +================== +- Description +- Integrating the Driver in the Linux Kernel +- Board and Platform Data + > Interrupt Pin + > Platform Data +- Board File Modifications for Secondary I2C Configuration + > MPU-6050 + AKM8963 on the secondary I2C interface + > MPU-6500 + AKM8963 on the secondary I2C interface + > MPU-6515 + AKM8975 and BMP280 on the secondary I2C interface + > MPU-9150 + > MPU-9250 + > MPU-3050 + BMA250 on the secondary I2C interface +- Board File Modifications for Invensense Devices + > MPU-3050 + > ITG-3500 + > MPU-6050 + > MPU-6515 + > MPU-6500 + > MPU-6XXX + > MPU-9150 + > MPU-9250 +- IIO Subsystem + > Communicating with the Driver in Userspace + > ITG-3500 + > MPU-6050 and MPU-6500 + > MPU-9150 + > MPU-9250 + > MPU-3050 + BMA250 on the secondary I2C interface +- Suspend and Resume +- Wake up CPU using event interrupt +- DMP Event +- Motion Event +- Streaming Data to an Userspace Application +- Recommended Sysfs Entry Setup Sequence + > With DMP Firmware + > Without DMP Firmware +- Test Applications + > Running Test Applications with MPU-9150/MPU-6050/MPU-6500/MPU-9250 + > Running Test Applications with MPU-3050/ITG-3500 + + +Description +=========== +This document describes how to install the Invensense device driver into a +Linux kernel. The Invensense driver currently supports the following sensors: +- ITG-3500 +- MPU-6050 +- MPU-9150 +- MPU-6500 +- MPU-9250 +- MPU-3050 +- MPU-6XXX(either MPU6050 or MPU6500, driver to do auto detection) + +The slave address of each device is either 0x68 or 0x69, depending on the AD0 +pin value of the device. Please refer to the appropriate product specification +document for further information regarding the AD0 pin. The driver supports both +addresses. + +The following files are included in this package: +- Kconfig +- Makefile +- inv_mpu_core.c +- inv_mpu_misc.c +- inv_mpu_trigger.c +- inv_mpu3050_iio.c +- inv_mpu_iio.h +- inv_mpu_ring.c +- inv_slave_bma250.c +- inv_slave_compass.c +- dmpDefaultMPU6050.c +- dmpkey.h +- dmpmap.h +- mpu.h + + +Integrating the Driver in the Linux Kernel +========================================== +Please add the files as follows: +- Add mpu.h to "kernel/include/linux". +- Add all other files to drivers/staging/iio/imu/inv_mpu +(another directory is acceptable, but this is the recommended destination) + +In order to see the driver in menuconfig when building the kernel, please +make modifications as shown below: + + modify "drivers/staging/iio/imu/Kconfig" with: + >> source "drivers/staging/iio/imu/inv_mpu/Kconfig" + + modify "drivers/staging/iio/imu/Makefile" with: + >> obj-y += inv_mpu/ + + +Board and Platform Data +======================= +In order to recognize the Invensense device on the I2C bus, the board file must +be modified. +The i2c_board_info instance must be defined as shown below. + +Interrupt Pin +------------- +The hardcoded value of 140 corresponds to the GPIO input pin connected to the +Invensense device's interrupt pin. +This pin will most likely be different for your platform, and the value should +be changed accordingly. + +Platform Data +------------- +The platform data (orientation matrix and secondary bus configurations) must be +modified as show below, according to your particular platform configuration. + +Please note that the MPU-9150 it is treated as a MPU-6050 with AKM8975 on the +device's secondary I2C interface. Thus the secondary I2C address must be +provided. + +Please note that the MPU-9250 it is treated as a MPU-6500 with AKM8963 on the +device's secondary I2C interface. Thus the secondary I2C address must be +provided. + +Board File Modifications for Secondary I2C Configuration +======================================================== +For the Panda Board, the board file can be found at +arch/arm/mach-omap2/board-omap4panda.c. +Please modify the pertinent baord file in your system according to the examples +shown below: + +MPU-6050 + AKM8963 on the secondary I2C interface +------------------------------------------------- +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x00, + .level_shifter = 0, + .orientation = { -1, 0, 0, + 0, 1, 0, + 0, 0, -1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK8963, + .secondary_i2c_addr = 0x0E, + .secondary_orientation = { 0, 1, 0, + 1, 0, 0, + 0, 0, -1 }, +}; + +MPU-6500 + AKM8963 on the secondary I2C interface +------------------------------------------------- +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x00, + .level_shifter = 0, + .orientation = { -1, 0, 0, + 0, 1, 0, + 0, 0, -1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK8963, + .secondary_i2c_addr = 0x0E, + .secondary_orientation = { 0, 1, 0, + 1, 0, 0, + 0, 0, -1 }, +}; + +MPU-6500 + AK09911 on the secondary I2C interface +------------------------------------------------- +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x00, + .level_shifter = 0, + .orientation = { -1, 0, 0, + 0, 1, 0, + 0, 0, -1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK09911, + .secondary_i2c_addr = 0x0D, + .secondary_orientation = { 0, 1, 0, + 1, 0, 0, + 0, 0, -1 }, +}; + +MPU-6515 + AK8975 + BMP280 on the secondary I2C interface +Note: sec_slave_XXX can only be compass or accelerometer in 3050 case. + aux_slave_XXX can only be pressure. +-------------------------------------------------- +static struct mpu_platform_data mpu_data = { + .int_config = 0x00, + .level_shifter = 0, + .orientation = { -1, 0, 0, + 0, 1, 0, + 0, 0, -1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK8975, + .secondary_i2c_addr = 0x0E, + .secondary_orientation = { 0, 1, 0, + 1, 0, 0, + 0, 0, -1 }, + .aux_slave_type = SECONDARY_SLAVE_TYPE_PRESSURE, + .aux_slave_id = PRESSURE_ID_BMP280, + .aux_i2c_addr = 0x77, +}; +static struct i2c_board_info __initdata chip_board_info[] = { + { + I2C_BOARD_INFO("mpu6515", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &mpu_data, + } +}; + +MPU-9150 +-------- +For MPU-9150, please provide the following secondary I2C bus information. + +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x10, + .level_shifter = 0, + .orientation = { 1, 0, 0, + 0, 1, 0, + 0, 0, 1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK8975, + .secondary_i2c_addr = 0x0C, + .secondary_orientation = { 0, 1, 0, + 1, 0, 0, + 0, 0, -1 }, +}; + +MPU-9250 +-------- +For MPU-9250, please provide the following secondary I2C bus information. + +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x00, + .level_shifter = 0, + .orientation = { 1, 0, 0, + 0, 1, 0, + 0, 0, 1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS, + .sec_slave_id = COMPASS_ID_AK8963, + .secondary_i2c_addr = 0x0C, + .secondary_orientation = { 0, 1, 0, + -1, 0, 0, + 0, 0, 1 }, +}; + +MPU-3050 + BMA250 on the secondary I2C interface +------------------------------------------------ +For BMA250 on the secondary I2C bus, please provide the following information. + +static struct mpu_platform_data gyro_platform_data = { + .int_config = 0x10, + .level_shifter = 0, + .orientation = { -1, 0, 0, + 0, 1, 0, + 0, 0, -1 }, + .sec_slave_type = SECONDARY_SLAVE_TYPE_ACCEL, + .sec_slave_id = ACCEL_ID_BMA250, + .secondary_i2c_addr = 0x18, +}; + + +Board File Modifications for Invensense Devices +=============================================== +For Invensense devices, please provide the i2c init data as shown in the +examples below. + +In the _i2c_init function, the device is registered in the following manner: + + // arch/arm/mach-omap2/board-omap4panda.c + // in static int __init omap4_panda_i2c_init(void) + omap_register_i2c_bus(4, 400, + single_chip_board_info, + ARRAY_SIZE(single_chip_board_info)); + +MPU-3050 +-------- +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu3050", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +ITG-3500 +-------- +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("itg3500", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +MPU6050 +------- +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu6050", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +MPU6500 +------- +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu6500", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +MPU6XXX +------- +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu6xxx", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +MPU9150 +------- +arch/arm/mach-omap2/board-omap4panda.c +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu9150", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +MPU9250 +------- +arch/arm/mach-omap2/board-omap4panda.c +static struct i2c_board_info __initdata single_chip_board_info[] = { + { + I2C_BOARD_INFO("mpu9250", 0x68), + .irq = (IH_GPIO_BASE + MPUIRQ_GPIO), + .platform_data = &gyro_platform_data, + }, +}; + +IIO subsystem +============= +A successful installation will create the following two new directories under +/sys/bus/iio/devices: + - iio:device0 + - trigger0 + +Also, a new file, "iio:device0", will be created in the /dev/ diretory. +(if you have more than one IIO device, the file will be named "iio:deviceX", +where X is a number) + + +Communicating with the Driver in Userspace +------------------------------------------ +The driver generates several files in sysfs upon installation. +These files are used to communicate with the driver. The files can be found +at /sys/bus/iio/devices/iio:device0 (or ../iio:deviceX as shown above). + +A brief description of the pertinent files for each Invensense device is shown +below: + +ITG-3500 +-------- +temperature (Read-only) +--Read temperature data directly from the temperature register. + +sampling_frequency (Read/write) +--Configure the ADC sampling rate and FIFO output rate. + +sampling_frequency_available(read-only) +--show commonly used frequency + +clock_source (Read-only) +--Check which clock-source is used by the chip. + +power_state (Read/write) +--turn on/off the power supply + +self_test (read-only) +--read this entry trigger self test. The return value is D. +D is the success/fail. +For different chip, the result is different for success/fail. +1 means success 0 means fail. The LSB of D is for gyro; the bit +next to LSB of D is for accel. The bit 2 of D is for compass result. + +key (read-only) +--show the key value of this driver. Used by MPL. + +gyro_matrix (read-only) +--show the orientation matrix obtained from the board file. + +MPU-6050 and MPU-6500 +--------------------- +MPU-6050 and MPU-6500 have all sysfs files belonging to ITG-3500 (shown above). +In addition, it has the files below: + +gyro_enable (read/write) +--enable/disable gyro functionality. Affects raw_gyro. Turning this off this + will shut down gyro and save power. + +accl_enable (read/write) +--enable/disable accelerometer functionality. Affects raw_accl. +Turning this off this will shut down accel and save power. + +firmware_loaded (read/write) +--Flag indicating the whether firmware is loaded or not in the DMP engine. +0 means no firmware loaded. 1 means firmware is already loaded . This +flag can only be written as 0. It internally updates to 1. + +dmp_on(read/write) +--This entry controls whether to run DMP or not. +Write 1 to enable DMP and write 0 to disable dmp. +Please note that firmware_loaded must be 1 in order to enable DMP. + +dmp_int_on(read/write) +--This entry controls whether dmp interrupt is on/off. +Please note that firmware_loaded must be 1. +Also, we'd like to remind you that it is sometimes advantageous to +turn interrupts off while the DMP is running. + +dmp_output_rate +--control dmp output rate when dmp is on. + +dmp_event_int_on(read/write) +--This entry controls whether dmp event interrupt is on/off. +Please note that turning this on will turn off the data interrupt. +Interrupts will be generated only when events occur. +This is useful for saving power when the system is waiting for a special event +to wake up. + +dmp_firmware (write only binary file) +--DMP firmware code is loaded into this entry. +If loading is successful, the firmware_loaded flag will be updated to 1. +In order to load new firmware, the firmware_loaded flag must be first set to 0. + +accel_matrix +--orientation matrix for accelerometer. + +quaternion_on +--Turn on/off quaterniion data output. DMP is required for this feature. + +pedometer_time +pedometer_steps, +--Pedometer related entries + +event_tap +event_display_orientation +event_accel_motion +event_smd +--Event related entries. +Please poll these entries to read their values. Direct reads will yield +meaningless results. +Further details are provided in the DMP Events section of this README. + +tap_on +--Controls tap function of DMP + +tap_time +tap_min_count +tap_threshold +--Tap related entries. Controls various parameters of tap function. + +display_orientation_on +--Turn on/off display orientation function of DMP. + +smd_enable +enable SMD(Significant Motion Detection) detection. + +smd_threshold +This set the threshold of the motion when SMD start to be triggered. The +value is in acclerometer counts. + +smd_delay_threshold +This sets the threshold of time after which SMD can be triggered. +The value is in seconds. + +smd_delay_threshold2 +This sets the threshold of time during which SMD can be triggered (after the +smd_delay_threshold timer has expired). +The value is in seconds. + +quaternion_on +--Turn on/off quaterniion data output. DMP is required for this feature. + +Low power accel motion interrupt related settings. +if motion_lpa_on is set, this will disable all engines except accel. Accel will +enter low power mode and the whole chip will be turned on/off at specific frequency. +----------------------------------------------------------------------------- +motion_lpa_threshold +--set motion threshold. in mg. The maximum is 1020mg and resolution is 32mg. + +motion_lpa_on +--turn on/off motion function. + +motion_lpa_freq +--motion lpa frequency. which determines power on/off frequency. +------------------------------------------------------------------------------ +MPU-9150 +-------- +MPU-9150 has all of MPU-6050's entries. It also has two additional entries, +described below. + +compass_enable (read/write) +--Enables compass function. + +compass_matrix (read-only) +--Compass orientation matrix + +MPU-3050 with BMA250 on secondary I2C interface +----------------------------------------------- +MPU-3050 with BMA250 on the secondary I2C interface has ever ITG-3500 entry. +It also has two additional entries, shown below: + +accl_matrix + +accl_enable + +Suspend and Resume +=================================================== +The suspend and resume functions are call backs registered to the system +and executed when the system goes in suspend and resumes. +It is enabled when CONFIG_PM is defined. +The current behavior is simple: +- suspend will turn off the chip +- resume will turn on the chip + +However, it is possible for the driver to do more complex things; +for example, leaving pedometers running when system is off. This can save whole +system power while letting pedometer working. Other behaviors are possible +too. + +Wake up CPU using event interrupt +==================================================== +It is common practice in current mobile device to save power as much as +possible. Ususally the device can sleep when not use and only be wake up when in use. +Invensense MPU series provides an efficient way of waking up CPU by events. +The events could be pedometer steps, tap action, SMD(significant Motion Detection), +or any customer defined actions. CPU can sleep to save power, when event happens, +DMP will calculate the motion data to detemine whether a pre-defined event happens +or not. If an pre-defined happens, an interrupt is generated to wake up the CPU +to do further processing. +The requirements of doing event wake is the following: +1. Add enable_irq_wake() either in the driver or in the board file. +2. Connect the interrupt pin to the CPU wake up pin. +3. Configure CPU to be woken up by wake up pin. + +DMP Event +========= +A DMP Event is an event that is output by the DMP unit within the Invensense +device (MPU). +Only the MPU-6050, MPU-6500, MPU-9250, MPU-9150, MPU-9250 feature the DMP. + +There are four sysfs entries for DMP events: +- event_tap +- event_display_orientation +- event_accel_motion +- event_smd + +These events must be polled before reading. + +The proper method to poll sysfs is as follows: +1. open file. +2. dummy read. +3. poll. +4. once the poll passed, use fopen and fread to read the sysfs entry. +5. interpret the data. + +Streaming Data to an Userspace Application +========================================== +When streaming data to an userspace application, we recommend that you access +gyro/accel/compass data via /dev/iio:device0. + +Please follow the steps below to read data at a constant rate from the driver: + +1. Write a 1 to power_state to turn on the chip. This is the default setting + after installing the driver. +2. Write the desired output rate to fifo_rate. +3. Write 1 to enable to turn on the event. +4. Read /dev/iio:device0 to get a string of gyro/accel/compass data. +5. Parse this string to obtain each gyro/accel/compass element. +6. If dmp firmware code is loaded, use "dmp_on" to enable/disable dmp. +7. If compass is enabled, the output will contain compass data. + + +Recommended Sysfs Entry Setup Senquence +======================================= + +Without DMP Firmware +-------------------- +1. Set "power_state" to 1, +2. Set the scale and fifo rate values according to your needs. +3. Set gyro_enable, accel_enable, and compass_enable according to your needs. + For example: + - If you only want gyro data, set accel_enable to 0 (and compass_enable to + 0, if applicable). + - If you only want accel data, set gyro_enable to 0 (and compass_enable to + 0, if applicable). + - If you only want compass data, set gyro_enable to 0 and accel_enable to 0. +4. Set "enable" to 1. +5. You will now get the output that you want. + +With DMP Firmware +----------------- +1. Set "power_state" to 1. +2. Write "0" to firmware_loaded if it is not zero already. +3. Load firmware into "dmp_firmware" as a whole. Don't split the DMP firmware + image. +4. Make sure firmware_loaded is 1 after loading the DMP image. +5. Make appropriate configurations as shown above in the "without DMP firmware" + case. +6. Set dmp_on to 1. +7. Set "enable" to 1. + +Please note that the enable function uses the enable entry under +"/sys/bus/iio/devices/iio:device0/buffer" + +Test Applications +================= +A test application is located under software/simple_apps/mpu_iio. +This application is stand-alone in that it cannot be run concurrently with other +entities trying to access the device node(s) or sysfs entries; in particular, +the + +Running Test Applications with MPU-9150/MPU-6050/MPU-6500/MPU-9250 +--------------------------------------------------------- +To run test applications with MPU-9150, MPU-9250, MPU-6050, or MPU-6500 devices, +please use the following commands: + +1. For tap/display orientation events: + mpu_iio -c 10 -l 3 -p + +2. In addition, to test the motion interrupt (and no_motion on MPU6050) use: + mpu_iio -c 10 -l 3 -p -m + +3. For printing data normally: + mpu_iio -c 10 -l 3 -r + +Running Test Applications with MPU-3050/ITG-3500 +------------------------------------------------ +To run test applications with MPU-3050 or ITG-3500 devices, +please use the following command: + +1. For printing data normally: + mpu_iio -c 10 -l 3 -r + +Please use mpu_iio.c and iio_utils.h as example code for your development +purposes. + +Stress test application +================================= +A stress test application is located under software/simple_apps/stress_iio. +This application simulates HAL's usage calls to the driver. It creates three +threads. One for data read; one for event read; one for sysfs control. +It can run without any parameters or run with some control parameters. Please +see README in the same directories for details. + diff --git a/drivers/iio/imu/inv_mpu/dmpDefaultMPU6050.c b/drivers/iio/imu/inv_mpu/dmpDefaultMPU6050.c new file mode 100644 index 000000000000..81e63960f391 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/dmpDefaultMPU6050.c @@ -0,0 +1,381 @@ +/* + * Copyright (C) 2012 Invensense, Inc. + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * 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. + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt +#include "inv_mpu_iio.h" +#include "dmpKey.h" +#include "dmpmap.h" + +#define CFG_OUT_PRESS (1823) +#define CFG_PED_ENABLE (1936) +#define CFG_OUT_GYRO (1755) +#define CFG_PEDSTEP_DET (2417) +#define OUT_GYRO_DAT (1764) +#define CFG_FIFO_INT (1934) +#define OUT_CPASS_DAT (1798) +#define CFG_AUTH (1051) +#define OUT_ACCL_DAT (1730) +#define FCFG_1 (1078) +#define FCFG_3 (1103) +#define FCFG_2 (1082) +#define CFG_OUT_CPASS (1789) +#define FCFG_7 (1089) +#define CFG_OUT_3QUAT (1617) +#define OUT_PRESS_DAT (1832) +#define OUT_3QUAT_DAT (1627) +#define CFG_7 (1300) +#define OUT_PQUAT_DAT (1696) +#define CFG_OUT_6QUAT (1652) +#define CFG_PED_INT (2406) +#define SMD_TP2 (1265) +#define SMD_TP1 (1244) +#define CFG_MOTION_BIAS (1302) +#define CFG_OUT_ACCL (1721) +#define CFG_OUT_STEPDET (1587) +#define OUT_6QUAT_DAT (1662) +#define CFG_OUT_PQUAT (1687) + +#define D_0_22 (22+512) +#define D_0_24 (24+512) + +#define D_0_36 (36) +#define D_0_52 (52) +#define D_0_96 (96) +#define D_0_104 (104) +#define D_0_108 (108) +#define D_0_163 (163) +#define D_0_188 (188) +#define D_0_192 (192) +#define D_0_224 (224) +#define D_0_228 (228) +#define D_0_232 (232) +#define D_0_236 (236) + +#define D_1_2 (256 + 2) +#define D_1_4 (256 + 4) +#define D_1_8 (256 + 8) +#define D_1_10 (256 + 10) +#define D_1_24 (256 + 24) +#define D_1_28 (256 + 28) +#define D_1_36 (256 + 36) +#define D_1_40 (256 + 40) +#define D_1_44 (256 + 44) +#define D_1_72 (256 + 72) +#define D_1_74 (256 + 74) +#define D_1_79 (256 + 79) +#define D_1_88 (256 + 88) +#define D_1_90 (256 + 90) +#define D_1_92 (256 + 92) +#define D_1_96 (256 + 96) +#define D_1_98 (256 + 98) +#define D_1_106 (256 + 106) +#define D_1_108 (256 + 108) +#define D_1_112 (256 + 112) +#define D_1_128 (256 + 144) +#define D_1_152 (256 + 12) +#define D_1_160 (256 + 160) +#define D_1_176 (256 + 176) +#define D_1_178 (256 + 178) +#define D_1_218 (256 + 218) +#define D_1_232 (256 + 232) +#define D_1_236 (256 + 236) +#define D_1_240 (256 + 240) +#define D_1_244 (256 + 244) +#define D_1_250 (256 + 250) +#define D_1_252 (256 + 252) +#define D_2_12 (512 + 12) +#define D_2_96 (512 + 96) +#define D_2_108 (512 + 108) +#define D_2_208 (512 + 208) +#define D_2_224 (512 + 224) +#define D_2_236 (512 + 236) +#define D_2_244 (512 + 244) +#define D_2_248 (512 + 248) +#define D_2_252 (512 + 252) +#define CPASS_BIAS_X (30 * 16 + 4) +#define CPASS_BIAS_Y (30 * 16 + 8) +#define CPASS_BIAS_Z (30 * 16 + 12) +#define CPASS_MTX_00 (32 * 16 + 4) +#define CPASS_MTX_01 (32 * 16 + 8) +#define CPASS_MTX_02 (36 * 16 + 12) +#define CPASS_MTX_10 (33 * 16) +#define CPASS_MTX_11 (33 * 16 + 4) +#define CPASS_MTX_12 (33 * 16 + 8) +#define CPASS_MTX_20 (33 * 16 + 12) +#define CPASS_MTX_21 (34 * 16 + 4) +#define CPASS_MTX_22 (34 * 16 + 8) +#define D_EXT_GYRO_BIAS_X (61 * 16) +#define D_EXT_GYRO_BIAS_Y (61 * 16 + 4) +#define D_EXT_GYRO_BIAS_Z (61 * 16 + 8) +#define D_ACT0 (40 * 16) +#define D_ACSX (40 * 16 + 4) +#define D_ACSY (40 * 16 + 8) +#define D_ACSZ (40 * 16 + 12) + +#define FLICK_MSG (45 * 16 + 4) +#define FLICK_COUNTER (45 * 16 + 8) +#define FLICK_LOWER (45 * 16 + 12) +#define FLICK_UPPER (46 * 16 + 12) + +#define D_SMD_ENABLE (18 * 16) +#define D_SMD_MOT_THLD (21 * 16 + 8) +#define D_SMD_DELAY_THLD (21 * 16 + 4) +#define D_SMD_DELAY2_THLD (21 * 16 + 12) +#define D_SMD_EXE_STATE (22 * 16) +#define D_SMD_DELAY_CNTR (21 * 16) + +#define D_WF_GESTURE_TIME_THRSH (25 * 16 + 8) +#define D_WF_GESTURE_TILT_ERROR (25 * 16 + 12) +#define D_WF_GESTURE_TILT_THRSH (26 * 16 + 8) +#define D_WF_GESTURE_TILT_REJECT_THRSH (26 * 16 + 12) + +#define D_AUTH_OUT (992) +#define D_AUTH_IN (996) +#define D_AUTH_A (1000) +#define D_AUTH_B (1004) + +#define D_PEDSTD_BP_B (768 + 0x1C) +#define D_PEDSTD_BP_A4 (768 + 0x40) +#define D_PEDSTD_BP_A3 (768 + 0x44) +#define D_PEDSTD_BP_A2 (768 + 0x48) +#define D_PEDSTD_BP_A1 (768 + 0x4C) +#define D_PEDSTD_SB (768 + 0x28) +#define D_PEDSTD_SB_TIME (768 + 0x2C) +#define D_PEDSTD_PEAKTHRSH (768 + 0x98) +#define D_PEDSTD_TIML (768 + 0x2A) +#define D_PEDSTD_TIMH (768 + 0x2E) +#define D_PEDSTD_PEAK (768 + 0X94) +#define D_PEDSTD_STEPCTR (768 + 0x60) +#define D_PEDSTD_STEPCTR2 (58 * 16 + 8) +#define D_PEDSTD_TIMECTR (964) +#define D_PEDSTD_DECI (768 + 0xA0) +#define D_PEDSTD_SB2 (60 * 16 + 14) +#define D_STPDET_TIMESTAMP (28 * 16 + 8) +#define D_PEDSTD_DRIVE_STATE (58) + +#define D_HOST_NO_MOT (976) +#define D_ACCEL_BIAS (660) + +#define D_BM_BATCH_CNTR (27 * 16 + 4) +#define D_BM_BATCH_THLD (27 * 16 + 12) +#define D_BM_ENABLE (28 * 16 + 6) +#define D_BM_NUMWORD_TOFILL (28 * 16 + 4) + +#define D_SO_DATA (4 * 16 + 2) + +#define D_P_HW_ID (22 * 16 + 10) +#define D_P_INIT (22 * 16 + 2) + +#define D_DMP_RUN_CNTR (24*16) + +#define D_ODR_S0 (45*16+8) +#define D_ODR_S1 (45*16+12) +#define D_ODR_S2 (45*16+10) +#define D_ODR_S3 (45*16+14) +#define D_ODR_S4 (46*16+8) +#define D_ODR_S5 (46*16+12) +#define D_ODR_S6 (46*16+10) +#define D_ODR_S7 (46*16+14) +#define D_ODR_S8 (42*16+8) +#define D_ODR_S9 (42*16+12) + +#define D_ODR_CNTR_S0 (45*16) +#define D_ODR_CNTR_S1 (45*16+4) +#define D_ODR_CNTR_S2 (45*16+2) +#define D_ODR_CNTR_S3 (45*16+6) +#define D_ODR_CNTR_S4 (46*16) +#define D_ODR_CNTR_S5 (46*16+4) +#define D_ODR_CNTR_S6 (46*16+2) +#define D_ODR_CNTR_S7 (46*16+6) +#define D_ODR_CNTR_S8 (42*16) +#define D_ODR_CNTR_S9 (42*16+4) + +#define D_FS_LPQ0 (59*16) +#define D_FS_LPQ1 (59*16 + 4) +#define D_FS_LPQ2 (59*16 + 8) +#define D_FS_LPQ3 (59*16 + 12) + +#define D_FS_Q0 (12*16) +#define D_FS_Q1 (12*16 + 4) +#define D_FS_Q2 (12*16 + 8) +#define D_FS_Q3 (12*16 + 12) + +#define D_FS_9Q0 (2*16) +#define D_FS_9Q1 (2*16 + 4) +#define D_FS_9Q2 (2*16 + 8) +#define D_FS_9Q3 (2*16 + 12) + +static const struct tKeyLabel dmpTConfig[] = { + {KEY_CFG_OUT_ACCL, CFG_OUT_ACCL}, + {KEY_CFG_OUT_GYRO, CFG_OUT_GYRO}, + {KEY_CFG_OUT_3QUAT, CFG_OUT_3QUAT}, + {KEY_CFG_OUT_6QUAT, CFG_OUT_6QUAT}, + {KEY_CFG_OUT_PQUAT, CFG_OUT_PQUAT}, + {KEY_CFG_PED_ENABLE, CFG_PED_ENABLE}, + {KEY_CFG_FIFO_INT, CFG_FIFO_INT}, + {KEY_CFG_AUTH, CFG_AUTH}, + {KEY_FCFG_1, FCFG_1}, + {KEY_FCFG_3, FCFG_3}, + {KEY_FCFG_2, FCFG_2}, + {KEY_FCFG_7, FCFG_7}, + {KEY_CFG_7, CFG_7}, + {KEY_CFG_MOTION_BIAS, CFG_MOTION_BIAS}, + {KEY_CFG_PEDSTEP_DET, CFG_PEDSTEP_DET}, + {KEY_D_0_22, D_0_22}, + {KEY_D_0_96, D_0_96}, + {KEY_D_0_104, D_0_104}, + {KEY_D_0_108, D_0_108}, + {KEY_D_1_36, D_1_36}, + {KEY_D_1_40, D_1_40}, + {KEY_D_1_44, D_1_44}, + {KEY_D_1_72, D_1_72}, + {KEY_D_1_74, D_1_74}, + {KEY_D_1_79, D_1_79}, + {KEY_D_1_88, D_1_88}, + {KEY_D_1_90, D_1_90}, + {KEY_D_1_92, D_1_92}, + {KEY_D_1_160, D_1_160}, + {KEY_D_1_176, D_1_176}, + {KEY_D_1_218, D_1_218}, + {KEY_D_1_232, D_1_232}, + {KEY_D_1_250, D_1_250}, + {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, + {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, + {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, + {KEY_DMP_ORIENT, DMP_ORIENT}, + {KEY_D_AUTH_OUT, D_AUTH_OUT}, + {KEY_D_AUTH_IN, D_AUTH_IN}, + {KEY_D_AUTH_A, D_AUTH_A}, + {KEY_D_AUTH_B, D_AUTH_B}, + {KEY_CPASS_BIAS_X, CPASS_BIAS_X}, + {KEY_CPASS_BIAS_Y, CPASS_BIAS_Y}, + {KEY_CPASS_BIAS_Z, CPASS_BIAS_Z}, + {KEY_CPASS_MTX_00, CPASS_MTX_00}, + {KEY_CPASS_MTX_01, CPASS_MTX_01}, + {KEY_CPASS_MTX_02, CPASS_MTX_02}, + {KEY_CPASS_MTX_10, CPASS_MTX_10}, + {KEY_CPASS_MTX_11, CPASS_MTX_11}, + {KEY_CPASS_MTX_12, CPASS_MTX_12}, + {KEY_CPASS_MTX_20, CPASS_MTX_20}, + {KEY_CPASS_MTX_21, CPASS_MTX_21}, + {KEY_CPASS_MTX_22, CPASS_MTX_22}, + {KEY_D_ACT0, D_ACT0}, + {KEY_D_ACSX, D_ACSX}, + {KEY_D_ACSY, D_ACSY}, + {KEY_D_ACSZ, D_ACSZ}, + {KEY_D_PEDSTD_BP_B, D_PEDSTD_BP_B}, + {KEY_D_PEDSTD_BP_A4, D_PEDSTD_BP_A4}, + {KEY_D_PEDSTD_BP_A3, D_PEDSTD_BP_A3}, + {KEY_D_PEDSTD_BP_A2, D_PEDSTD_BP_A2}, + {KEY_D_PEDSTD_BP_A1, D_PEDSTD_BP_A1}, + {KEY_D_PEDSTD_SB, D_PEDSTD_SB}, + {KEY_D_PEDSTD_SB_TIME, D_PEDSTD_SB_TIME}, + {KEY_D_PEDSTD_PEAKTHRSH, D_PEDSTD_PEAKTHRSH}, + {KEY_D_PEDSTD_TIML, D_PEDSTD_TIML}, + {KEY_D_PEDSTD_TIMH, D_PEDSTD_TIMH}, + {KEY_D_PEDSTD_PEAK, D_PEDSTD_PEAK}, + {KEY_D_PEDSTD_STEPCTR, D_PEDSTD_STEPCTR}, + {KEY_D_PEDSTD_STEPCTR2, D_PEDSTD_STEPCTR2}, + {KEY_D_PEDSTD_TIMECTR, D_PEDSTD_TIMECTR}, + {KEY_D_PEDSTD_DECI, D_PEDSTD_DECI}, + {KEY_D_PEDSTD_SB2, D_PEDSTD_SB2}, + {KEY_D_PEDSTD_DRIVE_STATE, D_PEDSTD_DRIVE_STATE}, + {KEY_D_STPDET_TIMESTAMP, D_STPDET_TIMESTAMP}, + {KEY_D_HOST_NO_MOT, D_HOST_NO_MOT}, + {KEY_D_ACCEL_BIAS, D_ACCEL_BIAS}, + {KEY_CFG_EXT_GYRO_BIAS_X, D_EXT_GYRO_BIAS_X}, + {KEY_CFG_EXT_GYRO_BIAS_Y, D_EXT_GYRO_BIAS_Y}, + {KEY_CFG_EXT_GYRO_BIAS_Z, D_EXT_GYRO_BIAS_Z}, + {KEY_CFG_PED_INT, CFG_PED_INT}, + {KEY_SMD_ENABLE, D_SMD_ENABLE}, + {KEY_SMD_ACCEL_THLD, D_SMD_MOT_THLD}, + {KEY_SMD_DELAY_THLD, D_SMD_DELAY_THLD}, + {KEY_SMD_DELAY2_THLD, D_SMD_DELAY2_THLD}, + {KEY_SMD_ENABLE_TESTPT1, SMD_TP1}, + {KEY_SMD_ENABLE_TESTPT2, SMD_TP2}, + {KEY_SMD_EXE_STATE, D_SMD_EXE_STATE}, + {KEY_SMD_DELAY_CNTR, D_SMD_DELAY_CNTR}, + {KEY_BM_ENABLE, D_BM_ENABLE}, + {KEY_BM_BATCH_CNTR, D_BM_BATCH_CNTR}, + {KEY_BM_BATCH_THLD, D_BM_BATCH_THLD}, + {KEY_BM_NUMWORD_TOFILL, D_BM_NUMWORD_TOFILL}, + {KEY_SO_DATA, D_SO_DATA}, + {KEY_P_INIT, D_P_INIT}, + {KEY_CFG_OUT_CPASS, CFG_OUT_CPASS}, + {KEY_CFG_OUT_PRESS, CFG_OUT_PRESS}, + {KEY_CFG_OUT_STEPDET, CFG_OUT_STEPDET}, + {KEY_CFG_3QUAT_ODR, D_ODR_S1}, + {KEY_CFG_6QUAT_ODR, D_ODR_S2}, + {KEY_CFG_PQUAT6_ODR, D_ODR_S3}, + {KEY_CFG_ACCL_ODR, D_ODR_S4}, + {KEY_CFG_GYRO_ODR, D_ODR_S5}, + {KEY_CFG_CPASS_ODR, D_ODR_S6}, + {KEY_CFG_PRESS_ODR, D_ODR_S7}, + {KEY_CFG_9QUAT_ODR, D_ODR_S8}, + {KEY_CFG_PQUAT9_ODR, D_ODR_S9}, + {KEY_ODR_CNTR_3QUAT, D_ODR_CNTR_S1}, + {KEY_ODR_CNTR_6QUAT, D_ODR_CNTR_S2}, + {KEY_ODR_CNTR_PQUAT, D_ODR_CNTR_S3}, + {KEY_ODR_CNTR_ACCL, D_ODR_CNTR_S4}, + {KEY_ODR_CNTR_GYRO, D_ODR_CNTR_S5}, + {KEY_ODR_CNTR_CPASS, D_ODR_CNTR_S6}, + {KEY_ODR_CNTR_PRESS, D_ODR_CNTR_S7}, + {KEY_ODR_CNTR_9QUAT, D_ODR_CNTR_S8}, + {KEY_ODR_CNTR_PQUAT9, D_ODR_CNTR_S9}, + {KEY_DMP_RUN_CNTR, D_DMP_RUN_CNTR}, + {KEY_DMP_LPQ0, D_FS_LPQ0}, + {KEY_DMP_LPQ1, D_FS_LPQ1}, + {KEY_DMP_LPQ2, D_FS_LPQ2}, + {KEY_DMP_LPQ3, D_FS_LPQ3}, + {KEY_DMP_Q0, D_FS_Q0}, + {KEY_DMP_Q1, D_FS_Q1}, + {KEY_DMP_Q2, D_FS_Q2}, + {KEY_DMP_Q3, D_FS_Q3}, + {KEY_DMP_9Q0, D_FS_9Q0}, + {KEY_DMP_9Q1, D_FS_9Q1}, + {KEY_DMP_9Q2, D_FS_9Q2}, + {KEY_DMP_9Q3, D_FS_9Q3}, + {KEY_TEST_01, OUT_ACCL_DAT}, + {KEY_TEST_02, OUT_GYRO_DAT}, + {KEY_TEST_03, OUT_CPASS_DAT}, + {KEY_TEST_04, OUT_PRESS_DAT}, + {KEY_TEST_05, OUT_3QUAT_DAT}, + {KEY_TEST_06, OUT_6QUAT_DAT}, + {KEY_TEST_07, OUT_PQUAT_DAT} +}; +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) + +static struct tKeyLabel keys[NUM_KEYS]; + +unsigned short inv_dmp_get_address(unsigned short key) +{ + static int isSorted; + + if (!isSorted) { + int kk; + for (kk = 0; kk < NUM_KEYS; ++kk) { + keys[kk].addr = 0xffff; + keys[kk].key = kk; + } + for (kk = 0; kk < NUM_LOCAL_KEYS; ++kk) + keys[dmpTConfig[kk].key].addr = dmpTConfig[kk].addr; + isSorted = 1; + } + if (key >= NUM_KEYS) { + pr_err("ERROR!! key not exist=%d!\n", key); + return 0xffff; + } + if (0xffff == keys[key].addr) + pr_err("ERROR!!key not local=%d!\n", key); + return keys[key].addr; +} diff --git a/drivers/iio/imu/inv_mpu/dmpKey.h b/drivers/iio/imu/inv_mpu/dmpKey.h new file mode 100644 index 000000000000..f6173b3c0f80 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/dmpKey.h @@ -0,0 +1,607 @@ +/* + * Copyright (C) 2012 Invensense, Inc. + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * 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. + */ +#ifndef DMPKEY_H__ +#define DMPKEY_H__ + +#define KEY_CFG_25 (0) +#define KEY_CFG_24 (KEY_CFG_25 + 1) +#define KEY_CFG_26 (KEY_CFG_24 + 1) +#define KEY_CFG_27 (KEY_CFG_26 + 1) +#define KEY_CFG_21 (KEY_CFG_27 + 1) +#define KEY_CFG_20 (KEY_CFG_21 + 1) +#define KEY_CFG_TAP4 (KEY_CFG_20 + 1) +#define KEY_CFG_TAP5 (KEY_CFG_TAP4 + 1) +#define KEY_CFG_TAP6 (KEY_CFG_TAP5 + 1) +#define KEY_CFG_TAP7 (KEY_CFG_TAP6 + 1) +#define KEY_CFG_TAP0 (KEY_CFG_TAP7 + 1) +#define KEY_CFG_TAP1 (KEY_CFG_TAP0 + 1) +#define KEY_CFG_TAP2 (KEY_CFG_TAP1 + 1) +#define KEY_CFG_TAP3 (KEY_CFG_TAP2 + 1) +#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3 + 1) +#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE + 1) +#define KEY_CFG_DR_INT (KEY_CFG_TAP_JERK + 1) +#define KEY_CFG_AUTH (KEY_CFG_DR_INT + 1) +#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_AUTH + 1) +#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB + 1) +#define KEY_CFG_FIFO_ON_EVENT (KEY_CFG_TAP_CLEAR_STICKY + 1) +#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_FIFO_ON_EVENT + 1) +#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT + 1) +#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT + 1) +#define KEY_FCFG_1 (KEY_CFG_23 + 1) +#define KEY_FCFG_3 (KEY_FCFG_1 + 1) +#define KEY_FCFG_2 (KEY_FCFG_3 + 1) +#define KEY_CFG_3D (KEY_FCFG_2 + 1) +#define KEY_CFG_3B (KEY_CFG_3D + 1) +#define KEY_CFG_3C (KEY_CFG_3B + 1) +#define KEY_FCFG_5 (KEY_CFG_3C + 1) +#define KEY_FCFG_4 (KEY_FCFG_5 + 1) +#define KEY_FCFG_7 (KEY_FCFG_4 + 1) +#define KEY_FCFG_FSCALE (KEY_FCFG_7 + 1) +#define KEY_FCFG_AZ (KEY_FCFG_FSCALE + 1) +#define KEY_FCFG_6 (KEY_FCFG_AZ + 1) +#define KEY_FCFG_LSB4 (KEY_FCFG_6 + 1) +#define KEY_CFG_12 (KEY_FCFG_LSB4 + 1) +#define KEY_CFG_14 (KEY_CFG_12 + 1) +#define KEY_CFG_15 (KEY_CFG_14 + 1) +#define KEY_CFG_16 (KEY_CFG_15 + 1) +#define KEY_CFG_18 (KEY_CFG_16 + 1) +#define KEY_CFG_6 (KEY_CFG_18 + 1) +#define KEY_CFG_7 (KEY_CFG_6 + 1) +#define KEY_CFG_4 (KEY_CFG_7 + 1) +#define KEY_CFG_5 (KEY_CFG_4 + 1) +#define KEY_CFG_2 (KEY_CFG_5 + 1) +#define KEY_CFG_3 (KEY_CFG_2 + 1) +#define KEY_CFG_1 (KEY_CFG_3 + 1) +#define KEY_CFG_EXTERNAL (KEY_CFG_1 + 1) +#define KEY_CFG_8 (KEY_CFG_EXTERNAL + 1) +#define KEY_CFG_9 (KEY_CFG_8 + 1) +#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) +#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) +#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) +#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) +#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) +#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) +#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) +#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) +#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) +#define KEY_CFG_LP_QUAT (KEY_FCFG_MAG_MOV + 1) +#define KEY_CFG_GYRO_RAW_DATA (KEY_CFG_LP_QUAT + 1) +#define KEY_CFG_EXT_GYRO_BIAS (KEY_CFG_GYRO_RAW_DATA + 1) +#define KEY_CFG_EXT_GYRO_BIAS_X (KEY_CFG_EXT_GYRO_BIAS + 1) +#define KEY_CFG_EXT_GYRO_BIAS_Y (KEY_CFG_EXT_GYRO_BIAS_X + 1) +#define KEY_CFG_EXT_GYRO_BIAS_Z (KEY_CFG_EXT_GYRO_BIAS_Y + 1) +#define KEY_bad_compass (KEY_CFG_EXT_GYRO_BIAS_Z + 1) +#define KEY_COMPASS_CHG_SENSITIVITY (KEY_bad_compass + 1) +#define KEY_CCS_HEADING_THLD (KEY_COMPASS_CHG_SENSITIVITY + 1) +#define KEY_CCS_TIME_THLD (KEY_CCS_HEADING_THLD + 1) +#define KEY_CCS_DOTP_THLD (KEY_CCS_TIME_THLD + 1) +#define KEY_CCS_COMP_CNTR (KEY_CCS_DOTP_THLD + 1) +#define KEY_CFG_NM_DET (KEY_CCS_COMP_CNTR + 1) +#define KEY_SMD_ENABLE (KEY_CFG_NM_DET + 1) +#define KEY_SMD_ACCEL_THLD (KEY_SMD_ENABLE + 1) +#define KEY_SMD_DELAY_THLD (KEY_SMD_ACCEL_THLD + 1) +#define KEY_SMD_DELAY2_THLD (KEY_SMD_DELAY_THLD + 1) +#define KEY_SMD_ENABLE_TESTPT1 (KEY_SMD_DELAY2_THLD + 1) +#define KEY_SMD_ENABLE_TESTPT2 (KEY_SMD_ENABLE_TESTPT1 + 1) +#define KEY_SMD_EXE_STATE (KEY_SMD_ENABLE_TESTPT2 + 1) +#define KEY_SMD_DELAY_CNTR (KEY_SMD_EXE_STATE + 1) + +#define KEY_BREAK (81) +#if KEY_SMD_DELAY_CNTR != KEY_BREAK +#error +#endif + +/* MPU6050 keys */ +#define KEY_CFG_ACCEL_FILTER (KEY_BREAK + 1) +#define KEY_CFG_MOTION_BIAS (KEY_CFG_ACCEL_FILTER + 1) +#define KEY_TEMPLABEL (KEY_CFG_MOTION_BIAS + 1) + +#define KEY_D_0_22 (KEY_TEMPLABEL + 1) +#define KEY_D_0_24 (KEY_D_0_22 + 1) +#define KEY_D_0_36 (KEY_D_0_24 + 1) +#define KEY_D_0_52 (KEY_D_0_36 + 1) +#define KEY_D_0_96 (KEY_D_0_52 + 1) +#define KEY_D_0_104 (KEY_D_0_96 + 1) +#define KEY_D_0_108 (KEY_D_0_104 + 1) +#define KEY_D_0_163 (KEY_D_0_108 + 1) +#define KEY_D_0_188 (KEY_D_0_163 + 1) +#define KEY_D_0_192 (KEY_D_0_188 + 1) +#define KEY_D_0_224 (KEY_D_0_192 + 1) +#define KEY_D_0_228 (KEY_D_0_224 + 1) +#define KEY_D_0_232 (KEY_D_0_228 + 1) +#define KEY_D_0_236 (KEY_D_0_232 + 1) + +#define KEY_DMP_PREVPTAT (KEY_D_0_236 + 1) +#define KEY_D_1_2 (KEY_DMP_PREVPTAT + 1) +#define KEY_D_1_4 (KEY_D_1_2 + 1) +#define KEY_D_1_8 (KEY_D_1_4 + 1) +#define KEY_D_1_10 (KEY_D_1_8 + 1) +#define KEY_D_1_24 (KEY_D_1_10 + 1) +#define KEY_D_1_28 (KEY_D_1_24 + 1) +#define KEY_D_1_36 (KEY_D_1_28 + 1) +#define KEY_D_1_40 (KEY_D_1_36 + 1) +#define KEY_D_1_44 (KEY_D_1_40 + 1) +#define KEY_D_1_72 (KEY_D_1_44 + 1) +#define KEY_D_1_74 (KEY_D_1_72 + 1) +#define KEY_D_1_79 (KEY_D_1_74 + 1) +#define KEY_D_1_88 (KEY_D_1_79 + 1) +#define KEY_D_1_90 (KEY_D_1_88 + 1) +#define KEY_D_1_92 (KEY_D_1_90 + 1) +#define KEY_D_1_96 (KEY_D_1_92 + 1) +#define KEY_D_1_98 (KEY_D_1_96 + 1) +#define KEY_D_1_100 (KEY_D_1_98 + 1) +#define KEY_D_1_106 (KEY_D_1_100 + 1) +#define KEY_D_1_108 (KEY_D_1_106 + 1) +#define KEY_D_1_112 (KEY_D_1_108 + 1) +#define KEY_D_1_128 (KEY_D_1_112 + 1) +#define KEY_D_1_152 (KEY_D_1_128 + 1) +#define KEY_D_1_160 (KEY_D_1_152 + 1) +#define KEY_D_1_168 (KEY_D_1_160 + 1) +#define KEY_D_1_175 (KEY_D_1_168 + 1) +#define KEY_D_1_176 (KEY_D_1_175 + 1) +#define KEY_D_1_178 (KEY_D_1_176 + 1) +#define KEY_D_1_179 (KEY_D_1_178 + 1) +#define KEY_D_1_218 (KEY_D_1_179 + 1) +#define KEY_D_1_232 (KEY_D_1_218 + 1) +#define KEY_D_1_236 (KEY_D_1_232 + 1) +#define KEY_D_1_240 (KEY_D_1_236 + 1) +#define KEY_D_1_244 (KEY_D_1_240 + 1) +#define KEY_D_1_250 (KEY_D_1_244 + 1) +#define KEY_D_1_252 (KEY_D_1_250 + 1) +#define KEY_D_2_12 (KEY_D_1_252 + 1) +#define KEY_D_2_96 (KEY_D_2_12 + 1) +#define KEY_D_2_108 (KEY_D_2_96 + 1) +#define KEY_D_2_208 (KEY_D_2_108 + 1) +#define KEY_FLICK_MSG (KEY_D_2_208 + 1) +#define KEY_FLICK_COUNTER (KEY_FLICK_MSG + 1) +#define KEY_FLICK_LOWER (KEY_FLICK_COUNTER + 1) +#define KEY_CFG_FLICK_IN (KEY_FLICK_LOWER + 1) +#define KEY_FLICK_UPPER (KEY_CFG_FLICK_IN + 1) +#define KEY_CGNOTICE_INTR (KEY_FLICK_UPPER + 1) +#define KEY_D_2_224 (KEY_CGNOTICE_INTR + 1) +#define KEY_D_2_244 (KEY_D_2_224 + 1) +#define KEY_D_2_248 (KEY_D_2_244 + 1) +#define KEY_D_2_252 (KEY_D_2_248 + 1) + +#define KEY_D_GYRO_BIAS_X (KEY_D_2_252 + 1) +#define KEY_D_GYRO_BIAS_Y (KEY_D_GYRO_BIAS_X + 1) +#define KEY_D_GYRO_BIAS_Z (KEY_D_GYRO_BIAS_Y + 1) +#define KEY_D_ACC_BIAS_X (KEY_D_GYRO_BIAS_Z + 1) +#define KEY_D_ACC_BIAS_Y (KEY_D_ACC_BIAS_X + 1) +#define KEY_D_ACC_BIAS_Z (KEY_D_ACC_BIAS_Y + 1) +#define KEY_D_GYRO_ENABLE (KEY_D_ACC_BIAS_Z + 1) +#define KEY_D_ACCEL_ENABLE (KEY_D_GYRO_ENABLE + 1) +#define KEY_D_QUAT_ENABLE (KEY_D_ACCEL_ENABLE + 1) +#define KEY_D_OUTPUT_ENABLE (KEY_D_QUAT_ENABLE + 1) +#define KEY_D_ACCEL_CNTR (KEY_D_OUTPUT_ENABLE + 1) +#define KEY_D_GYRO_CNTR (KEY_D_ACCEL_CNTR + 1) +#define KEY_D_QUAT0_CNTR (KEY_D_GYRO_CNTR + 1) +#define KEY_D_QUAT1_CNTR (KEY_D_QUAT0_CNTR + 1) +#define KEY_D_QUAT2_CNTR (KEY_D_QUAT1_CNTR + 1) +#define KEY_D_CR_TIME_G (KEY_D_QUAT2_CNTR + 1) +#define KEY_D_CR_TIME_A (KEY_D_CR_TIME_G + 1) +#define KEY_D_CR_TIME_Q (KEY_D_CR_TIME_A + 1) +#define KEY_D_CS_TAX (KEY_D_CR_TIME_Q + 1) +#define KEY_D_CS_TAY (KEY_D_CS_TAX + 1) +#define KEY_D_CS_TAZ (KEY_D_CS_TAY + 1) +#define KEY_D_CS_TGX (KEY_D_CS_TAZ + 1) +#define KEY_D_CS_TGY (KEY_D_CS_TGX + 1) +#define KEY_D_CS_TGZ (KEY_D_CS_TGY + 1) +#define KEY_D_CS_TQ0 (KEY_D_CS_TGZ + 1) +#define KEY_D_CS_TQ1 (KEY_D_CS_TQ0 + 1) +#define KEY_D_CS_TQ2 (KEY_D_CS_TQ1 + 1) +#define KEY_D_CS_TQ3 (KEY_D_CS_TQ2 + 1) + +/* Compass keys */ +#define KEY_CPASS_GAIN (KEY_D_CS_TQ3 + 1) +#define KEY_CPASS_BIAS_X (KEY_CPASS_GAIN + 1) +#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X + 1) +#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y + 1) +#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z + 1) +#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00 + 1) +#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01 + 1) +#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02 + 1) +#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10 + 1) +#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11 + 1) +#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12 + 1) +#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20 + 1) +#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21 + 1) + +/* Tap Keys */ +#define KEY_DMP_TAP_GATE (KEY_CPASS_MTX_22 + 1) +#define KEY_DMP_TAPW_MIN (KEY_DMP_TAP_GATE + 1) +#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAPW_MIN + 1) +#define KEY_DMP_TAP_PREV_JERK_Z (KEY_DMP_TAP_THR_Z + 1) +#define KEY_DMP_TAP_MIN_TAPS (KEY_DMP_TAP_PREV_JERK_Z + 1) +#define KEY_DMP_TAP_NEXT_TAP_THRES (KEY_DMP_TAP_MIN_TAPS + 1) +#define KEY_DMP_TAP_SHAKE_REJECT (KEY_DMP_TAP_NEXT_TAP_THRES + 1) +#define KEY_DMP_TAP_SHAKE_COUNT_MAX (KEY_DMP_TAP_SHAKE_REJECT + 1) +#define KEY_DMP_TAP_SHAKE_TIMEOUT_MAX (KEY_DMP_TAP_SHAKE_COUNT_MAX + 1) +#define KEY_DMP_TAP_DIRECTION (KEY_DMP_TAP_SHAKE_TIMEOUT_MAX + 1) +#define KEY_DMP_TAP_COUNT (KEY_DMP_TAP_DIRECTION + 1) +/* Gesture Keys */ +#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_COUNT + 1) +#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y + 1) +#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X + 1) +#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z + 1) +#define KEY_D_ACT0 (KEY_DMP_ORIENT + 1) +#define KEY_D_ACSX (KEY_D_ACT0 + 1) +#define KEY_D_ACSY (KEY_D_ACSX + 1) +#define KEY_D_ACSZ (KEY_D_ACSY + 1) + +#define KEY_CFG_DISPLAY_ORIENT_INT (KEY_D_ACSZ + 1) +#define KEY_NO_ORIENT_INTERRUPT (KEY_CFG_DISPLAY_ORIENT_INT + 1) +#define KEY_X_GRT_Y_TMP2 (KEY_NO_ORIENT_INTERRUPT + 1) + +/*Shake Keys */ +#define KEY_D_0_64 (KEY_X_GRT_Y_TMP2 + 1) +#define KEY_D_2_4 (KEY_D_0_64 + 1) +#define KEY_D_2_8 (KEY_D_2_4 + 1) +#define KEY_D_2_48 (KEY_D_2_8 + 1) +#define KEY_D_2_92 (KEY_D_2_48 + 1) +#define KEY_D_2_94 (KEY_D_2_92 + 1) +#define KEY_D_2_160 (KEY_D_2_94 + 1) +#define KEY_D_3_180 (KEY_D_2_160 + 1) +#define KEY_D_3_184 (KEY_D_3_180 + 1) +#define KEY_D_3_188 (KEY_D_3_184 + 1) +#define KEY_D_3_208 (KEY_D_3_188 + 1) +#define KEY_D_3_240 (KEY_D_3_208 + 1) +#define KEY_RETRACTION_1 (KEY_D_3_240 + 1) +#define KEY_RETRACTION_2 (KEY_RETRACTION_1 + 1) +#define KEY_RETRACTION_3 (KEY_RETRACTION_2 + 1) +#define KEY_RETRACTION_4 (KEY_RETRACTION_3 + 1) +#define KEY_CFG_SHAKE_INT (KEY_RETRACTION_4 + 1) + +/* Authenticate Keys */ +#define KEY_D_AUTH_OUT (KEY_CFG_SHAKE_INT + 1) +#define KEY_D_AUTH_IN (KEY_D_AUTH_OUT + 1) +#define KEY_D_AUTH_A (KEY_D_AUTH_IN + 1) +#define KEY_D_AUTH_B (KEY_D_AUTH_A + 1) + +/* Pedometer standalone only keys */ +#define KEY_D_PEDSTD_BP_B (KEY_D_AUTH_B + 1) +#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B + 1) +#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A + 1) +#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B + 1) +#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4 + 1) +#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3 + 1) +#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2 + 1) +#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1 + 1) +#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH + 1) +#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP + 1) +#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB + 1) +#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME + 1) +#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH + 1) +#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML + 1) +#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH + 1) +#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK + 1) +#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR + 1) +#define KEY_D_PEDSTD_STEPCTR2 (KEY_D_PEDSTD_STEPCTR + 1) +#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR2 + 1) +#define KEY_D_PEDSTD_DECI (KEY_D_PEDSTD_WALKTIME + 1) +#define KEY_D_PEDSTD_SB2 (KEY_D_PEDSTD_DECI + 1) +#define KEY_D_PEDSTD_DRIVE_STATE (KEY_D_PEDSTD_SB2 + 1) +#define KEY_CFG_PED_INT (KEY_D_PEDSTD_DRIVE_STATE + 1) +#define KEY_CFG_PED_ENABLE (KEY_CFG_PED_INT + 1) +#define KEY_D_STPDET_TIMESTAMP (KEY_CFG_PED_ENABLE + 1) + +/*Host Based No Motion*/ +#define KEY_D_HOST_NO_MOT (KEY_D_STPDET_TIMESTAMP + 1) + +/*Host Based Accel Bias*/ +#define KEY_D_ACCEL_BIAS (KEY_D_HOST_NO_MOT + 1) + +/*Screen/Display Orientation Keys*/ +#define KEY_D_ORIENT_GAP (KEY_D_ACCEL_BIAS + 1) +#define KEY_D_TILT0_H (KEY_D_ORIENT_GAP + 1) +#define KEY_D_TILT0_L (KEY_D_TILT0_H + 1) +#define KEY_D_TILT1_H (KEY_D_TILT0_L + 1) +#define KEY_D_TILT1_L (KEY_D_TILT1_H + 1) +#define KEY_D_TILT2_H (KEY_D_TILT1_L + 1) +#define KEY_D_TILT2_L (KEY_D_TILT2_H + 1) +#define KEY_D_TILT3_H (KEY_D_TILT2_L + 1) +#define KEY_D_TILT3_L (KEY_D_TILT3_H + 1) + +#define KEY_STREAM_P_ACCEL_Z (KEY_D_TILT3_L + 1) + +/* Batch mode */ +#define KEY_BM_ENABLE (KEY_STREAM_P_ACCEL_Z + 1) +#define KEY_BM_BATCH_THLD (KEY_BM_ENABLE + 1) +#define KEY_BM_BATCH_CNTR (KEY_BM_BATCH_THLD + 1) +#define KEY_BM_NUMWORD_TOFILL (KEY_BM_BATCH_CNTR + 1) + +/* Watermark */ +#define KEY_CFG_WATERMARK_H (KEY_BM_NUMWORD_TOFILL + 1) +#define KEY_CFG_WATERMARK_L (KEY_CFG_WATERMARK_H + 1) + +/* FIFO output control */ +#define KEY_CFG_OUT_ACCL (KEY_CFG_WATERMARK_L + 1) +#define KEY_CFG_OUT_GYRO (KEY_CFG_OUT_ACCL + 1) +#define KEY_CFG_OUT_3QUAT (KEY_CFG_OUT_GYRO + 1) +#define KEY_CFG_OUT_6QUAT (KEY_CFG_OUT_3QUAT + 1) +#define KEY_CFG_OUT_9QUAT (KEY_CFG_OUT_6QUAT + 1) +#define KEY_CFG_OUT_PQUAT (KEY_CFG_OUT_9QUAT + 1) +#define KEY_CFG_OUT_PQUAT9 (KEY_CFG_OUT_PQUAT + 1) +#define KEY_CFG_OUT_CPASS (KEY_CFG_OUT_PQUAT9 + 1) +#define KEY_CFG_OUT_PRESS (KEY_CFG_OUT_CPASS + 1) +#define KEY_CFG_OUT_STEPDET (KEY_CFG_OUT_PRESS + 1) +#define KEY_CFG_FIFO_INT (KEY_CFG_OUT_STEPDET + 1) + +/* Ped Step detection */ +#define KEY_CFG_PEDSTEP_DET (KEY_CFG_FIFO_INT + 1) + +/* Screen Orientation data */ +#define KEY_SO_DATA (KEY_CFG_PEDSTEP_DET + 1) + +/* MPU for DMP Android K */ +#define KEY_P_INIT (KEY_SO_DATA + 1) +#define KEY_P_HW_ID (KEY_P_INIT + 1) + +/* DMP running counter */ +#define KEY_DMP_RUN_CNTR (KEY_P_HW_ID + 1) + +/* Sensor's ODR */ +#define KEY_CFG_3QUAT_ODR (KEY_DMP_RUN_CNTR + 1) +#define KEY_CFG_6QUAT_ODR (KEY_CFG_3QUAT_ODR + 1) +#define KEY_CFG_9QUAT_ODR (KEY_CFG_6QUAT_ODR + 1) +#define KEY_CFG_PQUAT6_ODR (KEY_CFG_9QUAT_ODR + 1) +#define KEY_CFG_PQUAT9_ODR (KEY_CFG_PQUAT6_ODR + 1) +#define KEY_CFG_ACCL_ODR (KEY_CFG_PQUAT9_ODR + 1) +#define KEY_CFG_GYRO_ODR (KEY_CFG_ACCL_ODR + 1) +#define KEY_CFG_CPASS_ODR (KEY_CFG_GYRO_ODR + 1) +#define KEY_CFG_PRESS_ODR (KEY_CFG_CPASS_ODR + 1) + +#define KEY_ODR_CNTR_3QUAT (KEY_CFG_PRESS_ODR + 1) +#define KEY_ODR_CNTR_6QUAT (KEY_ODR_CNTR_3QUAT + 1) +#define KEY_ODR_CNTR_9QUAT (KEY_ODR_CNTR_6QUAT + 1) +#define KEY_ODR_CNTR_PQUAT (KEY_ODR_CNTR_9QUAT + 1) +#define KEY_ODR_CNTR_PQUAT9 (KEY_ODR_CNTR_PQUAT + 1) +#define KEY_ODR_CNTR_ACCL (KEY_ODR_CNTR_PQUAT9 + 1) +#define KEY_ODR_CNTR_GYRO (KEY_ODR_CNTR_ACCL + 1) +#define KEY_ODR_CNTR_CPASS (KEY_ODR_CNTR_GYRO + 1) +#define KEY_ODR_CNTR_PRESS (KEY_ODR_CNTR_CPASS + 1) + +/* DMP fusion LP-Quat */ +#define KEY_DMP_LPQ0 (KEY_ODR_CNTR_PRESS + 1) +#define KEY_DMP_LPQ1 (KEY_DMP_LPQ0 + 1) +#define KEY_DMP_LPQ2 (KEY_DMP_LPQ1 + 1) +#define KEY_DMP_LPQ3 (KEY_DMP_LPQ2 + 1) + +/* DMP fusion 6-axis Quat */ +#define KEY_DMP_Q0 (KEY_DMP_LPQ3 + 1) +#define KEY_DMP_Q1 (KEY_DMP_Q0 + 1) +#define KEY_DMP_Q2 (KEY_DMP_Q1 + 1) +#define KEY_DMP_Q3 (KEY_DMP_Q2 + 1) + +/* 9-axis fusion */ +#define KEY_CPASS_VALID (KEY_DMP_Q3 + 1) +#define KEY_9AXIS_ACCURACY (KEY_CPASS_VALID + 1) +#define KEY_DMP_9Q0 (KEY_9AXIS_ACCURACY + 1) +#define KEY_DMP_9Q1 (KEY_DMP_9Q0 + 1) +#define KEY_DMP_9Q2 (KEY_DMP_9Q1 + 1) +#define KEY_DMP_9Q3 (KEY_DMP_9Q2 + 1) + +/* Test key */ +#define KEY_TEST_01 (KEY_9AXIS_ACCURACY + 1) +#define KEY_TEST_02 (KEY_TEST_01 + 1) +#define KEY_TEST_03 (KEY_TEST_02 + 1) +#define KEY_TEST_04 (KEY_TEST_03 + 1) +#define KEY_TEST_05 (KEY_TEST_04 + 1) +#define KEY_TEST_06 (KEY_TEST_05 + 1) +#define KEY_TEST_07 (KEY_TEST_06 + 1) +#define KEY_TEST_XX (KEY_TEST_07 + 1) + +#define NUM_KEYS (KEY_TEST_XX + 1) + +struct tKeyLabel { + unsigned short key; + unsigned short addr; +}; + +#define DINA0A 0x0a +#define DINA22 0x22 +#define DINA42 0x42 +#define DINA5A 0x5a + +#define DINA06 0x06 +#define DINA0E 0x0e +#define DINA16 0x16 +#define DINA1E 0x1e +#define DINA26 0x26 +#define DINA2E 0x2e +#define DINA36 0x36 +#define DINA3E 0x3e +#define DINA46 0x46 +#define DINA4E 0x4e +#define DINA56 0x56 +#define DINA5E 0x5e +#define DINA66 0x66 +#define DINA6E 0x6e +#define DINA76 0x76 +#define DINA7E 0x7e + +#define DINA00 0x00 +#define DINA08 0x08 +#define DINA10 0x10 +#define DINA18 0x18 +#define DINA20 0x20 +#define DINA28 0x28 +#define DINA30 0x30 +#define DINA38 0x38 +#define DINA40 0x40 +#define DINA48 0x48 +#define DINA50 0x50 +#define DINA58 0x58 +#define DINA60 0x60 +#define DINA68 0x68 +#define DINA70 0x70 +#define DINA78 0x78 + +#define DINA04 0x04 +#define DINA0C 0x0c +#define DINA14 0x14 +#define DINA1C 0x1C +#define DINA24 0x24 +#define DINA2C 0x2c +#define DINA34 0x34 +#define DINA3C 0x3c +#define DINA44 0x44 +#define DINA4C 0x4c +#define DINA54 0x54 +#define DINA5C 0x5c +#define DINA64 0x64 +#define DINA6C 0x6c +#define DINA74 0x74 +#define DINA7C 0x7c + +#define DINA01 0x01 +#define DINA09 0x09 +#define DINA11 0x11 +#define DINA19 0x19 +#define DINA21 0x21 +#define DINA29 0x29 +#define DINA31 0x31 +#define DINA39 0x39 +#define DINA41 0x41 +#define DINA49 0x49 +#define DINA51 0x51 +#define DINA59 0x59 +#define DINA61 0x61 +#define DINA69 0x69 +#define DINA71 0x71 +#define DINA79 0x79 + +#define DINA25 0x25 +#define DINA2D 0x2d +#define DINA35 0x35 +#define DINA3D 0x3d +#define DINA4D 0x4d +#define DINA55 0x55 +#define DINA5D 0x5D +#define DINA6D 0x6d +#define DINA75 0x75 +#define DINA7D 0x7d + +#define DINADC 0xdc +#define DINAF2 0xf2 +#define DINAAB 0xab +#define DINAAA 0xaa +#define DINAF1 0xf1 +#define DINADF 0xdf +#define DINADA 0xda +#define DINAB1 0xb1 +#define DINAB9 0xb9 +#define DINAF3 0xf3 +#define DINA8B 0x8b +#define DINAA3 0xa3 +#define DINA91 0x91 +#define DINAB6 0xb6 +#define DINAB4 0xb4 + +#define DINC00 0x00 +#define DINC01 0x01 +#define DINC02 0x02 +#define DINC03 0x03 +#define DINC08 0x08 +#define DINC09 0x09 +#define DINC0A 0x0a +#define DINC0B 0x0b +#define DINC10 0x10 +#define DINC11 0x11 +#define DINC12 0x12 +#define DINC13 0x13 +#define DINC18 0x18 +#define DINC19 0x19 +#define DINC1A 0x1a +#define DINC1B 0x1b + +#define DINC20 0x20 +#define DINC21 0x21 +#define DINC22 0x22 +#define DINC23 0x23 +#define DINC28 0x28 +#define DINC29 0x29 +#define DINC2A 0x2a +#define DINC2B 0x2b +#define DINC30 0x30 +#define DINC31 0x31 +#define DINC32 0x32 +#define DINC33 0x33 +#define DINC38 0x38 +#define DINC39 0x39 +#define DINC3A 0x3a +#define DINC3B 0x3b + +#define DINC40 0x40 +#define DINC41 0x41 +#define DINC42 0x42 +#define DINC43 0x43 +#define DINC48 0x48 +#define DINC49 0x49 +#define DINC4A 0x4a +#define DINC4B 0x4b +#define DINC50 0x50 +#define DINC51 0x51 +#define DINC52 0x52 +#define DINC53 0x53 +#define DINC58 0x58 +#define DINC59 0x59 +#define DINC5A 0x5a +#define DINC5B 0x5b + +#define DINC60 0x60 +#define DINC61 0x61 +#define DINC62 0x62 +#define DINC63 0x63 +#define DINC68 0x68 +#define DINC69 0x69 +#define DINC6A 0x6a +#define DINC6B 0x6b +#define DINC70 0x70 +#define DINC71 0x71 +#define DINC72 0x72 +#define DINC73 0x73 +#define DINC78 0x78 +#define DINC79 0x79 +#define DINC7A 0x7a +#define DINC7B 0x7b +#define DIND40 0x40 +#define DINA80 0x80 +#define DINA90 0x90 +#define DINAA0 0xa0 +#define DINAC9 0xc9 +#define DINACB 0xcb +#define DINACD 0xcd +#define DINACF 0xcf +#define DINAC8 0xc8 +#define DINACA 0xca +#define DINACC 0xcc +#define DINACE 0xce +#define DINAD8 0xd8 +#define DINADD 0xdd +#define DINAF8 0xf0 +#define DINAFE 0xfe + +#define DINBF8 0xf8 +#define DINAC0 0xb0 +#define DINAC1 0xb1 +#define DINAC2 0xb4 +#define DINAC3 0xb5 +#define DINAC4 0xb8 +#define DINAC5 0xb9 +#define DINBC0 0xc0 +#define DINBC2 0xc2 +#define DINBC4 0xc4 +#define DINBC6 0xc6 + +#endif diff --git a/drivers/iio/imu/inv_mpu/dmpmap.h b/drivers/iio/imu/inv_mpu/dmpmap.h new file mode 100644 index 000000000000..929363722241 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/dmpmap.h @@ -0,0 +1,263 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#ifndef DMPMAP_H +#define DMPMAP_H + +#define DMP_PTAT 0 +#define DMP_XGYR 2 +#define DMP_YGYR 4 +#define DMP_ZGYR 6 +#define DMP_XACC 8 +#define DMP_YACC 10 +#define DMP_ZACC 12 +#define DMP_ADC1 14 +#define DMP_ADC2 16 +#define DMP_ADC3 18 +#define DMP_BIASUNC 20 +#define DMP_FIFORT 22 +#define DMP_INVGSFH 24 +#define DMP_INVGSFL 26 +#define DMP_1H 28 +#define DMP_1L 30 +#define DMP_BLPFSTCH 32 +#define DMP_BLPFSTCL 34 +#define DMP_BLPFSXH 36 +#define DMP_BLPFSXL 38 +#define DMP_BLPFSYH 40 +#define DMP_BLPFSYL 42 +#define DMP_BLPFSZH 44 +#define DMP_BLPFSZL 46 +#define DMP_BLPFMTC 48 +#define DMP_SMC 50 +#define DMP_BLPFMXH 52 +#define DMP_BLPFMXL 54 +#define DMP_BLPFMYH 56 +#define DMP_BLPFMYL 58 +#define DMP_BLPFMZH 60 +#define DMP_BLPFMZL 62 +#define DMP_BLPFC 64 +#define DMP_SMCTH 66 +#define DMP_TAP_DIRECTION 68 +#define DMP_TAP_COUNT 70 +#define DMP_TAP_GATE 72 +#define DMP_TAP_MIN_TAPS 74 +#define DMP_BERR2NH 76 +#define DMP_SMCINC 78 +#define DMP_TAP_SHAKE_REJECT 80 +#define DMP_ANGVBXL 82 +#define DMP_TAP_SHAKE_COUNT_MAX 84 +#define DMP_TAP_SHAKE_TIMEOUT_MAX 86 +#define DMP_TAP_THZ 88 +#define DMP_TAPW_MIN 90 +#define DMP_TAP_PREV_JERK_Z 92 +#define DMP_TAP_NEXT_TAP_THRES 94 +#define DMP_ATCH 96 +#define DMP_BIASUNCSF 98 +#define DMP_ACT2H 100 +#define DMP_ACT2L 102 +#define DMP_GSFH 104 +#define DMP_GSFL 106 +#define DMP_GH 108 +#define DMP_GL 110 +#define DMP_0_5H 112 +#define DMP_0_5L 114 +#define DMP_0_0H 116 +#define DMP_0_0L 118 +#define DMP_1_0H 120 +#define DMP_1_0L 122 +#define DMP_1_5H 124 +#define DMP_1_5L 126 +#define DMP_TMP1AH 128 +#define DMP_TMP1AL 130 +#define DMP_TMP2AH 132 +#define DMP_TMP2AL 134 +#define DMP_TMP3AH 136 +#define DMP_TMP3AL 138 +#define DMP_TMP4AH 140 +#define DMP_TMP4AL 142 +#define DMP_XACCW 144 +#define DMP_TMP5 146 +#define DMP_XACCB 148 +#define DMP_TMP8 150 +#define DMP_YACCB 152 +#define DMP_TMP9 154 +#define DMP_ZACCB 156 +#define DMP_TMP10 158 +#define DMP_DZH 160 +#define DMP_DZL 162 +#define DMP_XGCH 164 +#define DMP_XGCL 166 +#define DMP_YGCH 168 +#define DMP_YGCL 170 +#define DMP_ZGCH 172 +#define DMP_ZGCL 174 +#define DMP_YACCW 176 +#define DMP_TMP7 178 +#define DMP_AFB1H 180 +#define DMP_AFB1L 182 +#define DMP_AFB2H 184 +#define DMP_AFB2L 186 +#define DMP_MAGFBH 188 +#define DMP_MAGFBL 190 +#define DMP_QT1H 192 +#define DMP_QT1L 194 +#define DMP_QT2H 196 +#define DMP_QT2L 198 +#define DMP_QT3H 200 +#define DMP_QT3L 202 +#define DMP_QT4H 204 +#define DMP_QT4L 206 +#define DMP_CTRL1H 208 +#define DMP_CTRL1L 210 +#define DMP_CTRL2H 212 +#define DMP_CTRL2L 214 +#define DMP_CTRL3H 216 +#define DMP_CTRL3L 218 +#define DMP_CTRL4H 220 +#define DMP_CTRL4L 222 +#define DMP_CTRLS1 224 +#define DMP_CTRLSF1 226 +#define DMP_CTRLS2 228 +#define DMP_CTRLSF2 230 +#define DMP_CTRLS3 232 +#define DMP_CTRLSFNLL 234 +#define DMP_CTRLS4 236 +#define DMP_CTRLSFNL2 238 +#define DMP_CTRLSFNL 240 +#define DMP_TMP30 242 +#define DMP_CTRLSFJT 244 +#define DMP_TMP31 246 +#define DMP_TMP11 248 +#define DMP_CTRLSF2_2 250 +#define DMP_TMP12 252 +#define DMP_CTRLSF1_2 254 +#define DMP_PREVPTAT 256 +#define DMP_ACCZB 258 +#define DMP_ACCXB 264 +#define DMP_ACCYB 266 +#define DMP_1HB 272 +#define DMP_1LB 274 +#define DMP_0H 276 +#define DMP_0L 278 +#define DMP_ASR22H 280 +#define DMP_ASR22L 282 +#define DMP_ASR6H 284 +#define DMP_ASR6L 286 +#define DMP_TMP13 288 +#define DMP_TMP14 290 +#define DMP_FINTXH 292 +#define DMP_FINTXL 294 +#define DMP_FINTYH 296 +#define DMP_FINTYL 298 +#define DMP_FINTZH 300 +#define DMP_FINTZL 302 +#define DMP_TMP1BH 304 +#define DMP_TMP1BL 306 +#define DMP_TMP2BH 308 +#define DMP_TMP2BL 310 +#define DMP_TMP3BH 312 +#define DMP_TMP3BL 314 +#define DMP_TMP4BH 316 +#define DMP_TMP4BL 318 +#define DMP_STXG 320 +#define DMP_ZCTXG 322 +#define DMP_STYG 324 +#define DMP_ZCTYG 326 +#define DMP_STZG 328 +#define DMP_ZCTZG 330 +#define DMP_CTRLSFJT2 332 +#define DMP_CTRLSFJTCNT 334 +#define DMP_PVXG 336 +#define DMP_TMP15 338 +#define DMP_PVYG 340 +#define DMP_TMP16 342 +#define DMP_PVZG 344 +#define DMP_TMP17 346 +#define DMP_MNMFLAGH 352 +#define DMP_MNMFLAGL 354 +#define DMP_MNMTMH 356 +#define DMP_MNMTML 358 +#define DMP_MNMTMTHRH 360 +#define DMP_MNMTMTHRL 362 +#define DMP_MNMTHRH 364 +#define DMP_MNMTHRL 366 +#define DMP_ACCQD4H 368 +#define DMP_ACCQD4L 370 +#define DMP_ACCQD5H 372 +#define DMP_ACCQD5L 374 +#define DMP_ACCQD6H 376 +#define DMP_ACCQD6L 378 +#define DMP_ACCQD7H 380 +#define DMP_ACCQD7L 382 +#define DMP_ACCQD0H 384 +#define DMP_ACCQD0L 386 +#define DMP_ACCQD1H 388 +#define DMP_ACCQD1L 390 +#define DMP_ACCQD2H 392 +#define DMP_ACCQD2L 394 +#define DMP_ACCQD3H 396 +#define DMP_ACCQD3L 398 +#define DMP_XN2H 400 +#define DMP_XN2L 402 +#define DMP_XN1H 404 +#define DMP_XN1L 406 +#define DMP_YN2H 408 +#define DMP_YN2L 410 +#define DMP_YN1H 412 +#define DMP_YN1L 414 +#define DMP_YH 416 +#define DMP_YL 418 +#define DMP_B0H 420 +#define DMP_B0L 422 +#define DMP_A1H 424 +#define DMP_A1L 426 +#define DMP_A2H 428 +#define DMP_A2L 430 +#define DMP_SEM1 432 +#define DMP_FIFOCNT 434 +#define DMP_SH_TH_X 436 +#define DMP_PACKET 438 +#define DMP_SH_TH_Y 440 +#define DMP_FOOTER 442 +#define DMP_SH_TH_Z 444 +#define DMP_TEMP29 448 +#define DMP_TEMP30 450 +#define DMP_XACCB_PRE 452 +#define DMP_XACCB_PREL 454 +#define DMP_YACCB_PRE 456 +#define DMP_YACCB_PREL 458 +#define DMP_ZACCB_PRE 460 +#define DMP_ZACCB_PREL 462 +#define DMP_TMP22 464 +#define DMP_TAP_TIMER 466 +#define DMP_TAP_THX 468 +#define DMP_TAP_THY 472 +#define DMP_TMP25 480 +#define DMP_TMP26 482 +#define DMP_TMP27 484 +#define DMP_TMP28 486 +#define DMP_ORIENT 488 +#define DMP_THRSH 490 +#define DMP_ENDIANH 492 +#define DMP_ENDIANL 494 +#define DMP_BLPFNMTCH 496 +#define DMP_BLPFNMTCL 498 +#define DMP_BLPFNMXH 500 +#define DMP_BLPFNMXL 502 +#define DMP_BLPFNMYH 504 +#define DMP_BLPFNMYL 506 +#define DMP_BLPFNMZH 508 +#define DMP_BLPFNMZL 510 + +#endif diff --git a/drivers/iio/imu/inv_mpu/inv_compass/Kconfig b/drivers/iio/imu/inv_mpu/inv_compass/Kconfig new file mode 100644 index 000000000000..b5c8c44fd7ed --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/Kconfig @@ -0,0 +1,36 @@ +# +# Kconfig for Invensense IIO compass drivers of 3rd party compass devices. +# + +# Yamaha YAS530/YAS532/YAS533 +config INV_YAS53X_IIO + tristate "Invensense IIO driver for Yamaha YAS530/YAS532/YAS533 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Yamaha YAS530/YAS532/YAS533. It is the Invensense + implementation of YAS53x series compass devices. + This driver can be built as a module. The module will be called + inv_yas53x_iio. + +# Aichi AMI306 +config INV_AMI306_IIO + tristate "Invensense IIO driver for Aichi AMI306 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Aichi AMI306 compass. It is the Invensense + IIO implementation for the AMI306 compass device. + This driver can be built as a module. The module will be called + inv-ami306-iio. + +# Asahi Kasei AK8975/AK8972/AK8963 +config INV_AK89XX_IIO + tristate "Invensense IIO driver for Asahi Kasei AK8975/AK8972/AK8963 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Asahi Kasei AK8975/AK8972/AK8963 compasses. It is the Invensense + IIO implementation of AK89xx series compass devices. + This driver can be built as a module. The module will be called + inv-ak89xx-iio.
\ No newline at end of file diff --git a/drivers/iio/imu/inv_mpu/inv_compass/Makefile b/drivers/iio/imu/inv_mpu/inv_compass/Makefile new file mode 100644 index 000000000000..47c4271d3744 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/Makefile @@ -0,0 +1,36 @@ +# +# Makefile for Invensense IIO compass drivers of 3rd party compass devices. +# + +# Yamaha YAS530/YAS532/YAS533 +obj-$(CONFIG_INV_YAS53X_IIO) += inv_yas53x.o + +inv_yas53x-objs := inv_yas53x_core.o +inv_yas53x-objs += inv_yas53x_ring.o +inv_yas53x-objs += inv_yas53x_trigger.o + +CFLAGS_inv_yas53x_core.o += -Idrivers/staging/iio +CFLAGS_inv_yas53x_ring.o += -Idrivers/staging/iio +CFLAGS_inv_yas53x_trigger.o += -Idrivers/staging/iio + +# Aichi AMI306 +obj-$(CONFIG_INV_AMI306_IIO) += inv-ami306-iio.o + +inv-ami306-iio-objs := inv_ami306_core.o +inv-ami306-iio-objs += inv_ami306_ring.o +inv-ami306-iio-objs += inv_ami306_trigger.o + +CFLAGS_inv_ami306_core.o += -Idrivers/staging/iio +CFLAGS_inv_ami306_ring.o += -Idrivers/staging/iio +CFLAGS_inv_ami306_trigger.o += -Idrivers/staging/iio + +# Asahi Kasei AK8975/AK8972/AK8963 +obj-$(CONFIG_INV_AK89XX_IIO) += inv-ak89xx-iio.o + +inv-ak89xx-iio-objs := inv_ak89xx_core.o +inv-ak89xx-iio-objs += inv_ak89xx_ring.o +inv-ak89xx-iio-objs += inv_ak89xx_trigger.o + +CFLAGS_inv_ak89xx_core.o += -Idrivers/staging/iio +CFLAGS_inv_ak89xx_ring.o += -Idrivers/staging/iio +CFLAGS_inv_ak89xx_trigger.o += -Idrivers/staging/iio
\ No newline at end of file diff --git a/drivers/iio/imu/inv_mpu/inv_compass/README b/drivers/iio/imu/inv_mpu/inv_compass/README new file mode 100644 index 000000000000..54f2bb8ded2e --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/README @@ -0,0 +1,176 @@ +Kernel driver +Author: Invensense <http://invensense.com> + +Table of Contents: +================== +- Description +- Integrating the Driver in the Linux Kernel +- Board and Platform Data + > Platform Data +- Board File Modifications for compass + > AMI306 + > YAS530/532/533 +- IIO Subsystem + > Communicating with the Driver in Userspace +- Streaming Data to an Userspace Application +- Test Applications + > Running Test Applications with AMI306 or YAS53x + +Description +=========== +This document describes how to install the Invensense device driver for AMI306 +and YAS53x series compass chip into a Linux kernel. The Invensense driver +currently supports the following sensors: +- AMI306 +- YAS530 +- YAS532 +- YAS533 + +Please refer to the appropriate product specification +document for further information regarding the slave address. + +The following files are included in this package: +- Kconfig +- Makefile +- inv_ami306_core.c +- inv_ami306_ring.c +- inv_ami306_trigger.c +- inv_ami306_iio.h +- inv_yas53x_core.c +- inv_yas53x_ring.c +- inv_yas53x_trigger.c +- inv_yas53x_iio.h + +Integrating the Driver in the Linux Kernel +========================================== +Please add the files as follows: +- Add all above files to drivers/staging/iio/magnetometer/inv_compass +(another directory is acceptable, but this is the recommended destination) + +In order to see the driver in menuconfig when building the kernel, please +make modifications as shown below: + + modify "drivers/staging/iio/magnetometer/Kconfig" with: + >> source "drivers/staging/iio/magnetometer/inv_compass/Kconfig" + + modify "drivers/staging/iio/magnetometer/Makefile" with: + >> obj-y += inv_compass/ + + +Board and Platform Data +======================= +In order to recognize the Invensense device on the I2C bus, the board file must +be modified. +The i2c_board_info instance must be defined as shown below. + +Platform Data +------------- +The platform data (orientation matrix and secondary bus configurations) must be +modified as show below, according to your particular platform configuration. + +Board File Modifications for Secondary I2C Configuration +======================================================== +For the Panda Board, the board file can be found at +arch/arm/mach-omap2/board-omap4panda.c. +Please modify the pertinent baord file in your system according to the examples +shown below: + +AMI306 +------------------------------------------------- +static struct mpu_platform_data compass_data = { + .orientation = { 0, 0, 1, + 0, 1, 0, + 1, 0, 0 }, +}; + +static struct i2c_board_info __initdata chip_board_info[] = { + { + I2C_BOARD_INFO("ami306", 0x0E), + .platform_data = &compass_data, + }, +}; + +YAS53x(Use YAS532 as an example) +------------------------------------------------- +static struct mpu_platform_data compass_data = { + .orientation = { 0, -1, 0, + 1, 0, 0, + 0, 0, 1 }, +}; + +static struct i2c_board_info __initdata compass_board_info[] = { + { + I2C_BOARD_INFO("yas532", 0x2E), + .platform_data = &compass_data, + }, +}; + +IIO subsystem +============= +A successful installation will create the following two new directories under +/sys/bus/iio/devices: + - iio:device0 + - trigger0 + +Also, a new file, "iio:device0", will be created in the /dev/ diretory. +(if you have more than one IIO device, the file will be named "iio:deviceX", +where X is a number) + + +Communicating with the Driver in Userspace +------------------------------------------ +The driver generates several files in sysfs upon installation. +These files are used to communicate with the driver. The files can be found +at /sys/bus/iio/devices/iio:device0 (or ../iio:deviceX as shown above). + +A brief description of the pertinent files for each Invensense device is shown +below: + +AMI306 +-------- +compass_matrix (read-only) +--show the orientation matrix obtained from the board file. + +sampling_frequency(read and write) +--show and change the sampling rate of the sensor. + +YAS53x +--------------------- +YAS53x has all the attributes AMI306 has. It has one more additional attribute: + +overunderflow(read-only) +--value 1 shows an overflow or underflow happens. Need to write into it to make + it zero. + +Streaming Data to an Userspace Application +========================================== +When streaming data to an userspace application, we recommend that you access +compass data via /dev/iio:device0. + +Please follow the steps below to read data at a constant rate from the driver: + +1. Write the desired output rate to sampling_frequency. +2. Write 1 to enable to turn on the event. +3. Read /dev/iio:device0 to get a string of gyro/accel/compass data. +4. Parse this string to obtain each compass element. + +Test Applications +================= +A test application is located under software/simple_apps/mpu_iio. +This application is stand-alone in that it cannot be run concurrently with other +entities trying to access the device node(s) or sysfs entries; in particular, +the + +Running Test Applications +--------------------------------------------------------- +To run test applications with AMI306 or YAS53x devices, +please use the following commands: + +1. for ami306: + mpu_iio -n ami306 -c 10 -l 3 + +2. for yas532: + mpu_iio -n yas532 -c 10 -l 3 + +Please use mpu_iio.c and iio_utils.h as example code for your development +purposes. diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_core.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_core.c new file mode 100644 index 000000000000..a781fd398225 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_core.c @@ -0,0 +1,590 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_ak89xx_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +static s64 get_time_ns(void) +{ + struct timespec ts; + ktime_get_ts(&ts); + return timespec_to_ns(&ts); +} + +/** + * inv_serial_read() - Read one or more bytes from the device registers. + * @st: Device driver instance. + * @reg: First device register to be read from. + * @length: Number of bytes to read. + * @data: Data read from device. + * NOTE: The slave register will not increment when reading from the FIFO. + */ +int inv_serial_read(struct inv_ak89xx_state_s *st, u8 reg, u16 length, u8 *data) +{ + int result; + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + result = i2c_smbus_read_i2c_block_data(st->i2c, reg, length, data); + if (result != length) { + if (result < 0) + return result; + else + return -EINVAL; + } else { + return 0; + } +} + +/** + * inv_serial_single_write() - Write a byte to a device register. + * @st: Device driver instance. + * @reg: Device register to be written to. + * @data: Byte to write to device. + */ +int inv_serial_single_write(struct inv_ak89xx_state_s *st, u8 reg, u8 data) +{ + u8 d[1]; + d[0] = data; + INV_I2C_INC_COMPASSWRITE(3); + + return i2c_smbus_write_i2c_block_data(st->i2c, reg, 1, d); +} + +static int ak89xx_init(struct inv_ak89xx_state_s *st) +{ + int result = 0; + unsigned char serial_data[3]; + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_POWER_DOWN); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + /* Wait at least 100us */ + udelay(100); + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_FUSE_ACCESS); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + /* Wait at least 200us */ + udelay(200); + + result = inv_serial_read(st, AK89XX_FUSE_ASAX, 3, serial_data); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + st->asa[0] = serial_data[0]; + st->asa[1] = serial_data[1]; + st->asa[2] = serial_data[2]; + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_POWER_DOWN); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + udelay(100); + + return result; +} + +int ak89xx_read(struct inv_ak89xx_state_s *st, short rawfixed[3]) +{ + unsigned char regs[8]; + unsigned char *stat = ®s[0]; + unsigned char *stat2 = ®s[7]; + int result = 0; + int status = 0; + + result = inv_serial_read(st, AK89XX_REG_ST1, 8, regs); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + rawfixed[0] = (short)((regs[2]<<8) | regs[1]); + rawfixed[1] = (short)((regs[4]<<8) | regs[3]); + rawfixed[2] = (short)((regs[6]<<8) | regs[5]); + + /* + * ST : data ready - + * Measurement has been completed and data is ready to be read. + */ + if (*stat & 0x01) + status = 0; + + /* + * ST2 : data error - + * occurs when data read is started outside of a readable period; + * data read would not be correct. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour but we + * stil account for it and return an error, since the data would be + * corrupted. + * DERR bit is self-clearing when ST2 register is read. + */ + if (*stat2 & 0x04) + status = 0x04; + /* + * ST2 : overflow - + * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. + * This is likely to happen in presence of an external magnetic + * disturbance; it indicates, the sensor data is incorrect and should + * be ignored. + * An error is returned. + * HOFL bit clears when a new measurement starts. + */ + if (*stat2 & 0x08) + status = 0x08; + /* + * ST : overrun - + * the previous sample was not fetched and lost. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour and we + * don't consider this condition an error. + * DOR bit is self-clearing when ST2 or any meas. data register is + * read. + */ + if (*stat & 0x02) { + /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */ + status = 0; + } + + /* + * trigger next measurement if: + * - stat is non zero; + * - if stat is zero and stat2 is non zero. + * Won't trigger if data is not ready and there was no error. + */ + if (1) { + unsigned char scale = 0; + if (st->compass_id == COMPASS_ID_AK8963) + scale = st->compass_scale; + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + } else + pr_err("%s, no next measure(0x%x,0x%x)\n", __func__, + *stat, *stat2); + + if (status) + pr_err("%s, line=%d, status=%d\n", __func__, __LINE__, status); + + return status; +} + +/** + * ak89xx_read_raw() - read raw method. + */ +static int ak89xx_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int scale = 0; + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + if (st->compass_id == COMPASS_ID_AK8975) + scale = 9830; + else if (st->compass_id == COMPASS_ID_AK8972) + scale = 19661; + else if (st->compass_id == COMPASS_ID_AK8963) { + if (st->compass_scale) + scale = 4915; /* 16 bit */ + else + scale = 19661; /* 14 bit */ + } + scale *= (1L << 15); + *val = scale; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +static ssize_t ak89xx_value_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + short c[3]; + + mutex_lock(&indio_dev->mlock); + c[0] = st->compass_data[0]; + c[1] = st->compass_data[1]; + c[2] = st->compass_data[2]; + mutex_unlock(&indio_dev->mlock); + return sprintf(buf, "%d, %d, %d\n", c[0], c[1], c[2]); +} + +static ssize_t ak89xx_scale_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int scale = 0; + + if (st->compass_id == COMPASS_ID_AK8975) + scale = 9830; + else if (st->compass_id == COMPASS_ID_AK8972) + scale = 19661; + else if (st->compass_id == COMPASS_ID_AK8963) { + if (st->compass_scale) + scale = 4915; /* 16 bit */ + else + scale = 19661; /* 14 bit */ + } + scale *= (1L << 15); + return sprintf(buf, "%d\n", scale); +} + +static ssize_t ak89xx_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", (1000 / st->delay)); +} + +/** + * ak89xx_matrix_show() - show orientation matrix + */ +static ssize_t ak89xx_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int result = 0; + unsigned char scale = 0; + + if (st->compass_id == COMPASS_ID_AK8963) + scale = st->compass_scale; + + if (enable) { + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE); + if (result) + pr_err("%s, line=%d\n", __func__, __LINE__); + schedule_delayed_work(&st->work, + msecs_to_jiffies(st->delay)); + } else { + cancel_delayed_work_sync(&st->work); + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_POWER_DOWN); + if (result) + pr_err("%s, line=%d\n", __func__, __LINE__); + mdelay(1); /* wait at least 100us */ + } +} + +static ssize_t ak89xx_scale_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + unsigned long data, result; + + result = kstrtoul(buf, 10, &data); + if (result) + return result; + if (st->compass_id == COMPASS_ID_AK8963) + st->compass_scale = !!data; + return count; +} + +static ssize_t ak89xx_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + error = kstrtoul(buf, 10, &data); + if (error) + return error; + /* transform rate to delay in ms */ + data = 1000 / data; + if (data > AK89XX_MAX_DELAY) + data = AK89XX_MAX_DELAY; + if (data < AK89XX_MIN_DELAY) + data = AK89XX_MIN_DELAY; + st->delay = (unsigned int) data; + return count; +} + +static void ak89xx_work_func(struct work_struct *work) +{ + struct inv_ak89xx_state_s *st = + container_of((struct delayed_work *)work, + struct inv_ak89xx_state_s, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + unsigned long delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + st->timestamp = get_time_ns(); + schedule_delayed_work(&st->work, delay); + inv_read_ak89xx_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_AK89XX_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(value, S_IRUGO, ak89xx_value_show, NULL); +static DEVICE_ATTR(scale, S_IRUGO | S_IWUSR, ak89xx_scale_show, + ak89xx_scale_store); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, ak89xx_rate_show, + ak89xx_rate_store); +static DEVICE_ATTR(compass_matrix, S_IRUGO, ak89xx_matrix_show, NULL); + +static struct attribute *inv_ak89xx_attributes[] = { + &dev_attr_value.attr, + &dev_attr_scale.attr, + &dev_attr_sampling_frequency.attr, + &dev_attr_compass_matrix.attr, + NULL, +}; + +static const struct attribute_group inv_attribute_group = { + .name = "ak89xx", + .attrs = inv_ak89xx_attributes +}; + +static const struct iio_info ak89xx_info = { + .driver_module = THIS_MODULE, + .read_raw = &ak89xx_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_ak89xx_probe() - probe function. + */ +static int inv_ak89xx_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_ak89xx_state_s *st; + struct iio_dev *indio_dev; + int result; + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->i2c = client; + st->sl_handle = client->adapter; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->i2c_addr = client->addr; + st->delay = AK89XX_DEFAULT_DELAY; + st->compass_id = id->driver_data; + st->compass_scale = 0; + + i2c_set_clientdata(client, indio_dev); + result = ak89xx_init(st); + if (result) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &ak89xx_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_ak89xx_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_ak89xx_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, ak89xx_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_ak89xx_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_ak89xx_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_ak89xx_remove() - remove function. + */ +static int inv_ak89xx_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_ak89xx_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_ak89xx_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv-ak89xx-iio module removed.\n"); + return 0; +} + +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_ak89xx_id[] = { + {"akm8975", COMPASS_ID_AK8975}, + {"akm8972", COMPASS_ID_AK8972}, + {"akm8963", COMPASS_ID_AK8963}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_ak89xx_id); + +static struct i2c_driver inv_ak89xx_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_ak89xx_probe, + .remove = inv_ak89xx_remove, + .id_table = inv_ak89xx_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-ak89xx-iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_ak89xx_init(void) +{ + int result = i2c_add_driver(&inv_ak89xx_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_ak89xx_exit(void) +{ + i2c_del_driver(&inv_ak89xx_driver); +} + +module_init(inv_ak89xx_init); +module_exit(inv_ak89xx_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv-ak89xx-iio"); + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_iio.h b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_iio.h new file mode 100644 index 000000000000..9a9f14a73ec5 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_iio.h @@ -0,0 +1,144 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#ifndef _INV_AK89XX_IIO_H_ +#define _INV_AK89XX_IIO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +/** + * struct inv_ak89xx_state_s - Driver state variables. + * @plat_data: board file platform data. + * @i2c: i2c client handle. + * @trig: not used. for compatibility. + * @work: work data structure. + * @delay: delay between each scheduled work. + * @dev: Represents read-only node for accessing buffered data. + * @inv_dev: Handle to input device. + * @sl_handle: Handle to I2C port. + */ +struct inv_ak89xx_state_s { + struct mpu_platform_data plat_data; + struct i2c_client *i2c; + struct iio_trigger *trig; + struct delayed_work work; + int delay; /* msec */ + unsigned char compass_id; + int compass_scale; /* for ak8963, 1:16-bit, 0:14-bit */ + short compass_data[3]; + u8 asa[3]; /* axis sensitivity adjustment */ + s64 timestamp; + short i2c_addr; + void *sl_handle; + struct device *inv_dev; + struct input_dev *idev; +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_AK89XX_SCAN_MAGN_X, + INV_AK89XX_SCAN_MAGN_Y, + INV_AK89XX_SCAN_MAGN_Z, + INV_AK89XX_SCAN_TIMESTAMP, +}; + +#define AK89XX_I2C_NAME "ak89xx" + +#define SENSOR_DATA_SIZE 8 +#define YPR_DATA_SIZE 12 +#define RWBUF_SIZE 16 + +#define ACC_DATA_FLAG 0 +#define MAG_DATA_FLAG 1 +#define ORI_DATA_FLAG 2 +#define AKM_NUM_SENSORS 3 + +#define ACC_DATA_READY (1 << (ACC_DATA_FLAG)) +#define MAG_DATA_READY (1 << (MAG_DATA_FLAG)) +#define ORI_DATA_READY (1 << (ORI_DATA_FLAG)) + +#define AKM_MINOR_NUMBER 254 + +/*! \name AK89XX constant definition + \anchor AK89XX_Def + Constant definitions of the AK89XX.*/ +#define AK89XX_MEASUREMENT_TIME_US 10000 + +/*! \name AK89XX operation mode + \anchor AK89XX_Mode + Defines an operation mode of the AK89XX.*/ +/*! @{*/ +#define AK89XX_CNTL_MODE_SNG_MEASURE 0x01 +#define AK89XX_CNTL_MODE_SELF_TEST 0x08 +#define AK89XX_CNTL_MODE_FUSE_ACCESS 0x0F +#define AK89XX_CNTL_MODE_POWER_DOWN 0x00 +/*! @}*/ + +/*! \name AK89XX register address +\anchor AK89XX_REG +Defines a register address of the AK89XX.*/ +/*! @{*/ +#define AK89XX_REG_WIA 0x00 +#define AK89XX_REG_INFO 0x01 +#define AK89XX_REG_ST1 0x02 +#define AK89XX_REG_HXL 0x03 +#define AK89XX_REG_HXH 0x04 +#define AK89XX_REG_HYL 0x05 +#define AK89XX_REG_HYH 0x06 +#define AK89XX_REG_HZL 0x07 +#define AK89XX_REG_HZH 0x08 +#define AK89XX_REG_ST2 0x09 +#define AK89XX_REG_CNTL 0x0A +#define AK89XX_REG_RSV 0x0B +#define AK89XX_REG_ASTC 0x0C +#define AK89XX_REG_TS1 0x0D +#define AK89XX_REG_TS2 0x0E +#define AK89XX_REG_I2CDIS 0x0F +/*! @}*/ + +/*! \name AK89XX fuse-rom address +\anchor AK89XX_FUSE +Defines a read-only address of the fuse ROM of the AK89XX.*/ +/*! @{*/ +#define AK89XX_FUSE_ASAX 0x10 +#define AK89XX_FUSE_ASAY 0x11 +#define AK89XX_FUSE_ASAZ 0x12 +/*! @}*/ + +#define AK89XX_MAX_DELAY (200) +#define AK89XX_MIN_DELAY (10) +#define AK89XX_DEFAULT_DELAY (100) + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + +int inv_ak89xx_configure_ring(struct iio_dev *indio_dev); +void inv_ak89xx_unconfigure_ring(struct iio_dev *indio_dev); +int inv_ak89xx_probe_trigger(struct iio_dev *indio_dev); +void inv_ak89xx_remove_trigger(struct iio_dev *indio_dev); +void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable); +int ak89xx_read_raw_data(struct inv_ak89xx_state_s *st, + short dat[3]); +void inv_read_ak89xx_fifo(struct iio_dev *indio_dev); +int ak89xx_read(struct inv_ak89xx_state_s *st, short rawfixed[3]); + +#endif + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_ring.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_ring.c new file mode 100644 index 000000000000..a8c1090bed5b --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_ring.c @@ -0,0 +1,138 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_ak89xx_iio.h" + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) +{ + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + + return d_ind; +} + +/** + * inv_read_ak89xx_fifo() - Transfer data from FIFO to ring buffer. + */ +void inv_read_ak89xx_fifo(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int d_ind; + s8 *tmp; + s64 tmp_buf[2]; + + if (!ak89xx_read(st, st->compass_data)) { + st->compass_data[0] = (short)(((int)st->compass_data[0] * (st->asa[0] + 128)) >> 8); + st->compass_data[1] = (short)(((int)st->compass_data[1] * (st->asa[1] + 128)) >> 8); + st->compass_data[2] = (short)(((int)st->compass_data[2] * (st->asa[2] + 128)) >> 8); + tmp = (u8 *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_AK89XX_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7)/8] = st->timestamp; + ring->access->store_to(indio_dev->buffer, tmp, st->timestamp); + } +} + +void inv_ak89xx_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; + +static int inv_ak89xx_postenable(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_Z))) + return 0; + + set_ak89xx_enable(indio_dev, true); + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_ak89xx_predisable(struct iio_dev *indio_dev) +{ + struct iio_buffer *ring = indio_dev->buffer; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_AK89XX_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_AK89XX_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_AK89XX_SCAN_MAGN_Z, ring->scan_mask); + set_ak89xx_enable(indio_dev, false); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_ak89xx_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_ak89xx_postenable, + .predisable = &inv_ak89xx_predisable, +}; + +int inv_ak89xx_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_ak89xx_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_trigger.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_trigger.c new file mode 100644 index 000000000000..04c77ab5c79d --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ak89xx_trigger.c @@ -0,0 +1,75 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" +#include "inv_ak89xx_iio.h" + +static const struct iio_trigger_ops inv_ak89xx_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_ak89xx_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->i2c->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_ak89xx_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_ak89xx_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_core.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_core.c new file mode 100644 index 000000000000..612ba72b59e9 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_core.c @@ -0,0 +1,570 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_core.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_ami306_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +static unsigned char late_initialize = true; + +s32 i2c_write(const struct i2c_client *client, + u8 command, u8 length, const u8 *values) +{ + INV_I2C_INC_COMPASSWRITE(3); + return i2c_smbus_write_i2c_block_data(client, command, length, values); +} + +s32 i2c_read(const struct i2c_client *client, + u8 command, u8 length, u8 *values) +{ + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + return i2c_smbus_read_i2c_block_data(client, command, length, values); +} + +static int ami306_read_param(struct inv_ami306_state_s *st) +{ + int result = 0; + unsigned char regs[AMI_PARAM_LEN]; + struct ami_sensor_parametor *param = &st->param; + + result = i2c_read(st->i2c, REG_AMI_SENX, + AMI_PARAM_LEN, regs); + if (result < 0) + return result; + + /* Little endian 16 bit registers */ + param->m_gain.x = le16_to_cpup((__le16 *)(®s[0])); + param->m_gain.y = le16_to_cpup((__le16 *)(®s[2])); + param->m_gain.z = le16_to_cpup((__le16 *)(®s[4])); + + param->m_interference.xy = regs[7]; + param->m_interference.xz = regs[6]; + param->m_interference.yx = regs[9]; + param->m_interference.yz = regs[8]; + param->m_interference.zx = regs[11]; + param->m_interference.zy = regs[10]; + + param->m_offset.x = AMI_STANDARD_OFFSET; + param->m_offset.y = AMI_STANDARD_OFFSET; + param->m_offset.z = AMI_STANDARD_OFFSET; + + param->m_gain_cor.x = AMI_GAIN_COR_DEFAULT; + param->m_gain_cor.y = AMI_GAIN_COR_DEFAULT; + param->m_gain_cor.z = AMI_GAIN_COR_DEFAULT; + + return 0; +} + +static int ami306_write_offset(const struct i2c_client *client, + unsigned char *fine) +{ + int result = 0; + unsigned char dat[3]; + dat[0] = (0x7f & fine[0]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFX, 2, dat); + dat[0] = (0x7f & fine[1]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFY, 2, dat); + dat[0] = (0x7f & fine[2]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFZ, 2, dat); + + return result; +} + +static int ami306_wait_data_ready(struct inv_ami306_state_s *st, + unsigned long usecs, unsigned long times) +{ + int result = 0; + unsigned char buf; + + for (; 0 < times; --times) { + udelay(usecs); + result = i2c_read(st->i2c, REG_AMI_STA1, 1, &buf); + if (result < 0) + return INV_ERROR_COMPASS_DATA_NOT_READY; + if (buf & AMI_STA1_DRDY_BIT) + return 0; + else if (buf & AMI_STA1_DOR_BIT) + return INV_ERROR_COMPASS_DATA_OVERFLOW; + } + + return INV_ERROR_COMPASS_DATA_NOT_READY; +} +int ami306_read_raw_data(struct inv_ami306_state_s *st, + short dat[3]) +{ + int result; + unsigned char buf[6]; + result = i2c_read(st->i2c, REG_AMI_DATAX, sizeof(buf), buf); + if (result < 0) + return result; + dat[0] = le16_to_cpup((__le16 *)(&buf[0])); + dat[1] = le16_to_cpup((__le16 *)(&buf[2])); + dat[2] = le16_to_cpup((__le16 *)(&buf[4])); + + return 0; +} + +#define AMI_WAIT_DATAREADY_RETRY 3 /* retry times */ +#define AMI_DRDYWAIT 800 /* u(micro) sec */ +static int ami306_force_measurement(struct inv_ami306_state_s *st, + short ver[3]) +{ + int result; + int status; + char buf; + buf = AMI_CTRL3_FORCE_BIT; + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, &buf); + if (result < 0) + return result; + + result = ami306_wait_data_ready(st, + AMI_DRDYWAIT, AMI_WAIT_DATAREADY_RETRY); + if (result && result != INV_ERROR_COMPASS_DATA_OVERFLOW) + return result; + /* READ DATA X,Y,Z */ + status = ami306_read_raw_data(st, ver); + if (status) + return status; + + return result; +} + +static int ami306_initial_b0_adjust(struct inv_ami306_state_s *st) +{ + int result; + unsigned char fine[3] = { 0 }; + short data[3]; + int diff[3] = { 0x7fff, 0x7fff, 0x7fff }; + int fn = 0; + int ax = 0; + unsigned char buf[3]; + + buf[0] = AMI_CTRL2_DREN; + result = i2c_write(st->i2c, REG_AMI_CTRL2, 1, buf); + if (result) + return result; + + buf[0] = AMI_CTRL4_HS & 0xFF; + buf[1] = (AMI_CTRL4_HS >> 8) & 0xFF; + result = i2c_write(st->i2c, REG_AMI_CTRL4, 2, buf); + if (result < 0) + return result; + + for (fn = 0; fn < AMI_FINE_MAX; ++fn) { /* fine 0 -> 95 */ + fine[0] = fine[1] = fine[2] = fn; + result = ami306_write_offset(st->i2c, fine); + if (result) + return result; + + result = ami306_force_measurement(st, data); + if (result) + return result; + + for (ax = 0; ax < 3; ax++) { + /* search point most close to zero. */ + if (diff[ax] > abs(data[ax])) { + st->fine[ax] = fn; + diff[ax] = abs(data[ax]); + } + } + } + result = ami306_write_offset(st->i2c, st->fine); + if (result) + return result; + + /* Software Reset */ + buf[0] = AMI_CTRL3_SRST_BIT; + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, buf); + if (result < 0) + return result; + else + return 0; +} + +static int ami306_start_sensor(struct inv_ami306_state_s *st) +{ + int result = 0; + unsigned char buf[2]; + + /* Step 1 */ + buf[0] = (AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE); + result = i2c_write(st->i2c, REG_AMI_CTRL1, 1, buf); + if (result < 0) + return result; + /* Step 2 */ + buf[0] = AMI_CTRL2_DREN; + result = i2c_write(st->i2c, REG_AMI_CTRL2, 1, buf); + if (result < 0) + return result; + /* Step 3 */ + buf[0] = (AMI_CTRL4_HS & 0xFF); + buf[1] = (AMI_CTRL4_HS >> 8) & 0xFF; + + result = i2c_write(st->i2c, REG_AMI_CTRL4, 2, buf); + if (result < 0) + return result; + + /* Step 4 */ + result = ami306_write_offset(st->i2c, st->fine); + + return result; +} + +int set_ami306_enable(struct iio_dev *indio_dev, int state) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + int result; + char buf; + + buf = (AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE); + result = i2c_write(st->i2c, REG_AMI_CTRL1, 1, &buf); + if (result < 0) + return result; + + result = ami306_read_param(st); + if (result) + return result; + if (late_initialize) { + result = ami306_initial_b0_adjust(st); + if (result) + return result; + late_initialize = false; + } + result = ami306_start_sensor(st); + if (result) + return result; + buf = AMI_CTRL3_FORCE_BIT; + st->timestamp = iio_get_time_ns(); + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, &buf); + if (result) + return result; + + return 0; +} + +/** + * ami306_read_raw() - read raw method. + */ +static int ami306_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + *val = AMI_SCALE; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +/** + * inv_compass_matrix_show() - show orientation matrix + */ +static ssize_t inv_compass_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +static ssize_t ami306_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + error = kstrtoul(buf, 10, &data); + if (error) + return error; + if (0 == data) + return -EINVAL; + /* transform rate to delay in ms */ + data = 1000 / data; + if (data > AMI_MAX_DELAY) + data = AMI_MAX_DELAY; + if (data < AMI_MIN_DELAY) + data = AMI_MIN_DELAY; + st->delay = data; + return count; +} + +static ssize_t ami306_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", 1000 / st->delay); +} + + +static void ami306_work_func(struct work_struct *work) +{ + struct inv_ami306_state_s *st = + container_of((struct delayed_work *)work, + struct inv_ami306_state_s, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + unsigned long delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + st->timestamp = iio_get_time_ns(); + schedule_delayed_work(&st->work, delay); + inv_read_ami306_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_AMI306_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(compass_matrix, S_IRUGO, inv_compass_matrix_show, NULL); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, ami306_rate_show, + ami306_rate_store); + +static struct attribute *inv_ami306_attributes[] = { + &dev_attr_compass_matrix.attr, + &dev_attr_sampling_frequency.attr, + NULL, +}; +static const struct attribute_group inv_attribute_group = { + .name = "ami306", + .attrs = inv_ami306_attributes +}; + +static const struct iio_info ami306_info = { + .driver_module = THIS_MODULE, + .read_raw = &ami306_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_ami306_probe() - probe function. + */ +static int inv_ami306_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_ami306_state_s *st; + struct iio_dev *indio_dev; + int result; + char data; + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->i2c = client; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->delay = 10; + + /* Make state variables available to all _show and _store functions. */ + i2c_set_clientdata(client, indio_dev); + result = i2c_read(st->i2c, REG_AMI_WIA, 1, &data); + if (result < 0) + goto out_free; + if (data != DATA_WIA) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &ami306_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_ami306_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_ami306_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, ami306_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_ami306_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_ami306_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_ami306_remove() - remove function. + */ +static int inv_ami306_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_ami306_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_ami306_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv-ami306-iio module removed.\n"); + return 0; +} +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_ami306_id[] = { + {"ami306", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_ami306_id); + +static struct i2c_driver inv_ami306_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_ami306_probe, + .remove = inv_ami306_remove, + .id_table = inv_ami306_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-ami306-iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_ami306_init(void) +{ + int result = i2c_add_driver(&inv_ami306_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_ami306_exit(void) +{ + i2c_del_driver(&inv_ami306_driver); +} + +module_init(inv_ami306_init); +module_exit(inv_ami306_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv-ami306-iio"); +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_iio.h b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_iio.h new file mode 100644 index 000000000000..fa4f4ee1e5da --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_iio.h @@ -0,0 +1,159 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_iio.h + * @brief Struct definitions for the Invensense implementation + * of ami306 driver. + */ + +#ifndef _INV_GYRO_H_ +#define _INV_GYRO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +/** axis sensitivity(gain) calibration parameter information */ +struct ami_vector3d { + signed short x; /**< X-axis */ + signed short y; /**< Y-axis */ + signed short z; /**< Z-axis */ +}; + +/** axis interference information */ +struct ami_interference { + /**< Y-axis magnetic field for X-axis correction value */ + signed short xy; + /**< Z-axis magnetic field for X-axis correction value */ + signed short xz; + /**< X-axis magnetic field for Y-axis correction value */ + signed short yx; + /**< Z-axis magnetic field for Y-axis correction value */ + signed short yz; + /**< X-axis magnetic field for Z-axis correction value */ + signed short zx; + /**< Y-axis magnetic field for Z-axis correction value */ + signed short zy; +}; + +/** sensor calibration Parameter information */ +struct ami_sensor_parametor { + /**< geomagnetic field sensor gain */ + struct ami_vector3d m_gain; + /**< geomagnetic field sensor gain correction parameter */ + struct ami_vector3d m_gain_cor; + /**< geomagnetic field sensor offset */ + struct ami_vector3d m_offset; + /**< geomagnetic field sensor axis interference parameter */ + struct ami_interference m_interference; +}; + +/** + * struct inv_ami306_state_s - Driver state variables. + * @plat_data: board file platform data. + * @i2c: i2c client handle. + * @trig: not used. for compatibility. + * @param: ami specific sensor data. + * @work: work data structure. + * @delay: delay between each scheduled work. + * @fine: fine tunign parameters. + * @compass_data: compass data store. + * @timestamp: time stamp. + */ +struct inv_ami306_state_s { + struct mpu_platform_data plat_data; + struct i2c_client *i2c; + struct iio_trigger *trig; + struct ami_sensor_parametor param; + struct delayed_work work; + int delay; + s8 fine[3]; + short compass_data[3]; + s64 timestamp; +}; +/* scan element definition */ +enum inv_mpu_scan { + INV_AMI306_SCAN_MAGN_X, + INV_AMI306_SCAN_MAGN_Y, + INV_AMI306_SCAN_MAGN_Z, + INV_AMI306_SCAN_TIMESTAMP, +}; + +#define REG_AMI_WIA 0x0f +#define REG_AMI_DATAX 0x10 +#define REG_AMI_STA1 0x18 +#define REG_AMI_CTRL1 0x1b +#define REG_AMI_CTRL2 0x1c +#define REG_AMI_CTRL3 0x1d +#define REG_AMI_B0X 0x20 +#define REG_AMI_B0Y 0x22 +#define REG_AMI_B0Z 0x24 +#define REG_AMI_CTRL5 0x40 +#define REG_AMI_CTRL4 0x5c +#define REG_AMI_TEMP 0x60 +#define REG_AMI_SENX 0x96 +#define REG_AMI_OFFX 0x6c +#define REG_AMI_OFFY 0x72 +#define REG_AMI_OFFZ 0x78 + + +#define DATA_WIA 0x46 +#define AMI_CTRL1_PC1 0x80 +#define AMI_CTRL1_FS1_FORCE 0x02 +#define AMI_CTRL1_ODR1 0x10 +#define AMI_CTRL2_DREN 0x08 +#define AMI_CTRL2_DRP 0x04 +#define AMI_CTRL3_FORCE_BIT 0x40 +#define AMI_CTRL3_B0_LO_BIT 0x10 +#define AMI_CTRL3_SRST_BIT 0x80 +#define AMI_CTRL4_HS 0xa07e +#define AMI_CTRL4_AB 0x0001 +#define AMI_STA1_DRDY_BIT 0x40 +#define AMI_STA1_DOR_BIT 0x20 + +#define AMI_PARAM_LEN 12 +#define AMI_STANDARD_OFFSET 0x800 +#define AMI_GAIN_COR_DEFAULT 1000 +#define AMI_FINE_MAX 96 +#define AMI_MAX_DELAY 1000 +#define AMI_MIN_DELAY 10 +#define AMI_SCALE (5461 * (1<<15)) + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + + +int inv_ami306_configure_ring(struct iio_dev *indio_dev); +void inv_ami306_unconfigure_ring(struct iio_dev *indio_dev); +int inv_ami306_probe_trigger(struct iio_dev *indio_dev); +void inv_ami306_remove_trigger(struct iio_dev *indio_dev); +int set_ami306_enable(struct iio_dev *indio_dev, int state); +int ami306_read_raw_data(struct inv_ami306_state_s *st, + short dat[3]); +int inv_read_ami306_fifo(struct iio_dev *indio_dev); + +#endif /* #ifndef _INV_GYRO_H_ */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_ring.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_ring.c new file mode 100644 index 000000000000..ed91cf49516f --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_ring.c @@ -0,0 +1,163 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_ring.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_ami306_iio.h" + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) { + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + return d_ind; +} + +/** + * inv_read_fifo() - Transfer data from FIFO to ring buffer. + */ +int inv_read_ami306_fifo(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int result, status, d_ind; + char b; + char *tmp; + s64 tmp_buf[2]; + + result = i2c_smbus_read_i2c_block_data(st->i2c, REG_AMI_STA1, 1, &b); + if (result < 0) + goto end_session; + if (b & AMI_STA1_DRDY_BIT) { + status = ami306_read_raw_data(st, st->compass_data); + if (status) { + pr_err("error reading raw\n"); + goto end_session; + } + tmp = (unsigned char *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_AMI306_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7)/8] = st->timestamp; + ring->access->store_to(indio_dev->buffer, tmp, st->timestamp); + } else if (b & AMI_STA1_DOR_BIT) + pr_err("not ready\n"); +end_session: + b = AMI_CTRL3_FORCE_BIT; + result = i2c_smbus_write_i2c_block_data(st->i2c, REG_AMI_CTRL3, 1, &b); + + return IRQ_HANDLED; +} + +void inv_ami306_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; +static int inv_ami306_postenable(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int result; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_Z))) + return 0; + + result = set_ami306_enable(indio_dev, true); + if (result) + return result; + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_ami306_predisable(struct iio_dev *indio_dev) +{ + struct iio_buffer *ring = indio_dev->buffer; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_AMI306_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_AMI306_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_AMI306_SCAN_MAGN_Z, ring->scan_mask); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_ami306_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_ami306_postenable, + .predisable = &inv_ami306_predisable, +}; + +int inv_ami306_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_ami306_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_trigger.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_trigger.c new file mode 100644 index 000000000000..f7fe59ef5dff --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_ami306_trigger.c @@ -0,0 +1,90 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_trigger.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" +#include "inv_ami306_iio.h" + +static const struct iio_trigger_ops inv_ami306_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_ami306_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->i2c->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_ami306_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_ami306_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_core.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_core.c new file mode 100644 index 000000000000..6af420bb5cf1 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_core.c @@ -0,0 +1,969 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_core.c + * @brief Invensense implementation for yas530/yas532/yas533. + * @details This driver currently works for yas530/yas532/yas533. + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_yas53x_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +/* -------------------------------------------------------------------------- */ +static int Cx, Cy1, Cy2; +static int /*a1, */ a2, a3, a4, a5, a6, a7, a8, a9; +static int k; + +static u8 dx, dy1, dy2; +static u8 d2, d3, d4, d5, d6, d7, d8, d9, d0; +static u8 dck, ver; + +/** + * inv_serial_read() - Read one or more bytes from the device registers. + * @st: Device driver instance. + * @reg: First device register to be read from. + * @length: Number of bytes to read. + * @data: Data read from device. + * NOTE: The slave register will not increment when reading from the FIFO. + */ +int inv_serial_read(struct inv_compass_state *st, u8 reg, u16 length, u8 *data) +{ + int result; + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + result = i2c_smbus_read_i2c_block_data(st->client, reg, length, data); + if (result != length) { + if (result < 0) + return result; + else + return -EINVAL; + } else { + return 0; + } +} + +/** + * inv_serial_single_write() - Write a byte to a device register. + * @st: Device driver instance. + * @reg: Device register to be written to. + * @data: Byte to write to device. + */ +int inv_serial_single_write(struct inv_compass_state *st, u8 reg, u8 data) +{ + u8 d[1]; + d[0] = data; + INV_I2C_INC_COMPASSWRITE(3); + + return i2c_smbus_write_i2c_block_data(st->client, reg, 1, d); +} + +static int set_hardware_offset(struct inv_compass_state *st, + char offset_x, char offset_y1, char offset_y2) +{ + char data; + int result = 0; + + data = offset_x & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_X, data); + if (result) + return result; + + data = offset_y1 & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_Y1, data); + if (result) + return result; + + data = offset_y2 & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_Y2, data); + return result; +} + +static int set_measure_command(struct inv_compass_state *st) +{ + int result = 0; + result = inv_serial_single_write(st, + YAS530_REGADDR_MEASURE_COMMAND, 0x01); + return result; +} + +static int measure_normal(struct inv_compass_state *st, + int *busy, unsigned short *t, + unsigned short *x, unsigned short *y1, + unsigned short *y2) +{ + int result; + ktime_t sleeptime; + result = set_measure_command(st); + sleeptime = ktime_set(0, 2 * NSEC_PER_MSEC); + set_current_state(TASK_UNINTERRUPTIBLE); + schedule_hrtimeout(&sleeptime, HRTIMER_MODE_REL); + + result = st->read_data(st, busy, t, x, y1, y2); + + return result; +} + +static int measure_int(struct inv_compass_state *st, + int *busy, unsigned short *t, + unsigned short *x, unsigned short *y1, + unsigned short *y2) +{ + int result; + if (st->first_read_after_reset) { + st->first_read_after_reset = 0; + result = 1; + } else { + result = st->read_data(st, busy, t, x, y1, y2); + } + result |= set_measure_command(st); + + return result; +} + +static int yas530_read_data(struct inv_compass_state *st, + int *busy, u16 *t, u16 *x, u16 *y1, u16 *y2) +{ + u8 data[8]; + u16 b, to, xo, y1o, y2o; + int result; + + result = inv_serial_read(st, + YAS530_REGADDR_MEASURE_DATA, 8, data); + if (result) + return result; + + b = (data[0] >> 7) & 0x01; + to = (s16)(((data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03)); + xo = (s16)(((data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f)); + y1o = (s16)(((data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f)); + y2o = (s16)(((data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f)); + + *busy = b; + *t = to; + *x = xo; + *y1 = y1o; + *y2 = y2o; + + return 0; +} + +static int yas532_533_read_data(struct inv_compass_state *st, + int *busy, u16 *t, u16 *x, u16 *y1, u16 *y2) +{ + u8 data[8]; + u16 b, to, xo, y1o, y2o; + int result; + + result = inv_serial_read(st, + YAS530_REGADDR_MEASURE_DATA, 8, data); + if (result) + return result; + + b = (data[0] >> 7) & 0x01; + to = (s16)((((s32)data[0] << 3) & 0x3f8) | ((data[1] >> 5) & 0x07)); + xo = (s16)((((s32)data[2] << 6) & 0x1fc0) | ((data[3] >> 2) & 0x3f)); + y1o = (s16)((((s32)data[4] << 6) & 0x1fc0) | ((data[5] >> 2) & 0x3f)); + y2o = (s16)((((s32)data[6] << 6) & 0x1fc0) | ((data[7] >> 2) & 0x3f)); + + *busy = b; + *t = to; + *x = xo; + *y1 = y1o; + *y2 = y2o; + + return 0; +} + +static int check_offset(struct inv_compass_state *st, + char offset_x, char offset_y1, char offset_y2, + int *flag_x, int *flag_y1, int *flag_y2) +{ + int result; + int busy; + short t, x, y1, y2; + + result = set_hardware_offset(st, offset_x, offset_y1, offset_y2); + if (result) + return result; + result = measure_normal(st, &busy, &t, &x, &y1, &y2); + if (result) + return result; + *flag_x = 0; + *flag_y1 = 0; + *flag_y2 = 0; + + if (x > st->center) + *flag_x = 1; + if (y1 > st->center) + *flag_y1 = 1; + if (y2 > st->center) + *flag_y2 = 1; + if (x < st->center) + *flag_x = -1; + if (y1 < st->center) + *flag_y1 = -1; + if (y2 < st->center) + *flag_y2 = -1; + + return result; +} + +static int measure_and_set_offset(struct inv_compass_state *st, + char *offset) +{ + int i; + int result = 0; + char offset_x = 0, offset_y1 = 0, offset_y2 = 0; + int flag_x = 0, flag_y1 = 0, flag_y2 = 0; + static const int correct[5] = {16, 8, 4, 2, 1}; + + for (i = 0; i < 5; i++) { + result = check_offset(st, + offset_x, offset_y1, offset_y2, + &flag_x, &flag_y1, &flag_y2); + if (result) + return result; + if (flag_x) + offset_x += flag_x * correct[i]; + if (flag_y1) + offset_y1 += flag_y1 * correct[i]; + if (flag_y2) + offset_y2 += flag_y2 * correct[i]; + } + + result = set_hardware_offset(st, offset_x, offset_y1, offset_y2); + if (result) + return result; + offset[0] = offset_x; + offset[1] = offset_y1; + offset[2] = offset_y2; + + return result; +} + +static void coordinate_conversion(short x, short y1, short y2, short t, + int *xo, int *yo, int *zo) +{ + int sx, sy1, sy2, sy, sz; + int hx, hy, hz; + + sx = x - (Cx * t) / 100; + sy1 = y1 - (Cy1 * t) / 100; + sy2 = y2 - (Cy2 * t) / 100; + + sy = sy1 - sy2; + sz = -sy1 - sy2; + + hx = k * ((100 * sx + a2 * sy + a3 * sz) / 10); + hy = k * ((a4 * sx + a5 * sy + a6 * sz) / 10); + hz = k * ((a7 * sx + a8 * sy + a9 * sz) / 10); + + *xo = hx; + *yo = hy; + *zo = hz; +} + +static int get_cal_data_yas532_533(struct inv_compass_state *st) +{ + u8 data[YAS_YAS532_533_CAL_DATA_SIZE]; + int result; + + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS532_533_CAL_DATA_SIZE, data); + if (result) + return result; + /* CAL data Second Read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS532_533_CAL_DATA_SIZE, data); + if (result) + return result; + + dx = data[0]; + dy1 = data[1]; + dy2 = data[2]; + d2 = (data[3] >> 2) & 0x03f; + d3 = (u8)(((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03)); + d4 = (u8)(data[4] & 0x3f); + d5 = (data[5] >> 2) & 0x3f; + d6 = (u8)(((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f)); + d7 = (u8)(((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07)); + d8 = (u8)(((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01)); + d9 = (u8)(((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01)); + d0 = (u8)((data[9] >> 2) & 0x1f); + dck = (u8)(((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01)); + ver = (u8)((data[13]) & 0x01); + + Cx = dx * 10 - 1280; + Cy1 = dy1 * 10 - 1280; + Cy2 = dy2 * 10 - 1280; + a2 = d2 - 32; + a3 = d3 - 8; + a4 = d4 - 32; + a5 = d5 + 38; + a6 = d6 - 32; + a7 = d7 - 64; + a8 = d8 - 32; + a9 = d9; + k = d0; + + return 0; +} + +static int get_cal_data_yas530(struct inv_compass_state *st) +{ + u8 data[YAS_YAS530_CAL_DATA_SIZE]; + int result; + /* CAL data read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS530_CAL_DATA_SIZE, data); + if (result) + return result; + /* CAL data Second Read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS530_CAL_DATA_SIZE, data); + if (result) + return result; + /*Cal data */ + dx = data[0]; + dy1 = data[1]; + dy2 = data[2]; + d2 = (data[3] >> 2) & 0x03f; + d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + d4 = data[4] & 0x3f; + d5 = (data[5] >> 2) & 0x3f; + d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + d0 = (data[9] >> 2) & 0x1f; + dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + ver = (u8)((data[15]) & 0x03); + + /*Correction Data */ + Cx = (int)dx * 6 - 768; + Cy1 = (int)dy1 * 6 - 768; + Cy2 = (int)dy2 * 6 - 768; + a2 = (int)d2 - 32; + a3 = (int)d3 - 8; + a4 = (int)d4 - 32; + a5 = (int)d5 + 38; + a6 = (int)d6 - 32; + a7 = (int)d7 - 64; + a8 = (int)d8 - 32; + a9 = (int)d9; + k = (int)d0 + 10; + + return 0; +} + + +static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, + int threshold) +{ + thresh_filter->threshold = threshold; + thresh_filter->last = 0; +} + +static void +adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, + int noise) +{ + int i; + + adap_filter->num = 0; + adap_filter->index = 0; + adap_filter->filter_noise = noise; + adap_filter->filter_len = len; + + for (i = 0; i < adap_filter->filter_len; ++i) + adap_filter->sequence[i] = 0; +} + +static void yas_init_adap_filter(struct inv_compass_state *st) +{ + struct yas_filter *f; + int i; + int noise[] = {YAS_MAG_DEFAULT_FILTER_NOISE_X, + YAS_MAG_DEFAULT_FILTER_NOISE_Y, + YAS_MAG_DEFAULT_FILTER_NOISE_Z}; + + f = &st->filter; + f->filter_len = YAS_MAG_DEFAULT_FILTER_LEN; + for (i = 0; i < 3; i++) + f->filter_noise[i] = noise[i]; + + for (i = 0; i < 3; i++) { + adaptive_filter_init(&f->adap_filter[i], f->filter_len, + f->filter_noise[i]); + thresh_filter_init(&f->thresh_filter[i], f->filter_thresh); + } +} + +int yas53x_resume(struct inv_compass_state *st) +{ + int result = 0; + + unsigned char dummyData = 0x00; + unsigned char read_reg[1]; + + /* =============================================== */ + + /* Step 1 - Test register initialization */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_TEST1, dummyData); + if (result) + return result; + result = + inv_serial_single_write(st, + YAS530_REGADDR_TEST2, dummyData); + if (result) + return result; + /* Device ID read */ + result = inv_serial_read(st, + YAS530_REGADDR_DEVICE_ID, 1, read_reg); + + /*Step 2 Read the CAL register */ + st->get_cal_data(st); + + /*Obtain the [49:47] bits */ + dck &= 0x07; + + /*Step 3 : Storing the CONFIG with the CLK value */ + dummyData = 0x00 | (dck << 2); + result = inv_serial_single_write(st, + YAS530_REGADDR_CONFIG, dummyData); + if (result) + return result; + /*Step 4 : Set Acquisition Interval Register */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_MEASURE_INTERVAL, + dummyData); + if (result) + return result; + + /*Step 5 : Reset Coil */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_ACTUATE_INIT_COIL, + dummyData); + if (result) + return result; + /* Offset Measurement and Set */ + result = measure_and_set_offset(st, st->offset); + if (result) + return result; + st->first_measure_after_reset = 1; + st->first_read_after_reset = 1; + st->reset_timer = 0; + + yas_init_adap_filter(st); + + return result; +} + +static int inv_check_range(struct inv_compass_state *st, s16 x, s16 y1, s16 y2) +{ + int result = 0; + + if (x == 0) + result |= 0x01; + if (x == st->overflow_bound) + result |= 0x02; + if (y1 == 0) + result |= 0x04; + if (y1 == st->overflow_bound) + result |= 0x08; + if (y2 == 0) + result |= 0x10; + if (y2 == st->overflow_bound) + result |= 0x20; + + return result; +} +static int square(int data) +{ + return data * data; +} + +static int +adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, int in) +{ + int avg, sum; + int i; + + if (adap_filter->filter_len == 0) + return in; + if (adap_filter->num < adap_filter->filter_len) { + adap_filter->sequence[adap_filter->index++] = in / 100; + adap_filter->num++; + return in; + } + if (adap_filter->filter_len <= adap_filter->index) + adap_filter->index = 0; + adap_filter->sequence[adap_filter->index++] = in / 100; + + avg = 0; + for (i = 0; i < adap_filter->filter_len; i++) + avg += adap_filter->sequence[i]; + avg /= adap_filter->filter_len; + + sum = 0; + for (i = 0; i < adap_filter->filter_len; i++) + sum += square(avg - adap_filter->sequence[i]); + sum /= adap_filter->filter_len; + + if (sum <= adap_filter->filter_noise) + return avg * 100; + + return ((in/100 - avg) * (sum - adap_filter->filter_noise) / sum + avg) + * 100; +} + +static int +thresh_filter_filter(struct yas_thresh_filter *thresh_filter, int in) +{ + if (in < thresh_filter->last - thresh_filter->threshold + || thresh_filter->last + + thresh_filter->threshold < in) { + thresh_filter->last = in; + return in; + } else { + return thresh_filter->last; + } +} + +static void +filter_filter(struct yas_filter *d, int *orig, int *filtered) +{ + int i; + + for (i = 0; i < 3; i++) { + filtered[i] = adaptive_filter_filter(&d->adap_filter[i], + orig[i]); + filtered[i] = thresh_filter_filter(&d->thresh_filter[i], + filtered[i]); + } +} + +int yas53x_read(struct inv_compass_state *st, short rawfixed[3], + int *overunderflow) +{ + int result = 0; + + int busy, i, ov; + short t, x, y1, y2; + s32 xyz[3], disturb[3]; + + result = measure_int(st, &busy, &t, &x, &y1, &y2); + if (result) + return result; + if (busy) + return -1; + coordinate_conversion(x, y1, y2, t, &xyz[0], &xyz[1], &xyz[2]); + filter_filter(&st->filter, xyz, xyz); + for (i = 0; i < 3; i++) + rawfixed[i] = (short)(xyz[i] / 100); + + if (st->first_measure_after_reset) { + for (i = 0; i < 3; i++) + st->base_compass_data[i] = rawfixed[i]; + st->first_measure_after_reset = 0; + } + ov = 0; + for (i = 0; i < 3; i++) { + disturb[i] = abs(st->base_compass_data[i] - rawfixed[i]); + if (disturb[i] > YAS_MAG_DISTURBURNCE_THRESHOLD) + ov = 1; + } + if (ov) + st->reset_timer += st->delay; + else + st->reset_timer = 0; + + if (st->reset_timer > YAS_RESET_COIL_TIME_THRESHOLD) + *overunderflow = (1<<8); + else + *overunderflow = 0; + *overunderflow |= inv_check_range(st, x, y1, y2); + + return 0; +} + +/** + * yas53x_read_raw() - read raw method. + */ +static int yas53x_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_compass_state *st = iio_priv(indio_dev); + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + *val = YAS530_SCALE; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +/** + * inv_compass_matrix_show() - show orientation matrix + */ +static ssize_t inv_compass_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_compass_state *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +static ssize_t yas53x_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + u32 data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + error = kstrtoint(buf, 10, &data); + if (error) + return error; + if (0 == data) + return -EINVAL; + /* transform rate to delay in ms */ + data = MSEC_PER_SEC / data; + + if (data > YAS530_MAX_DELAY) + data = YAS530_MAX_DELAY; + if (data < YAS530_MIN_DELAY) + data = YAS530_MIN_DELAY; + st->delay = data; + + return count; +} + +static ssize_t yas53x_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", (int)MSEC_PER_SEC / st->delay); +} + +static ssize_t yas53x_overunderflow_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + u32 data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + error = kstrtoint(buf, 10, &data); + if (error) + return error; + if (data) + return -EINVAL; + st->overunderflow = data; + + return count; +} + +static ssize_t yas53x_overunderflow_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", st->overunderflow); +} + +void set_yas53x_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + + yas_init_adap_filter(st); + st->first_measure_after_reset = 1; + st->first_read_after_reset = 1; + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); +} + +static void yas53x_work_func(struct work_struct *work) +{ + struct inv_compass_state *st = + container_of((struct delayed_work *)work, + struct inv_compass_state, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + u32 delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + schedule_delayed_work(&st->work, delay); + inv_read_yas53x_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_YAS53X_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(compass_matrix, S_IRUGO, inv_compass_matrix_show, NULL); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, yas53x_rate_show, + yas53x_rate_store); +static DEVICE_ATTR(overunderflow, S_IRUGO | S_IWUSR, + yas53x_overunderflow_show, yas53x_overunderflow_store); + +static struct attribute *inv_yas53x_attributes[] = { + &dev_attr_compass_matrix.attr, + &dev_attr_sampling_frequency.attr, + &dev_attr_overunderflow.attr, + NULL, +}; +static const struct attribute_group inv_attribute_group = { + .name = "yas53x", + .attrs = inv_yas53x_attributes +}; + +static const struct iio_info yas53x_info = { + .driver_module = THIS_MODULE, + .read_raw = &yas53x_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_yas53x_probe() - probe function. + */ +static int inv_yas53x_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_compass_state *st; + struct iio_dev *indio_dev; + int result; + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->client = client; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->delay = 10; + + i2c_set_clientdata(client, indio_dev); + + if (!strcmp(id->name, "yas530")) { + st->read_data = yas530_read_data; + st->get_cal_data = get_cal_data_yas530; + st->overflow_bound = YAS_YAS530_DATA_OVERFLOW; + st->center = YAS_YAS530_DATA_CENTER; + st->filter.filter_thresh = YAS530_MAG_DEFAULT_FILTER_THRESH; + } else { + st->read_data = yas532_533_read_data; + st->get_cal_data = get_cal_data_yas532_533; + st->overflow_bound = YAS_YAS532_533_DATA_OVERFLOW; + st->center = YAS_YAS532_533_DATA_CENTER; + st->filter.filter_thresh = YAS532_MAG_DEFAULT_FILTER_THRESH; + } + st->upper_bound = st->center + (st->center >> 1); + st->lower_bound = (st->center >> 1); + + result = yas53x_resume(st); + if (result) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &yas53x_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_yas53x_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_yas53x_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, yas53x_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_yas53x_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_yas53x_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_yas53x_remove() - remove function. + */ +static int inv_yas53x_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_compass_state *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_yas53x_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_yas53x_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv_yas53x_iio module removed.\n"); + return 0; +} +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_yas53x_id[] = { + {"yas530", 0}, + {"yas532", 0}, + {"yas533", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_yas53x_id); + +static struct i2c_driver inv_yas53x_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_yas53x_probe, + .remove = inv_yas53x_remove, + .id_table = inv_yas53x_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv_yas53x_iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_yas53x_init(void) +{ + int result = i2c_add_driver(&inv_yas53x_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_yas53x_exit(void) +{ + i2c_del_driver(&inv_yas53x_driver); +} + +module_init(inv_yas53x_init); +module_exit(inv_yas53x_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv_yas53x_iio"); +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_iio.h b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_iio.h new file mode 100644 index 000000000000..92bf0af7ec7e --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_iio.h @@ -0,0 +1,172 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_iio.h + * @brief Struct definitions for the Invensense implementation + * of yas53x driver. + */ + +#ifndef _INV_GYRO_H_ +#define _INV_GYRO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +#define YAS_MAG_MAX_FILTER_LEN 30 +struct yas_adaptive_filter { + int num; + int index; + int filter_len; + int filter_noise; + int sequence[YAS_MAG_MAX_FILTER_LEN]; +}; + +struct yas_thresh_filter { + int threshold; + int last; +}; + +struct yas_filter { + int filter_len; + int filter_thresh; + int filter_noise[3]; + struct yas_adaptive_filter adap_filter[3]; + struct yas_thresh_filter thresh_filter[3]; +}; + +/** + * struct inv_compass_state - Driver state variables. + * @plat_data: mpu platform data from board file. + * @client: i2c client handle. + * @chan_info: channel information. + * @trig: IIO trigger. + * @work: work structure. + * @delay: delay to schedule the next work. + * @overflow_bound: bound to determine overflow. + * @center: center of the measurement. + * @compass_data[3]: compass data store. + * @offset[3]: yas530 specific data. + * @base_compass_data[3]: first measure data after reset. + * @first_measure_after_reset:1: flag for first measurement after reset. + * @first_read_after_reset:1: flag for first read after reset. + * @reset_timer: timer to accumulate overflow conditions. + * @overunderflow:1: overflow and underflow flag. + * @filter: filter data structure. + * @read_data: function pointer of reading data from device. + * @get_cal_data: function pointer of reading cal data. + */ +struct inv_compass_state { + struct mpu_platform_data plat_data; + struct i2c_client *client; + struct iio_trigger *trig; + struct delayed_work work; + s16 delay; + s16 overflow_bound; + s16 upper_bound; + s16 lower_bound; + s16 center; + s16 compass_data[3]; + s8 offset[3]; + s16 base_compass_data[3]; + u8 first_measure_after_reset:1; + u8 first_read_after_reset:1; + u8 overunderflow:1; + s32 reset_timer; + struct yas_filter filter; + int (*read_data)(struct inv_compass_state *st, + int *, u16 *, u16 *, u16 *, u16 *); + int (*get_cal_data)(struct inv_compass_state *); +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_YAS53X_SCAN_MAGN_X, + INV_YAS53X_SCAN_MAGN_Y, + INV_YAS53X_SCAN_MAGN_Z, + INV_YAS53X_SCAN_TIMESTAMP, +}; + +#define YAS530_REGADDR_DEVICE_ID 0x80 +#define YAS530_REGADDR_ACTUATE_INIT_COIL 0x81 +#define YAS530_REGADDR_MEASURE_COMMAND 0x82 +#define YAS530_REGADDR_CONFIG 0x83 +#define YAS530_REGADDR_MEASURE_INTERVAL 0x84 +#define YAS530_REGADDR_OFFSET_X 0x85 +#define YAS530_REGADDR_OFFSET_Y1 0x86 +#define YAS530_REGADDR_OFFSET_Y2 0x87 +#define YAS530_REGADDR_TEST1 0x88 +#define YAS530_REGADDR_TEST2 0x89 +#define YAS530_REGADDR_CAL 0x90 +#define YAS530_REGADDR_MEASURE_DATA 0xb0 + +#define YAS530_MAX_DELAY 200 +#define YAS530_MIN_DELAY 5 +#define YAS530_SCALE 107374182L + +#define YAS_YAS530_VERSION_A 0 /* YAS530 (MS-3E Aver) */ +#define YAS_YAS530_VERSION_B 1 /* YAS530B (MS-3E Bver) */ +#define YAS_YAS530_VERSION_A_COEF 380 +#define YAS_YAS530_VERSION_B_COEF 550 +#define YAS_YAS530_DATA_CENTER 2048 +#define YAS_YAS530_DATA_OVERFLOW 4095 +#define YAS_YAS530_CAL_DATA_SIZE 16 + +/*filter related defines */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_X 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_Y 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_Z 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_LEN 20 + +#define YAS530_MAG_DEFAULT_FILTER_THRESH 100 +#define YAS532_MAG_DEFAULT_FILTER_THRESH 300 + +#define YAS_YAS532_533_VERSION_AB 0 /* YAS532_533AB (MS-3R/3F ABver) */ +#define YAS_YAS532_533_VERSION_AC 1 /* YAS532_533AC (MS-3R/3F ACver) */ +#define YAS_YAS532_533_VERSION_AB_COEF 1800 +#define YAS_YAS532_533_VERSION_AC_COEF 900 +#define YAS_YAS532_533_DATA_CENTER 4096 +#define YAS_YAS532_533_DATA_OVERFLOW 8190 +#define YAS_YAS532_533_CAL_DATA_SIZE 14 + +#define YAS_MAG_DISTURBURNCE_THRESHOLD 1600 +#define YAS_RESET_COIL_TIME_THRESHOLD 3000 + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + +int inv_yas53x_configure_ring(struct iio_dev *indio_dev); +void inv_yas53x_unconfigure_ring(struct iio_dev *indio_dev); +int inv_yas53x_probe_trigger(struct iio_dev *indio_dev); +void inv_yas53x_remove_trigger(struct iio_dev *indio_dev); +void set_yas53x_enable(struct iio_dev *indio_dev, bool enable); +void inv_read_yas53x_fifo(struct iio_dev *indio_dev); +int yas53x_read(struct inv_compass_state *st, short rawfixed[3], + s32 *overunderflow); +int yas53x_resume(struct inv_compass_state *st); + +#endif /* #ifndef _INV_GYRO_H_ */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_ring.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_ring.c new file mode 100644 index 000000000000..efcf49c68393 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_ring.c @@ -0,0 +1,165 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_ring.c + * @brief Invensense implementation for yas530/yas532/yas533. + * @details This driver currently works for the yas530/yas532/yas533. + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_yas53x_iio.h" + +static s64 get_time_ns(void) +{ + struct timespec ts; + ktime_get_ts(&ts); + + return timespec_to_ns(&ts); +} + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) +{ + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + + return d_ind; +} + +/** + * inv_read_yas53x_fifo() - Transfer data from FIFO to ring buffer. + */ +void inv_read_yas53x_fifo(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int d_ind; + s32 overunderflow; + s8 *tmp; + s64 tmp_buf[2]; + + if (!yas53x_read(st, st->compass_data, &overunderflow)) { + tmp = (u8 *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_YAS53X_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7) / 8] = get_time_ns(); + ring->access->store_to(indio_dev->buffer, tmp, 0); + + if (overunderflow) { + yas53x_resume(st); + if (!st->overunderflow) + st->overunderflow = 1; + } + } +} + +void inv_yas53x_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; + +static int inv_yas53x_postenable(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_Z))) + return 0; + + set_yas53x_enable(indio_dev, true); + schedule_delayed_work(&st->work, + msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_yas53x_predisable(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_YAS53X_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_YAS53X_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_YAS53X_SCAN_MAGN_Z, ring->scan_mask); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_yas53x_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_yas53x_postenable, + .predisable = &inv_yas53x_predisable, +}; + +int inv_yas53x_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_yas53x_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_trigger.c b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_trigger.c new file mode 100644 index 000000000000..a20ce2baa7e0 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_compass/inv_yas53x_trigger.c @@ -0,0 +1,91 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_trigger.c + * @brief Invensense implementation for yas530/yas532/yas533 + * @details This driver currently works for the yas530/yas532/yas533 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" + +#include "inv_yas53x_iio.h" + +static const struct iio_trigger_ops inv_yas53x_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_yas53x_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_compass_state *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->client->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_yas53x_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_yas53x_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} +/** + * @} + */ + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu3050_iio.c b/drivers/iio/imu/inv_mpu/inv_mpu3050_iio.c new file mode 100644 index 000000000000..1fc5ff980972 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu3050_iio.c @@ -0,0 +1,271 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_mpu_iio.h" +#define MPU3050_NACK_MIN_TIME (2 * 1000) +#define MPU3050_NACK_MAX_TIME (3 * 1000) + +#define MPU3050_ONE_MPU_TIME 20 +#define MPU3050_BOGUS_ADDR 0x7F +int __attribute__((weak)) inv_register_mpu3050_slave(struct inv_mpu_state *st) +{ + return 0; +} + +int set_3050_bypass(struct inv_mpu_state *st, bool enable) +{ + struct inv_reg_map_s *reg; + int result; + u8 b; + + reg = &st->reg; + result = inv_i2c_read(st, reg->user_ctrl, 1, &b); + if (result) + return result; + if (((b & BIT_3050_AUX_IF_EN) == 0) && enable) + return 0; + if ((b & BIT_3050_AUX_IF_EN) && (enable == 0)) + return 0; + b &= ~BIT_3050_AUX_IF_EN; + if (!enable) { + b |= BIT_3050_AUX_IF_EN; + result = inv_i2c_single_write(st, reg->user_ctrl, b); + return result; + } else { + /* Coming out of I2C is tricky due to several erratta. Do not + * modify this algorithm + */ + /* + * 1) wait for the right time and send the command to change + * the aux i2c slave address to an invalid address that will + * get nack'ed + * + * 0x00 is broadcast. 0x7F is unlikely to be used by any aux. + */ + result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR, + MPU3050_BOGUS_ADDR); + if (result) + return result; + /* + * 2) wait enough time for a nack to occur, then go into + * bypass mode: + */ + usleep_range(MPU3050_NACK_MIN_TIME, MPU3050_NACK_MAX_TIME); + result = inv_i2c_single_write(st, reg->user_ctrl, b); + if (result) + return result; + /* + * 3) wait for up to one MPU cycle then restore the slave + * address + */ + msleep(MPU3050_ONE_MPU_TIME); + + result = inv_i2c_single_write(st, REG_3050_SLAVE_ADDR, + st->plat_data.secondary_i2c_addr); + if (result) + return result; + result = inv_i2c_single_write(st, reg->user_ctrl, b); + if (result) + return result; + usleep_range(MPU3050_NACK_MIN_TIME, MPU3050_NACK_MAX_TIME); + } + return 0; +} + +void inv_setup_reg_mpu3050(struct inv_reg_map_s *reg) +{ + reg->fifo_en = REG_3050_FIFO_EN; + reg->sample_rate_div = REG_3050_SAMPLE_RATE_DIV; + reg->lpf = REG_3050_LPF; + reg->fifo_count_h = REG_3050_FIFO_COUNT_H; + reg->fifo_r_w = REG_3050_FIFO_R_W; + reg->user_ctrl = REG_3050_USER_CTRL; + reg->pwr_mgmt_1 = REG_3050_PWR_MGMT_1; + reg->raw_accel = REG_3050_AUX_XOUT_H; + reg->temperature = REG_3050_TEMPERATURE; + reg->int_enable = REG_3050_INT_ENABLE; + reg->int_status = REG_3050_INT_STATUS; +} + +int inv_switch_3050_gyro_engine(struct inv_mpu_state *st, bool en) +{ + struct inv_reg_map_s *reg; + u8 data, p; + int result; + reg = &st->reg; + if (en) { + data = INV_CLK_PLL; + p = (BITS_3050_POWER1 | data); + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p); + if (result) + return result; + p = (BITS_3050_POWER2 | data); + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p); + if (result) + return result; + p = data; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p); + msleep(SENSOR_UP_TIME); + } else { + p = BITS_3050_GYRO_STANDBY; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, p); + } + + return result; +} + +int inv_switch_3050_accel_engine(struct inv_mpu_state *st, bool en) +{ + int result; + if (NULL == st->slave_accel) + return -EPERM; + if (en) + result = st->slave_accel->resume(st); + else + result = st->slave_accel->suspend(st); + + return result; +} + +/** + * inv_init_config_mpu3050() - Initialize hardware, disable FIFO. + * @st: Device driver instance. + * Initial configuration: + * FSR: +/- 2000DPS + * DLPF: 42Hz + * FIFO rate: 50Hz + * Clock source: Gyro PLL + */ +int inv_init_config_mpu3050(struct iio_dev *indio_dev) +{ + struct inv_reg_map_s *reg; + int result; + u8 data; + struct inv_mpu_state *st = iio_priv(indio_dev); + + if (st->chip_config.is_asleep) + return -EPERM; + /*reading AUX VDDIO register */ + result = inv_i2c_read(st, REG_3050_AUX_VDDIO, 1, &data); + if (result) + return result; + data &= ~BIT_3050_VDDIO; + if (st->plat_data.level_shifter) + data |= BIT_3050_VDDIO; + result = inv_i2c_single_write(st, REG_3050_AUX_VDDIO, data); + if (result) + return result; + + reg = &st->reg; + /*2000dps full scale range*/ + result = inv_i2c_single_write(st, reg->lpf, + (INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT) + | INV_FILTER_42HZ); + if (result) + return result; + st->chip_config.fsr = INV_FSR_2000DPS; + st->chip_config.lpf = INV_FILTER_42HZ; + st->chip_info.multi = 1; + result = inv_i2c_single_write(st, reg->sample_rate_div, + ONE_K_HZ/INIT_FIFO_RATE - 1); + if (result) + return result; + st->chip_config.fifo_rate = INIT_FIFO_RATE; + st->irq_dur_ns = INIT_DUR_TIME; + st->chip_config.prog_start_addr = DMP_START_ADDR; + if ((SECONDARY_SLAVE_TYPE_ACCEL == st->plat_data.sec_slave_type) && + st->slave_accel) { + result = st->slave_accel->setup(st); + if (result) + return result; + result = st->slave_accel->set_fs(st, INV_FS_02G); + if (result) + return result; + result = st->slave_accel->set_lpf(st, INIT_FIFO_RATE); + if (result) + return result; + } + + return 0; +} + +/** + * set_power_mpu3050() - set power of mpu3050. + * @st: Device driver instance. + * @power_on: on/off + */ +int set_power_mpu3050(struct inv_mpu_state *st, bool power_on) +{ + struct inv_reg_map_s *reg; + u8 data, p; + int result; + reg = &st->reg; + if (power_on) { + data = 0; + } else { + if (st->slave_accel) { + result = st->slave_accel->suspend(st); + if (result) + return result; + } + data = BIT_SLEEP; + } + if (st->chip_config.gyro_enable) { + p = (BITS_3050_POWER1 | INV_CLK_PLL); + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p); + if (result) + return result; + + p = (BITS_3050_POWER2 | INV_CLK_PLL); + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p); + if (result) + return result; + + p = INV_CLK_PLL; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data | p); + if (result) + return result; + } else { + data |= (BITS_3050_GYRO_STANDBY | INV_CLK_INTERNAL); + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data); + if (result) + return result; + } + if (power_on) { + msleep(POWER_UP_TIME); + if (st->slave_accel) { + result = st->slave_accel->resume(st); + if (result) + return result; + } + } + st->chip_config.is_asleep = !power_on; + + return 0; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_core.c b/drivers/iio/imu/inv_mpu/inv_mpu_core.c new file mode 100644 index 000000000000..16d6b65015c3 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_core.c @@ -0,0 +1,3003 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> +#include <linux/iio/sysfs.h> + +#include "inv_mpu_iio.h" +#include "inv_test/inv_counters.h" + +#ifdef CONFIG_DTS_INV_MPU_IIO +#include "inv_mpu_dts.h" +#endif + +s64 get_time_ns(void) +{ + struct timespec ts; + ktime_get_ts(&ts); + return timespec_to_ns(&ts); +} + +/* This is for compatibility for power state. Should remove once HAL + does not use power_state sysfs entry */ +static bool fake_asleep; + +static const struct inv_hw_s hw_info[INV_NUM_PARTS] = { + {119, "ITG3500"}, + { 63, "MPU3050"}, + {117, "MPU6050"}, + {118, "MPU9150"}, + {128, "MPU6500"}, + {128, "MPU9250"}, + {128, "MPU9350"}, + {128, "MPU6515"}, +}; + +static const u8 reg_gyro_offset[] = {REG_XG_OFFS_USRH, + REG_XG_OFFS_USRH + 2, + REG_XG_OFFS_USRH + 4}; + +const u8 reg_6050_accel_offset[] = {REG_XA_OFFS_H, + REG_XA_OFFS_H + 2, + REG_XA_OFFS_H + 4}; + +const u8 reg_6500_accel_offset[] = {REG_6500_XA_OFFS_H, + REG_6500_YA_OFFS_H, + REG_6500_ZA_OFFS_H}; +#ifdef CONFIG_INV_TESTING +static bool suspend_state; +static int inv_mpu_suspend(struct device *dev); +static int inv_mpu_resume(struct device *dev); +struct test_data_out { + bool gyro; + bool accel; + bool compass; + bool pressure; + bool LPQ; + bool SIXQ; + bool PEDQ; +}; +static struct test_data_out data_out_control; +#endif + +static void inv_setup_reg(struct inv_reg_map_s *reg) +{ + reg->sample_rate_div = REG_SAMPLE_RATE_DIV; + reg->lpf = REG_CONFIG; + reg->bank_sel = REG_BANK_SEL; + reg->user_ctrl = REG_USER_CTRL; + reg->fifo_en = REG_FIFO_EN; + reg->gyro_config = REG_GYRO_CONFIG; + reg->accel_config = REG_ACCEL_CONFIG; + reg->fifo_count_h = REG_FIFO_COUNT_H; + reg->fifo_r_w = REG_FIFO_R_W; + reg->raw_accel = REG_RAW_ACCEL; + reg->temperature = REG_TEMPERATURE; + reg->int_enable = REG_INT_ENABLE; + reg->int_status = REG_INT_STATUS; + reg->pwr_mgmt_1 = REG_PWR_MGMT_1; + reg->pwr_mgmt_2 = REG_PWR_MGMT_2; + reg->mem_start_addr = REG_MEM_START_ADDR; + reg->mem_r_w = REG_MEM_RW; + reg->prgm_strt_addrh = REG_PRGM_STRT_ADDRH; +}; + +/** + * inv_i2c_read_base() - Read one or more bytes from the device registers. + * @st: Device driver instance. + * @i2c_addr: i2c address of device. + * @reg: First device register to be read from. + * @length: Number of bytes to read. + * @data: Data read from device. + * NOTE:This is not re-implementation of i2c_smbus_read because i2c + * address could be specified in this case. We could have two different + * i2c address due to secondary i2c interface. + */ +int inv_i2c_read_base(struct inv_mpu_state *st, u16 i2c_addr, + u8 reg, u16 length, u8 *data) +{ + struct i2c_msg msgs[2]; + int res; + + if (!data) + return -EINVAL; + + msgs[0].addr = i2c_addr; + msgs[0].flags = 0; /* write */ + msgs[0].buf = ® + msgs[0].len = 1; + + msgs[1].addr = i2c_addr; + msgs[1].flags = I2C_M_RD; + msgs[1].buf = data; + msgs[1].len = length; + + res = i2c_transfer(st->sl_handle, msgs, 2); + + if (res < 2) { + if (res >= 0) + res = -EIO; + } else + res = 0; + + INV_I2C_INC_MPUWRITE(3); + INV_I2C_INC_MPUREAD(length); +#if CONFIG_DYNAMIC_DEBUG + { + char *read = 0; + pr_debug("%s RD%02X%02X%02X -> %s%s\n", st->hw->name, + i2c_addr, reg, length, + wr_pr_debug_begin(data, length, read), + wr_pr_debug_end(read)); + } +#endif + return res; +} + +/** + * inv_i2c_single_write_base() - Write a byte to a device register. + * @st: Device driver instance. + * @i2c_addr: I2C address of the device. + * @reg: Device register to be written to. + * @data: Byte to write to device. + * NOTE:This is not re-implementation of i2c_smbus_write because i2c + * address could be specified in this case. We could have two different + * i2c address due to secondary i2c interface. + */ +int inv_i2c_single_write_base(struct inv_mpu_state *st, + u16 i2c_addr, u8 reg, u8 data) +{ + u8 tmp[2]; + struct i2c_msg msg; + int res; + tmp[0] = reg; + tmp[1] = data; + + msg.addr = i2c_addr; + msg.flags = 0; /* write */ + msg.buf = tmp; + msg.len = 2; + + pr_debug("%s WR%02X%02X%02X\n", st->hw->name, i2c_addr, reg, data); + INV_I2C_INC_MPUWRITE(3); + + res = i2c_transfer(st->sl_handle, &msg, 1); + if (res < 1) { + if (res == 0) + res = -EIO; + return res; + } else + return 0; +} + +static int inv_switch_engine(struct inv_mpu_state *st, bool en, u32 mask) +{ + struct inv_reg_map_s *reg; + u8 data, mgmt_1; + int result; + + reg = &st->reg; + /* Only when gyro is on, can + clock source be switched to gyro. Otherwise, it must be set to + internal clock */ + if (BIT_PWR_GYRO_STBY == mask) { + result = inv_i2c_read(st, reg->pwr_mgmt_1, 1, &mgmt_1); + if (result) + return result; + + mgmt_1 &= ~BIT_CLK_MASK; + } + + if ((BIT_PWR_GYRO_STBY == mask) && (!en)) { + /* turning off gyro requires switch to internal clock first. + Then turn off gyro engine */ + mgmt_1 |= INV_CLK_INTERNAL; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, + mgmt_1); + if (result) + return result; + } + + result = inv_i2c_read(st, reg->pwr_mgmt_2, 1, &data); + if (result) + return result; + if (en) + data &= (~mask); + else + data |= mask; + result = inv_i2c_single_write(st, reg->pwr_mgmt_2, data); + if (result) + return result; + + if ((BIT_PWR_GYRO_STBY == mask) && en) { + /* only gyro on needs sensor up time */ + msleep(SENSOR_UP_TIME); + /* after gyro is on & stable, switch internal clock to PLL */ + mgmt_1 |= INV_CLK_PLL; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, + mgmt_1); + if (result) + return result; + } + if ((BIT_PWR_ACCEL_STBY == mask) && en) + msleep(REG_UP_TIME); + + return 0; +} + +/* + * inv_lpa_freq() - store current low power frequency setting. + */ +static int inv_lpa_freq(struct inv_mpu_state *st, int lpa_freq) +{ + unsigned long result; + u8 d; + /* 2, 4, 6, 7 corresponds to 0.98, 3.91, 15.63, 31.25 */ + const u8 mpu6500_lpa_mapping[] = {2, 4, 6, 7}; + + if (lpa_freq > MAX_LPA_FREQ_PARAM) + return -EINVAL; + + if (INV_MPU6500 == st->chip_type) { + d = mpu6500_lpa_mapping[lpa_freq]; + result = inv_i2c_single_write(st, REG_6500_LP_ACCEL_ODR, d); + if (result) + return result; + } + st->chip_config.lpa_freq = lpa_freq; + + return 0; +} + +static int set_power_itg(struct inv_mpu_state *st, bool power_on) +{ + struct inv_reg_map_s *reg; + u8 data; + int result; + + if ((!power_on) == st->chip_config.is_asleep) + return 0; + reg = &st->reg; + if (power_on) + data = 0; + else + data = BIT_SLEEP; + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, data); + if (result) + return result; + + if (power_on) + msleep(REG_UP_TIME); + + st->chip_config.is_asleep = !power_on; + + return 0; +} + +/** + * inv_init_config() - Initialize hardware, disable FIFO. + * @indio_dev: Device driver instance. + * Initial configuration: + * FSR: +/- 2000DPS + * DLPF: 42Hz + * FIFO rate: 50Hz + */ +static int inv_init_config(struct iio_dev *indio_dev) +{ + struct inv_reg_map_s *reg; + int result, i; + struct inv_mpu_state *st = iio_priv(indio_dev); + const u8 *ch; + u8 d[2]; + + reg = &st->reg; + + result = inv_i2c_single_write(st, reg->gyro_config, + INV_FSR_2000DPS << GYRO_CONFIG_FSR_SHIFT); + if (result) + return result; + + st->chip_config.fsr = INV_FSR_2000DPS; + + result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_42HZ); + if (result) + return result; + st->chip_config.lpf = INV_FILTER_42HZ; + + result = inv_i2c_single_write(st, reg->sample_rate_div, + ONE_K_HZ / INIT_FIFO_RATE - 1); + if (result) + return result; + st->chip_config.fifo_rate = INIT_FIFO_RATE; + st->irq_dur_ns = INIT_DUR_TIME; + st->chip_config.prog_start_addr = DMP_START_ADDR; + if (INV_MPU6050 == st->chip_type) + st->self_test.samples = INIT_ST_MPU6050_SAMPLES; + else + st->self_test.samples = INIT_ST_SAMPLES; + st->self_test.threshold = INIT_ST_THRESHOLD; + st->batch.wake_fifo_on = true; + if (INV_ITG3500 != st->chip_type) { + st->chip_config.accel_fs = INV_FS_02G; + result = inv_i2c_single_write(st, reg->accel_config, + (INV_FS_02G << ACCEL_CONFIG_FSR_SHIFT)); + if (result) + return result; + st->tap.time = INIT_TAP_TIME; + st->tap.thresh = INIT_TAP_THRESHOLD; + st->tap.min_count = INIT_TAP_MIN_COUNT; + st->sample_divider = INIT_SAMPLE_DIVIDER; + st->smd.threshold = MPU_INIT_SMD_THLD; + st->smd.delay = MPU_INIT_SMD_DELAY_THLD; + st->smd.delay2 = MPU_INIT_SMD_DELAY2_THLD; + st->ped.int_thresh = INIT_PED_INT_THRESH; + st->ped.step_thresh = INIT_PED_THRESH; + st->sensor[SENSOR_STEP].rate = MAX_DMP_OUTPUT_RATE; + + result = inv_i2c_single_write(st, REG_ACCEL_MOT_DUR, + INIT_MOT_DUR); + if (result) + return result; + st->mot_int.mot_dur = INIT_MOT_DUR; + + result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR, + INIT_MOT_THR); + if (result) + return result; + st->mot_int.mot_thr = INIT_MOT_THR; + + for (i = 0; i < 3; i++) { + result = inv_i2c_read(st, reg_gyro_offset[i], 2, d); + if (result) + return result; + st->rom_gyro_offset[i] = + (short)be16_to_cpup((__be16 *)(d)); + st->input_gyro_offset[i] = 0; + st->input_gyro_dmp_bias[i] = 0; + } + if (INV_MPU6050 == st->chip_type) + ch = reg_6050_accel_offset; + else + ch = reg_6500_accel_offset; + for (i = 0; i < 3; i++) { + result = inv_i2c_read(st, ch[i], 2, d); + if (result) + return result; + st->rom_accel_offset[i] = + (short)be16_to_cpup((__be16 *)(d)); + st->input_accel_offset[i] = 0; + st->input_accel_dmp_bias[i] = 0; + } + st->ped.step = 0; + st->ped.time = 0; + } + + return 0; +} + +/* + * inv_write_fsr() - Configure the gyro's scale range. + */ +static int inv_write_fsr(struct inv_mpu_state *st, int fsr) +{ + struct inv_reg_map_s *reg; + int result; + + reg = &st->reg; + if ((fsr < 0) || (fsr > MAX_GYRO_FS_PARAM)) + return -EINVAL; + if (fsr == st->chip_config.fsr) + return 0; + + if (INV_MPU3050 == st->chip_type) + result = inv_i2c_single_write(st, reg->lpf, + (fsr << GYRO_CONFIG_FSR_SHIFT) | st->chip_config.lpf); + else + result = inv_i2c_single_write(st, reg->gyro_config, + fsr << GYRO_CONFIG_FSR_SHIFT); + + if (result) + return result; + st->chip_config.fsr = fsr; + + return 0; +} + +/* + * inv_write_accel_fs() - Configure the accelerometer's scale range. + */ +static int inv_write_accel_fs(struct inv_mpu_state *st, int fs) +{ + int result; + struct inv_reg_map_s *reg; + + reg = &st->reg; + if (fs < 0 || fs > MAX_ACCEL_FS_PARAM) + return -EINVAL; + if (fs == st->chip_config.accel_fs) + return 0; + if (INV_MPU3050 == st->chip_type) + result = st->slave_accel->set_fs(st, fs); + else + result = inv_i2c_single_write(st, reg->accel_config, + (fs << ACCEL_CONFIG_FSR_SHIFT)); + if (result) + return result; + + st->chip_config.accel_fs = fs; + + return 0; +} + +static int inv_set_offset_reg(struct inv_mpu_state *st, int reg, int val) +{ + int result; + u8 d; + + d = ((val >> 8) & 0xff); + result = inv_i2c_single_write(st, reg, d); + if (result) + return result; + + d = (val & 0xff); + result = inv_i2c_single_write(st, reg + 1, d); + + return result; +} + +int inv_reset_offset_reg(struct inv_mpu_state *st, bool en) +{ + const u8 *ch; + int i, result; + s16 gyro[3], accel[3]; + + if (en) { + for (i = 0; i < 3; i++) { + gyro[i] = st->rom_gyro_offset[i]; + accel[i] = st->rom_accel_offset[i]; + } + } else { + for (i = 0; i < 3; i++) { + gyro[i] = st->rom_gyro_offset[i] + + st->input_gyro_offset[i]; + accel[i] = st->rom_accel_offset[i] + + (st->input_accel_offset[i] << 1); + } + } + if (INV_MPU6050 == st->chip_type) + ch = reg_6050_accel_offset; + else + ch = reg_6500_accel_offset; + + for (i = 0; i < 3; i++) { + result = inv_set_offset_reg(st, reg_gyro_offset[i], gyro[i]); + if (result) + return result; + result = inv_set_offset_reg(st, ch[i], accel[i]); + if (result) + return result; + } + + return 0; +} +/* + * inv_fifo_rate_store() - Set fifo rate. + */ +static int inv_fifo_rate_store(struct inv_mpu_state *st, int fifo_rate) +{ + if ((fifo_rate < MIN_FIFO_RATE) || (fifo_rate > MAX_FIFO_RATE)) + return -EINVAL; + if (fifo_rate == st->chip_config.fifo_rate) + return 0; + + st->irq_dur_ns = NSEC_PER_SEC / fifo_rate; + st->chip_config.fifo_rate = fifo_rate; + + return 0; +} + +/* + * inv_reg_dump_show() - Register dump for testing. + */ +static ssize_t inv_reg_dump_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + int ii; + char data; + ssize_t bytes_printed = 0; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + for (ii = 0; ii < st->hw->num_reg; ii++) { + /* don't read fifo r/w register */ + if (ii == st->reg.fifo_r_w) + data = 0; + else + inv_i2c_read(st, ii, 1, &data); + bytes_printed += sprintf(buf + bytes_printed, "%#2x: %#2x\n", + ii, data); + } + mutex_unlock(&indio_dev->mlock); + + return bytes_printed; +} + +int write_be32_key_to_mem(struct inv_mpu_state *st, + u32 data, int key) +{ + cpu_to_be32s(&data); + return mem_w_key(key, sizeof(data), (u8 *)&data); +} + +int inv_write_2bytes(struct inv_mpu_state *st, int k, int data) +{ + u8 d[2]; + + if (data < 0 || data > USHRT_MAX) + return -EINVAL; + + d[0] = (u8)((data >> 8) & 0xff); + d[1] = (u8)(data & 0xff); + + return mem_w_key(k, ARRAY_SIZE(d), d); +} + +static ssize_t _dmp_bias_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, data, tmp; + + if (!st->chip_config.firmware_loaded) + return -EINVAL; + + if (!st->chip_config.enable) { + result = st->set_power_state(st, true); + if (result) + return result; + } + + result = kstrtoint(buf, 10, &data); + if (result) + goto dmp_bias_store_fail; + switch (this_attr->address) { + case ATTR_DMP_ACCEL_X_DMP_BIAS: + tmp = st->input_accel_dmp_bias[0]; + st->input_accel_dmp_bias[0] = data; + result = inv_set_accel_bias_dmp(st); + if (result) + st->input_accel_dmp_bias[0] = tmp; + break; + case ATTR_DMP_ACCEL_Y_DMP_BIAS: + tmp = st->input_accel_dmp_bias[1]; + st->input_accel_dmp_bias[1] = data; + result = inv_set_accel_bias_dmp(st); + if (result) + st->input_accel_dmp_bias[1] = tmp; + break; + case ATTR_DMP_ACCEL_Z_DMP_BIAS: + tmp = st->input_accel_dmp_bias[2]; + st->input_accel_dmp_bias[2] = data; + result = inv_set_accel_bias_dmp(st); + if (result) + st->input_accel_dmp_bias[2] = tmp; + break; + case ATTR_DMP_GYRO_X_DMP_BIAS: + result = write_be32_key_to_mem(st, data, + KEY_CFG_EXT_GYRO_BIAS_X); + if (result) + goto dmp_bias_store_fail; + st->input_gyro_dmp_bias[0] = data; + break; + case ATTR_DMP_GYRO_Y_DMP_BIAS: + result = write_be32_key_to_mem(st, data, + KEY_CFG_EXT_GYRO_BIAS_Y); + if (result) + goto dmp_bias_store_fail; + st->input_gyro_dmp_bias[1] = data; + break; + case ATTR_DMP_GYRO_Z_DMP_BIAS: + result = write_be32_key_to_mem(st, data, + KEY_CFG_EXT_GYRO_BIAS_Z); + if (result) + goto dmp_bias_store_fail; + st->input_gyro_dmp_bias[2] = data; + break; + default: + break; + } + +dmp_bias_store_fail: + if (!st->chip_config.enable) + result |= st->set_power_state(st, false); + if (result) + return result; + + return count; +} + +static ssize_t inv_dmp_bias_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _dmp_bias_store(dev, attr, buf, count); + mutex_unlock(&indio_dev->mlock); + + return result; +} + +static ssize_t _dmp_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, data; + + if (st->chip_config.enable) + return -EBUSY; + if (this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) { + if (!st->chip_config.firmware_loaded) + return -EINVAL; + result = st->set_power_state(st, true); + if (result) + return result; + } + + result = kstrtoint(buf, 10, &data); + if (result) + goto dmp_attr_store_fail; + switch (this_attr->address) { + case ATTR_DMP_PED_INT_ON: + result = inv_enable_pedometer_interrupt(st, !!data); + if (result) + goto dmp_attr_store_fail; + st->ped.int_on = !!data; + break; + case ATTR_DMP_PED_ON: + { + result = inv_enable_pedometer(st, !!data); + if (result) + goto dmp_attr_store_fail; + st->ped.on = !!data; + break; + } + case ATTR_DMP_PED_STEP_THRESH: + { + result = inv_write_2bytes(st, KEY_D_PEDSTD_SB, data); + if (result) + goto dmp_attr_store_fail; + st->ped.step_thresh = data; + break; + } + case ATTR_DMP_PED_INT_THRESH: + { + result = inv_write_2bytes(st, KEY_D_PEDSTD_SB2, data); + if (result) + goto dmp_attr_store_fail; + st->ped.int_thresh = data; + break; + } + case ATTR_DMP_SMD_ENABLE: + result = inv_write_2bytes(st, KEY_SMD_ENABLE, !!data); + if (result) + goto dmp_attr_store_fail; + st->chip_config.smd_enable = !!data; + break; + case ATTR_DMP_SMD_THLD: + if (data < 0 || data > SHRT_MAX) + goto dmp_attr_store_fail; + result = write_be32_key_to_mem(st, data << 16, + KEY_SMD_ACCEL_THLD); + if (result) + goto dmp_attr_store_fail; + st->smd.threshold = data; + break; + case ATTR_DMP_SMD_DELAY_THLD: + if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ) + goto dmp_attr_store_fail; + result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ, + KEY_SMD_DELAY_THLD); + if (result) + goto dmp_attr_store_fail; + st->smd.delay = data; + break; + case ATTR_DMP_SMD_DELAY_THLD2: + if (data < 0 || data > INT_MAX / MPU_DEFAULT_DMP_FREQ) + goto dmp_attr_store_fail; + result = write_be32_key_to_mem(st, data * MPU_DEFAULT_DMP_FREQ, + KEY_SMD_DELAY2_THLD); + if (result) + goto dmp_attr_store_fail; + st->smd.delay2 = data; + break; + case ATTR_DMP_TAP_ON: + result = inv_enable_tap_dmp(st, !!data); + if (result) + goto dmp_attr_store_fail; + st->tap.on = !!data; + break; + case ATTR_DMP_TAP_THRESHOLD: + if (data < 0 || data > USHRT_MAX) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + result = inv_set_tap_threshold_dmp(st, data); + if (result) + goto dmp_attr_store_fail; + st->tap.thresh = data; + break; + case ATTR_DMP_TAP_MIN_COUNT: + if (data < 0 || data > USHRT_MAX) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + result = inv_set_min_taps_dmp(st, data); + if (result) + goto dmp_attr_store_fail; + st->tap.min_count = data; + break; + case ATTR_DMP_TAP_TIME: + if (data < 0 || data > USHRT_MAX) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + result = inv_set_tap_time_dmp(st, data); + if (result) + goto dmp_attr_store_fail; + st->tap.time = data; + break; + case ATTR_DMP_DISPLAY_ORIENTATION_ON: + result = inv_set_display_orient_interrupt_dmp(st, !!data); + if (result) + goto dmp_attr_store_fail; + st->chip_config.display_orient_on = !!data; + break; + /* from here, power of chip is not turned on */ + case ATTR_DMP_ON: + st->chip_config.dmp_on = !!data; + break; + case ATTR_DMP_INT_ON: + st->chip_config.dmp_int_on = !!data; + break; + case ATTR_DMP_EVENT_INT_ON: + st->chip_config.dmp_event_int_on = !!data; + break; + case ATTR_DMP_STEP_INDICATOR_ON: + st->chip_config.step_indicator_on = !!data; + break; + case ATTR_DMP_BATCHMODE_TIMEOUT: + if (data < 0 || data > INT_MAX) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + st->batch.timeout = data; + break; + case ATTR_DMP_BATCHMODE_WAKE_FIFO_FULL: + st->batch.wake_fifo_on = !!data; + st->batch.overflow_on = 0; + break; + case ATTR_DMP_SIX_Q_ON: + st->sensor[SENSOR_SIXQ].on = !!data; + break; + case ATTR_DMP_SIX_Q_RATE: + if (data > MPU_DEFAULT_DMP_FREQ || data < 0) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + st->sensor[SENSOR_SIXQ].rate = data; + st->sensor[SENSOR_SIXQ].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_SIXQ].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_DMP_LPQ_ON: + st->sensor[SENSOR_LPQ].on = !!data; + break; + case ATTR_DMP_LPQ_RATE: + if (data > MPU_DEFAULT_DMP_FREQ || data < 0) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + st->sensor[SENSOR_LPQ].rate = data; + st->sensor[SENSOR_LPQ].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_LPQ].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_DMP_PED_Q_ON: + st->sensor[SENSOR_PEDQ].on = !!data; + break; + case ATTR_DMP_PED_Q_RATE: + if (data > MPU_DEFAULT_DMP_FREQ || data < 0) { + result = -EINVAL; + goto dmp_attr_store_fail; + } + st->sensor[SENSOR_PEDQ].rate = data; + st->sensor[SENSOR_PEDQ].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_PEDQ].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_DMP_STEP_DETECTOR_ON: + st->sensor[SENSOR_STEP].on = !!data; + break; +#ifdef CONFIG_INV_TESTING + case ATTR_DEBUG_SMD_ENABLE_TESTP1: + { + u8 d[] = {0x42}; + result = st->set_power_state(st, true); + if (result) + goto dmp_attr_store_fail; + result = mem_w_key(KEY_SMD_ENABLE_TESTPT1, ARRAY_SIZE(d), d); + if (result) + goto dmp_attr_store_fail; + } + break; + case ATTR_DEBUG_SMD_ENABLE_TESTP2: + { + u8 d[] = {0x42}; + result = st->set_power_state(st, true); + if (result) + goto dmp_attr_store_fail; + result = mem_w_key(KEY_SMD_ENABLE_TESTPT2, ARRAY_SIZE(d), d); + if (result) + goto dmp_attr_store_fail; + } + break; +#endif + default: + result = -EINVAL; + goto dmp_attr_store_fail; + } + +dmp_attr_store_fail: + if (this_attr->address <= ATTR_DMP_DISPLAY_ORIENTATION_ON) + result |= st->set_power_state(st, false); + if (result) + return result; + + return count; +} + +/* + * inv_dmp_attr_store() - calling this function will store current + * dmp parameter settings + */ +static ssize_t inv_dmp_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _dmp_attr_store(dev, attr, buf, count); + mutex_unlock(&indio_dev->mlock); + + return result; +} + +static ssize_t inv_attr64_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result; + u64 tmp; + u32 ped; + + mutex_lock(&indio_dev->mlock); + if (!st->chip_config.enable || !st->chip_config.dmp_on) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + result = 0; + switch (this_attr->address) { + case ATTR_DMP_PEDOMETER_STEPS: + result = inv_get_pedometer_steps(st, &ped); + result |= inv_read_pedometer_counter(st); + tmp = st->ped.step + ped; + break; + case ATTR_DMP_PEDOMETER_TIME: + result = inv_get_pedometer_time(st, &ped); + tmp = st->ped.time + ped; + break; + case ATTR_DMP_PEDOMETER_COUNTER: + tmp = st->ped.last_step_time; + break; + default: + result = -EINVAL; + break; + } + mutex_unlock(&indio_dev->mlock); + if (result) + return -EINVAL; + return sprintf(buf, "%lld\n", tmp); +} + +static ssize_t inv_attr64_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result; + u64 data; + + mutex_lock(&indio_dev->mlock); + if (st->chip_config.enable || (!st->chip_config.firmware_loaded)) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + result = st->set_power_state(st, true); + if (result) { + mutex_unlock(&indio_dev->mlock); + return result; + } + result = kstrtoull(buf, 10, &data); + if (result) + goto attr64_store_fail; + switch (this_attr->address) { + case ATTR_DMP_PEDOMETER_STEPS: + result = write_be32_key_to_mem(st, 0, KEY_D_PEDSTD_STEPCTR); + if (result) + goto attr64_store_fail; + st->ped.step = data; + break; + case ATTR_DMP_PEDOMETER_TIME: + result = write_be32_key_to_mem(st, 0, KEY_D_PEDSTD_TIMECTR); + if (result) + goto attr64_store_fail; + st->ped.time = data; + break; + default: + result = -EINVAL; + break; + } +attr64_store_fail: + mutex_unlock(&indio_dev->mlock); + result = st->set_power_state(st, false); + if (result) + return result; + + return count; +} +/* + * inv_attr_show() - calling this function will show current + * dmp parameters. + */ +static ssize_t inv_attr_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int result, axis; + s8 *m; + + switch (this_attr->address) { + case ATTR_GYRO_SCALE: + { + const s16 gyro_scale[] = {250, 500, 1000, 2000}; + + return sprintf(buf, "%d\n", gyro_scale[st->chip_config.fsr]); + } + case ATTR_ACCEL_SCALE: + { + const s16 accel_scale[] = {2, 4, 8, 16}; + return sprintf(buf, "%d\n", + accel_scale[st->chip_config.accel_fs] * + st->chip_info.multi); + } + case ATTR_COMPASS_SCALE: + st->slave_compass->get_scale(st, &result); + + return sprintf(buf, "%d\n", result); + case ATTR_ACCEL_X_CALIBBIAS: + case ATTR_ACCEL_Y_CALIBBIAS: + case ATTR_ACCEL_Z_CALIBBIAS: + axis = this_attr->address - ATTR_ACCEL_X_CALIBBIAS; + return sprintf(buf, "%d\n", st->accel_bias[axis] * + st->chip_info.multi); + case ATTR_GYRO_X_CALIBBIAS: + case ATTR_GYRO_Y_CALIBBIAS: + case ATTR_GYRO_Z_CALIBBIAS: + axis = this_attr->address - ATTR_GYRO_X_CALIBBIAS; + return sprintf(buf, "%d\n", st->gyro_bias[axis]); + case ATTR_SELF_TEST_GYRO_SCALE: + return sprintf(buf, "%d\n", SELF_TEST_GYRO_FULL_SCALE); + case ATTR_SELF_TEST_ACCEL_SCALE: + if (INV_MPU6500 == st->chip_type) + return sprintf(buf, "%d\n", SELF_TEST_ACCEL_6500_SCALE); + else + return sprintf(buf, "%d\n", SELF_TEST_ACCEL_FULL_SCALE); + case ATTR_GYRO_X_OFFSET: + case ATTR_GYRO_Y_OFFSET: + case ATTR_GYRO_Z_OFFSET: + axis = this_attr->address - ATTR_GYRO_X_OFFSET; + return sprintf(buf, "%d\n", st->input_gyro_offset[axis]); + case ATTR_ACCEL_X_OFFSET: + case ATTR_ACCEL_Y_OFFSET: + case ATTR_ACCEL_Z_OFFSET: + axis = this_attr->address - ATTR_ACCEL_X_OFFSET; + return sprintf(buf, "%d\n", st->input_accel_offset[axis]); + case ATTR_DMP_ACCEL_X_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_accel_dmp_bias[0]); + case ATTR_DMP_ACCEL_Y_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_accel_dmp_bias[1]); + case ATTR_DMP_ACCEL_Z_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_accel_dmp_bias[2]); + case ATTR_DMP_GYRO_X_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_gyro_dmp_bias[0]); + case ATTR_DMP_GYRO_Y_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_gyro_dmp_bias[1]); + case ATTR_DMP_GYRO_Z_DMP_BIAS: + return sprintf(buf, "%d\n", st->input_gyro_dmp_bias[2]); + case ATTR_DMP_PED_INT_ON: + return sprintf(buf, "%d\n", st->ped.int_on); + case ATTR_DMP_PED_ON: + return sprintf(buf, "%d\n", st->ped.on); + case ATTR_DMP_PED_STEP_THRESH: + return sprintf(buf, "%d\n", st->ped.step_thresh); + case ATTR_DMP_PED_INT_THRESH: + return sprintf(buf, "%d\n", st->ped.int_thresh); + case ATTR_DMP_SMD_ENABLE: + return sprintf(buf, "%d\n", st->chip_config.smd_enable); + case ATTR_DMP_SMD_THLD: + return sprintf(buf, "%d\n", st->smd.threshold); + case ATTR_DMP_SMD_DELAY_THLD: + return sprintf(buf, "%d\n", st->smd.delay); + case ATTR_DMP_SMD_DELAY_THLD2: + return sprintf(buf, "%d\n", st->smd.delay2); + case ATTR_DMP_TAP_ON: + return sprintf(buf, "%d\n", st->tap.on); + case ATTR_DMP_TAP_THRESHOLD: + return sprintf(buf, "%d\n", st->tap.thresh); + case ATTR_DMP_TAP_MIN_COUNT: + return sprintf(buf, "%d\n", st->tap.min_count); + case ATTR_DMP_TAP_TIME: + return sprintf(buf, "%d\n", st->tap.time); + case ATTR_DMP_DISPLAY_ORIENTATION_ON: + return sprintf(buf, "%d\n", + st->chip_config.display_orient_on); + case ATTR_DMP_ON: + return sprintf(buf, "%d\n", st->chip_config.dmp_on); + case ATTR_DMP_INT_ON: + return sprintf(buf, "%d\n", st->chip_config.dmp_int_on); + case ATTR_DMP_EVENT_INT_ON: + return sprintf(buf, "%d\n", st->chip_config.dmp_event_int_on); + case ATTR_DMP_STEP_INDICATOR_ON: + return sprintf(buf, "%d\n", st->chip_config.step_indicator_on); + case ATTR_DMP_BATCHMODE_TIMEOUT: + return sprintf(buf, "%d\n", + st->batch.timeout); + case ATTR_DMP_BATCHMODE_WAKE_FIFO_FULL: + return sprintf(buf, "%d\n", + st->batch.wake_fifo_on); + case ATTR_DMP_SIX_Q_ON: + return sprintf(buf, "%d\n", st->sensor[SENSOR_SIXQ].on); + case ATTR_DMP_SIX_Q_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_SIXQ].rate); + case ATTR_DMP_LPQ_ON: + return sprintf(buf, "%d\n", st->sensor[SENSOR_LPQ].on); + case ATTR_DMP_LPQ_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_LPQ].rate); + case ATTR_DMP_PED_Q_ON: + return sprintf(buf, "%d\n", st->sensor[SENSOR_PEDQ].on); + case ATTR_DMP_PED_Q_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_PEDQ].rate); + case ATTR_DMP_STEP_DETECTOR_ON: + return sprintf(buf, "%d\n", st->sensor[SENSOR_STEP].on); + case ATTR_MOTION_LPA_ON: + return sprintf(buf, "%d\n", st->mot_int.mot_on); + case ATTR_MOTION_LPA_FREQ:{ + const char *f[] = {"1.25", "5", "20", "40"}; + return sprintf(buf, "%s\n", f[st->chip_config.lpa_freq]); + } + case ATTR_MOTION_LPA_THRESHOLD: + return sprintf(buf, "%d\n", st->mot_int.mot_thr); + + case ATTR_SELF_TEST_SAMPLES: + return sprintf(buf, "%d\n", st->self_test.samples); + case ATTR_SELF_TEST_THRESHOLD: + return sprintf(buf, "%d\n", st->self_test.threshold); + case ATTR_GYRO_ENABLE: + return sprintf(buf, "%d\n", st->chip_config.gyro_enable); + case ATTR_GYRO_FIFO_ENABLE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_GYRO].on); + case ATTR_GYRO_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_GYRO].rate); + case ATTR_ACCEL_ENABLE: + return sprintf(buf, "%d\n", st->chip_config.accel_enable); + case ATTR_ACCEL_FIFO_ENABLE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_ACCEL].on); + case ATTR_ACCEL_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_ACCEL].rate); + case ATTR_COMPASS_ENABLE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_COMPASS].on); + case ATTR_COMPASS_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_COMPASS].rate); + case ATTR_PRESSURE_ENABLE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_PRESSURE].on); + case ATTR_PRESSURE_RATE: + return sprintf(buf, "%d\n", st->sensor[SENSOR_PRESSURE].rate); + case ATTR_POWER_STATE: + return sprintf(buf, "%d\n", !fake_asleep); + case ATTR_FIRMWARE_LOADED: + return sprintf(buf, "%d\n", st->chip_config.firmware_loaded); + case ATTR_SAMPLING_FREQ: + return sprintf(buf, "%d\n", st->chip_config.fifo_rate); + case ATTR_SELF_TEST: + mutex_lock(&indio_dev->mlock); + if (st->chip_config.enable) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + if (INV_MPU3050 == st->chip_type) + result = 1; + else + result = inv_hw_self_test(st); + mutex_unlock(&indio_dev->mlock); + return sprintf(buf, "%d\n", result); + case ATTR_GYRO_MATRIX: + m = st->plat_data.orientation; + return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); + case ATTR_ACCEL_MATRIX: + if (st->plat_data.sec_slave_type == + SECONDARY_SLAVE_TYPE_ACCEL) + m = + st->plat_data.secondary_orientation; + else + m = st->plat_data.orientation; + return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); + case ATTR_COMPASS_MATRIX: + if (st->plat_data.sec_slave_type == + SECONDARY_SLAVE_TYPE_COMPASS) + m = + st->plat_data.secondary_orientation; + else + return -ENODEV; + return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); + case ATTR_SECONDARY_NAME: + { + const char *n[] = {"NULL", "AK8975", "AK8972", "AK8963", + "BMA250", "MLX90399", "AK09911"}; + switch (st->plat_data.sec_slave_id) { + case COMPASS_ID_AK8975: + return sprintf(buf, "%s\n", n[1]); + case COMPASS_ID_AK8972: + return sprintf(buf, "%s\n", n[2]); + case COMPASS_ID_AK8963: + return sprintf(buf, "%s\n", n[3]); + case ACCEL_ID_BMA250: + return sprintf(buf, "%s\n", n[4]); + case COMPASS_ID_MLX90399: + return sprintf(buf, "%s\n", n[5]); + case COMPASS_ID_AK09911: + return sprintf(buf, "%s\n", n[6]); + default: + return sprintf(buf, "%s\n", n[0]); + } + } +#ifdef CONFIG_INV_TESTING + case ATTR_REG_WRITE: + return sprintf(buf, "1\n"); + case ATTR_COMPASS_SENS: + { + /* these 2 conditions should never be met, since the + 'compass_sens' sysfs entry should be hidden if the compass + is not an AKM */ + if (st->plat_data.sec_slave_type != + SECONDARY_SLAVE_TYPE_COMPASS) + return -ENODEV; + if (st->plat_data.sec_slave_id != COMPASS_ID_AK8975 && + st->plat_data.sec_slave_id != COMPASS_ID_AK8972 && + st->plat_data.sec_slave_id != COMPASS_ID_AK8963) + return -ENODEV; + m = st->chip_info.compass_sens; + return sprintf(buf, "%d,%d,%d\n", m[0], m[1], m[2]); + } + case ATTR_DEBUG_SMD_EXE_STATE: + { + u8 d[2]; + + result = st->set_power_state(st, true); + mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_SMD_EXE_STATE), 2, d); + return sprintf(buf, "%d\n", (short)be16_to_cpup((__be16 *)(d))); + } + case ATTR_DEBUG_SMD_DELAY_CNTR: + { + u8 d[4]; + + result = st->set_power_state(st, true); + mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_SMD_DELAY_CNTR), 4, d); + return sprintf(buf, "%d\n", (int)be32_to_cpup((__be32 *)(d))); + } +#endif + default: + return -EPERM; + } +} + +/* + * inv_dmp_display_orient_show() - calling this function will + * show orientation This event must use poll. + */ +static ssize_t inv_dmp_display_orient_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_mpu_state *st = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", st->display_orient_data); +} + +/* + * inv_accel_motion_show() - calling this function showes motion interrupt. + * This event must use poll. + */ +static ssize_t inv_accel_motion_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "1\n"); +} + +/* + * inv_smd_show() - calling this function showes smd interrupt. + * This event must use poll. + */ +static ssize_t inv_smd_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "1\n"); +} + +/* + * inv_ped_show() - calling this function showes pedometer interrupt. + * This event must use poll. + */ +static ssize_t inv_ped_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + return sprintf(buf, "1\n"); +} + +/* + * inv_dmp_tap_show() - calling this function will show tap + * This event must use poll. + */ +static ssize_t inv_dmp_tap_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_mpu_state *st = iio_priv(dev_get_drvdata(dev)); + return sprintf(buf, "%d\n", st->tap_data); +} + +/* + * inv_temperature_show() - Read temperature data directly from registers. + */ +static ssize_t inv_temperature_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct inv_reg_map_s *reg; + int result, cur_scale, cur_off; + short temp; + long scale_t; + u8 data[2]; + const long scale[] = {3834792L, 3158064L, 3340827L}; + const long offset[] = {5383314L, 2394184L, 1376256L}; + + reg = &st->reg; + mutex_lock(&indio_dev->mlock); + if (!st->chip_config.enable) + result = st->set_power_state(st, true); + else + result = 0; + if (result) { + mutex_unlock(&indio_dev->mlock); + return result; + } + result = inv_i2c_read(st, reg->temperature, 2, data); + if (!st->chip_config.enable) + result |= st->set_power_state(st, false); + mutex_unlock(&indio_dev->mlock); + if (result) { + pr_err("Could not read temperature register.\n"); + return result; + } + temp = (signed short)(be16_to_cpup((short *)&data[0])); + switch (st->chip_type) { + case INV_MPU3050: + cur_scale = scale[0]; + cur_off = offset[0]; + break; + case INV_MPU6050: + cur_scale = scale[1]; + cur_off = offset[1]; + break; + case INV_MPU6500: + cur_scale = scale[2]; + cur_off = offset[2]; + break; + default: + return -EINVAL; + }; + scale_t = cur_off + + inv_q30_mult((int)temp << MPU_TEMP_SHIFT, cur_scale); + + INV_I2C_INC_TEMPREAD(1); + + return sprintf(buf, "%ld %lld\n", scale_t, get_time_ns()); +} + +static ssize_t inv_flush_batch_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + bool has_data; + + mutex_lock(&indio_dev->mlock); + result = inv_flush_batch_data(indio_dev, &has_data); + mutex_unlock(&indio_dev->mlock); + if (result) + return sprintf(buf, "%d\n", result); + else + return sprintf(buf, "%d\n", has_data); +} + +/* + * inv_firmware_loaded() - calling this function will change + * firmware load + */ +static int inv_firmware_loaded(struct inv_mpu_state *st, int data) +{ + if (data) + return -EINVAL; + st->chip_config.firmware_loaded = 0; + + return 0; +} + +static int inv_switch_gyro_engine(struct inv_mpu_state *st, bool en) +{ + return inv_switch_engine(st, en, BIT_PWR_GYRO_STBY); +} + +static int inv_switch_accel_engine(struct inv_mpu_state *st, bool en) +{ + return inv_switch_engine(st, en, BIT_PWR_ACCEL_STBY); +} + +static ssize_t _attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int data; + u8 d, axis; + int result; + + result = 0; + if (st->chip_config.enable) + return -EBUSY; + if (this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) { + result = st->set_power_state(st, true); + if (result) + return result; + } + + /* check the input and validate it's format */ + switch (this_attr->address) { +#ifdef CONFIG_INV_TESTING + /* these inputs are strings */ + case ATTR_COMPASS_MATRIX: + case ATTR_COMPASS_SENS: + break; +#endif + /* these inputs are integers */ + default: + result = kstrtoint(buf, 10, &data); + if (result) + goto attr_store_fail; + break; + } + + switch (this_attr->address) { + case ATTR_GYRO_X_OFFSET: + case ATTR_GYRO_Y_OFFSET: + case ATTR_GYRO_Z_OFFSET: + if ((data > MPU_MAX_G_OFFSET_VALUE) || + (data < MPU_MIN_G_OFFSET_VALUE)) + return -EINVAL; + axis = this_attr->address - ATTR_GYRO_X_OFFSET; + result = inv_set_offset_reg(st, + reg_gyro_offset[axis], + st->rom_gyro_offset[axis] + data); + + if (result) + goto attr_store_fail; + st->input_gyro_offset[axis] = data; + break; + case ATTR_ACCEL_X_OFFSET: + case ATTR_ACCEL_Y_OFFSET: + case ATTR_ACCEL_Z_OFFSET: + { + const u8 *ch; + + if ((data > MPU_MAX_A_OFFSET_VALUE) || + (data < MPU_MIN_A_OFFSET_VALUE)) + return -EINVAL; + + axis = this_attr->address - ATTR_ACCEL_X_OFFSET; + if (INV_MPU6050 == st->chip_type) + ch = reg_6050_accel_offset; + else + ch = reg_6500_accel_offset; + + result = inv_set_offset_reg(st, ch[axis], + st->rom_accel_offset[axis] + (data << 1)); + if (result) + goto attr_store_fail; + st->input_accel_offset[axis] = data; + break; + } + case ATTR_GYRO_SCALE: + result = inv_write_fsr(st, data); + break; + case ATTR_ACCEL_SCALE: + result = inv_write_accel_fs(st, data); + break; + case ATTR_COMPASS_SCALE: + result = st->slave_compass->set_scale(st, data); + break; + case ATTR_MOTION_LPA_ON: + if (INV_MPU6500 == st->chip_type) { + if (data) + /* enable and put in MPU6500 mode */ + d = BIT_ACCEL_INTEL_ENABLE + | BIT_ACCEL_INTEL_MODE; + else + d = 0; + result = inv_i2c_single_write(st, + REG_6500_ACCEL_INTEL_CTRL, d); + if (result) + goto attr_store_fail; + } + st->mot_int.mot_on = !!data; + st->chip_config.lpa_mode = !!data; + break; + case ATTR_MOTION_LPA_FREQ: + result = inv_lpa_freq(st, data); + break; + case ATTR_MOTION_LPA_THRESHOLD: + if ((data > MPU6XXX_MAX_MOTION_THRESH) || (data < 0)) { + result = -EINVAL; + goto attr_store_fail; + } + if (INV_MPU6500 == st->chip_type) { + d = (u8)(data >> MPU6500_MOTION_THRESH_SHIFT); + data = (d << MPU6500_MOTION_THRESH_SHIFT); + } else { + d = (u8)(data >> MPU6050_MOTION_THRESH_SHIFT); + data = (d << MPU6050_MOTION_THRESH_SHIFT); + } + + result = inv_i2c_single_write(st, REG_ACCEL_MOT_THR, d); + if (result) + goto attr_store_fail; + st->mot_int.mot_thr = data; + break; + /* from now on, power is not turned on */ + case ATTR_SELF_TEST_SAMPLES: + if (data > ST_MAX_SAMPLES || data < 0) { + result = -EINVAL; + goto attr_store_fail; + } + st->self_test.samples = data; + break; + case ATTR_SELF_TEST_THRESHOLD: + if (data > ST_MAX_THRESHOLD || data < 0) { + result = -EINVAL; + goto attr_store_fail; + } + st->self_test.threshold = data; + case ATTR_GYRO_ENABLE: + st->chip_config.gyro_enable = !!data; + break; + case ATTR_GYRO_FIFO_ENABLE: + st->sensor[SENSOR_GYRO].on = !!data; + break; + case ATTR_GYRO_RATE: + st->sensor[SENSOR_GYRO].rate = data; + st->sensor[SENSOR_GYRO].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_GYRO].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_ACCEL_ENABLE: + st->chip_config.accel_enable = !!data; + break; + case ATTR_ACCEL_FIFO_ENABLE: + st->sensor[SENSOR_ACCEL].on = !!data; + break; + case ATTR_ACCEL_RATE: + st->sensor[SENSOR_ACCEL].rate = data; + st->sensor[SENSOR_ACCEL].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_ACCEL].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_COMPASS_ENABLE: + st->sensor[SENSOR_COMPASS].on = !!data; + break; + case ATTR_COMPASS_RATE: + if (data <= 0) { + result = -EINVAL; + goto attr_store_fail; + } + if ((MSEC_PER_SEC / st->slave_compass->rate_scale) < data) + data = MSEC_PER_SEC / st->slave_compass->rate_scale; + + st->sensor[SENSOR_COMPASS].rate = data; + st->sensor[SENSOR_COMPASS].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_COMPASS].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_PRESSURE_ENABLE: + st->sensor[SENSOR_PRESSURE].on = !!data; + break; + case ATTR_PRESSURE_RATE: + if (data <= 0) { + result = -EINVAL; + goto attr_store_fail; + } + if ((MSEC_PER_SEC / st->slave_pressure->rate_scale) < data) + data = MSEC_PER_SEC / st->slave_pressure->rate_scale; + + st->sensor[SENSOR_PRESSURE].rate = data; + st->sensor[SENSOR_PRESSURE].dur = MPU_DEFAULT_DMP_FREQ / data; + st->sensor[SENSOR_PRESSURE].dur *= DMP_INTERVAL_INIT; + break; + case ATTR_POWER_STATE: + fake_asleep = !data; + break; + case ATTR_FIRMWARE_LOADED: + result = inv_firmware_loaded(st, data); + break; + case ATTR_SAMPLING_FREQ: + result = inv_fifo_rate_store(st, data); + break; +#ifdef CONFIG_INV_TESTING + case ATTR_COMPASS_MATRIX: + { + char *str; + __s8 m[9]; + d = 0; + if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_NONE) + return -ENODEV; + while ((str = strsep((char **)&buf, ","))) { + if (d >= 9) { + result = -EINVAL; + goto attr_store_fail; + } + result = kstrtoint(str, 10, &data); + if (result) + goto attr_store_fail; + if (data < -1 || data > 1) { + result = -EINVAL; + goto attr_store_fail; + } + m[d] = data; + d++; + } + if (d < 9) { + result = -EINVAL; + goto attr_store_fail; + } + memcpy(st->plat_data.secondary_orientation, m, sizeof(m)); + pr_debug(KERN_INFO + "compass_matrix: %d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); + break; + } + case ATTR_COMPASS_SENS: + { + char *str; + __s8 s[3]; + d = 0; + /* these 2 conditions should never be met, since the + 'compass_sens' sysfs entry should be hidden if the compass + is not an AKM */ + if (st->plat_data.sec_slave_type == SECONDARY_SLAVE_TYPE_NONE) + return -ENODEV; + if (st->plat_data.sec_slave_id != COMPASS_ID_AK8975 && + st->plat_data.sec_slave_id != COMPASS_ID_AK8972 && + st->plat_data.sec_slave_id != COMPASS_ID_AK8963) + return -ENODEV; + /* read the input data, expecting 3 comma separated values */ + while ((str = strsep((char **)&buf, ","))) { + if (d >= 3) { + result = -EINVAL; + goto attr_store_fail; + } + result = kstrtoint(str, 10, &data); + if (result) + goto attr_store_fail; + if (data < 0 || data > 255) { + result = -EINVAL; + goto attr_store_fail; + } + s[d] = data; + d++; + } + if (d < 3) { + result = -EINVAL; + goto attr_store_fail; + } + /* store the new compass sensitivity adjustment */ + memcpy(st->chip_info.compass_sens, s, sizeof(s)); + pr_debug(KERN_INFO + "compass_sens: %d,%d,%d\n", s[0], s[1], s[2]); + break; + } +#endif + default: + result = -EINVAL; + goto attr_store_fail; + }; + +attr_store_fail: + if (this_attr->address <= ATTR_MOTION_LPA_THRESHOLD) + result |= st->set_power_state(st, false); + if (result) + return result; + + return count; +} + +/* + * inv_attr_store() - calling this function will store current + * non-dmp parameter settings + */ +static ssize_t inv_attr_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + int result; + + mutex_lock(&indio_dev->mlock); + result = _attr_store(dev, attr, buf, count); + mutex_unlock(&indio_dev->mlock); + + return result; +} + +static ssize_t inv_master_enable_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + int data; + int result; + + result = kstrtoint(buf, 10, &data); + if (result) + return result; + + mutex_lock(&indio_dev->mlock); + if (st->chip_config.enable == (!!data)) { + result = count; + goto end_enable; + } + if (!!data) { + result = st->set_power_state(st, true); + if (result) + goto end_enable; + } + result = set_inv_enable(indio_dev, !!data); + if (result) + goto end_enable; + if (!data) { + result = st->set_power_state(st, false); + if (result) + goto end_enable; + } + result = count; + +end_enable: + mutex_unlock(&indio_dev->mlock); + + return result; +} + +static ssize_t inv_master_enable_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct inv_mpu_state *st = iio_priv(dev_get_drvdata(dev)); + + return sprintf(buf, "%d\n", st->chip_config.enable); +} + +#ifdef CONFIG_INV_TESTING +static ssize_t inv_test_suspend_resume_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + int data; + int result; + + result = kstrtoint(buf, 10, &data); + if (result) + return result; + if (data) + inv_mpu_suspend(dev); + else + inv_mpu_resume(dev); + suspend_state = !!data; + + return count; +} + +static ssize_t inv_test_suspend_resume_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + + return sprintf(buf, "%d\n", suspend_state); +} + +/* + * inv_reg_write_store() - register write command for testing. + * Format: WSRRDD, where RR is the register in hex, + * and DD is the data in hex. + */ +static ssize_t inv_reg_write_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + u32 result; + u8 wreg, wval; + int temp; + char local_buf[10]; + + if ((buf[0] != 'W' && buf[0] != 'w') || + (buf[1] != 'S' && buf[1] != 's')) + return -EINVAL; + if (strlen(buf) < 6) + return -EINVAL; + + strncpy(local_buf, buf, 7); + local_buf[6] = 0; + result = sscanf(&local_buf[4], "%x", &temp); + if (result == 0) + return -EINVAL; + wval = temp; + local_buf[4] = 0; + sscanf(&local_buf[2], "%x", &temp); + if (result == 0) + return -EINVAL; + wreg = temp; + + result = inv_i2c_single_write(st, wreg, wval); + if (result) + return result; + + return count; +} + +static ssize_t inv_test_store(struct device *dev, + struct device_attribute *attr, const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + int data; + u8 *m; + int result; + + if (st->chip_config.enable) + return -EBUSY; + result = kstrtoint(buf, 10, &data); + if (result) + return -EINVAL; + + result = st->set_power_state(st, true); + if (result) + return result; + + switch (this_attr->address) { + case ATTR_DEBUG_ACCEL_COUNTER: + { + u8 D[6] = {0xf2, 0xb0, 0x80, 0xc0, 0xc8, 0xc2}; + u8 E[6] = {0xf3, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_01, ARRAY_SIZE(D), m); + data_out_control.accel = !!data; + break; + } + case ATTR_DEBUG_GYRO_COUNTER: + { + u8 D[6] = {0xf2, 0xb0, 0x80, 0xc4, 0xcc, 0xc6}; + u8 E[6] = {0xf3, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_02, ARRAY_SIZE(D), m); + data_out_control.gyro = !!data; + break; + } + case ATTR_DEBUG_COMPASS_COUNTER: + { + u8 D[6] = {0xf2, 0xb0, 0x81, 0xc0, 0xc8, 0xc2}; + u8 E[6] = {0xf3, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_03, ARRAY_SIZE(D), m); + data_out_control.compass = !!data; + break; + } + case ATTR_DEBUG_PRESSURE_COUNTER: + { + u8 D[6] = {0xf2, 0xb0, 0x81, 0xc4, 0xcc, 0xc6}; + u8 E[6] = {0xf3, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_04, ARRAY_SIZE(D), m); + data_out_control.pressure = !!data; + break; + } + case ATTR_DEBUG_LPQ_COUNTER: + { + u8 D[6] = {0xf1, 0xb1, 0x83, 0xc2, 0xc4, 0xc6}; + u8 E[6] = {0xf1, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_05, ARRAY_SIZE(D), m); + data_out_control.LPQ = !!data; + break; + } + case ATTR_DEBUG_SIXQ_COUNTER: + { + u8 D[6] = {0xf1, 0xb1, 0x89, 0xc2, 0xc4, 0xc6}; + u8 E[6] = {0xf1, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_06, ARRAY_SIZE(D), m); + data_out_control.SIXQ = !!data; + break; + } + case ATTR_DEBUG_PEDQ_COUNTER: + { + u8 D[6] = {0xf2, 0xf2, 0x88, 0xc2, 0xc4, 0xc6}; + u8 E[6] = {0xf3, 0xb1, 0x88, 0xc0, 0xc0, 0xc0}; + + if (data) + m = E; + else + m = D; + result = mem_w_key(KEY_TEST_07, ARRAY_SIZE(D), m); + data_out_control.PEDQ = !!data; + break; + } + default: + return -EINVAL; + } + + return count; +} + +static ssize_t inv_test_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev_attr *this_attr = to_iio_dev_attr(attr); + + switch (this_attr->address) { + case ATTR_DEBUG_ACCEL_COUNTER: + return sprintf(buf, "%d\n", data_out_control.accel); + case ATTR_DEBUG_GYRO_COUNTER: + return sprintf(buf, "%d\n", data_out_control.gyro); + case ATTR_DEBUG_COMPASS_COUNTER: + return sprintf(buf, "%d\n", data_out_control.compass); + case ATTR_DEBUG_PRESSURE_COUNTER: + return sprintf(buf, "%d\n", data_out_control.pressure); + case ATTR_DEBUG_LPQ_COUNTER: + return sprintf(buf, "%d\n", data_out_control.LPQ); + case ATTR_DEBUG_SIXQ_COUNTER: + return sprintf(buf, "%d\n", data_out_control.SIXQ); + case ATTR_DEBUG_PEDQ_COUNTER: + return sprintf(buf, "%d\n", data_out_control.PEDQ); + default: + return -EINVAL; + } +} + +#endif /* CONFIG_INV_TESTING */ + +static const struct iio_chan_spec inv_mpu_channels[] = { + IIO_CHAN_SOFT_TIMESTAMP(INV_MPU_SCAN_TIMESTAMP), +}; + +/*constant IIO attribute */ +static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500"); + +/* special sysfs */ +static DEVICE_ATTR(reg_dump, S_IRUGO, inv_reg_dump_show, NULL); +static DEVICE_ATTR(temperature, S_IRUGO, inv_temperature_show, NULL); + +/* event based sysfs, needs poll to read */ +static DEVICE_ATTR(event_tap, S_IRUGO, inv_dmp_tap_show, NULL); +static DEVICE_ATTR(event_display_orientation, S_IRUGO, + inv_dmp_display_orient_show, NULL); +static DEVICE_ATTR(event_accel_motion, S_IRUGO, inv_accel_motion_show, NULL); +static DEVICE_ATTR(event_smd, S_IRUGO, inv_smd_show, NULL); +static DEVICE_ATTR(event_pedometer, S_IRUGO, inv_ped_show, NULL); + +/* master enable method */ +static DEVICE_ATTR(master_enable, S_IRUGO | S_IWUSR, inv_master_enable_show, + inv_master_enable_store); + +/* special run time sysfs entry, read only */ +static DEVICE_ATTR(flush_batch, S_IRUGO, inv_flush_batch_show, NULL); + +/* DMP sysfs with power on/off */ +static IIO_DEVICE_ATTR(in_accel_x_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_ACCEL_X_DMP_BIAS); +static IIO_DEVICE_ATTR(in_accel_y_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_ACCEL_Y_DMP_BIAS); +static IIO_DEVICE_ATTR(in_accel_z_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_ACCEL_Z_DMP_BIAS); + +static IIO_DEVICE_ATTR(in_anglvel_x_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_GYRO_X_DMP_BIAS); +static IIO_DEVICE_ATTR(in_anglvel_y_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_GYRO_Y_DMP_BIAS); +static IIO_DEVICE_ATTR(in_anglvel_z_dmp_bias, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_bias_store, ATTR_DMP_GYRO_Z_DMP_BIAS); + +static IIO_DEVICE_ATTR(pedometer_int_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_PED_INT_ON); +static IIO_DEVICE_ATTR(pedometer_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_PED_ON); +static IIO_DEVICE_ATTR(pedometer_step_thresh, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_PED_STEP_THRESH); +static IIO_DEVICE_ATTR(pedometer_int_thresh, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_PED_INT_THRESH); + +static IIO_DEVICE_ATTR(smd_enable, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_ENABLE); +static IIO_DEVICE_ATTR(smd_threshold, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_THLD); +static IIO_DEVICE_ATTR(smd_delay_threshold, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD); +static IIO_DEVICE_ATTR(smd_delay_threshold2, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_SMD_DELAY_THLD2); + +static IIO_DEVICE_ATTR(pedometer_steps, S_IRUGO | S_IWUSR, inv_attr64_show, + inv_attr64_store, ATTR_DMP_PEDOMETER_STEPS); +static IIO_DEVICE_ATTR(pedometer_time, S_IRUGO | S_IWUSR, inv_attr64_show, + inv_attr64_store, ATTR_DMP_PEDOMETER_TIME); +static IIO_DEVICE_ATTR(pedometer_counter, S_IRUGO | S_IWUSR, inv_attr64_show, + NULL, ATTR_DMP_PEDOMETER_COUNTER); + +static IIO_DEVICE_ATTR(tap_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_TAP_ON); +static IIO_DEVICE_ATTR(tap_threshold, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_TAP_THRESHOLD); +static IIO_DEVICE_ATTR(tap_min_count, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_TAP_MIN_COUNT); +static IIO_DEVICE_ATTR(tap_time, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_TAP_TIME); +static IIO_DEVICE_ATTR(display_orientation_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_DISPLAY_ORIENTATION_ON); + +/* DMP sysfs without power on/off */ +static IIO_DEVICE_ATTR(dmp_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_ON); +static IIO_DEVICE_ATTR(dmp_int_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_INT_ON); +static IIO_DEVICE_ATTR(dmp_event_int_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_EVENT_INT_ON); +static IIO_DEVICE_ATTR(step_indicator_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_STEP_INDICATOR_ON); +static IIO_DEVICE_ATTR(batchmode_timeout, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_BATCHMODE_TIMEOUT); +static IIO_DEVICE_ATTR(batchmode_wake_fifo_full_on, S_IRUGO | S_IWUSR, + inv_attr_show, inv_dmp_attr_store, ATTR_DMP_BATCHMODE_WAKE_FIFO_FULL); + +static IIO_DEVICE_ATTR(six_axes_q_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_SIX_Q_ON); +static IIO_DEVICE_ATTR(six_axes_q_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_SIX_Q_RATE); + +static IIO_DEVICE_ATTR(three_axes_q_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_LPQ_ON); +static IIO_DEVICE_ATTR(three_axes_q_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_LPQ_RATE); + +static IIO_DEVICE_ATTR(ped_q_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_PED_Q_ON); +static IIO_DEVICE_ATTR(ped_q_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_PED_Q_RATE); + +static IIO_DEVICE_ATTR(step_detector_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_dmp_attr_store, ATTR_DMP_STEP_DETECTOR_ON); + +/* non DMP sysfs with power on/off */ +static IIO_DEVICE_ATTR(motion_lpa_on, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_MOTION_LPA_ON); +static IIO_DEVICE_ATTR(motion_lpa_freq, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_MOTION_LPA_FREQ); +static IIO_DEVICE_ATTR(motion_lpa_threshold, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_MOTION_LPA_THRESHOLD); + +static IIO_DEVICE_ATTR(in_accel_scale, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_SCALE); +static IIO_DEVICE_ATTR(in_anglvel_scale, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_SCALE); +static IIO_DEVICE_ATTR(in_magn_scale, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_COMPASS_SCALE); + +static IIO_DEVICE_ATTR(in_anglvel_x_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_X_OFFSET); +static IIO_DEVICE_ATTR(in_anglvel_y_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_Y_OFFSET); +static IIO_DEVICE_ATTR(in_anglvel_z_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_Z_OFFSET); + +static IIO_DEVICE_ATTR(in_accel_x_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_X_OFFSET); +static IIO_DEVICE_ATTR(in_accel_y_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_Y_OFFSET); +static IIO_DEVICE_ATTR(in_accel_z_offset, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_Z_OFFSET); + +/* non DMP sysfs without power on/off */ +static IIO_DEVICE_ATTR(self_test_samples, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_SELF_TEST_SAMPLES); +static IIO_DEVICE_ATTR(self_test_threshold, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_SELF_TEST_THRESHOLD); + +static IIO_DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_ENABLE); +static IIO_DEVICE_ATTR(gyro_fifo_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_FIFO_ENABLE); +static IIO_DEVICE_ATTR(gyro_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_GYRO_RATE); + +static IIO_DEVICE_ATTR(accel_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_ENABLE); +static IIO_DEVICE_ATTR(accel_fifo_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_FIFO_ENABLE); +static IIO_DEVICE_ATTR(accel_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_ACCEL_RATE); + +static IIO_DEVICE_ATTR(compass_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_COMPASS_ENABLE); +static IIO_DEVICE_ATTR(compass_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_COMPASS_RATE); + +static IIO_DEVICE_ATTR(pressure_enable, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_PRESSURE_ENABLE); +static IIO_DEVICE_ATTR(pressure_rate, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_PRESSURE_RATE); + +static IIO_DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_POWER_STATE); +static IIO_DEVICE_ATTR(firmware_loaded, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_FIRMWARE_LOADED); +static IIO_DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_SAMPLING_FREQ); + +/* show method only sysfs but with power on/off */ +static IIO_DEVICE_ATTR(self_test, S_IRUGO, inv_attr_show, NULL, + ATTR_SELF_TEST); + +/* show method only sysfs */ +static IIO_DEVICE_ATTR(in_accel_x_calibbias, S_IRUGO, inv_attr_show, + NULL, ATTR_ACCEL_X_CALIBBIAS); +static IIO_DEVICE_ATTR(in_accel_y_calibbias, S_IRUGO, inv_attr_show, + NULL, ATTR_ACCEL_Y_CALIBBIAS); +static IIO_DEVICE_ATTR(in_accel_z_calibbias, S_IRUGO, inv_attr_show, + NULL, ATTR_ACCEL_Z_CALIBBIAS); + +static IIO_DEVICE_ATTR(in_anglvel_x_calibbias, S_IRUGO, + inv_attr_show, NULL, ATTR_GYRO_X_CALIBBIAS); +static IIO_DEVICE_ATTR(in_anglvel_y_calibbias, S_IRUGO, + inv_attr_show, NULL, ATTR_GYRO_Y_CALIBBIAS); +static IIO_DEVICE_ATTR(in_anglvel_z_calibbias, S_IRUGO, + inv_attr_show, NULL, ATTR_GYRO_Z_CALIBBIAS); + +static IIO_DEVICE_ATTR(in_anglvel_self_test_scale, S_IRUGO, + inv_attr_show, NULL, ATTR_SELF_TEST_GYRO_SCALE); +static IIO_DEVICE_ATTR(in_accel_self_test_scale, S_IRUGO, + inv_attr_show, NULL, ATTR_SELF_TEST_ACCEL_SCALE); + +static IIO_DEVICE_ATTR(gyro_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_GYRO_MATRIX); +static IIO_DEVICE_ATTR(accel_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_ACCEL_MATRIX); +#ifdef CONFIG_INV_TESTING /* read/write in test mode */ +static IIO_DEVICE_ATTR(compass_matrix, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_COMPASS_MATRIX); +static IIO_DEVICE_ATTR(compass_sens, S_IRUGO | S_IWUSR, inv_attr_show, + inv_attr_store, ATTR_COMPASS_SENS); +#else +static IIO_DEVICE_ATTR(compass_matrix, S_IRUGO, inv_attr_show, NULL, + ATTR_COMPASS_MATRIX); +#endif +static IIO_DEVICE_ATTR(secondary_name, S_IRUGO, inv_attr_show, NULL, + ATTR_SECONDARY_NAME); + +#ifdef CONFIG_INV_TESTING +static IIO_DEVICE_ATTR(reg_write, S_IRUGO | S_IWUSR, inv_attr_show, + inv_reg_write_store, ATTR_REG_WRITE); +/* smd debug related sysfs */ +static IIO_DEVICE_ATTR(debug_smd_enable_testp1, S_IWUSR, NULL, + inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP1); +static IIO_DEVICE_ATTR(debug_smd_enable_testp2, S_IWUSR, NULL, + inv_dmp_attr_store, ATTR_DEBUG_SMD_ENABLE_TESTP2); +static IIO_DEVICE_ATTR(debug_smd_exe_state, S_IRUGO, inv_attr_show, + NULL, ATTR_DEBUG_SMD_EXE_STATE); +static IIO_DEVICE_ATTR(debug_smd_delay_cntr, S_IRUGO, inv_attr_show, + NULL, ATTR_DEBUG_SMD_DELAY_CNTR); +static DEVICE_ATTR(test_suspend_resume, S_IRUGO | S_IWUSR, + inv_test_suspend_resume_show, inv_test_suspend_resume_store); + +static IIO_DEVICE_ATTR(test_gyro_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_GYRO_COUNTER); +static IIO_DEVICE_ATTR(test_accel_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_ACCEL_COUNTER); +static IIO_DEVICE_ATTR(test_compass_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_COMPASS_COUNTER); +static IIO_DEVICE_ATTR(test_pressure_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_PRESSURE_COUNTER); +static IIO_DEVICE_ATTR(test_LPQ_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_LPQ_COUNTER); +static IIO_DEVICE_ATTR(test_SIXQ_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_SIXQ_COUNTER); +static IIO_DEVICE_ATTR(test_PEDQ_counter, S_IRUGO | S_IWUSR, inv_test_show, + inv_test_store, ATTR_DEBUG_PEDQ_COUNTER); +#endif + +static const struct attribute *inv_gyro_attributes[] = { + &iio_const_attr_sampling_frequency_available.dev_attr.attr, + &dev_attr_reg_dump.attr, + &dev_attr_temperature.attr, + &dev_attr_master_enable.attr, + &iio_dev_attr_in_anglvel_scale.dev_attr.attr, + &iio_dev_attr_in_anglvel_x_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_y_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_z_calibbias.dev_attr.attr, + &iio_dev_attr_in_anglvel_x_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_y_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_z_offset.dev_attr.attr, + &iio_dev_attr_in_anglvel_self_test_scale.dev_attr.attr, + &iio_dev_attr_self_test_samples.dev_attr.attr, + &iio_dev_attr_self_test_threshold.dev_attr.attr, + &iio_dev_attr_gyro_enable.dev_attr.attr, + &iio_dev_attr_gyro_fifo_enable.dev_attr.attr, + &iio_dev_attr_gyro_rate.dev_attr.attr, + &iio_dev_attr_power_state.dev_attr.attr, + &iio_dev_attr_sampling_frequency.dev_attr.attr, + &iio_dev_attr_self_test.dev_attr.attr, + &iio_dev_attr_gyro_matrix.dev_attr.attr, + &iio_dev_attr_secondary_name.dev_attr.attr, +#ifdef CONFIG_INV_TESTING + &iio_dev_attr_reg_write.dev_attr.attr, + &iio_dev_attr_debug_smd_enable_testp1.dev_attr.attr, + &iio_dev_attr_debug_smd_enable_testp2.dev_attr.attr, + &iio_dev_attr_debug_smd_exe_state.dev_attr.attr, + &iio_dev_attr_debug_smd_delay_cntr.dev_attr.attr, + &dev_attr_test_suspend_resume.attr, + &iio_dev_attr_test_gyro_counter.dev_attr.attr, + &iio_dev_attr_test_accel_counter.dev_attr.attr, + &iio_dev_attr_test_compass_counter.dev_attr.attr, + &iio_dev_attr_test_pressure_counter.dev_attr.attr, + &iio_dev_attr_test_LPQ_counter.dev_attr.attr, + &iio_dev_attr_test_SIXQ_counter.dev_attr.attr, + &iio_dev_attr_test_PEDQ_counter.dev_attr.attr, +#endif +}; + +static const struct attribute *inv_mpu6xxx_attributes[] = { + &dev_attr_event_accel_motion.attr, + &dev_attr_event_smd.attr, + &dev_attr_event_pedometer.attr, + &dev_attr_flush_batch.attr, + &iio_dev_attr_in_accel_scale.dev_attr.attr, + &iio_dev_attr_in_accel_x_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_y_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_z_calibbias.dev_attr.attr, + &iio_dev_attr_in_accel_self_test_scale.dev_attr.attr, + &iio_dev_attr_in_accel_x_offset.dev_attr.attr, + &iio_dev_attr_in_accel_y_offset.dev_attr.attr, + &iio_dev_attr_in_accel_z_offset.dev_attr.attr, + &iio_dev_attr_in_accel_x_dmp_bias.dev_attr.attr, + &iio_dev_attr_in_accel_y_dmp_bias.dev_attr.attr, + &iio_dev_attr_in_accel_z_dmp_bias.dev_attr.attr, + &iio_dev_attr_in_anglvel_x_dmp_bias.dev_attr.attr, + &iio_dev_attr_in_anglvel_y_dmp_bias.dev_attr.attr, + &iio_dev_attr_in_anglvel_z_dmp_bias.dev_attr.attr, + &iio_dev_attr_pedometer_int_on.dev_attr.attr, + &iio_dev_attr_pedometer_on.dev_attr.attr, + &iio_dev_attr_pedometer_steps.dev_attr.attr, + &iio_dev_attr_pedometer_time.dev_attr.attr, + &iio_dev_attr_pedometer_counter.dev_attr.attr, + &iio_dev_attr_pedometer_step_thresh.dev_attr.attr, + &iio_dev_attr_pedometer_int_thresh.dev_attr.attr, + &iio_dev_attr_smd_enable.dev_attr.attr, + &iio_dev_attr_smd_threshold.dev_attr.attr, + &iio_dev_attr_smd_delay_threshold.dev_attr.attr, + &iio_dev_attr_smd_delay_threshold2.dev_attr.attr, + &iio_dev_attr_dmp_on.dev_attr.attr, + &iio_dev_attr_dmp_int_on.dev_attr.attr, + &iio_dev_attr_dmp_event_int_on.dev_attr.attr, + &iio_dev_attr_step_indicator_on.dev_attr.attr, + &iio_dev_attr_batchmode_timeout.dev_attr.attr, + &iio_dev_attr_batchmode_wake_fifo_full_on.dev_attr.attr, + &iio_dev_attr_six_axes_q_on.dev_attr.attr, + &iio_dev_attr_six_axes_q_rate.dev_attr.attr, + &iio_dev_attr_three_axes_q_on.dev_attr.attr, + &iio_dev_attr_three_axes_q_rate.dev_attr.attr, + &iio_dev_attr_ped_q_on.dev_attr.attr, + &iio_dev_attr_ped_q_rate.dev_attr.attr, + &iio_dev_attr_step_detector_on.dev_attr.attr, + &iio_dev_attr_accel_enable.dev_attr.attr, + &iio_dev_attr_accel_fifo_enable.dev_attr.attr, + &iio_dev_attr_accel_rate.dev_attr.attr, + &iio_dev_attr_firmware_loaded.dev_attr.attr, + &iio_dev_attr_accel_matrix.dev_attr.attr, +}; + +static const struct attribute *inv_mpu6500_attributes[] = { + &iio_dev_attr_motion_lpa_on.dev_attr.attr, + &iio_dev_attr_motion_lpa_freq.dev_attr.attr, + &iio_dev_attr_motion_lpa_threshold.dev_attr.attr, +}; + +static const struct attribute *inv_tap_attributes[] = { + &dev_attr_event_tap.attr, + &iio_dev_attr_tap_on.dev_attr.attr, + &iio_dev_attr_tap_threshold.dev_attr.attr, + &iio_dev_attr_tap_min_count.dev_attr.attr, + &iio_dev_attr_tap_time.dev_attr.attr, +}; + +static const struct attribute *inv_display_orient_attributes[] = { + &dev_attr_event_display_orientation.attr, + &iio_dev_attr_display_orientation_on.dev_attr.attr, +}; + +static const struct attribute *inv_compass_attributes[] = { + &iio_dev_attr_in_magn_scale.dev_attr.attr, + &iio_dev_attr_compass_enable.dev_attr.attr, + &iio_dev_attr_compass_rate.dev_attr.attr, + &iio_dev_attr_compass_matrix.dev_attr.attr, +}; + +static const struct attribute *inv_akxxxx_attributes[] = { +#ifdef CONFIG_INV_TESTING + &iio_dev_attr_compass_sens.dev_attr.attr, +#endif +}; + +static const struct attribute *inv_pressure_attributes[] = { + &iio_dev_attr_pressure_enable.dev_attr.attr, + &iio_dev_attr_pressure_rate.dev_attr.attr, +}; + +static const struct attribute *inv_mpu3050_attributes[] = { + &iio_dev_attr_in_accel_scale.dev_attr.attr, + &iio_dev_attr_accel_enable.dev_attr.attr, + &iio_dev_attr_accel_fifo_enable.dev_attr.attr, + &iio_dev_attr_accel_matrix.dev_attr.attr, +}; + +static struct attribute *inv_attributes[ + ARRAY_SIZE(inv_gyro_attributes) + + ARRAY_SIZE(inv_mpu6xxx_attributes) + + ARRAY_SIZE(inv_mpu6500_attributes) + + ARRAY_SIZE(inv_compass_attributes) + + ARRAY_SIZE(inv_akxxxx_attributes) + + ARRAY_SIZE(inv_pressure_attributes) + + ARRAY_SIZE(inv_tap_attributes) + + ARRAY_SIZE(inv_display_orient_attributes) + + 1 +]; + +static const struct attribute_group inv_attribute_group = { + .name = "mpu", + .attrs = inv_attributes +}; + +static const struct iio_info mpu_info = { + .driver_module = THIS_MODULE, + .attrs = &inv_attribute_group, +}; + +static void inv_setup_func_ptr(struct inv_mpu_state *st) +{ + if (st->chip_type == INV_MPU3050) { + st->set_power_state = set_power_mpu3050; + st->switch_gyro_engine = inv_switch_3050_gyro_engine; + st->switch_accel_engine = inv_switch_3050_accel_engine; + st->init_config = inv_init_config_mpu3050; + st->setup_reg = inv_setup_reg_mpu3050; + } else { + st->set_power_state = set_power_itg; + st->switch_gyro_engine = inv_switch_gyro_engine; + st->switch_accel_engine = inv_switch_accel_engine; + st->init_config = inv_init_config; + st->setup_reg = inv_setup_reg; + } +} + +static int inv_detect_6xxx(struct inv_mpu_state *st) +{ + int result; + u8 d; + + result = inv_i2c_read(st, REG_WHOAMI, 1, &d); + if (result) + return result; + if ((d == MPU6500_ID) || (d == MPU6515_ID)) { + st->chip_type = INV_MPU6500; + strcpy(st->name, "mpu6500"); + } else { + strcpy(st->name, "mpu6050"); + } + + return 0; +} + +static int inv_setup_vddio(struct inv_mpu_state *st) +{ + int result; + u8 data[1]; + + if (INV_MPU6050 == st->chip_type) { + result = inv_i2c_read(st, REG_YGOFFS_TC, 1, data); + if (result) + return result; + data[0] &= ~BIT_I2C_MST_VDDIO; + if (st->plat_data.level_shifter) + data[0] |= BIT_I2C_MST_VDDIO; + /*set up VDDIO register */ + result = inv_i2c_single_write(st, REG_YGOFFS_TC, data[0]); + if (result) + return result; + } + + return 0; +} + +/* + * inv_check_chip_type() - check and setup chip type. + */ +static int inv_check_chip_type(struct inv_mpu_state *st, + const struct i2c_device_id *id) +{ + struct inv_reg_map_s *reg; + int result; + int t_ind; + struct inv_chip_config_s *conf; + struct mpu_platform_data *plat; + + conf = &st->chip_config; + plat = &st->plat_data; + if (!strcmp(id->name, "itg3500")) { + st->chip_type = INV_ITG3500; + } else if (!strcmp(id->name, "mpu3050")) { + st->chip_type = INV_MPU3050; + } else if (!strcmp(id->name, "mpu6050")) { + st->chip_type = INV_MPU6050; + } else if (!strcmp(id->name, "mpu9150")) { + st->chip_type = INV_MPU6050; + plat->sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS; + plat->sec_slave_id = COMPASS_ID_AK8975; + } else if (!strcmp(id->name, "mpu6500")) { + st->chip_type = INV_MPU6500; + } else if (!strcmp(id->name, "mpu9250")) { + st->chip_type = INV_MPU6500; + plat->sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS; + plat->sec_slave_id = COMPASS_ID_AK8963; + } else if (!strcmp(id->name, "mpu6xxx")) { + st->chip_type = INV_MPU6050; + } else if (!strcmp(id->name, "mpu9350")) { + st->chip_type = INV_MPU6500; + plat->sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS; + plat->sec_slave_id = COMPASS_ID_MLX90399; + /* block use of MPU9350 in this build + since it's not production ready */ + pr_err("MPU9350 support is not officially available yet.\n"); + return -EPERM; + } else if (!strcmp(id->name, "mpu6515")) { + st->chip_type = INV_MPU6500; + } else { + return -EPERM; + } + inv_setup_func_ptr(st); + st->hw = &hw_info[st->chip_type]; + reg = &st->reg; + st->setup_reg(reg); + /* reset to make sure previous state are not there */ + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET); + if (result) + return result; + msleep(POWER_UP_TIME); + /* toggle power state */ + result = st->set_power_state(st, false); + if (result) + return result; + + result = st->set_power_state(st, true); + if (result) + return result; + + if (!strcmp(id->name, "mpu6xxx")) { + /* for MPU6500, reading register need more time */ + msleep(POWER_UP_TIME); + result = inv_detect_6xxx(st); + if (result) + return result; + } + + if ((plat->sec_slave_type != SECONDARY_SLAVE_TYPE_NONE) || + (plat->aux_slave_type != SECONDARY_SLAVE_TYPE_NONE)) { + result = inv_setup_vddio(st); + if (result) + return result; + } + + switch (st->chip_type) { + case INV_ITG3500: + break; + case INV_MPU6050: + case INV_MPU6500: + if (SECONDARY_SLAVE_TYPE_COMPASS == plat->sec_slave_type) + conf->has_compass = 1; + else + conf->has_compass = 0; + if (SECONDARY_SLAVE_TYPE_PRESSURE == plat->aux_slave_type) + conf->has_pressure = 1; + else + conf->has_pressure = 0; + + break; + case INV_MPU3050: + if (SECONDARY_SLAVE_TYPE_ACCEL == plat->sec_slave_type) { + if (ACCEL_ID_BMA250 == plat->sec_slave_id) + inv_register_mpu3050_slave(st); + } + break; + default: + result = st->set_power_state(st, false); + return -ENODEV; + } + if (conf->has_compass && conf->has_pressure && + (COMPASS_ID_MLX90399 == plat->sec_slave_id)) { + pr_err("MLX90399 can't share slaves with others\n"); + return -EINVAL; + } + switch (st->chip_type) { + case INV_MPU6050: + result = inv_get_silicon_rev_mpu6050(st); + break; + case INV_MPU6500: + result = inv_get_silicon_rev_mpu6500(st); + break; + default: + result = 0; + break; + } + if (result) { + pr_err("read silicon rev error\n"); + st->set_power_state(st, false); + return result; + } + + /* turn off the gyro engine after OTP reading */ + result = st->switch_gyro_engine(st, false); + if (result) + return result; + result = st->switch_accel_engine(st, false); + if (result) + return result; + + if (conf->has_compass) { + result = inv_mpu_setup_compass_slave(st); + if (result) { + pr_err("compass setup failed\n"); + //st->set_power_state(st, false); + //return result; + } + } + if (conf->has_pressure) { + result = inv_mpu_setup_pressure_slave(st); + if (result) { + pr_err("pressure setup failed\n"); + st->set_power_state(st, false); + return result; + } + } + + t_ind = 0; + memcpy(&inv_attributes[t_ind], inv_gyro_attributes, + sizeof(inv_gyro_attributes)); + t_ind += ARRAY_SIZE(inv_gyro_attributes); + + if (INV_MPU3050 == st->chip_type && st->slave_accel != NULL) { + memcpy(&inv_attributes[t_ind], inv_mpu3050_attributes, + sizeof(inv_mpu3050_attributes)); + t_ind += ARRAY_SIZE(inv_mpu3050_attributes); + inv_attributes[t_ind] = NULL; + return 0; + } + + /* all MPU6xxx based parts */ + if ((INV_MPU6050 == st->chip_type) || (INV_MPU6500 == st->chip_type)) { + memcpy(&inv_attributes[t_ind], inv_mpu6xxx_attributes, + sizeof(inv_mpu6xxx_attributes)); + t_ind += ARRAY_SIZE(inv_mpu6xxx_attributes); + } + + /* MPU6500 only */ + if (INV_MPU6500 == st->chip_type) { + memcpy(&inv_attributes[t_ind], inv_mpu6500_attributes, + sizeof(inv_mpu6500_attributes)); + t_ind += ARRAY_SIZE(inv_mpu6500_attributes); + } + + if (conf->has_compass) { + memcpy(&inv_attributes[t_ind], inv_compass_attributes, + sizeof(inv_compass_attributes)); + t_ind += ARRAY_SIZE(inv_compass_attributes); + + /* AKM only */ + if (st->plat_data.sec_slave_id == COMPASS_ID_AK8975 || + st->plat_data.sec_slave_id == COMPASS_ID_AK8972 || + st->plat_data.sec_slave_id == COMPASS_ID_AK09911 || + st->plat_data.sec_slave_id == COMPASS_ID_AK8963) { + memcpy(&inv_attributes[t_ind], inv_akxxxx_attributes, + sizeof(inv_akxxxx_attributes)); + t_ind += ARRAY_SIZE(inv_akxxxx_attributes); + } + } + + if (conf->has_pressure) { + memcpy(&inv_attributes[t_ind], inv_pressure_attributes, + sizeof(inv_pressure_attributes)); + t_ind += ARRAY_SIZE(inv_pressure_attributes); + } + + inv_attributes[t_ind] = NULL; + + return 0; +} + +/* + * inv_create_dmp_sysfs() - create binary sysfs dmp entry. + */ +static const struct bin_attribute dmp_firmware = { + .attr = { + .name = "dmp_firmware", + .mode = S_IRUGO | S_IWUSR + }, + .size = DMP_IMAGE_SIZE + 32, + .read = inv_dmp_firmware_read, + .write = inv_dmp_firmware_write, +}; + +static const struct bin_attribute six_q_value = { + .attr = { + .name = "six_axes_q_value", + .mode = S_IWUSR + }, + .size = QUATERNION_BYTES, + .read = NULL, + .write = inv_six_q_write, +}; + +static int inv_create_dmp_sysfs(struct iio_dev *ind) +{ + int result; + + result = sysfs_create_bin_file(&ind->dev.kobj, &dmp_firmware); + if (result) + return result; + result = sysfs_create_bin_file(&ind->dev.kobj, &six_q_value); + + return result; +} + +/* + * inv_mpu_probe() - probe function. + */ +static int inv_mpu_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_mpu_state *st; + struct iio_dev *indio_dev; + int result; + +#ifdef CONFIG_DTS_INV_MPU_IIO + enable_irq_wake(client->irq); +#endif + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENOSYS; + pr_err("I2c function error\n"); + goto out_no_free; + } + indio_dev = iio_device_alloc(sizeof(*st)); + if (indio_dev == NULL) { + pr_err("memory allocation failed\n"); + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->client = client; + st->sl_handle = client->adapter; + st->i2c_addr = client->addr; +#ifdef CONFIG_DTS_INV_MPU_IIO + result = invensense_mpu_parse_dt(&client->dev, &st->plat_data); + if (result) + goto out_free; + + /*Power on device.*/ + if (st->plat_data.power_on) { + result = st->plat_data.power_on(&st->plat_data); + if (result < 0) { + dev_err(&client->dev, + "power_on failed: %d\n", result); + return result; + } + } + +msleep(100); +#else + /* power is turned on inside check chip type */ + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); +#endif + result = inv_check_chip_type(st, id); + if (result) + goto out_free; + + result = st->init_config(indio_dev); + if (result) { + dev_err(&client->adapter->dev, + "Could not initialize device.\n"); + goto out_free; + } + /* Make state variables available to all _show and _store functions. */ + i2c_set_clientdata(client, indio_dev); + indio_dev->dev.parent = &client->dev; + if (!strcmp(id->name, "mpu6xxx")) + indio_dev->name = st->name; + else + indio_dev->name = id->name; + indio_dev->channels = inv_mpu_channels; + indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels); + + indio_dev->info = &mpu_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_mpu_configure_ring(indio_dev); + if (result) { + pr_err("configure ring buffer fail\n"); + goto out_free; + } + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) { + pr_err("ring buffer register fail\n"); + goto out_unreg_ring; + } + st->irq = client->irq; + result = inv_mpu_probe_trigger(indio_dev); + if (result) { + pr_err("trigger probe fail\n"); + goto out_remove_ring; + } + + /* Tell the i2c counter, we have an IRQ */ + INV_I2C_SETIRQ(IRQ_MPU, client->irq); + + result = iio_device_register(indio_dev); + if (result) { + pr_err("IIO device register fail\n"); + goto out_remove_trigger; + } + + if (INV_MPU6050 == st->chip_type || + INV_MPU6500 == st->chip_type) { + result = inv_create_dmp_sysfs(indio_dev); + if (result) { + pr_err("create dmp sysfs failed\n"); + goto out_unreg_iio; + } + } + INIT_KFIFO(st->timestamps); + spin_lock_init(&st->time_stamp_lock); + mutex_init(&st->suspend_resume_lock); + result = st->set_power_state(st, false); + if (result) { + dev_err(&client->adapter->dev, + "%s could not be turned off.\n", st->hw->name); + goto out_unreg_iio; + } + inv_init_sensor_struct(st); + dev_info(&client->dev, "%s is ready to go!\n", + indio_dev->name); + + return 0; +out_unreg_iio: + iio_device_unregister(indio_dev); +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_mpu_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_mpu_unconfigure_ring(indio_dev); +out_free: + iio_device_free(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +static void inv_mpu_shutdown(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_mpu_state *st = iio_priv(indio_dev); + struct inv_reg_map_s *reg; + int result; + + reg = &st->reg; + mutex_lock(&indio_dev->mlock); + dev_dbg(&client->adapter->dev, "Shutting down %s...\n", st->hw->name); + + /* reset to make sure previous state are not there */ + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, BIT_H_RESET); + if (result) + dev_err(&client->adapter->dev, "Failed to reset %s\n", + st->hw->name); + msleep(POWER_UP_TIME); + /* turn off power to ensure gyro engine is off */ + result = st->set_power_state(st, false); + if (result) + dev_err(&client->adapter->dev, "Failed to turn off %s\n", + st->hw->name); + mutex_unlock(&indio_dev->mlock); +} + +/* + * inv_mpu_remove() - remove function. + */ +static int inv_mpu_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_mpu_state *st = iio_priv(indio_dev); + + kfifo_free(&st->timestamps); + iio_device_unregister(indio_dev); + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_mpu_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_mpu_unconfigure_ring(indio_dev); + iio_device_free(indio_dev); + + dev_info(&client->adapter->dev, "inv-mpu-iio module removed.\n"); + + return 0; +} + +static int inv_setup_suspend_batchmode(struct iio_dev *indio_dev, bool suspend) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + int result; + + if (st->chip_config.dmp_on && + st->chip_config.enable && + st->batch.on && + (!st->chip_config.dmp_event_int_on)) { + if (!st->batch.wake_fifo_on) { + st->batch.overflow_on = suspend; + result = inv_i2c_single_write(st, + st->reg.user_ctrl, 0); + if (result) + return result; + result = inv_batchmode_setup(st); + if (result) + return result; + result = inv_reset_fifo(indio_dev); + if (result) + return result; + } + /* turn off data interrupt in suspend mode;turn on resume */ + result = inv_set_interrupt_on_gesture_event(st, suspend); + if (result) + return result; + } + + return 0; +} + +#ifdef CONFIG_PM +static int inv_mpu_resume(struct device *dev) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev)); + struct inv_mpu_state *st = iio_priv(indio_dev); + int result; + + pr_debug("%s inv_mpu_resume\n", st->hw->name); + mutex_lock(&indio_dev->mlock); + + result = 0; + if (st->chip_config.dmp_on && st->chip_config.enable) { + result = st->set_power_state(st, true); + result |= inv_read_time_and_ticks(st, true); + if (st->ped.int_on) + result |= inv_enable_pedometer_interrupt(st, true); + if (st->chip_config.display_orient_on) + result |= inv_set_display_orient_interrupt_dmp(st, + true); + result |= inv_setup_suspend_batchmode(indio_dev, false); + } else if (st->chip_config.enable) { + result = st->set_power_state(st, true); + } + + mutex_unlock(&indio_dev->mlock); + mutex_unlock(&st->suspend_resume_lock); + + return result; +} + +static int inv_mpu_suspend(struct device *dev) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev)); + struct inv_mpu_state *st = iio_priv(indio_dev); + int result; + + pr_debug("%s inv_mpu_suspend\n", st->hw->name); + mutex_lock(&indio_dev->mlock); + + result = 0; + if (st->chip_config.dmp_on && st->chip_config.enable) { + /* turn off pedometer interrupt during suspend */ + if (st->ped.int_on) + result |= inv_enable_pedometer_interrupt(st, false); + /* turn off orientation interrupt during suspend */ + if (st->chip_config.display_orient_on) + result |= inv_set_display_orient_interrupt_dmp(st, + false); + /* setup batch mode related during suspend */ + result = inv_setup_suspend_batchmode(indio_dev, true); + /* only in DMP non-batch data mode, turn off the power */ + if ((!st->batch.on) && (!st->chip_config.smd_enable) && + (!st->ped.on)) + result |= st->set_power_state(st, false); + } else if (st->chip_config.enable) { + /* in non DMP case, just turn off the power */ + result |= st->set_power_state(st, false); + } + + mutex_unlock(&indio_dev->mlock); + mutex_lock(&st->suspend_resume_lock); + + return result; +} +static const struct dev_pm_ops inv_mpu_pmops = { + SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume) +}; +#define INV_MPU_PMOPS (&inv_mpu_pmops) +#else +#define INV_MPU_PMOPS NULL +#endif /* CONFIG_PM */ + +static const u16 normal_i2c[] = { I2C_CLIENT_END }; +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_mpu_id[] = { + {"itg3500", INV_ITG3500}, + {"mpu3050", INV_MPU3050}, + {"mpu6050", INV_MPU6050}, + {"mpu9150", INV_MPU9150}, + {"mpu6500", INV_MPU6500}, + {"mpu9250", INV_MPU9250}, + {"mpu6xxx", INV_MPU6XXX}, + {"mpu9350", INV_MPU9350}, + {"mpu6515", INV_MPU6515}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_mpu_id); + +static struct i2c_driver inv_mpu_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_mpu_probe, + .remove = inv_mpu_remove, + .shutdown = inv_mpu_shutdown, + .id_table = inv_mpu_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-mpu-iio", + .pm = INV_MPU_PMOPS, + }, + .address_list = normal_i2c, +}; + +static int __init inv_mpu_init(void) +{ + int result = i2c_add_driver(&inv_mpu_driver); + if (result) { + pr_err("failed\n"); + return result; + } + return 0; +} + +static void __exit inv_mpu_exit(void) +{ + i2c_del_driver(&inv_mpu_driver); +} + +module_init(inv_mpu_init); +module_exit(inv_mpu_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv-mpu-iio"); + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_dts.c b/drivers/iio/imu/inv_mpu/inv_mpu_dts.c new file mode 100644 index 000000000000..276d5bcfea7e --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_dts.c @@ -0,0 +1,270 @@ +#include <linux/err.h> +#include <linux/of_gpio.h> +#include <linux/of.h> +#include <linux/of_device.h> +#include <linux/regulator/consumer.h> + +#include <linux/i2c.h> +#include <linux/mpu.h> +#include "inv_mpu_dts.h" + +int inv_mpu_power_on(struct mpu_platform_data *pdata) +{ + int err ; + + err = regulator_enable(pdata->vdd_ana); + err = regulator_enable(pdata->vdd_i2c); + pr_debug(KERN_INFO "inv_mpu_power_on call"); + + return err ; + +} + +int inv_mpu_power_off(struct mpu_platform_data *pdata) +{ + int err ; + + err = regulator_disable(pdata->vdd_ana); + err = regulator_disable(pdata->vdd_i2c); + pr_debug(KERN_INFO "inv_mpu_power_off call"); + + return err; +} + +int inv_parse_orientation_matrix(struct device *dev, s8 *orient) +{ + int rc, i; + struct device_node *np = dev->of_node; + u32 temp_val, temp_val2; + + for (i = 0; i < 9; i++) + orient[i] = 0; + + /* parsing axis x orientation matrix*/ + rc = of_property_read_u32(np, "axis_map_x", &temp_val); + if (rc) { + dev_err(dev, "Unable to read axis_map_x\n"); + return rc; + } + rc = of_property_read_u32(np, "negate_x", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read negate_x\n"); + return rc; + } + if (temp_val2) + orient[temp_val] = -1; + else + orient[temp_val] = 1; + + /* parsing axis y orientation matrix*/ + rc = of_property_read_u32(np, "axis_map_y", &temp_val); + if (rc) { + dev_err(dev, "Unable to read axis_map_y\n"); + return rc; + } + rc = of_property_read_u32(np, "negate_y", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read negate_y\n"); + return rc; + } + if (temp_val2) + orient[3 + temp_val] = -1; + else + orient[3 + temp_val] = 1; + + /* parsing axis z orientation matrix*/ + rc = of_property_read_u32(np, "axis_map_z", &temp_val); + if (rc) { + dev_err(dev, "Unable to read axis_map_z\n"); + return rc; + } + rc = of_property_read_u32(np, "negate_z", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read negate_z\n"); + return rc; + } + if (temp_val2) + orient[6 + temp_val] = -1; + else + orient[6 + temp_val] = 1; + + return 0; +} + +int inv_parse_secondary_orientation_matrix(struct device *dev, + s8 *orient) +{ + int rc, i; + struct device_node *np = dev->of_node; + u32 temp_val, temp_val2; + + for (i = 0; i < 9; i++) + orient[i] = 0; + + /* parsing axis x orientation matrix*/ + rc = of_property_read_u32(np, "inven,secondary_axis_map_x", &temp_val); + if (rc) { + dev_err(dev, "Unable to read secondary axis_map_x\n"); + return rc; + } + rc = of_property_read_u32(np, "inven,secondary_negate_x", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read secondary negate_x\n"); + return rc; + } + if (temp_val2) + orient[temp_val] = -1; + else + orient[temp_val] = 1; + + /* parsing axis y orientation matrix*/ + rc = of_property_read_u32(np, "inven,secondary_axis_map_y", &temp_val); + if (rc) { + dev_err(dev, "Unable to read secondary axis_map_y\n"); + return rc; + } + rc = of_property_read_u32(np, "inven,secondary_negate_y", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read secondary negate_y\n"); + return rc; + } + if (temp_val2) + orient[3 + temp_val] = -1; + else + orient[3 + temp_val] = 1; + + /* parsing axis z orientation matrix*/ + rc = of_property_read_u32(np, "inven,secondary_axis_map_z", &temp_val); + if (rc) { + dev_err(dev, "Unable to read secondary axis_map_z\n"); + return rc; + } + rc = of_property_read_u32(np, "inven,secondary_negate_z", &temp_val2); + if (rc) { + dev_err(dev, "Unable to read secondary negate_z\n"); + return rc; + } + if (temp_val2) + orient[6 + temp_val] = -1; + else + orient[6 + temp_val] = 1; + + return 0; +} + +int inv_parse_secondary(struct device *dev, struct mpu_platform_data *pdata) +{ + int rc; + struct device_node *np = dev->of_node; + u32 temp_val; + const char *name; + + if (of_property_read_string(np, "inven,secondary_type", &name)) { + dev_err(dev, "Missing secondary type.\n"); + return -EINVAL; + } + if (!strcmp(name, "compass")) { + pdata->sec_slave_type = SECONDARY_SLAVE_TYPE_COMPASS; + } else if (!strcmp(name, "none")) { + pdata->sec_slave_type = SECONDARY_SLAVE_TYPE_NONE; + return 0; + } else { + return -EINVAL; + } + + if (of_property_read_string(np, "inven,secondary_name", &name)) { + dev_err(dev, "Missing secondary name.\n"); + return -EINVAL; + } + if (!strcmp(name, "ak8963")) + pdata->sec_slave_id = COMPASS_ID_AK8963; + else if (!strcmp(name, "ak8975")) + pdata->sec_slave_id = COMPASS_ID_AK8975; + else if (!strcmp(name, "ak8972")) + pdata->sec_slave_id = COMPASS_ID_AK8972; + else + return -EINVAL; + rc = of_property_read_u32(np, "inven,secondary_reg", &temp_val); + if (rc) { + dev_err(dev, "Unable to read secondary register\n"); + return rc; + } + pdata->secondary_i2c_addr = temp_val; + rc = inv_parse_secondary_orientation_matrix(dev, + pdata->secondary_orientation); + + return rc; +} + +int inv_parse_aux(struct device *dev, struct mpu_platform_data *pdata) +{ + int rc; + struct device_node *np = dev->of_node; + u32 temp_val; + const char *name; + + if (of_property_read_string(np, "inven,aux_type", &name)) { + dev_err(dev, "Missing aux type.\n"); + return -EINVAL; + } + if (!strcmp(name, "pressure")) { + pdata->aux_slave_type = SECONDARY_SLAVE_TYPE_PRESSURE; + } else if (!strcmp(name, "none")) { + pdata->aux_slave_type = SECONDARY_SLAVE_TYPE_NONE; + return 0; + } else { + return -EINVAL; + } + + if (of_property_read_string(np, "inven,aux_name", &name)) { + dev_err(dev, "Missing aux name.\n"); + return -EINVAL; + } + if (!strcmp(name, "bmp280")) + pdata->aux_slave_id = PRESSURE_ID_BMP280; + else + return -EINVAL; + + rc = of_property_read_u32(np, "inven,aux_reg", &temp_val); + if (rc) { + dev_err(dev, "Unable to read aux register\n"); + return rc; + } + pdata->aux_i2c_addr = temp_val; + + return 0; +} + +int invensense_mpu_parse_dt(struct device *dev, struct mpu_platform_data *pdata) +{ + int rc; + pr_debug("Invensense MPU parse_dt started.\n"); + + rc = inv_parse_orientation_matrix(dev, pdata->orientation); + if (rc) + return rc; + + rc = inv_parse_secondary(dev, pdata); + if (rc) + return rc; + + inv_parse_aux(dev, pdata); + + pdata->vdd_ana = regulator_get(dev, "inven,vdd_ana"); + if (IS_ERR(pdata->vdd_ana)) { + rc = PTR_ERR(pdata->vdd_ana); + dev_err(dev, "Regulator get failed vdd_ana-supply rc=%d\n", rc); + return rc; + } + pdata->vdd_i2c = regulator_get(dev, "inven,vcc_i2c"); + if (IS_ERR(pdata->vdd_i2c)) { + rc = PTR_ERR(pdata->vdd_i2c); + dev_err(dev, "Regulator get failed vcc-i2c-supply rc=%d\n", rc); + return rc; + } + pdata->power_on = inv_mpu_power_on; + pdata->power_off = inv_mpu_power_off; + pr_debug("Invensense MPU parse_dt complete.\n"); + return rc; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_dts.h b/drivers/iio/imu/inv_mpu/inv_mpu_dts.h new file mode 100644 index 000000000000..151ac74ea05b --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_dts.h @@ -0,0 +1,30 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#ifndef _INV_MPU_DTS_H_ +#define _INV_MPU_DTS_H_ + +#include <linux/i2c.h> +#include <linux/mpu.h> + +int inv_mpu_power_on(struct mpu_platform_data *pdata); +int inv_mpu_power_off(struct mpu_platform_data *pdata); +int inv_parse_orientation_matrix(struct device *dev, s8 *orient); +int inv_parse_secondary_orientation_matrix(struct device *dev, + s8 *orient); +int inv_parse_secondary(struct device *dev, struct mpu_platform_data *pdata); +int inv_parse_aux(struct device *dev, struct mpu_platform_data *pdata); +int invensense_mpu_parse_dt(struct device *dev, + struct mpu_platform_data *pdata); + +#endif /* #ifndef _INV_MPU_DTS_H_ */ diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h new file mode 100644 index 000000000000..deaf746d89ed --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h @@ -0,0 +1,1067 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#ifndef _INV_MPU_IIO_H_ +#define _INV_MPU_IIO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> +#include <linux/iio/iio.h> +#include <linux/iio/buffer.h> +#include <linux/mpu_iio.h> + +#include "dmpKey.h" + +/*register and associated bit definition*/ +#define REG_3050_FIFO_EN 0x12 +#define BITS_3050_ACCEL_OUT 0x0E + +#define REG_3050_AUX_VDDIO 0x13 +#define BIT_3050_VDDIO 0x04 + +#define REG_3050_SLAVE_ADDR 0x14 +#define REG_3050_SAMPLE_RATE_DIV 0x15 +#define REG_3050_LPF 0x16 +#define REG_3050_INT_ENABLE 0x17 +#define REG_3050_AUX_BST_ADDR 0x18 +#define REG_3050_INT_STATUS 0x1A +#define REG_3050_TEMPERATURE 0x1B +#define REG_3050_RAW_GYRO 0x1D +#define REG_3050_AUX_XOUT_H 0x23 +#define REG_3050_FIFO_COUNT_H 0x3A +#define REG_3050_FIFO_R_W 0x3C + +#define REG_3050_USER_CTRL 0x3D +#define BIT_3050_AUX_IF_EN 0x20 +#define BIT_3050_FIFO_RST 0x02 + +#define REG_3050_PWR_MGMT_1 0x3E +#define BITS_3050_POWER1 0x30 +#define BITS_3050_POWER2 0x10 +#define BITS_3050_GYRO_STANDBY 0x38 + +#define REG_3500_OTP 0x0 + +#define REG_YGOFFS_TC 0x1 +#define BIT_I2C_MST_VDDIO 0x80 + +#define REG_XA_OFFS_H 0x6 +#define REG_XA_OFFS_L_TC 0x7 +#define REG_PRODUCT_ID 0xC +#define REG_ST_GCT_X 0xD +#define REG_XG_OFFS_USRH 0x13 +#define REG_SAMPLE_RATE_DIV 0x19 +#define REG_CONFIG 0x1A + +#define REG_GYRO_CONFIG 0x1B +#define BITS_SELF_TEST_EN 0xE0 + +#define REG_ACCEL_CONFIG 0x1C +#define REG_ACCEL_MOT_THR 0x1F +#define REG_ACCEL_MOT_DUR 0x20 + +#define REG_FIFO_EN 0x23 +#define BIT_ACCEL_OUT 0x08 +#define BITS_GYRO_OUT 0x70 + +#define REG_I2C_MST_CTRL 0x24 +#define BIT_WAIT_FOR_ES 0x40 + +#define REG_I2C_SLV0_ADDR 0x25 +#define REG_I2C_SLV0_REG 0x26 +#define REG_I2C_SLV0_CTRL 0x27 +#define REG_I2C_SLV1_ADDR 0x28 +#define REG_I2C_SLV1_REG 0x29 +#define REG_I2C_SLV1_CTRL 0x2A +#define REG_I2C_SLV2_ADDR 0x2B +#define REG_I2C_SLV2_REG 0x2C +#define REG_I2C_SLV2_CTRL 0x2D +#define REG_I2C_SLV3_ADDR 0x2E +#define REG_I2C_SLV3_REG 0x2F +#define REG_I2C_SLV3_CTRL 0x30 +#define REG_I2C_SLV4_CTRL 0x34 + +#define INV_MPU_BIT_SLV_EN 0x80 +#define INV_MPU_BIT_BYTE_SW 0x40 +#define INV_MPU_BIT_REG_DIS 0x20 +#define INV_MPU_BIT_I2C_READ 0x80 + +#define REG_INT_PIN_CFG 0x37 +#define BIT_BYPASS_EN 0x2 + +#define REG_INT_ENABLE 0x38 +#define BIT_DATA_RDY_EN 0x01 +#define BIT_DMP_INT_EN 0x02 +#define BIT_ZMOT_EN 0x20 +#define BIT_MOT_EN 0x40 +#define BIT_6500_WOM_EN 0x40 + +#define REG_DMP_INT_STATUS 0x39 +#define BIT_DMP_INT_CI 0x01 + +#define REG_INT_STATUS 0x3A +#define BIT_MOT_INT 0x40 +#define BIT_ZMOT_INT 0x20 + +#define REG_RAW_ACCEL 0x3B +#define REG_TEMPERATURE 0x41 +#define REG_EXT_SENS_DATA_00 0x49 +#define REG_EXT_SENS_DATA_08 0x51 + +#define REG_ACCEL_INTEL_STATUS 0x61 + +#define INV_MPU_REG_I2C_SLV0_DO 0x63 +#define INV_MPU_REG_I2C_SLV1_DO 0x64 +#define INV_MPU_REG_I2C_SLV2_DO 0x65 +#define INV_MPU_REG_I2C_SLV3_DO 0x66 + +#define REG_I2C_MST_DELAY_CTRL 0x67 +#define BIT_SLV0_DLY_EN 0x01 +#define BIT_SLV1_DLY_EN 0x02 +#define BIT_SLV2_DLY_EN 0x04 +#define BIT_SLV3_DLY_EN 0x08 + +#define REG_USER_CTRL 0x6A +#define BIT_FIFO_RST 0x04 +#define BIT_DMP_RST 0x08 +#define BIT_I2C_MST_EN 0x20 +#define BIT_FIFO_EN 0x40 +#define BIT_DMP_EN 0x80 + +#define REG_PWR_MGMT_1 0x6B +#define BIT_H_RESET 0x80 +#define BIT_SLEEP 0x40 +#define BIT_CYCLE 0x20 +#define BIT_CLK_MASK 0x7 + +#define REG_PWR_MGMT_2 0x6C +#define BIT_PWR_ACCEL_STBY 0x38 +#define BIT_PWR_GYRO_STBY 0x07 +#define BIT_LPA_FREQ 0xC0 + +#define REG_BANK_SEL 0x6D +#define REG_MEM_START_ADDR 0x6E +#define REG_MEM_RW 0x6F +#define REG_PRGM_STRT_ADDRH 0x70 +#define REG_FIFO_COUNT_H 0x72 +#define REG_FIFO_R_W 0x74 +#define REG_WHOAMI 0x75 + +#define REG_6500_XG_ST_DATA 0x0 +#define REG_6500_XA_ST_DATA 0xD +#define REG_6500_XA_OFFS_H 0x77 +#define REG_6500_YA_OFFS_H 0x7A +#define REG_6500_ZA_OFFS_H 0x7D +#define REG_6500_ACCEL_CONFIG2 0x1D +#define BIT_ACCEL_FCHOCIE_B 0x08 +#define BIT_FIFO_SIZE_1K 0x40 + +#define REG_6500_LP_ACCEL_ODR 0x1E +#define REG_6500_ACCEL_WOM_THR 0x1F + +#define REG_6500_ACCEL_INTEL_CTRL 0x69 +#define BIT_ACCEL_INTEL_ENABLE 0x80 +#define BIT_ACCEL_INTEL_MODE 0x40 + +/* data definitions */ +#define DMP_START_ADDR 0x400 +#define DMP_MASK_TAP 0x3f +#define DMP_MASK_DIS_ORIEN 0xC0 +#define DMP_DIS_ORIEN_SHIFT 6 + +#define BYTES_FOR_DMP 8 +#define BYTES_FOR_EVENTS 4 +#define QUATERNION_BYTES 16 +#define BYTES_PER_SENSOR 6 +#define MPU3050_FOOTER_SIZE 2 +#define FIFO_COUNT_BYTE 2 +#define FIFO_THRESHOLD 800 +#define FIFO_SIZE 800 +#define HARDWARE_FIFO_SIZE 1024 +#define MAX_READ_SIZE 64 +#define POWER_UP_TIME 100 +#define SENSOR_UP_TIME 30 +#define REG_UP_TIME 5 +#define INV_MPU_SAMPLE_RATE_CHANGE_STABLE 50 +#define MPU_MEM_BANK_SIZE 256 +#define SELF_TEST_GYRO_FULL_SCALE 250 +#define SELF_TEST_ACCEL_FULL_SCALE 8 +#define SELF_TEST_ACCEL_6500_SCALE 2 + +/* data header defines */ +#define PRESSURE_HDR 0x8000 +#define ACCEL_HDR 0x4000 +#define GYRO_HDR 0x2000 +#define COMPASS_HDR 0x1000 +#define LPQUAT_HDR 0x0800 +#define SIXQUAT_HDR 0x0400 +#define PEDQUAT_HDR 0x0200 +#define STEP_DETECTOR_HDR 0x0100 +#define STEP_INDICATOR_MASK 0xf + +#define MAX_BYTES_PER_SAMPLE 80 +#define MAX_HW_FIFO_BYTES (BYTES_PER_SENSOR * 2) +#define IIO_BUFFER_BYTES 8 +#define HEADERED_NORMAL_BYTES 8 +#define HEADERED_Q_BYTES 16 + +#define MPU6XXX_MAX_MOTION_THRESH (255*4) +#define MPU6050_MOTION_THRESH_SHIFT 5 +#define MPU6500_MOTION_THRESH_SHIFT 2 +#define MPU6050_MOTION_DUR_DEFAULT 1 +#define MPU6050_ID 0x68 +#define MPU6050_MAX_MOTION_DUR 255 +#define MPU_TEMP_SHIFT 16 +#define LPA_FREQ_SHIFT 6 +#define COMPASS_RATE_SCALE 10 +#define MAX_GYRO_FS_PARAM 3 +#define MAX_ACCEL_FS_PARAM 3 +#define MAX_LPA_FREQ_PARAM 3 +#define MPU_MAX_A_OFFSET_VALUE 16383 +#define MPU_MIN_A_OFFSET_VALUE -16384 +#define MPU_MAX_G_OFFSET_VALUE 32767 +#define MPU_MIN_G_OFFSET_VALUE -32767 +#define MPU6XXX_MAX_MPU_MEM (256 * 12) + +#define INIT_MOT_DUR 128 +#define INIT_MOT_THR 128 +#define INIT_ZMOT_DUR 128 +#define INIT_ZMOT_THR 128 +#define INIT_ST_SAMPLES 200 +#define INIT_ST_MPU6050_SAMPLES 600 +#define INIT_ST_THRESHOLD 50 +#define INIT_PED_INT_THRESH 2 +#define INIT_PED_THRESH 7 +#define ST_THRESHOLD_MULTIPLIER 10 +#define ST_MAX_SAMPLES 500 +#define ST_MAX_THRESHOLD 100 +#define DMP_INTERVAL_INIT (5 * NSEC_PER_MSEC) +#define DMP_INTERVAL_MIN_ADJ (50 * NSEC_PER_USEC) + +/*---- MPU6500 ----*/ +#define MPU6500_ID 0x70 /* unique WHOAMI */ +#define MPU6515_ID 0x74 /* unique WHOAMI */ +#define MPU6500_PRODUCT_REVISION 1 +#define MPU6500_MEM_REV_ADDR 0x16 +#define INV_MPU_REV_MASK 0x0F +#define MPU6500_REV 2 +#define MPU_DMP_LOAD_START 0x20 + +/*---- MPU9250 ----*/ +#define MPU9250_ID 0x71 /* unique WHOAMI */ + +/* ----MPU9350 ----*/ +#define MPU9350_ID 0x72 + +#define THREE_AXIS 3 +#define GYRO_CONFIG_FSR_SHIFT 3 +#define ACCEL_CONFIG_FSR_SHIFT 3 +#define GYRO_DPS_SCALE 250 +#define MEM_ADDR_PROD_REV 0x6 +#define SOFT_PROD_VER_BYTES 5 +#define CRC_FIRMWARE_SEED 0 +#define SELF_TEST_SUCCESS 1 +#define MS_PER_DMP_TICK 20 +#define DMP_IMAGE_SIZE 2463 + +/* init parameters */ +#define INIT_FIFO_RATE 50 +#define INIT_DMP_OUTPUT_RATE 25 +#define INIT_DUR_TIME (NSEC_PER_SEC / INIT_FIFO_RATE) +#define INIT_TAP_THRESHOLD 100 +#define INIT_TAP_TIME 100 +#define INIT_TAP_MIN_COUNT 2 +#define INIT_SAMPLE_DIVIDER 4 +#define MPU_INIT_SMD_DELAY_THLD 3 +#define MPU_INIT_SMD_DELAY2_THLD 1 +#define MPU_INIT_SMD_THLD 1500 +#define MPU_DEFAULT_DMP_FREQ 200 +#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev) +#define NUM_OF_PROD_REVS (ARRAY_SIZE(prod_rev_map)) +/*---- MPU6050 Silicon Revisions ----*/ +#define MPU_SILICON_REV_A2 1 /* MPU6050A2 Device */ +#define MPU_SILICON_REV_B1 2 /* MPU6050B1 Device */ + +#define BIT_PRFTCH_EN 0x40 +#define BIT_CFG_USER_BANK 0x20 +#define BITS_MEM_SEL 0x1f + +#define TIME_STAMP_TOR 5 +#define MAX_CATCH_UP 5 +#define DEFAULT_ACCEL_TRIM 16384 +#define DEFAULT_GYRO_TRIM 131 +#define MAX_FIFO_RATE 1000 +#define MAX_DMP_OUTPUT_RATE 200 +#define MIN_FIFO_RATE 4 +#define ONE_K_HZ 1000 +#define NS_PER_MS_SHIFT 20 +#define END_MARKER 0x0010 +#define EMPTY_MARKER 0x0020 + +/*tap related defines */ +#define INV_TAP 0x08 +#define INV_NUM_TAP_AXES 3 + +#define INV_TAP_AXIS_X_POS 0x20 +#define INV_TAP_AXIS_X_NEG 0x10 +#define INV_TAP_AXIS_Y_POS 0x08 +#define INV_TAP_AXIS_Y_NEG 0x04 +#define INV_TAP_AXIS_Z_POS 0x02 +#define INV_TAP_AXIS_Z_NEG 0x01 +#define INV_TAP_ALL_DIRECTIONS 0x3f + +#define INV_TAP_AXIS_X 0x1 +#define INV_TAP_AXIS_Y 0x2 +#define INV_TAP_AXIS_Z 0x4 + +#define INV_TAP_AXIS_ALL \ + (INV_TAP_AXIS_X | \ + INV_TAP_AXIS_Y | \ + INV_TAP_AXIS_Z) + +#define INT_SRC_TAP 0x01 +#define INT_SRC_DISPLAY_ORIENT 0x08 +#define INT_SRC_SHAKE 0x10 + +#define INV_X_AXIS_INDEX 0x00 +#define INV_Y_AXIS_INDEX 0x01 +#define INV_Z_AXIS_INDEX 0x02 + +#define INV_ELEMENT_1 0x0001 +#define INV_ELEMENT_2 0x0002 +#define INV_ELEMENT_3 0x0004 +#define INV_ELEMENT_4 0x0008 +#define INV_ELEMENT_5 0x0010 +#define INV_ELEMENT_6 0x0020 +#define INV_ELEMENT_7 0x0040 +#define INV_ELEMENT_8 0x0080 +#define INV_ALL 0xFFFF +#define INV_ELEMENT_MASK 0x00FF +#define INV_GYRO_ACC_MASK 0x007E +#define INV_ACCEL_MASK 0x70 +#define INV_GYRO_MASK 0xE + +struct inv_mpu_state; + +/** + * struct inv_reg_map_s - Notable slave registers. + * @sample_rate_div: Divider applied to gyro output rate. + * @lpf: Configures internal LPF. + * @bank_sel: Selects between memory banks. + * @user_ctrl: Enables/resets the FIFO. + * @fifo_en: Determines which data will appear in FIFO. + * @gyro_config: gyro config register. + * @accel_config: accel config register + * @fifo_count_h: Upper byte of FIFO count. + * @fifo_r_w: FIFO register. + * @raw_accel Address of first accel register. + * @temperature temperature register + * @int_enable: Interrupt enable register. + * @int_status: Interrupt flags. + * @pwr_mgmt_1: Controls chip's power state and clock source. + * @pwr_mgmt_2: Controls power state of individual sensors. + * @mem_start_addr: Address of first memory read. + * @mem_r_w: Access to memory. + * @prgm_strt_addrh firmware program start address register + */ +struct inv_reg_map_s { + u8 sample_rate_div; + u8 lpf; + u8 bank_sel; + u8 user_ctrl; + u8 fifo_en; + u8 gyro_config; + u8 accel_config; + u8 fifo_count_h; + u8 fifo_r_w; + u8 raw_accel; + u8 temperature; + u8 int_enable; + u8 int_status; + u8 pwr_mgmt_1; + u8 pwr_mgmt_2; + u8 mem_start_addr; + u8 mem_r_w; + u8 prgm_strt_addrh; +}; + +/* device enum */ +enum inv_devices { + INV_ITG3500, + INV_MPU3050, + INV_MPU6050, + INV_MPU9150, + INV_MPU6500, + INV_MPU9250, + INV_MPU6XXX, + INV_MPU9350, + INV_MPU6515, + INV_NUM_PARTS +}; + +/** + * struct inv_hw_s - Other important hardware information. + * @num_reg: Number of registers on device. + * @name: name of the chip + */ +struct inv_hw_s { + u8 num_reg; + u8 *name; +}; + +/* enum for sensor */ +enum INV_SENSORS { + SENSOR_GYRO = 0, + SENSOR_ACCEL, + SENSOR_COMPASS, + SENSOR_PRESSURE, + SENSOR_STEP, + SENSOR_PEDQ, + SENSOR_SIXQ, + SENSOR_LPQ, + SENSOR_NUM_MAX, + SENSOR_INVALID, +}; + +/** + * struct inv_sensor - information for each sensor. + * @ts: this sensors timestamp. + * @dur: duration between samples in ns. + * @rate: sensor data rate. + * @counter: dmp tick counter corresponding to rate. + * @on: sensor on/off. + * @sample_size: number of bytes for the sensor. + * @send_data: function pointer to send data or not. + * @set_rate: funcition pointer to set data rate. + */ +struct inv_sensor { + u64 ts; + int dur; + int rate; + int counter; + bool on; + u8 sample_size; + int (*send_data)(struct inv_mpu_state *st, bool on); + int (*set_rate)(struct inv_mpu_state *st); +}; + +/** + * struct inv_batch - information batchmode. + * @on: derived variable for batch mode. + * @overflow_on: overflow mode for batchmode. + * @wake_fifo_on: overflow for suspend mode. + * @counter: counter for batch mode. + * @timeout: nominal timeout value for batchmode in milliseconds. + * @min_rate: minimum sensor rate that is turned on. + */ +struct inv_batch { + bool on; + bool overflow_on; + bool wake_fifo_on; + u32 counter; + u32 timeout; + u32 min_rate; +}; + +/** + * struct inv_chip_config_s - Cached chip configuration data. + * @fsr: Full scale range. + * @lpf: Digital low pass filter frequency. + * @accel_fs: accel full scale range. + * @has_footer: MPU3050 specific work around. + * @has_compass: has compass or not. + * @has_pressure: has pressure sensor or not. + * @enable: master enable to enable output + * @accel_enable: enable accel functionality + * @gyro_enable: enable gyro functionality + * @is_asleep: 1 if chip is powered down. + * @dmp_on: dmp is on/off. + * @dmp_int_on: dmp interrupt on/off. + * @step_indicator_on: step indicate bit added to the sensor or not. + * @dmp_event_int_on: dmp event interrupt on/off. + * @firmware_loaded: flag indicate firmware loaded or not. + * @lpa_mod: low power mode. + * @display_orient_on: display orientation on/off. + * @normal_compass_measure: discard first compass data after reset. + * @normal_pressure_measure: discard first pressure data after reset. + * @smd_enable: disable/enable SMD function. + * @adjust_time: flag to indicate whether adjust chip clock or not. + * @smd_triggered: smd is triggered. + * @lpa_freq: low power frequency + * @prog_start_addr: firmware program start address. + * @fifo_rate: current FIFO update rate. + * @bytes_per_datum: number of bytes for 1 sample data. + */ +struct inv_chip_config_s { + u32 fsr:2; + u32 lpf:3; + u32 accel_fs:2; + u32 has_footer:1; + u32 has_compass:1; + u32 has_pressure:1; + u32 enable:1; + u32 accel_enable:1; + u32 gyro_enable:1; + u32 is_asleep:1; + u32 dmp_on:1; + u32 dmp_int_on:1; + u32 dmp_event_int_on:1; + u32 step_indicator_on:1; + u32 firmware_loaded:1; + u32 lpa_mode:1; + u32 display_orient_on:1; + u32 normal_compass_measure:1; + u32 normal_pressure_measure:1; + u32 smd_enable:1; + u32 adjust_time:1; + u32 smd_triggered:1; + u16 lpa_freq; + u16 prog_start_addr; + u16 fifo_rate; + u16 bytes_per_datum; +}; + +/** + * struct inv_chip_info_s - Chip related information. + * @product_id: Product id. + * @product_revision: Product revision. + * @silicon_revision: Silicon revision. + * @software_revision: software revision. + * @multi: accel specific multiplier. + * @compass_sens: compass sensitivity. + * @gyro_sens_trim: Gyro sensitivity trim factor. + * @accel_sens_trim: accel sensitivity trim factor. + */ +struct inv_chip_info_s { + u8 product_id; + u8 product_revision; + u8 silicon_revision; + u8 software_revision; + u8 multi; + u8 compass_sens[3]; + u32 gyro_sens_trim; + u32 accel_sens_trim; +}; + +/** + * struct inv_tap structure to store tap data. + * @min_count: minimum taps counted. + * @thresh: tap threshold. + * @time: tap time. + * @on: tap on/off. + */ +struct inv_tap { + u16 min_count; + u16 thresh; + u16 time; + bool on; +}; + +/** + * struct accel_mot_int_s structure to store motion interrupt data + * @mot_thr: motion threshold. + * @mot_dur: motion duration. + * @mot_on: flag to indicate motion detection on; + */ +struct accel_mot_int { + u16 mot_thr; + u32 mot_dur; + u8 mot_on:1; +}; + +/** + * struct self_test_setting - self test settables from sysfs + * samples: number of samples used in self test. + * threshold: threshold fail/pass criterion in self test. + * This value is in the percentage multiplied by 100. + * So 14% would be 14. + */ +struct self_test_setting { + u16 samples; + u16 threshold; +}; + +/** + * struct inv_smd significant motion detection structure. + * @threshold: accel threshold for motion detection. + * @delay: delay time to confirm 2nd motion. + * @delay2: delay window parameter. + */ +struct inv_smd { + u32 threshold; + u32 delay; + u32 delay2; +}; + +/** + * struct inv_ped pedometer related data structure. + * @step: steps taken. + * @time: time taken during the period. + * @last_step_time: last time the step is taken. + * @step_thresh: step threshold to show steps. + * @int_thresh: step threshold to generate interrupt. + * @int_on: pedometer interrupt enable/disable. + * @on: pedometer on/off. + */ +struct inv_ped { + u64 step; + u64 time; + u64 last_step_time; + u16 step_thresh; + u16 int_thresh; + bool int_on; + bool on; +}; + +struct inv_mpu_slave; +/** + * struct inv_mpu_state - Driver state variables. + * @chip_config: Cached attribute information. + * @chip_info: Chip information from read-only registers. + * @trig; iio trigger. + * @tap: tap data structure. + * @smd: SMD data structure. + * @ped: pedometer data structure. + * @batch: batchmode data structure. + * @reg: Map of important registers. + * @self_test: self test settings. + * @hw: Other hardware-specific information. + * @chip_type: chip type. + * @time_stamp_lock: spin lock to time stamp. + * @suspend_resume_lock: mutex lock for suspend/resume. + * @client: i2c client handle. + * @plat_data: platform data. + * @slave_accel: mpu slave handle for accelerometer(MPU3050 only). + * @slave_compass: mpu slave handle for magnetometer. + * @slave_pressure: mpu slave handle for pressure sensor. + * (*set_power_state)(struct inv_mpu_state *, int on): function ptr + * (*switch_gyro_engine)(struct inv_mpu_state *, int on): function ptr + * (*switch_accel_engine)(struct inv_mpu_state *, int on): function ptr + * (*init_config)(struct iio_dev *indio_dev): function ptr + * (*setup_reg)(struct inv_reg_map_s *reg): function ptr + * @timestamps: kfifo queue to store time stamp. + * @irq: irq number store. + * @accel_bias: accel bias store. + * @gyro_bias: gyro bias store. + * @input_gyro_offset[3]: gyro offset from sysfs. + * @input_accel_offset[3]: accel offset from sysfs. + * @input_accel_dmp_bias[3]: accel bias for dmp. + * @input_gyro_dmp_bias[3];: gyro bias for dmp. + * @rom_gyro_bias[3]: gyro bias from sysfs. + * @rom_accel_bias[3]: accel bias from sysfs. + * @fifo_data[6]: fifo data storage. + * @i2c_addr: i2c address. + * @sample_divider: sample divider for dmp. + * @fifo_divider: fifo divider for dmp. + * @display_orient_data:display orient data. + * @tap_data: tap data. + * @bytes_per_sec: bytes per seconds when in DMP mode. + * @left_over[HEADERED_Q_BYTES]: left over bytes storage. + * @left_over_size: left over size. + * @sensor{SENSOR_NUM_MAX]: sensor individual properties. + * @fifo_count: current fifo_count; + * @dmp_counter: dmp_counter; + * @dmp_ticks: DMP ticks past since the previous read. + * @sl_handle: Handle to I2C port. + * @irq_dur_ns: duration between each irq. + * @ts_counter: time stamp counter. + * @dmp_interval: dmp interval. nomial value is 5 ms. + * @dmp_interval_accum: dmp interval accumlater. + * @diff_accumulater: accumlator for the difference of nominal and actual. + * @last_ts: last time stamp. + * @step_detector_base_ts: base time stamp for step detector calculation. + * @prev_ts: previous time stamp. + * @pedometer_step: pedometer steps stored in driver. + * @pedometer_time: pedometer time stored in driver. + * @last_run_time: last time the post ISR runs. + * @name: name for distiguish MPU6050 and MPU6500 in MPU6XXX. + * @secondary_name: name for the slave device in the secondary I2C. + */ +struct inv_mpu_state { +#define TIMESTAMP_FIFO_SIZE 64 + struct inv_chip_config_s chip_config; + struct inv_chip_info_s chip_info; + struct iio_trigger *trig; + struct inv_tap tap; + struct inv_smd smd; + struct inv_ped ped; + struct inv_batch batch; + struct inv_reg_map_s reg; + struct self_test_setting self_test; + const struct inv_hw_s *hw; + enum inv_devices chip_type; + spinlock_t time_stamp_lock; + struct mutex suspend_resume_lock; + struct i2c_client *client; + struct mpu_platform_data plat_data; + struct inv_mpu_slave *slave_accel; + struct inv_mpu_slave *slave_compass; + struct inv_mpu_slave *slave_pressure; + struct accel_mot_int mot_int; + int (*set_power_state)(struct inv_mpu_state *, bool on); + int (*switch_gyro_engine)(struct inv_mpu_state *, bool on); + int (*switch_accel_engine)(struct inv_mpu_state *, bool on); + int (*init_config)(struct iio_dev *indio_dev); + void (*setup_reg)(struct inv_reg_map_s *reg); + DECLARE_KFIFO(timestamps, u64, TIMESTAMP_FIFO_SIZE); + short irq; + int accel_bias[3]; + int gyro_bias[3]; + s16 input_gyro_offset[3]; + s16 input_accel_offset[3]; + int input_accel_dmp_bias[3]; + int input_gyro_dmp_bias[3]; + s16 rom_gyro_offset[3]; + s16 rom_accel_offset[3]; + u8 fifo_data[6]; + u8 i2c_addr; + u8 sample_divider; + u8 fifo_divider; + u8 display_orient_data; + u8 tap_data; + u16 bytes_per_sec; + u8 left_over[HEADERED_Q_BYTES]; + u32 left_over_size; + struct inv_sensor sensor[SENSOR_NUM_MAX]; + u32 fifo_count; + u32 dmp_counter; + u32 dmp_ticks; + void *sl_handle; + u32 irq_dur_ns; + u32 ts_counter; + u32 dmp_interval; + s32 dmp_interval_accum; + s64 diff_accumulater; + u64 last_ts; + u64 step_detector_base_ts; + u64 prev_ts; + u64 last_run_time; + u8 name[20]; + u8 secondary_name[20]; +}; + +/* produces an unique identifier for each device based on the + combination of product version and product revision */ +struct prod_rev_map_t { + u16 mpl_product_key; + u8 silicon_rev; + u16 gyro_trim; + u16 accel_trim; +}; + +/** + * struct inv_mpu_slave - MPU slave structure. + * @st_upper: compass self test upper limit. + * @st_lower: compass self test lower limit. + * @scale: compass scale. + * @rate_scale: decide how fast a compass can read. + * @min_read_time: minimum time between each reading. + * @self_test: self test method of the slave. + * @set_scale: set scale of slave + * @get_scale: read scale back of the slave. + * @suspend: suspend operation. + * @resume: resume operation. + * @setup: setup chip. initialization. + * @combine_data: combine raw data into meaningful data. + * @read_data: read external sensor and output + * @get_mode: get current chip mode. + * @set_lpf: set low pass filter. + * @set_fs: set full scale + * @prev_ts: last time it is read. + */ +struct inv_mpu_slave { + const short *st_upper; + const short *st_lower; + int scale; + int rate_scale; + int min_read_time; + int (*self_test)(struct inv_mpu_state *); + int (*set_scale)(struct inv_mpu_state *, int scale); + int (*get_scale)(struct inv_mpu_state *, int *val); + int (*suspend)(struct inv_mpu_state *); + int (*resume)(struct inv_mpu_state *); + int (*setup)(struct inv_mpu_state *); + int (*combine_data)(u8 *in, short *out); + int (*read_data)(struct inv_mpu_state *, short *out); + int (*get_mode)(void); + int (*set_lpf)(struct inv_mpu_state *, int rate); + int (*set_fs)(struct inv_mpu_state *, int fs); + u64 prev_ts; +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_MPU_SCAN_QUAT_R = 0, + INV_MPU_SCAN_QUAT_X, + INV_MPU_SCAN_QUAT_Y, + INV_MPU_SCAN_QUAT_Z, + INV_MPU_SCAN_ACCEL_X, + INV_MPU_SCAN_ACCEL_Y, + INV_MPU_SCAN_ACCEL_Z, + INV_MPU_SCAN_GYRO_X, + INV_MPU_SCAN_GYRO_Y, + INV_MPU_SCAN_GYRO_Z, + INV_MPU_SCAN_MAGN_X, + INV_MPU_SCAN_MAGN_Y, + INV_MPU_SCAN_MAGN_Z, + INV_MPU_SCAN_TIMESTAMP, +}; + +enum inv_filter_e { + INV_FILTER_256HZ_NOLPF2 = 0, + INV_FILTER_188HZ, + INV_FILTER_98HZ, + INV_FILTER_42HZ, + INV_FILTER_20HZ, + INV_FILTER_10HZ, + INV_FILTER_5HZ, + INV_FILTER_2100HZ_NOLPF, + NUM_FILTER +}; + +enum inv_slave_mode { + INV_MODE_SUSPEND, + INV_MODE_NORMAL, +}; + +/*==== MPU6050B1 MEMORY ====*/ +enum MPU_MEMORY_BANKS { + MEM_RAM_BANK_0 = 0, + MEM_RAM_BANK_1, + MEM_RAM_BANK_2, + MEM_RAM_BANK_3, + MEM_RAM_BANK_4, + MEM_RAM_BANK_5, + MEM_RAM_BANK_6, + MEM_RAM_BANK_7, + MEM_RAM_BANK_8, + MEM_RAM_BANK_9, + MEM_RAM_BANK_10, + MEM_RAM_BANK_11, + MPU_MEM_NUM_RAM_BANKS, + MPU_MEM_OTP_BANK_0 = 16 +}; + +/* IIO attribute address */ +enum MPU_IIO_ATTR_ADDR { + ATTR_DMP_GYRO_X_DMP_BIAS, + ATTR_DMP_GYRO_Y_DMP_BIAS, + ATTR_DMP_GYRO_Z_DMP_BIAS, + ATTR_DMP_ACCEL_X_DMP_BIAS, + ATTR_DMP_ACCEL_Y_DMP_BIAS, + ATTR_DMP_ACCEL_Z_DMP_BIAS, + ATTR_DMP_PED_INT_ON, + ATTR_DMP_PED_STEP_THRESH, + ATTR_DMP_PED_INT_THRESH, + ATTR_DMP_PED_ON, + ATTR_DMP_SMD_ENABLE, + ATTR_DMP_SMD_THLD, + ATTR_DMP_SMD_DELAY_THLD, + ATTR_DMP_SMD_DELAY_THLD2, + ATTR_DMP_PEDOMETER_STEPS, + ATTR_DMP_PEDOMETER_TIME, + ATTR_DMP_PEDOMETER_COUNTER, + ATTR_DMP_TAP_ON, + ATTR_DMP_TAP_THRESHOLD, + ATTR_DMP_TAP_MIN_COUNT, + ATTR_DMP_TAP_TIME, + ATTR_DMP_DISPLAY_ORIENTATION_ON, +/* *****above this line, are DMP features, power needs on/off */ +/* *****below this line, are DMP features, no power needed */ + ATTR_DMP_ON, + ATTR_DMP_INT_ON, + ATTR_DMP_EVENT_INT_ON, + ATTR_DMP_STEP_INDICATOR_ON, + ATTR_DMP_BATCHMODE_TIMEOUT, + ATTR_DMP_BATCHMODE_WAKE_FIFO_FULL, + ATTR_DMP_SIX_Q_ON, + ATTR_DMP_SIX_Q_RATE, + ATTR_DMP_LPQ_ON, + ATTR_DMP_LPQ_RATE, + ATTR_DMP_PED_Q_ON, + ATTR_DMP_PED_Q_RATE, + ATTR_DMP_STEP_DETECTOR_ON, + ATTR_DMP_STEP_DETECTOR_RATE, +/* *****above this line, it is all DMP related features */ +/* *****below this line, it is all non-DMP related features */ + ATTR_GYRO_SCALE, + ATTR_ACCEL_SCALE, + ATTR_COMPASS_SCALE, + ATTR_GYRO_X_OFFSET, + ATTR_GYRO_Y_OFFSET, + ATTR_GYRO_Z_OFFSET, + ATTR_ACCEL_X_OFFSET, + ATTR_ACCEL_Y_OFFSET, + ATTR_ACCEL_Z_OFFSET, + ATTR_MOTION_LPA_ON, + ATTR_MOTION_LPA_FREQ, + ATTR_MOTION_LPA_THRESHOLD, +/* *****above this line, it is non-DMP, power needs on/off */ +/* *****below this line, it is non-DMP, no needs to on/off power */ + ATTR_SELF_TEST_SAMPLES, + ATTR_SELF_TEST_THRESHOLD, + ATTR_GYRO_ENABLE, + ATTR_GYRO_FIFO_ENABLE, + ATTR_GYRO_RATE, + ATTR_ACCEL_ENABLE, + ATTR_ACCEL_FIFO_ENABLE, + ATTR_ACCEL_RATE, + ATTR_COMPASS_ENABLE, + ATTR_COMPASS_RATE, + ATTR_PRESSURE_ENABLE, + ATTR_PRESSURE_RATE, + ATTR_POWER_STATE, /* this is fake sysfs for compatibility */ + ATTR_FIRMWARE_LOADED, + ATTR_SAMPLING_FREQ, +/* *****below this line, it is attributes only has show methods */ + ATTR_SELF_TEST, /* this has show-only methods needs power on/off */ + ATTR_GYRO_X_CALIBBIAS, + ATTR_GYRO_Y_CALIBBIAS, + ATTR_GYRO_Z_CALIBBIAS, + ATTR_ACCEL_X_CALIBBIAS, + ATTR_ACCEL_Y_CALIBBIAS, + ATTR_ACCEL_Z_CALIBBIAS, + ATTR_SELF_TEST_GYRO_SCALE, + ATTR_SELF_TEST_ACCEL_SCALE, + ATTR_GYRO_MATRIX, + ATTR_ACCEL_MATRIX, + ATTR_COMPASS_MATRIX, + ATTR_SECONDARY_NAME, +#ifdef CONFIG_INV_TESTING + ATTR_COMPASS_SENS, + ATTR_I2C_COUNTERS, + ATTR_REG_WRITE, + ATTR_DEBUG_SMD_ENABLE_TESTP1, + ATTR_DEBUG_SMD_ENABLE_TESTP2, + ATTR_DEBUG_SMD_EXE_STATE, + ATTR_DEBUG_SMD_DELAY_CNTR, + ATTR_DEBUG_GYRO_COUNTER, + ATTR_DEBUG_ACCEL_COUNTER, + ATTR_DEBUG_COMPASS_COUNTER, + ATTR_DEBUG_PRESSURE_COUNTER, + ATTR_DEBUG_LPQ_COUNTER, + ATTR_DEBUG_SIXQ_COUNTER, + ATTR_DEBUG_PEDQ_COUNTER, +#endif +}; + +enum inv_accel_fs_e { + INV_FS_02G = 0, + INV_FS_04G, + INV_FS_08G, + INV_FS_16G, + NUM_ACCEL_FSR +}; + +enum inv_fsr_e { + INV_FSR_250DPS = 0, + INV_FSR_500DPS, + INV_FSR_1000DPS, + INV_FSR_2000DPS, + NUM_FSR +}; + +enum inv_clock_sel_e { + INV_CLK_INTERNAL = 0, + INV_CLK_PLL, + NUM_CLK +}; + +ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj, + struct bin_attribute *attr, char *buf, loff_t pos, size_t size); +ssize_t inv_dmp_firmware_read(struct file *filp, + struct kobject *kobj, + struct bin_attribute *bin_attr, + char *buf, loff_t off, size_t count); +ssize_t inv_six_q_write(struct file *fp, struct kobject *kobj, + struct bin_attribute *attr, char *buf, loff_t pos, size_t size); + +int inv_mpu_configure_ring(struct iio_dev *indio_dev); +int inv_mpu_probe_trigger(struct iio_dev *indio_dev); +void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev); +void inv_mpu_remove_trigger(struct iio_dev *indio_dev); +int inv_init_config_mpu3050(struct iio_dev *indio_dev); +int inv_get_silicon_rev_mpu6050(struct inv_mpu_state *st); +int inv_get_silicon_rev_mpu6500(struct inv_mpu_state *st); +int set_3050_bypass(struct inv_mpu_state *st, bool enable); +int inv_register_mpu3050_slave(struct inv_mpu_state *st); +void inv_setup_reg_mpu3050(struct inv_reg_map_s *reg); +int inv_switch_3050_gyro_engine(struct inv_mpu_state *st, bool en); +int inv_switch_3050_accel_engine(struct inv_mpu_state *st, bool en); +int set_power_mpu3050(struct inv_mpu_state *st, bool power_on); +int inv_set_interrupt_on_gesture_event(struct inv_mpu_state *st, bool on); +int inv_set_display_orient_interrupt_dmp(struct inv_mpu_state *st, bool on); +u16 inv_dmp_get_address(u16 key); +int inv_q30_mult(int a, int b); +int inv_set_tap_threshold_dmp(struct inv_mpu_state *st, u16 threshold); +int inv_write_2bytes(struct inv_mpu_state *st, int k, int data); +int inv_set_min_taps_dmp(struct inv_mpu_state *st, u16 min_taps); +int inv_set_tap_time_dmp(struct inv_mpu_state *st, u16 time); +int inv_enable_tap_dmp(struct inv_mpu_state *st, bool on); +int inv_i2c_read_base(struct inv_mpu_state *st, u16 i2c_addr, + u8 reg, u16 length, u8 *data); +int inv_i2c_single_write_base(struct inv_mpu_state *st, + u16 i2c_addr, u8 reg, u8 data); +int inv_hw_self_test(struct inv_mpu_state *st); +s64 get_time_ns(void); +int write_be32_key_to_mem(struct inv_mpu_state *st, + u32 data, int key); +int inv_set_accel_bias_dmp(struct inv_mpu_state *st); +int inv_mpu_setup_compass_slave(struct inv_mpu_state *st); +int inv_mpu_setup_pressure_slave(struct inv_mpu_state *st); +int mpu_memory_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 const *data); +int mpu_memory_read(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 *data); +int mpu_memory_write_unaligned(struct inv_mpu_state *st, u16 key, int len, + u8 const *d); +int inv_quaternion_on(struct inv_mpu_state *st, + struct iio_buffer *ring, bool en); +int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en); +int inv_read_pedometer_counter(struct inv_mpu_state *st); +int inv_enable_pedometer(struct inv_mpu_state *st, bool en); +int inv_get_pedometer_steps(struct inv_mpu_state *st, u32 *steps); +int inv_get_pedometer_time(struct inv_mpu_state *st, u32 *time); +int inv_reset_fifo(struct iio_dev *indio_dev); +int inv_reset_offset_reg(struct inv_mpu_state *st, bool en); +int inv_batchmode_setup(struct inv_mpu_state *st); +void inv_init_sensor_struct(struct inv_mpu_state *st); +int inv_read_time_and_ticks(struct inv_mpu_state *st, bool resume); +int inv_flush_batch_data(struct iio_dev *indio_dev, bool *has_data); +int set_inv_enable(struct iio_dev *indio_dev, bool enable); +/* used to print i2c data using pr_debug */ +char *wr_pr_debug_begin(u8 const *data, u32 len, char *string); +char *wr_pr_debug_end(char *string); + +#define mem_w(a, b, c) \ + mpu_memory_write(st, st->i2c_addr, a, b, c) +#define mem_w_key(key, b, c) mpu_memory_write_unaligned(st, key, b, c) +#define inv_i2c_read(st, reg, len, data) \ + inv_i2c_read_base(st, st->i2c_addr, reg, len, data) +#define inv_i2c_single_write(st, reg, data) \ + inv_i2c_single_write_base(st, st->i2c_addr, reg, data) +#define inv_secondary_read(reg, len, data) \ + inv_i2c_read_base(st, st->plat_data.secondary_i2c_addr, reg, len, data) +#define inv_secondary_write(reg, data) \ + inv_i2c_single_write_base(st, st->plat_data.secondary_i2c_addr, \ + reg, data) +#define inv_aux_read(reg, len, data) \ + inv_i2c_read_base(st, st->plat_data.aux_i2c_addr, reg, len, data) +#define inv_aux_write(reg, data) \ + inv_i2c_single_write_base(st, st->plat_data.aux_i2c_addr, \ + reg, data) + +#endif /* #ifndef _INV_MPU_IIO_H_ */ + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_misc.c b/drivers/iio/imu/inv_mpu/inv_mpu_misc.c new file mode 100644 index 000000000000..70f239a35750 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_misc.c @@ -0,0 +1,2028 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/crc32.h> + +#include "inv_mpu_iio.h" +#include "inv_test/inv_counters.h" + +/* DMP defines */ +#define DMP_ORIENTATION_TIME 500 +#define DMP_ORIENTATION_ANGLE 60 +#define DMP_DEFAULT_FIFO_RATE 200 +#define DMP_TAP_SCALE (767603923 / 5) +#define DMP_MULTI_SHIFT 30 +#define DMP_MULTI_TAP_TIME 500 +#define DMP_SHAKE_REJECT_THRESH 100 +#define DMP_SHAKE_REJECT_TIME 10 +#define DMP_SHAKE_REJECT_TIMEOUT 10 +#define DMP_ANGLE_SCALE 15 +#define DMP_PRECISION 1000 +#define DMP_MAX_DIVIDER 4 +#define DMP_MAX_MIN_TAPS 4 +#define DMP_IMAGE_CRC_VALUE 0x972aae92 + +/*--- Test parameters defaults --- */ +#define DEF_OLDEST_SUPP_PROD_REV 8 +#define DEF_OLDEST_SUPP_SW_REV 2 + +/* sample rate */ +#define DEF_SELFTEST_SAMPLE_RATE 0 +/* full scale setting dps */ +#define DEF_SELFTEST_GYRO_FS (0 << 3) +#define DEF_SELFTEST_ACCEL_FS (2 << 3) +#define DEF_SELFTEST_GYRO_SENS (32768 / 250) +/* wait time before collecting data */ +#define DEF_GYRO_WAIT_TIME 10 +#define DEF_ST_STABLE_TIME 20 +#define DEF_ST_6500_STABLE_TIME 20 +#define DEF_GYRO_SCALE 131 +#define DEF_ST_PRECISION 1000 +#define DEF_ST_ACCEL_FS_MG 8000UL +#define DEF_ST_SCALE (1L << 15) +#define DEF_ST_TRY_TIMES 2 +#define DEF_ST_COMPASS_RESULT_SHIFT 2 +#define DEF_ST_ACCEL_RESULT_SHIFT 1 +#define DEF_ST_OTP0_THRESH 60 +#define DEF_ST_ABS_THRESH 20 +#define DEF_ST_TOR 2 + +#define X 0 +#define Y 1 +#define Z 2 +/*---- MPU6050 notable product revisions ----*/ +#define MPU_PRODUCT_KEY_B1_E1_5 105 +#define MPU_PRODUCT_KEY_B2_F1 431 +/* accelerometer Hw self test min and max bias shift (mg) */ +#define DEF_ACCEL_ST_SHIFT_MIN 300 +#define DEF_ACCEL_ST_SHIFT_MAX 950 + +#define DEF_ACCEL_ST_SHIFT_DELTA 500 +#define DEF_GYRO_CT_SHIFT_DELTA 500 +/* gyroscope Coriolis self test min and max bias shift (dps) */ +#define DEF_GYRO_CT_SHIFT_MIN 10 +#define DEF_GYRO_CT_SHIFT_MAX 105 + +/*---- MPU6500 Self Test Pass/Fail Criteria ----*/ +/* Gyro Offset Max Value (dps) */ +#define DEF_GYRO_OFFSET_MAX 20 +/* Gyro Self Test Absolute Limits ST_AL (dps) */ +#define DEF_GYRO_ST_AL 60 +/* Accel Self Test Absolute Limits ST_AL (mg) */ +#define DEF_ACCEL_ST_AL_MIN 225 +#define DEF_ACCEL_ST_AL_MAX 675 +#define DEF_6500_ACCEL_ST_SHIFT_DELTA 500 +#define DEF_6500_GYRO_CT_SHIFT_DELTA 500 +#define DEF_ST_MPU6500_ACCEL_LPF 2 +#define DEF_ST_6500_ACCEL_FS_MG 2000UL +#define DEF_SELFTEST_6500_ACCEL_FS (0 << 3) + +/* Note: The ST_AL values are only used when ST_OTP = 0, + * i.e no factory self test values for reference + */ + +/* NOTE: product entries are in chronological order */ +static const struct prod_rev_map_t prod_rev_map[] = { + /* prod_ver = 0 */ + {MPL_PROD_KEY(0, 1), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 2), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 3), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 4), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 5), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 6), MPU_SILICON_REV_A2, 131, 16384}, + /* prod_ver = 1 */ + {MPL_PROD_KEY(0, 7), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 8), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 9), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 10), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 11), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 12), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 13), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 14), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 15), MPU_SILICON_REV_A2, 131, 16384}, + {MPL_PROD_KEY(0, 27), MPU_SILICON_REV_A2, 131, 16384}, + /* prod_ver = 1 */ + {MPL_PROD_KEY(1, 16), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 17), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 18), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 19), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 20), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 28), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 1), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 2), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 3), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 4), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 5), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(1, 6), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 2 */ + {MPL_PROD_KEY(2, 7), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 8), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 9), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 10), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 11), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 12), MPU_SILICON_REV_B1, 131, 16384}, + {MPL_PROD_KEY(2, 29), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 3 */ + {MPL_PROD_KEY(3, 30), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 4 */ + {MPL_PROD_KEY(4, 31), MPU_SILICON_REV_B1, 131, 8192}, + {MPL_PROD_KEY(4, 1), MPU_SILICON_REV_B1, 131, 8192}, + {MPL_PROD_KEY(4, 3), MPU_SILICON_REV_B1, 131, 8192}, + /* prod_ver = 5 */ + {MPL_PROD_KEY(5, 3), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 6 */ + {MPL_PROD_KEY(6, 19), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 7 */ + {MPL_PROD_KEY(7, 19), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 8 */ + {MPL_PROD_KEY(8, 19), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 9 */ + {MPL_PROD_KEY(9, 19), MPU_SILICON_REV_B1, 131, 16384}, + /* prod_ver = 10 */ + {MPL_PROD_KEY(10, 19), MPU_SILICON_REV_B1, 131, 16384} +}; + +/* +* List of product software revisions +* +* NOTE : +* software revision 0 falls back to the old detection method +* based off the product version and product revision per the +* table above +*/ +static const struct prod_rev_map_t sw_rev_map[] = { + {0, 0, 0, 0}, + {1, MPU_SILICON_REV_B1, 131, 8192}, /* rev C */ + {2, MPU_SILICON_REV_B1, 131, 16384} /* rev D */ +}; + +static const u16 mpu_6500_st_tb[256] = { + 2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808, + 2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041, + 3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293, + 3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566, + 3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862, + 3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182, + 4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528, + 4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903, + 4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310, + 5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750, + 5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226, + 6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742, + 6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301, + 7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906, + 7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561, + 8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270, + 9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038, + 10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870, + 10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771, + 11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746, + 12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802, + 13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946, + 15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184, + 16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526, + 17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978, + 19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550, + 20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253, + 22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097, + 24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093, + 26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255, + 28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597, + 30903, 31212, 31524, 31839, 32157, 32479, 32804 +}; + +static const int accel_st_tb[31] = { + 340, 351, 363, 375, 388, 401, 414, 428, + 443, 458, 473, 489, 506, 523, 541, 559, + 578, 597, 617, 638, 660, 682, 705, 729, + 753, 779, 805, 832, 860, 889, 919 +}; + +static const int gyro_6050_st_tb[31] = { + 3275, 3425, 3583, 3748, 3920, 4100, 4289, 4486, + 4693, 4909, 5134, 5371, 5618, 5876, 6146, 6429, + 6725, 7034, 7358, 7696, 8050, 8421, 8808, 9213, + 9637, 10080, 10544, 11029, 11537, 12067, 12622 +}; + +static const int gyro_3500_st_tb[255] = { + 2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808, + 2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041, + 3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293, + 3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566, + 3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862, + 3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182, + 4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528, + 4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903, + 4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310, + 5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750, + 5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226, + 6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742, + 6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301, + 7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906, + 7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561, + 8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270, + 9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038, + 10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870, + 10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771, + 11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746, + 12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802, + 13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946, + 15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184, + 16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526, + 17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978, + 19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550, + 20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253, + 22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097, + 24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093, + 26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255, + 28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597, + 30903, 31212, 31524, 31839, 32157, 32479, 32804 +}; + +char *wr_pr_debug_begin(u8 const *data, u32 len, char *string) +{ + int ii; + string = kmalloc(len * 2 + 1, GFP_KERNEL); + for (ii = 0; ii < len; ii++) + sprintf(&string[ii * 2], "%02X", data[ii]); + string[len * 2] = 0; + return string; +} + +char *wr_pr_debug_end(char *string) +{ + kfree(string); + return ""; +} + +int mpu_memory_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 const *data) +{ + u8 bank[2]; + u8 addr[2]; + u8 buf[513]; + + struct i2c_msg msgs[3]; + int res; + + if (!data || !st) + return -EINVAL; + + if (len >= (sizeof(buf) - 1)) + return -ENOMEM; + + bank[0] = REG_BANK_SEL; + bank[1] = mem_addr >> 8; + + addr[0] = REG_MEM_START_ADDR; + addr[1] = mem_addr & 0xFF; + + buf[0] = REG_MEM_RW; + memcpy(buf + 1, data, len); + + /* write message */ + msgs[0].addr = mpu_addr; + msgs[0].flags = 0; + msgs[0].buf = bank; + msgs[0].len = sizeof(bank); + + msgs[1].addr = mpu_addr; + msgs[1].flags = 0; + msgs[1].buf = addr; + msgs[1].len = sizeof(addr); + + msgs[2].addr = mpu_addr; + msgs[2].flags = 0; + msgs[2].buf = (u8 *)buf; + msgs[2].len = len + 1; + + INV_I2C_INC_MPUWRITE(3 + 3 + (2 + len)); +#if CONFIG_DYNAMIC_DEBUG + { + char *write = 0; + pr_debug("%s WM%02X%02X%02X%s%s - %d\n", st->hw->name, + mpu_addr, bank[1], addr[1], + wr_pr_debug_begin(data, len, write), + wr_pr_debug_end(write), + len); + } +#endif + + res = i2c_transfer(st->sl_handle, msgs, 3); + if (res != 3) { + if (res >= 0) + res = -EIO; + return res; + } else { + return 0; + } +} + +int mpu_memory_read(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, + u32 len, u8 *data) +{ + u8 bank[2]; + u8 addr[2]; + u8 buf; + + struct i2c_msg msgs[4]; + int res; + + if (!data || !st) + return -EINVAL; + + bank[0] = REG_BANK_SEL; + bank[1] = mem_addr >> 8; + + addr[0] = REG_MEM_START_ADDR; + addr[1] = mem_addr & 0xFF; + + buf = REG_MEM_RW; + + /* write message */ + msgs[0].addr = mpu_addr; + msgs[0].flags = 0; + msgs[0].buf = bank; + msgs[0].len = sizeof(bank); + + msgs[1].addr = mpu_addr; + msgs[1].flags = 0; + msgs[1].buf = addr; + msgs[1].len = sizeof(addr); + + msgs[2].addr = mpu_addr; + msgs[2].flags = 0; + msgs[2].buf = &buf; + msgs[2].len = 1; + + msgs[3].addr = mpu_addr; + msgs[3].flags = I2C_M_RD; + msgs[3].buf = data; + msgs[3].len = len; + + res = i2c_transfer(st->sl_handle, msgs, 4); + if (res != 4) { + if (res >= 0) + res = -EIO; + } else + res = 0; + + INV_I2C_INC_MPUWRITE(3 + 3 + 3); + INV_I2C_INC_MPUREAD(len); +#if CONFIG_DYNAMIC_DEBUG + { + char *read = 0; + pr_debug("%s RM%02X%02X%02X%02X - %s%s\n", st->hw->name, + mpu_addr, bank[1], addr[1], len, + wr_pr_debug_begin(data, len, read), + wr_pr_debug_end(read)); + } +#endif + + return res; +} + +int mpu_memory_write_unaligned(struct inv_mpu_state *st, u16 key, int len, + u8 const *d) +{ + u32 addr; + int start, end; + int len1, len2; + int result = 0; + + if (len > MPU_MEM_BANK_SIZE) + return -EINVAL; + addr = inv_dmp_get_address(key); + if (addr > MPU6XXX_MAX_MPU_MEM) + return -EINVAL; + + start = (addr >> 8); + end = ((addr + len - 1) >> 8); + if (start == end) { + result = mpu_memory_write(st, st->i2c_addr, addr, len, d); + } else { + end <<= 8; + len1 = end - addr; + len2 = len - len1; + result = mpu_memory_write(st, st->i2c_addr, addr, len1, d); + result |= mpu_memory_write(st, st->i2c_addr, end, len2, + d + len1); + } + + return result; +} + +/** + * index_of_key()- Inverse lookup of the index of an MPL product key . + * @key: the MPL product indentifier also referred to as 'key'. + */ +static short index_of_key(u16 key) +{ + int i; + for (i = 0; i < NUM_OF_PROD_REVS; i++) + if (prod_rev_map[i].mpl_product_key == key) + return (short)i; + return -EINVAL; +} + +int inv_get_silicon_rev_mpu6500(struct inv_mpu_state *st) +{ + struct inv_chip_info_s *chip_info = &st->chip_info; + int result; + u8 whoami, sw_rev; + + result = inv_i2c_read(st, REG_WHOAMI, 1, &whoami); + if (result) + return result; + if (whoami != MPU6500_ID && whoami != MPU9250_ID && + whoami != MPU9350_ID && whoami != MPU6515_ID) + return -EINVAL; + + /*memory read need more time after power up */ + msleep(POWER_UP_TIME); + result = mpu_memory_read(st, st->i2c_addr, + MPU6500_MEM_REV_ADDR, 1, &sw_rev); + sw_rev &= INV_MPU_REV_MASK; + if (result) + return result; + if (sw_rev != 0) + return -EINVAL; + /* these values are place holders and not real values */ + chip_info->product_id = MPU6500_PRODUCT_REVISION; + chip_info->product_revision = MPU6500_PRODUCT_REVISION; + chip_info->silicon_revision = MPU6500_PRODUCT_REVISION; + chip_info->software_revision = sw_rev; + chip_info->gyro_sens_trim = DEFAULT_GYRO_TRIM; + chip_info->accel_sens_trim = DEFAULT_ACCEL_TRIM; + chip_info->multi = 1; + + return 0; +} + +int inv_get_silicon_rev_mpu6050(struct inv_mpu_state *st) +{ + int result; + struct inv_reg_map_s *reg; + u8 prod_ver = 0x00, prod_rev = 0x00; + struct prod_rev_map_t *p_rev; + u8 bank = + (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0); + u16 mem_addr = ((bank << 8) | MEM_ADDR_PROD_REV); + u16 key; + u8 regs[5]; + u16 sw_rev; + short index; + struct inv_chip_info_s *chip_info = &st->chip_info; + reg = &st->reg; + + result = inv_i2c_read(st, REG_PRODUCT_ID, 1, &prod_ver); + if (result) + return result; + prod_ver &= 0xf; + /*memory read need more time after power up */ + msleep(POWER_UP_TIME); + result = mpu_memory_read(st, st->i2c_addr, mem_addr, 1, &prod_rev); + if (result) + return result; + prod_rev >>= 2; + /* clean the prefetch and cfg user bank bits */ + result = inv_i2c_single_write(st, reg->bank_sel, 0); + if (result) + return result; + /* get the software-product version, read from XA_OFFS_L */ + result = inv_i2c_read(st, REG_XA_OFFS_L_TC, + SOFT_PROD_VER_BYTES, regs); + if (result) + return result; + + sw_rev = (regs[4] & 0x01) << 2 | /* 0x0b, bit 0 */ + (regs[2] & 0x01) << 1 | /* 0x09, bit 0 */ + (regs[0] & 0x01); /* 0x07, bit 0 */ + /* if 0, use the product key to determine the type of part */ + if (sw_rev == 0) { + key = MPL_PROD_KEY(prod_ver, prod_rev); + if (key == 0) + return -EINVAL; + index = index_of_key(key); + if (index < 0 || index >= NUM_OF_PROD_REVS) + return -EINVAL; + /* check MPL is compiled for this device */ + if (prod_rev_map[index].silicon_rev != MPU_SILICON_REV_B1) + return -EINVAL; + p_rev = (struct prod_rev_map_t *)&prod_rev_map[index]; + /* if valid, use the software product key */ + } else if (sw_rev < ARRAY_SIZE(sw_rev_map)) { + p_rev = (struct prod_rev_map_t *)&sw_rev_map[sw_rev]; + } else { + return -EINVAL; + } + chip_info->product_id = prod_ver; + chip_info->product_revision = prod_rev; + chip_info->silicon_revision = p_rev->silicon_rev; + chip_info->software_revision = sw_rev; + chip_info->gyro_sens_trim = p_rev->gyro_trim; + chip_info->accel_sens_trim = p_rev->accel_trim; + if (chip_info->accel_sens_trim == 0) + chip_info->accel_sens_trim = DEFAULT_ACCEL_TRIM; + chip_info->multi = DEFAULT_ACCEL_TRIM / chip_info->accel_sens_trim; + if (chip_info->multi != 1) + pr_info("multi is %d\n", chip_info->multi); + return result; +} + +/** + * read_accel_hw_self_test_prod_shift()- read the accelerometer hardware + * self-test bias shift calculated + * during final production test and + * stored in chip non-volatile memory. + * @st: main data structure. + * @st_prod: A pointer to an array of 3 elements to hold the values + * for production hardware self-test bias shifts returned to the + * user. + * @accel_sens: accel sensitivity. + */ +static int read_accel_hw_self_test_prod_shift(struct inv_mpu_state *st, + int *st_prod, int *accel_sens) +{ + u8 regs[4]; + u8 shift_code[3]; + int result, i; + + for (i = 0; i < 3; i++) + st_prod[i] = 0; + + result = inv_i2c_read(st, REG_ST_GCT_X, ARRAY_SIZE(regs), regs); + if (result) + return result; + if ((0 == regs[0]) && (0 == regs[1]) && + (0 == regs[2]) && (0 == regs[3])) + return -EINVAL; + shift_code[X] = ((regs[0] & 0xE0) >> 3) | ((regs[3] & 0x30) >> 4); + shift_code[Y] = ((regs[1] & 0xE0) >> 3) | ((regs[3] & 0x0C) >> 2); + shift_code[Z] = ((regs[2] & 0xE0) >> 3) | (regs[3] & 0x03); + for (i = 0; i < 3; i++) + if (shift_code[i] != 0) + st_prod[i] = accel_sens[i] * + accel_st_tb[shift_code[i] - 1]; + + return 0; +} + +/** +* inv_check_accel_self_test()- check accel self test. this function returns +* zero as success. A non-zero return value +* indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +static int inv_check_accel_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg){ + int gravity, j, ret_val; + int tmp; + int st_shift_prod[THREE_AXIS], st_shift_cust[THREE_AXIS]; + int st_shift_ratio[THREE_AXIS]; + int accel_sens[THREE_AXIS]; + + if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV && + st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV) + return 0; + ret_val = 0; + tmp = DEF_ST_SCALE * DEF_ST_PRECISION / DEF_ST_ACCEL_FS_MG; + for (j = 0; j < 3; j++) + accel_sens[j] = tmp; + + if (MPL_PROD_KEY(st->chip_info.product_id, + st->chip_info.product_revision) == + MPU_PRODUCT_KEY_B1_E1_5) { + /* half sensitivity Z accelerometer parts */ + accel_sens[Z] /= 2; + } else { + /* half sensitivity X, Y, Z accelerometer parts */ + accel_sens[X] /= st->chip_info.multi; + accel_sens[Y] /= st->chip_info.multi; + accel_sens[Z] /= st->chip_info.multi; + } + gravity = accel_sens[Z]; + ret_val = read_accel_hw_self_test_prod_shift(st, st_shift_prod, + accel_sens); + if (ret_val) + return ret_val; + + for (j = 0; j < 3; j++) { + st_shift_cust[j] = abs(reg_avg[j] - st_avg[j]); + if (st_shift_prod[j]) { + tmp = st_shift_prod[j] / DEF_ST_PRECISION; + st_shift_ratio[j] = abs(st_shift_cust[j] / tmp + - DEF_ST_PRECISION); + if (st_shift_ratio[j] > DEF_ACCEL_ST_SHIFT_DELTA) + ret_val = 1; + } else { + if (st_shift_cust[j] < + DEF_ACCEL_ST_SHIFT_MIN * gravity) + ret_val = 1; + if (st_shift_cust[j] > + DEF_ACCEL_ST_SHIFT_MAX * gravity) + ret_val = 1; + } + } + + return ret_val; +} + +/** +* inv_check_3500_gyro_self_test() check gyro self test. this function returns +* zero as success. A non-zero return value +* indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ + +static int inv_check_3500_gyro_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg){ + int result; + int gst[3], ret_val; + int gst_otp[3], i; + u8 st_code[THREE_AXIS]; + ret_val = 0; + + for (i = 0; i < 3; i++) + gst[i] = st_avg[i] - reg_avg[i]; + result = inv_i2c_read(st, REG_3500_OTP, THREE_AXIS, st_code); + if (result) + return result; + gst_otp[0] = 0; + gst_otp[1] = 0; + gst_otp[2] = 0; + for (i = 0; i < 3; i++) { + if (st_code[i] != 0) + gst_otp[i] = gyro_3500_st_tb[st_code[i] - 1]; + } + /* check self test value passing criterion. Using the DEF_ST_TOR + * for certain degree of tolerance */ + for (i = 0; i < 3; i++) { + if (gst_otp[i] == 0) { + if (abs(gst[i]) * DEF_ST_TOR < DEF_ST_OTP0_THRESH * + DEF_ST_PRECISION * + DEF_GYRO_SCALE) + ret_val |= (1 << i); + } else { + if (abs(gst[i]/gst_otp[i] - DEF_ST_PRECISION) > + DEF_GYRO_CT_SHIFT_DELTA) + ret_val |= (1 << i); + } + } + /* check for absolute value passing criterion. Using DEF_ST_TOR + * for certain degree of tolerance */ + for (i = 0; i < 3; i++) { + if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH * + DEF_ST_PRECISION * DEF_GYRO_SCALE) + ret_val |= (1 << i); + } + + return ret_val; +} + +/** +* inv_check_6050_gyro_self_test() - check 6050 gyro self test. this function +* returns zero as success. A non-zero return +* value indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +static int inv_check_6050_gyro_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg){ + int result; + int ret_val; + int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i; + u8 regs[3]; + + if (st->chip_info.software_revision < DEF_OLDEST_SUPP_SW_REV && + st->chip_info.product_revision < DEF_OLDEST_SUPP_PROD_REV) + return 0; + + ret_val = 0; + result = inv_i2c_read(st, REG_ST_GCT_X, 3, regs); + if (result) + return result; + regs[X] &= 0x1f; + regs[Y] &= 0x1f; + regs[Z] &= 0x1f; + for (i = 0; i < 3; i++) { + if (regs[i] != 0) + st_shift_prod[i] = gyro_6050_st_tb[regs[i] - 1]; + else + st_shift_prod[i] = 0; + } + st_shift_prod[1] = -st_shift_prod[1]; + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = st_avg[i] - reg_avg[i]; + if (st_shift_prod[i]) { + st_shift_ratio[i] = abs(st_shift_cust[i] / + st_shift_prod[i] - DEF_ST_PRECISION); + if (st_shift_ratio[i] > DEF_GYRO_CT_SHIFT_DELTA) + ret_val = 1; + } else { + if (st_shift_cust[i] < DEF_ST_PRECISION * + DEF_GYRO_CT_SHIFT_MIN * DEF_SELFTEST_GYRO_SENS) + ret_val = 1; + if (st_shift_cust[i] > DEF_ST_PRECISION * + DEF_GYRO_CT_SHIFT_MAX * DEF_SELFTEST_GYRO_SENS) + ret_val = 1; + } + } + /* check for absolute value passing criterion. Using DEF_ST_TOR + * for certain degree of tolerance */ + for (i = 0; i < 3; i++) + if (abs(reg_avg[i]) > DEF_ST_TOR * DEF_ST_ABS_THRESH * + DEF_ST_PRECISION * DEF_GYRO_SCALE) + ret_val = 1; + + return ret_val; +} + +/** +* inv_check_6500_gyro_self_test() - check 6500 gyro self test. this function +* returns zero as success. A non-zero return +* value indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +static int inv_check_6500_gyro_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg) { + u8 regs[3]; + int ret_val, result; + int otp_value_zero = 0; + int st_shift_prod[3], st_shift_cust[3], i; + + ret_val = 0; + result = inv_i2c_read(st, REG_6500_XG_ST_DATA, 3, regs); + if (result) + return result; + pr_debug("%s self_test gyro shift_code - %02x %02x %02x\n", + st->hw->name, regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + } else { + st_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + pr_debug("%s self_test gyro st_shift_prod - %+d %+d %+d\n", + st->hw->name, st_shift_prod[0], st_shift_prod[1], + st_shift_prod[2]); + + for (i = 0; i < 3; i++) { + st_shift_cust[i] = st_avg[i] - reg_avg[i]; + if (!otp_value_zero) { + /* Self Test Pass/Fail Criteria A */ + if (st_shift_cust[i] < DEF_6500_GYRO_CT_SHIFT_DELTA + * st_shift_prod[i]) + ret_val = 1; + } else { + /* Self Test Pass/Fail Criteria B */ + if (st_shift_cust[i] < DEF_GYRO_ST_AL * + DEF_SELFTEST_GYRO_SENS * + DEF_ST_PRECISION) + ret_val = 1; + } + } + pr_debug("%s self_test gyro st_shift_cust - %+d %+d %+d\n", + st->hw->name, st_shift_cust[0], st_shift_cust[1], + st_shift_cust[2]); + + if (ret_val == 0) { + /* Self Test Pass/Fail Criteria C */ + for (i = 0; i < 3; i++) + if (abs(reg_avg[i]) > DEF_GYRO_OFFSET_MAX * + DEF_SELFTEST_GYRO_SENS * + DEF_ST_PRECISION) + ret_val = 1; + } + + return ret_val; +} + +/** +* inv_check_6500_accel_self_test() - check 6500 accel self test. this function +* returns zero as success. A non-zero return +* value indicates failure in self test. +* @*st: main data structure. +* @*reg_avg: average value of normal test. +* @*st_avg: average value of self test +*/ +static int inv_check_6500_accel_self_test(struct inv_mpu_state *st, + int *reg_avg, int *st_avg) { + int ret_val, result; + int st_shift_prod[3], st_shift_cust[3], st_shift_ratio[3], i; + u8 regs[3]; + int otp_value_zero = 0; + +#define ACCEL_ST_AL_MIN ((DEF_ACCEL_ST_AL_MIN * DEF_ST_SCALE \ + / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION) +#define ACCEL_ST_AL_MAX ((DEF_ACCEL_ST_AL_MAX * DEF_ST_SCALE \ + / DEF_ST_6500_ACCEL_FS_MG) * DEF_ST_PRECISION) + + ret_val = 0; + result = inv_i2c_read(st, REG_6500_XA_ST_DATA, 3, regs); + if (result) + return result; + pr_debug("%s self_test accel shift_code - %02x %02x %02x\n", + st->hw->name, regs[0], regs[1], regs[2]); + + for (i = 0; i < 3; i++) { + if (regs[i] != 0) { + st_shift_prod[i] = mpu_6500_st_tb[regs[i] - 1]; + } else { + st_shift_prod[i] = 0; + otp_value_zero = 1; + } + } + pr_debug("%s self_test accel st_shift_prod - %+d %+d %+d\n", + st->hw->name, st_shift_prod[0], st_shift_prod[1], + st_shift_prod[2]); + + if (!otp_value_zero) { + /* Self Test Pass/Fail Criteria A */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = st_avg[i] - reg_avg[i]; + st_shift_ratio[i] = abs(st_shift_cust[i] / + st_shift_prod[i] - DEF_ST_PRECISION); + if (st_shift_ratio[i] > DEF_6500_ACCEL_ST_SHIFT_DELTA) + ret_val = 1; + } + } else { + /* Self Test Pass/Fail Criteria B */ + for (i = 0; i < 3; i++) { + st_shift_cust[i] = abs(st_avg[i] - reg_avg[i]); + if (st_shift_cust[i] < ACCEL_ST_AL_MIN || + st_shift_cust[i] > ACCEL_ST_AL_MAX) + ret_val = 1; + } + } + pr_debug("%s self_test accel st_shift_cust - %+d %+d %+d\n", + st->hw->name, st_shift_cust[0], st_shift_cust[1], + st_shift_cust[2]); + + return ret_val; +} + +/* + * inv_do_test() - do the actual test of self testing + */ +static int inv_do_test(struct inv_mpu_state *st, int self_test_flag, + int *gyro_result, int *accel_result) +{ + struct inv_reg_map_s *reg; + int result, i, j, packet_size; + u8 data[BYTES_PER_SENSOR * 2], d; + bool has_accel; + int fifo_count, packet_count, ind, s; + + reg = &st->reg; + has_accel = (st->chip_type != INV_ITG3500); + if (has_accel) + packet_size = BYTES_PER_SENSOR * 2; + else + packet_size = BYTES_PER_SENSOR; + + result = inv_i2c_single_write(st, reg->int_enable, 0); + if (result) + return result; + /* disable the sensor output to FIFO */ + result = inv_i2c_single_write(st, reg->fifo_en, 0); + if (result) + return result; + /* disable fifo reading */ + result = inv_i2c_single_write(st, reg->user_ctrl, 0); + if (result) + return result; + /* clear FIFO */ + result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_RST); + if (result) + return result; + /* setup parameters */ + result = inv_i2c_single_write(st, reg->lpf, INV_FILTER_98HZ); + if (result) + return result; + + if (INV_MPU6500 == st->chip_type) { + /* config accel LPF register for MPU6500 */ + result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2, + DEF_ST_MPU6500_ACCEL_LPF | + BIT_FIFO_SIZE_1K); + if (result) + return result; + } + + result = inv_i2c_single_write(st, reg->sample_rate_div, + DEF_SELFTEST_SAMPLE_RATE); + if (result) + return result; + /* wait for the sampling rate change to stabilize */ + mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE); + result = inv_i2c_single_write(st, reg->gyro_config, + self_test_flag | DEF_SELFTEST_GYRO_FS); + if (result) + return result; + if (has_accel) { + if (INV_MPU6500 == st->chip_type) + d = DEF_SELFTEST_6500_ACCEL_FS; + else + d = DEF_SELFTEST_ACCEL_FS; + d |= self_test_flag; + result = inv_i2c_single_write(st, reg->accel_config, d); + if (result) + return result; + } + /* wait for the output to get stable */ + if (self_test_flag) { + if (INV_MPU6500 == st->chip_type) + msleep(DEF_ST_6500_STABLE_TIME); + else + msleep(DEF_ST_STABLE_TIME); + } + + /* enable FIFO reading */ + result = inv_i2c_single_write(st, reg->user_ctrl, BIT_FIFO_EN); + if (result) + return result; + /* enable sensor output to FIFO */ + if (has_accel) + d = BITS_GYRO_OUT | BIT_ACCEL_OUT; + else + d = BITS_GYRO_OUT; + for (i = 0; i < THREE_AXIS; i++) { + gyro_result[i] = 0; + accel_result[i] = 0; + } + s = 0; + while (s < st->self_test.samples) { + result = inv_i2c_single_write(st, reg->fifo_en, d); + if (result) + return result; + mdelay(DEF_GYRO_WAIT_TIME); + result = inv_i2c_single_write(st, reg->fifo_en, 0); + if (result) + return result; + + result = inv_i2c_read(st, reg->fifo_count_h, + FIFO_COUNT_BYTE, data); + if (result) + return result; + fifo_count = be16_to_cpup((__be16 *)(&data[0])); + pr_debug("%s self_test fifo_count - %d\n", + st->hw->name, fifo_count); + packet_count = fifo_count / packet_size; + i = 0; + while ((i < packet_count) && (s < st->self_test.samples)) { + short vals[3]; + result = inv_i2c_read(st, reg->fifo_r_w, + packet_size, data); + if (result) + return result; + ind = 0; + if (has_accel) { + for (j = 0; j < THREE_AXIS; j++) { + vals[j] = (short)be16_to_cpup( + (__be16 *)(&data[ind + 2 * j])); + accel_result[j] += vals[j]; + } + ind += BYTES_PER_SENSOR; + pr_debug( + "%s self_test accel data - %d %+d %+d %+d", + st->hw->name, s, vals[0], vals[1], vals[2]); + } + + for (j = 0; j < THREE_AXIS; j++) { + vals[j] = (short)be16_to_cpup( + (__be16 *)(&data[ind + 2 * j])); + gyro_result[j] += vals[j]; + } + pr_debug("%s self_test gyro data - %d %+d %+d %+d", + st->hw->name, s, vals[0], vals[1], vals[2]); + + s++; + i++; + } + } + + if (has_accel) { + for (j = 0; j < THREE_AXIS; j++) { + accel_result[j] = accel_result[j] / s; + accel_result[j] *= DEF_ST_PRECISION; + } + } + for (j = 0; j < THREE_AXIS; j++) { + gyro_result[j] = gyro_result[j] / s; + gyro_result[j] *= DEF_ST_PRECISION; + } + + return 0; +} + +/* + * inv_recover_setting() recover the old settings after everything is done + */ +static void inv_recover_setting(struct inv_mpu_state *st) +{ + struct inv_reg_map_s *reg; + int data; + + reg = &st->reg; + inv_i2c_single_write(st, reg->gyro_config, + st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT); + inv_i2c_single_write(st, reg->lpf, st->chip_config.lpf); + data = ONE_K_HZ/st->chip_config.fifo_rate - 1; + inv_i2c_single_write(st, reg->sample_rate_div, data); + /* wait for the sampling rate change to stabilize */ + mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE); + if (INV_ITG3500 != st->chip_type) { + inv_i2c_single_write(st, reg->accel_config, + (st->chip_config.accel_fs << + ACCEL_CONFIG_FSR_SHIFT)); + } + inv_reset_offset_reg(st, false); + st->switch_gyro_engine(st, false); + st->switch_accel_engine(st, false); + st->set_power_state(st, false); +} + + +static int inv_power_up_self_test(struct inv_mpu_state *st) +{ + int result; + + result = st->set_power_state(st, true); + if (result) + return result; + result = st->switch_accel_engine(st, true); + if (result) + return result; + result = st->switch_gyro_engine(st, true); + if (result) + return result; + + return 0; +} + +/* + * inv_hw_self_test() - main function to do hardware self test + */ +int inv_hw_self_test(struct inv_mpu_state *st) +{ + int result; + int gyro_bias_st[THREE_AXIS], gyro_bias_regular[THREE_AXIS]; + int accel_bias_st[THREE_AXIS], accel_bias_regular[THREE_AXIS]; + int test_times, i; + char compass_result, accel_result, gyro_result; + + result = inv_power_up_self_test(st); + if (result) + return result; + result = inv_reset_offset_reg(st, true); + if (result) + return result; + compass_result = 0; + accel_result = 0; + gyro_result = 0; + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, 0, gyro_bias_regular, + accel_bias_regular); + if (result == -EAGAIN) + test_times--; + else + test_times = 0; + } + if (result) + goto test_fail; + pr_debug("%s self_test accel bias_regular - %+d %+d %+d\n", + st->hw->name, accel_bias_regular[0], + accel_bias_regular[1], accel_bias_regular[2]); + pr_debug("%s self_test gyro bias_regular - %+d %+d %+d\n", + st->hw->name, gyro_bias_regular[0], gyro_bias_regular[1], + gyro_bias_regular[2]); + + for (i = 0; i < 3; i++) { + st->gyro_bias[i] = gyro_bias_regular[i]; + st->accel_bias[i] = accel_bias_regular[i]; + } + + test_times = DEF_ST_TRY_TIMES; + while (test_times > 0) { + result = inv_do_test(st, BITS_SELF_TEST_EN, gyro_bias_st, + accel_bias_st); + if (result == -EAGAIN) + test_times--; + else + break; + } + if (result) + goto test_fail; + pr_debug("%s self_test accel bias_st - %+d %+d %+d\n", + st->hw->name, accel_bias_st[0], accel_bias_st[1], + accel_bias_st[2]); + pr_debug("%s self_test gyro bias_st - %+d %+d %+d\n", + st->hw->name, gyro_bias_st[0], gyro_bias_st[1], + gyro_bias_st[2]); + + if (st->chip_type == INV_ITG3500) { + gyro_result = !inv_check_3500_gyro_self_test(st, + gyro_bias_regular, gyro_bias_st); + } else { + if (st->chip_config.has_compass) + compass_result = !st->slave_compass->self_test(st); + + if (INV_MPU6050 == st->chip_type) { + accel_result = !inv_check_accel_self_test(st, + accel_bias_regular, accel_bias_st); + gyro_result = !inv_check_6050_gyro_self_test(st, + gyro_bias_regular, gyro_bias_st); + } else if (INV_MPU6500 == st->chip_type) { + accel_result = !inv_check_6500_accel_self_test(st, + accel_bias_regular, accel_bias_st); + gyro_result = !inv_check_6500_gyro_self_test(st, + gyro_bias_regular, gyro_bias_st); + } + } + +test_fail: + inv_recover_setting(st); + + return (compass_result << DEF_ST_COMPASS_RESULT_SHIFT) | + (accel_result << DEF_ST_ACCEL_RESULT_SHIFT) | gyro_result; +} + +static int inv_load_firmware(struct inv_mpu_state *st, + u8 *data, int size) +{ + int bank, write_size; + int result; + u16 memaddr; + + /* first bank start at MPU_DMP_LOAD_START */ + write_size = MPU_MEM_BANK_SIZE - MPU_DMP_LOAD_START; + memaddr = MPU_DMP_LOAD_START; + result = mem_w(memaddr, write_size, data); + if (result) + return result; + size -= write_size; + data += write_size; + + /* Write and verify memory */ + for (bank = 1; size > 0; bank++, size -= write_size, + data += write_size) { + if (size > MPU_MEM_BANK_SIZE) + write_size = MPU_MEM_BANK_SIZE; + else + write_size = size; + + memaddr = ((bank << 8) | 0x00); + + result = mem_w(memaddr, write_size, data); + if (result) + return result; + } + return 0; +} + +static int inv_verify_firmware(struct inv_mpu_state *st, + u8 *data, int size) +{ + int bank, write_size; + int result; + u16 memaddr; + u8 firmware[MPU_MEM_BANK_SIZE]; + + /* Write and verify memory */ + write_size = MPU_MEM_BANK_SIZE - MPU_DMP_LOAD_START; + size -= write_size; + data += write_size; + for (bank = 1; size > 0; bank++, + size -= write_size, + data += write_size) { + if (size > MPU_MEM_BANK_SIZE) + write_size = MPU_MEM_BANK_SIZE; + else + write_size = size; + + memaddr = ((bank << 8) | 0x00); + result = mpu_memory_read(st, + st->i2c_addr, memaddr, write_size, firmware); + if (result) + return result; + if (0 != memcmp(firmware, data, write_size)) + return -EINVAL; + } + return 0; +} + +int inv_enable_pedometer_interrupt(struct inv_mpu_state *st, bool en) +{ + u8 reg[3]; + + if (en) { + reg[0] = 0xf4; + reg[1] = 0x44; + reg[2] = 0xf1; + + } else { + reg[0] = 0xf1; + reg[1] = 0xf1; + reg[2] = 0xf1; + } + + return mem_w_key(KEY_CFG_PED_INT, ARRAY_SIZE(reg), reg); +} + +int inv_read_pedometer_counter(struct inv_mpu_state *st) +{ + int result; + u8 d[4]; + u32 last_step_counter, curr_counter; + + result = mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_D_STPDET_TIMESTAMP), 4, d); + if (result) + return result; + last_step_counter = (u32)be32_to_cpup((__be32 *)(d)); + + result = mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_DMP_RUN_CNTR), 4, d); + if (result) + return result; + curr_counter = (u32)be32_to_cpup((__be32 *)(d)); + if (0 != last_step_counter) + st->ped.last_step_time = get_time_ns() - + ((u64)(curr_counter - last_step_counter)) * + DMP_INTERVAL_INIT; + + return 0; +} + +int inv_enable_pedometer(struct inv_mpu_state *st, bool en) +{ + u8 d[1]; + + if (en) + d[0] = 0xf1; + else + d[0] = 0xff; + + return mem_w_key(KEY_CFG_PED_ENABLE, ARRAY_SIZE(d), d); +} + +int inv_get_pedometer_steps(struct inv_mpu_state *st, u32 *steps) +{ + u8 d[4]; + int result; + + result = mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_D_PEDSTD_STEPCTR), 4, d); + *steps = (u32)be32_to_cpup((__be32 *)(d)); + + return result; +} + +int inv_get_pedometer_time(struct inv_mpu_state *st, u32 *time) +{ + u8 d[4]; + int result; + + result = mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_D_PEDSTD_TIMECTR), 4, d); + *time = (u32)be32_to_cpup((__be32 *)(d)); + + return result; +} + +int inv_set_display_orient_interrupt_dmp(struct inv_mpu_state *st, bool on) +{ + int r; + u8 rn[] = {0xf4, 0x41}; + u8 rf[] = {0xd8, 0xd8}; + + if (on) + r = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, ARRAY_SIZE(rn), rn); + else + r = mem_w_key(KEY_CFG_DISPLAY_ORIENT_INT, ARRAY_SIZE(rf), rf); + + return r; +} + +static int inv_set_tap_interrupt_dmp(struct inv_mpu_state *st, u8 on) +{ + int result; + u16 d; + + if (on) + d = 192; + else + d = 128; + + result = inv_write_2bytes(st, KEY_DMP_TAP_GATE, d); + + return result; +} + +/* + * inv_set_tap_threshold_dmp(): + * Sets the tap threshold in the dmp + * Simultaneously sets secondary tap threshold to help correct the tap + * direction for soft taps. + */ +int inv_set_tap_threshold_dmp(struct inv_mpu_state *st, u16 threshold) +{ + int result; + int sampleDivider; + int scaledThreshold; + u32 dmpThreshold; + u8 sample_div; + const u32 accel_sens = (0x20000000 / 0x00010000); + + if (threshold > (1 << 15)) + return -EINVAL; + sample_div = st->sample_divider; + + sampleDivider = (1 + sample_div); + /* Scale factor corresponds linearly using + * 0 : 0 + * 25 : 0.0250 g/ms + * 50 : 0.0500 g/ms + * 100: 1.0000 g/ms + * 200: 2.0000 g/ms + * 400: 4.0000 g/ms + * 800: 8.0000 g/ms + */ + /*multiply by 1000 to avoid floating point 1000/1000*/ + scaledThreshold = threshold; + /* Convert to per sample */ + scaledThreshold *= sampleDivider; + + /* Scale to DMP 16 bit value */ + if (accel_sens != 0) + dmpThreshold = (u32)(scaledThreshold * accel_sens); + else + return -EINVAL; + dmpThreshold = dmpThreshold / DMP_PRECISION; + result = inv_write_2bytes(st, KEY_DMP_TAP_THR_Z, dmpThreshold); + if (result) + return result; + result = inv_write_2bytes(st, KEY_DMP_TAP_PREV_JERK_Z, + dmpThreshold * 3 / 4); + + return result; +} + + +/* + * inv_set_min_taps_dmp(): + * Indicates the minimum number of consecutive taps required + * before the DMP will generate an interrupt. + */ +int inv_set_min_taps_dmp(struct inv_mpu_state *st, u16 min_taps) +{ + u8 result; + + /* check if any spurious bit other the ones expected are set */ + if ((min_taps > DMP_MAX_MIN_TAPS) || (min_taps < 1)) + return -EINVAL; + + /* DMP tap count is zero-based. So single-tap is 0. + Furthermore, DMP code checks for tap_count > min_taps. + So we have to do minus 2 here. + For example, if the user expects any single tap will generate an + interrupt, (s)he will call inv_set_min_taps_dmp(1). + When DMP gets a single tap, tap_count = 0. To get + tap_count > min_taps, we have to decrement min_taps by 2 to -1. */ + result = inv_write_2bytes(st, KEY_DMP_TAP_MIN_TAPS, (u16)(min_taps-2)); + + return result; +} + +/* + * inv_set_tap_time_dmp(): + * Determines how long after a tap the DMP requires before + * another tap can be registered. + */ +int inv_set_tap_time_dmp(struct inv_mpu_state *st, u16 time) +{ + int result; + u16 dmpTime; + u8 sampleDivider; + + sampleDivider = st->sample_divider; + sampleDivider++; + + /* 60 ms minimum time added */ + dmpTime = ((time) / sampleDivider); + result = inv_write_2bytes(st, KEY_DMP_TAPW_MIN, dmpTime); + + return result; +} + +/* + * inv_set_multiple_tap_time_dmp(): + * Determines how close together consecutive taps must occur + * to be considered double/triple taps. + */ +static int inv_set_multiple_tap_time_dmp(struct inv_mpu_state *st, u32 time) +{ + int result; + u16 dmpTime; + u8 sampleDivider; + + sampleDivider = st->sample_divider; + sampleDivider++; + + /* 60 ms minimum time added */ + dmpTime = ((time) / sampleDivider); + result = inv_write_2bytes(st, KEY_DMP_TAP_NEXT_TAP_THRES, dmpTime); + + return result; +} + +int inv_q30_mult(int a, int b) +{ + u64 temp; + int result; + + temp = (u64)a * b; + result = (int)(temp >> DMP_MULTI_SHIFT); + + return result; +} + +static u16 inv_row_2_scale(const s8 *row) +{ + u16 b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; + + return b; +} + +/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar +* representation. +* @param[in] mtx Orientation matrix to convert to a scalar. +* @return Description of orientation matrix. The lowest 2 bits (0 and 1) +* represent the column the one is on for the +* first row, with the bit number 2 being the sign. The next 2 bits +* (3 and 4) represent +* the column the one is on for the second row with bit number 5 being +* the sign. +* The next 2 bits (6 and 7) represent the column the one is on for the +* third row with +* bit number 8 being the sign. In binary the identity matrix would therefor +* be: 010_001_000 or 0x88 in hex. +*/ +static u16 inv_orientation_matrix_to_scaler(const signed char *mtx) +{ + + u16 scalar; + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + return scalar; +} + +static int inv_gyro_dmp_cal(struct inv_mpu_state *st) +{ + int inv_gyro_orient; + u8 regs[3]; + int result; + + u8 tmpD = DINA4C; + u8 tmpE = DINACD; + u8 tmpF = DINA6C; + + inv_gyro_orient = + inv_orientation_matrix_to_scaler(st->plat_data.orientation); + + if ((inv_gyro_orient & 3) == 0) + regs[0] = tmpD; + else if ((inv_gyro_orient & 3) == 1) + regs[0] = tmpE; + else if ((inv_gyro_orient & 3) == 2) + regs[0] = tmpF; + if ((inv_gyro_orient & 0x18) == 0) + regs[1] = tmpD; + else if ((inv_gyro_orient & 0x18) == 0x8) + regs[1] = tmpE; + else if ((inv_gyro_orient & 0x18) == 0x10) + regs[1] = tmpF; + if ((inv_gyro_orient & 0xc0) == 0) + regs[2] = tmpD; + else if ((inv_gyro_orient & 0xc0) == 0x40) + regs[2] = tmpE; + else if ((inv_gyro_orient & 0xc0) == 0x80) + regs[2] = tmpF; + + result = mem_w_key(KEY_FCFG_1, ARRAY_SIZE(regs), regs); + if (result) + return result; + + if (inv_gyro_orient & 4) + regs[0] = DINA36 | 1; + else + regs[0] = DINA36; + if (inv_gyro_orient & 0x20) + regs[1] = DINA56 | 1; + else + regs[1] = DINA56; + if (inv_gyro_orient & 0x100) + regs[2] = DINA76 | 1; + else + regs[2] = DINA76; + result = mem_w_key(KEY_FCFG_3, ARRAY_SIZE(regs), regs); + + return result; +} + +static int inv_accel_dmp_cal(struct inv_mpu_state *st) +{ + int inv_accel_orient; + int result; + u8 regs[3]; + const u8 tmp[3] = { DINA0C, DINAC9, DINA2C }; + inv_accel_orient = + inv_orientation_matrix_to_scaler(st->plat_data.orientation); + + regs[0] = tmp[inv_accel_orient & 3]; + regs[1] = tmp[(inv_accel_orient >> 3) & 3]; + regs[2] = tmp[(inv_accel_orient >> 6) & 3]; + result = mem_w_key(KEY_FCFG_2, ARRAY_SIZE(regs), regs); + if (result) + return result; + + regs[0] = DINA26; + regs[1] = DINA46; + regs[2] = DINA66; + if (inv_accel_orient & 4) + regs[0] |= 1; + if (inv_accel_orient & 0x20) + regs[1] |= 1; + if (inv_accel_orient & 0x100) + regs[2] |= 1; + result = mem_w_key(KEY_FCFG_7, ARRAY_SIZE(regs), regs); + + return result; +} + +int inv_set_accel_bias_dmp(struct inv_mpu_state *st) +{ + int inv_accel_orient, result, i, accel_bias_body[3], out[3]; + int tmp[] = {1, 1, 1}; + int mask[] = {4, 0x20, 0x100}; + int accel_sf = 0x20000000;/* 536870912 */ + u8 *regs; + + inv_accel_orient = + inv_orientation_matrix_to_scaler(st->plat_data.orientation); + + for (i = 0; i < 3; i++) + if (inv_accel_orient & mask[i]) + tmp[i] = -1; + + for (i = 0; i < 3; i++) + accel_bias_body[i] = + st->input_accel_dmp_bias[(inv_accel_orient >> + (i * 3)) & 3] * tmp[i]; + for (i = 0; i < 3; i++) + accel_bias_body[i] = inv_q30_mult(accel_sf, + accel_bias_body[i]); + for (i = 0; i < 3; i++) + out[i] = cpu_to_be32p(&accel_bias_body[i]); + regs = (u8 *)out; + result = mem_w_key(KEY_D_ACCEL_BIAS, sizeof(out), regs); + + return result; +} + +/* + * inv_set_gyro_sf_dmp(): + * The gyro threshold, in dps, above which taps will be rejected. + */ +static int inv_set_gyro_sf_dmp(struct inv_mpu_state *st) +{ + int result; + u8 sampleDivider; + u32 gyro_sf; + const u32 gyro_sens = 0x03e80000; + + sampleDivider = st->sample_divider; + gyro_sf = inv_q30_mult(gyro_sens, + (int)(DMP_TAP_SCALE * (sampleDivider + 1))); + result = write_be32_key_to_mem(st, gyro_sf, KEY_D_0_104); + + return result; +} + +/* + * inv_set_shake_reject_thresh_dmp(): + * The gyro threshold, in dps, above which taps will be rejected. + */ +static int inv_set_shake_reject_thresh_dmp(struct inv_mpu_state *st, + int thresh) +{ + int result; + u8 sampleDivider; + int thresh_scaled; + u32 gyro_sf; + const u32 gyro_sens = 0x03e80000; + + sampleDivider = st->sample_divider; + gyro_sf = inv_q30_mult(gyro_sens, (int)(DMP_TAP_SCALE * + (sampleDivider + 1))); + /* We're in units of DPS, convert it back to chip units*/ + /*split the operation to aviod overflow of integer*/ + thresh_scaled = gyro_sens / (1L << 16); + thresh_scaled = thresh_scaled / thresh; + thresh_scaled = gyro_sf / thresh_scaled; + result = write_be32_key_to_mem(st, thresh_scaled, + KEY_DMP_TAP_SHAKE_REJECT); + + return result; +} + +/* + * inv_set_shake_reject_time_dmp(): + * How long a gyro axis must remain above its threshold + * before taps are rejected. + */ +static int inv_set_shake_reject_time_dmp(struct inv_mpu_state *st, + u32 time) +{ + int result; + u16 dmpTime; + u8 sampleDivider; + + sampleDivider = st->sample_divider; + sampleDivider++; + + /* 60 ms minimum time added */ + dmpTime = ((time) / sampleDivider); + result = inv_write_2bytes(st, KEY_DMP_TAP_SHAKE_COUNT_MAX, dmpTime); + + return result; +} + +/* + * inv_set_shake_reject_timeout_dmp(): + * How long the gyros must remain below their threshold, + * after taps have been rejected, before taps can be detected again. + */ +static int inv_set_shake_reject_timeout_dmp(struct inv_mpu_state *st, + u32 time) +{ + int result; + u16 dmpTime; + u8 sampleDivider; + + sampleDivider = st->sample_divider; + sampleDivider++; + + /* 60 ms minimum time added */ + dmpTime = ((time) / sampleDivider); + result = inv_write_2bytes(st, KEY_DMP_TAP_SHAKE_TIMEOUT_MAX, dmpTime); + + return result; +} + +int inv_set_interrupt_on_gesture_event(struct inv_mpu_state *st, bool on) +{ + u8 r; + const u8 rn[] = {0xA3}; + const u8 rf[] = {0xFE}; + + if (on) + r = mem_w_key(KEY_CFG_FIFO_INT, ARRAY_SIZE(rn), rn); + else + r = mem_w_key(KEY_CFG_FIFO_INT, ARRAY_SIZE(rf), rf); + + return r; +} + +/* + * inv_enable_tap_dmp() - calling this function will enable/disable tap + * function. + */ +int inv_enable_tap_dmp(struct inv_mpu_state *st, bool on) +{ + int result; + + result = inv_set_tap_interrupt_dmp(st, on); + if (result) + return result; + result = inv_set_tap_threshold_dmp(st, st->tap.thresh); + if (result) + return result; + + result = inv_set_min_taps_dmp(st, st->tap.min_count); + if (result) + return result; + + result = inv_set_tap_time_dmp(st, st->tap.time); + if (result) + return result; + + result = inv_set_multiple_tap_time_dmp(st, DMP_MULTI_TAP_TIME); + if (result) + return result; + + result = inv_set_gyro_sf_dmp(st); + if (result) + return result; + + result = inv_set_shake_reject_thresh_dmp(st, DMP_SHAKE_REJECT_THRESH); + if (result) + return result; + + result = inv_set_shake_reject_time_dmp(st, DMP_SHAKE_REJECT_TIME); + if (result) + return result; + + result = inv_set_shake_reject_timeout_dmp(st, + DMP_SHAKE_REJECT_TIMEOUT); + return result; +} + +static int inv_dry_run_dmp(struct inv_mpu_state *st) +{ + int result; + struct inv_reg_map_s *reg; + + reg = &st->reg; + result = st->switch_gyro_engine(st, true); + if (result) + return result; + result = inv_i2c_single_write(st, reg->user_ctrl, BIT_DMP_EN); + if (result) + return result; + msleep(400); + result = inv_i2c_single_write(st, reg->user_ctrl, 0); + if (result) + return result; + result = st->switch_gyro_engine(st, false); + if (result) + return result; + + return 0; +} + +static void inv_test_reset(struct inv_mpu_state *st) +{ + int result, ii; + u8 d[0x80]; + + if (INV_MPU6500 != st->chip_type) + return; + + for (ii = 3; ii < 0x80; ii++) { + /* don't read fifo r/w register */ + if (ii != st->reg.fifo_r_w) + inv_i2c_read(st, ii, 1, &d[ii]); + } + result = inv_i2c_single_write(st, st->reg.pwr_mgmt_1, BIT_H_RESET); + if (result) + return; + msleep(POWER_UP_TIME); + + for (ii = 3; ii < 0x80; ii++) { + /* don't write certain registers */ + if ((ii != st->reg.fifo_r_w) && + (ii != st->reg.mem_r_w) && + (ii != st->reg.mem_start_addr) && + (ii != st->reg.fifo_count_h) && + ii != (st->reg.fifo_count_h + 1)) + result = inv_i2c_single_write(st, ii, d[ii]); + } +} + +/* + * inv_dmp_firmware_write() - calling this function will load the firmware. + * This is the write function of file "dmp_firmware". + */ +ssize_t inv_dmp_firmware_write(struct file *fp, struct kobject *kobj, + struct bin_attribute *attr, + char *buf, loff_t pos, size_t size) +{ + u8 *firmware; + int result; + struct inv_reg_map_s *reg; + struct iio_dev *indio_dev; + struct inv_mpu_state *st; + + indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj)); + st = iio_priv(indio_dev); + + if (st->chip_config.firmware_loaded) + return -EINVAL; + if (st->chip_config.enable) + return -EBUSY; + + reg = &st->reg; + if (DMP_IMAGE_SIZE != size) { + pr_err("wrong DMP image size - expected %d, actual %d\n", + DMP_IMAGE_SIZE, size); + return -EINVAL; + } + + firmware = kmalloc(size, GFP_KERNEL); + if (!firmware) + return -ENOMEM; + + mutex_lock(&indio_dev->mlock); + + memcpy(firmware, buf, size); + result = crc32(CRC_FIRMWARE_SEED, firmware, size); + if (DMP_IMAGE_CRC_VALUE != result) { + pr_err("firmware CRC error - 0x%08x vs 0x%08x\n", + result, DMP_IMAGE_CRC_VALUE); + result = -EINVAL; + goto firmware_write_fail; + } + + result = st->set_power_state(st, true); + if (result) + goto firmware_write_fail; + inv_test_reset(st); + + result = inv_load_firmware(st, firmware, size); + if (result) + goto firmware_write_fail; + + result = inv_verify_firmware(st, firmware, size); + if (result) + goto firmware_write_fail; + + result = inv_i2c_single_write(st, reg->prgm_strt_addrh, + st->chip_config.prog_start_addr >> 8); + if (result) + goto firmware_write_fail; + result = inv_i2c_single_write(st, reg->prgm_strt_addrh + 1, + st->chip_config.prog_start_addr & 0xff); + if (result) + goto firmware_write_fail; + + result = inv_gyro_dmp_cal(st); + if (result) + goto firmware_write_fail; + result = inv_accel_dmp_cal(st); + if (result) + goto firmware_write_fail; + result = inv_dry_run_dmp(st); + if (result) + goto firmware_write_fail; + + st->chip_config.firmware_loaded = 1; + +firmware_write_fail: + result |= st->set_power_state(st, false); + mutex_unlock(&indio_dev->mlock); + kfree(firmware); + if (result) + return result; + + return size; +} + +ssize_t inv_dmp_firmware_read(struct file *filp, + struct kobject *kobj, + struct bin_attribute *bin_attr, + char *buf, loff_t off, size_t count) +{ + int bank, write_size, size, data, result; + u16 memaddr; + struct iio_dev *indio_dev; + struct inv_mpu_state *st; + + size = count; + indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj)); + st = iio_priv(indio_dev); + + data = 0; + mutex_lock(&indio_dev->mlock); + if (!st->chip_config.enable) { + result = st->set_power_state(st, true); + if (result) { + mutex_unlock(&indio_dev->mlock); + return result; + } + } + for (bank = 0; size > 0; bank++, size -= write_size, + data += write_size) { + if (size > MPU_MEM_BANK_SIZE) + write_size = MPU_MEM_BANK_SIZE; + else + write_size = size; + + memaddr = (bank << 8); + result = mpu_memory_read(st, + st->i2c_addr, memaddr, write_size, &buf[data]); + if (result) { + mutex_unlock(&indio_dev->mlock); + return result; + } + } + if (!st->chip_config.enable) + result = st->set_power_state(st, false); + mutex_unlock(&indio_dev->mlock); + if (result) + return result; + + return count; +} + +ssize_t inv_six_q_write(struct file *fp, struct kobject *kobj, + struct bin_attribute *attr, char *buf, loff_t pos, size_t size) +{ + u8 q[QUATERNION_BYTES]; + struct inv_reg_map_s *reg; + struct iio_dev *indio_dev; + struct inv_mpu_state *st; + int result; + + indio_dev = dev_get_drvdata(container_of(kobj, struct device, kobj)); + st = iio_priv(indio_dev); + + mutex_lock(&indio_dev->mlock); + + if (!st->chip_config.firmware_loaded) { + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + if (st->chip_config.enable) { + mutex_unlock(&indio_dev->mlock); + return -EBUSY; + } + reg = &st->reg; + if (QUATERNION_BYTES != size) { + pr_err("wrong quaternion size=%d, should=%d\n", size, + QUATERNION_BYTES); + mutex_unlock(&indio_dev->mlock); + return -EINVAL; + } + + memcpy(q, buf, size); + result = st->set_power_state(st, true); + if (result) + goto firmware_write_fail; + result = mem_w_key(KEY_DMP_Q0, QUATERNION_BYTES, q); + +firmware_write_fail: + result |= st->set_power_state(st, false); + mutex_unlock(&indio_dev->mlock); + if (result) + return result; + + return size; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c new file mode 100644 index 000000000000..9c218e92d143 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c @@ -0,0 +1,1867 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/iio/iio.h> +#include <linux/iio/kfifo_buf.h> +#include <linux/iio/trigger_consumer.h> +#include <linux/iio/sysfs.h> + +#include "inv_mpu_iio.h" + +static u8 fifo_data[HARDWARE_FIFO_SIZE + HEADERED_Q_BYTES]; +static int inv_process_batchmode(struct inv_mpu_state *st); + +static int inv_push_marker_to_buffer(struct inv_mpu_state *st, u16 hdr) +{ + struct iio_dev *indio_dev = iio_priv_to_dev(st); + u8 buf[IIO_BUFFER_BYTES]; + + memcpy(buf, &hdr, sizeof(hdr)); + iio_push_to_buffers(indio_dev, buf); + + return 0; +} + +static int inv_push_8bytes_buffer(struct inv_mpu_state *st, u16 hdr, + u64 t, s16 *d) +{ + struct iio_dev *indio_dev = iio_priv_to_dev(st); + u8 buf[IIO_BUFFER_BYTES]; + int i; + + memcpy(buf, &hdr, sizeof(hdr)); + for (i = 0; i < 3; i++) + memcpy(&buf[2 + i * 2], &d[i], sizeof(d[i])); + iio_push_to_buffers(indio_dev, buf); + memcpy(buf, &t, sizeof(t)); + iio_push_to_buffers(indio_dev, buf); + + return 0; +} + +static int inv_push_16bytes_buffer(struct inv_mpu_state *st, u16 hdr, u64 t, + int *q) +{ + struct iio_dev *indio_dev = iio_priv_to_dev(st); + u8 buf[IIO_BUFFER_BYTES]; + int i; + + memcpy(buf, &hdr, sizeof(hdr)); + memcpy(buf + 4, &q[0], sizeof(q[0])); + iio_push_to_buffers(indio_dev, buf); + for (i = 0; i < 2; i++) + memcpy(buf + 4 * i, &q[i + 1], sizeof(q[i])); + iio_push_to_buffers(indio_dev, buf); + memcpy(buf, &t, sizeof(t)); + iio_push_to_buffers(indio_dev, buf); + + return 0; +} + +static int inv_send_pressure_data(struct inv_mpu_state *st) +{ + short sen[3]; + struct inv_chip_config_s *conf; + struct inv_mpu_slave *slave; + u64 curr_ts; + int result; + + conf = &st->chip_config; + slave = st->slave_pressure; + if (!st->sensor[SENSOR_PRESSURE].on) + return 0; + if (conf->dmp_on && conf->dmp_event_int_on) + return 0; + if (!conf->normal_pressure_measure) { + conf->normal_pressure_measure = 1; + return 0; + } + curr_ts = get_time_ns(); + if (curr_ts - slave->prev_ts > slave->min_read_time) { + result = slave->read_data(st, sen); + if (!result) + inv_push_8bytes_buffer(st, PRESSURE_HDR, + st->last_ts, sen); + slave->prev_ts = curr_ts; + } + + return 0; +} + +static int inv_send_compass_data(struct inv_mpu_state *st) +{ + short sen[3]; + struct inv_chip_config_s *conf; + struct inv_mpu_slave *slave; + u64 curr_ts; + int result; + + conf = &st->chip_config; + slave = st->slave_compass; + if (!st->sensor[SENSOR_COMPASS].on) + return 0; + if (conf->dmp_on && conf->dmp_event_int_on) + return 0; + if (!conf->normal_compass_measure) { + conf->normal_compass_measure = 1; + return 0; + } + curr_ts = get_time_ns(); + if (curr_ts - slave->prev_ts > slave->min_read_time) { + result = slave->read_data(st, sen); + if (!result) + inv_push_8bytes_buffer(st, COMPASS_HDR, + st->last_ts, sen); + slave->prev_ts = curr_ts; + } + + return 0; +} + +static int inv_batchmode_calc(struct inv_mpu_state *st) +{ + int b, timeout; + int rate_dur; + + rate_dur = MSEC_PER_SEC / st->batch.min_rate; + if (st->batch.timeout < rate_dur) + st->batch.timeout = rate_dur; + b = st->batch.timeout * st->bytes_per_sec; + if ((b > (FIFO_SIZE * ONE_K_HZ)) && (!st->batch.overflow_on)) + timeout = FIFO_SIZE * ONE_K_HZ / st->bytes_per_sec; + else + timeout = st->batch.timeout; + + st->batch.counter = timeout / 5; + if (timeout) + st->batch.on = true; + + return 0; +} + +int inv_batchmode_setup(struct inv_mpu_state *st) +{ + int r; + + r = inv_write_2bytes(st, KEY_BM_NUMWORD_TOFILL, 0); + if (r) + return r; + r = write_be32_key_to_mem(st, 0, KEY_BM_BATCH_CNTR); + if (r) + return r; + + if (st->chip_config.dmp_on && (st->batch.timeout > 0) && + (st->chip_config.dmp_event_int_on == 0)) { + r = inv_batchmode_calc(st); + if (r) + return r; + } + + if (st->batch.on) { + r = write_be32_key_to_mem(st, st->batch.counter, + KEY_BM_BATCH_THLD); + if (r) + return r; + } + r = inv_write_2bytes(st, KEY_BM_ENABLE, st->batch.on); + + return r; +} + +/** + * reset_fifo_mpu3050() - Reset FIFO related registers + * @indio_dev: Device driver instance. + */ +static int reset_fifo_mpu3050(struct iio_dev *indio_dev) +{ + struct inv_reg_map_s *reg; + int result; + u8 val, user_ctrl; + struct inv_mpu_state *st = iio_priv(indio_dev); + reg = &st->reg; + + /* disable interrupt */ + result = inv_i2c_single_write(st, reg->int_enable, + st->plat_data.int_config); + if (result) + return result; + /* disable the sensor output to FIFO */ + result = inv_i2c_single_write(st, reg->fifo_en, 0); + if (result) + goto reset_fifo_fail; + result = inv_i2c_read(st, reg->user_ctrl, 1, &user_ctrl); + if (result) + goto reset_fifo_fail; + /* disable fifo reading */ + user_ctrl &= ~BIT_FIFO_EN; + st->chip_config.has_footer = 0; + /* reset fifo */ + val = (BIT_3050_FIFO_RST | user_ctrl); + result = inv_i2c_single_write(st, reg->user_ctrl, val); + if (result) + goto reset_fifo_fail; + if (st->chip_config.dmp_on) { + /* enable interrupt when DMP is done */ + result = inv_i2c_single_write(st, reg->int_enable, + st->plat_data.int_config | BIT_DMP_INT_EN); + if (result) + return result; + + result = inv_i2c_single_write(st, reg->user_ctrl, + BIT_FIFO_EN|user_ctrl); + if (result) + return result; + } else { + /* enable interrupt */ + if (st->sensor[SENSOR_ACCEL].on || + st->sensor[SENSOR_GYRO].on) { + result = inv_i2c_single_write(st, reg->int_enable, + st->plat_data.int_config | BIT_DATA_RDY_EN); + if (result) + return result; + } + /* enable FIFO reading and I2C master interface*/ + result = inv_i2c_single_write(st, reg->user_ctrl, + BIT_FIFO_EN | user_ctrl); + if (result) + return result; + /* enable sensor output to FIFO and FIFO footer*/ + val = 1; + if (st->sensor[SENSOR_ACCEL].on) + val |= BITS_3050_ACCEL_OUT; + if (st->sensor[SENSOR_GYRO].on) + val |= BITS_GYRO_OUT; + result = inv_i2c_single_write(st, reg->fifo_en, val); + if (result) + return result; + } + + return 0; +reset_fifo_fail: + if (st->chip_config.dmp_on) + val = BIT_DMP_INT_EN; + else + val = BIT_DATA_RDY_EN; + inv_i2c_single_write(st, reg->int_enable, + st->plat_data.int_config | val); + pr_err("reset fifo failed\n"); + + return result; +} + +/* + * inv_set_lpf() - set low pass filer based on fifo rate. + */ +static int inv_set_lpf(struct inv_mpu_state *st, int rate) +{ + const short hz[] = {188, 98, 42, 20, 10, 5}; + const int d[] = {INV_FILTER_188HZ, INV_FILTER_98HZ, + INV_FILTER_42HZ, INV_FILTER_20HZ, + INV_FILTER_10HZ, INV_FILTER_5HZ}; + int i, h, data, result; + struct inv_reg_map_s *reg; + + reg = &st->reg; + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) + i++; + data = d[i]; + if (INV_MPU3050 == st->chip_type) { + if (st->slave_accel != NULL) { + result = st->slave_accel->set_lpf(st, rate); + if (result) + return result; + } + result = inv_i2c_single_write(st, reg->lpf, data | + (st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT)); + } else { + result = inv_i2c_single_write(st, reg->lpf, data); + } + if (result) + return result; + st->chip_config.lpf = data; + + return 0; +} + +/* + * set_fifo_rate_reg() - Set fifo rate in hardware register + */ +static int set_fifo_rate_reg(struct inv_mpu_state *st) +{ + u8 data; + u16 fifo_rate; + int result; + struct inv_reg_map_s *reg; + + reg = &st->reg; + fifo_rate = st->chip_config.fifo_rate; + data = ONE_K_HZ / fifo_rate - 1; + result = inv_i2c_single_write(st, reg->sample_rate_div, data); + if (result) + return result; + result = inv_set_lpf(st, fifo_rate); + if (result) + return result; + /* wait for the sampling rate change to stabilize */ + mdelay(INV_MPU_SAMPLE_RATE_CHANGE_STABLE); + + return 0; +} + +/* + * inv_lpa_mode() - store current low power mode settings + */ +static int inv_lpa_mode(struct inv_mpu_state *st, int lpa_mode) +{ + unsigned long result; + u8 d; + struct inv_reg_map_s *reg; + + reg = &st->reg; + result = inv_i2c_read(st, reg->pwr_mgmt_1, 1, &d); + if (result) + return result; + if (lpa_mode) + d |= BIT_CYCLE; + else + d &= ~BIT_CYCLE; + + result = inv_i2c_single_write(st, reg->pwr_mgmt_1, d); + if (result) + return result; + if (INV_MPU6500 == st->chip_type) { + d = BIT_FIFO_SIZE_1K; + if (lpa_mode) + d |= BIT_ACCEL_FCHOCIE_B; + result = inv_i2c_single_write(st, REG_6500_ACCEL_CONFIG2, d); + if (result) + return result; + } + + return 0; +} + +static int inv_saturate_secondary_counter(struct inv_mpu_state *st) +{ + int result; + struct inv_reg_map_s *reg; + +#define COUNT_SATURATE_TIME_MS 32 + reg = &st->reg; + /* set sampling to 1KHz */ + result = inv_i2c_single_write(st, reg->sample_rate_div, 0); + if (result) + return result; + result = inv_i2c_single_write(st, REG_I2C_MST_DELAY_CTRL, + BIT_SLV0_DLY_EN); + if (result) + return result; + result = inv_i2c_single_write(st, REG_I2C_SLV4_CTRL, 0); + if (result) + return result; + result = inv_i2c_single_write(st, reg->user_ctrl, BIT_I2C_MST_EN); + if (result) + return result; + msleep(COUNT_SATURATE_TIME_MS); + + return 0; +} +static int inv_set_master_delay(struct inv_mpu_state *st) +{ + int d, result, rate; + u8 delay; + + if ((!st->sensor[SENSOR_COMPASS].on) && + (!st->sensor[SENSOR_PRESSURE].on)) + return 0; + + delay = 0; + d = 0; + if (st->sensor[SENSOR_COMPASS].on) { + switch (st->plat_data.sec_slave_id) { + case COMPASS_ID_AK8975: + case COMPASS_ID_AK8972: + case COMPASS_ID_AK8963: + case COMPASS_ID_AK09911: + delay = (BIT_SLV0_DLY_EN | BIT_SLV1_DLY_EN); + break; + case COMPASS_ID_MLX90399: + delay = (BIT_SLV0_DLY_EN | + BIT_SLV1_DLY_EN | + BIT_SLV2_DLY_EN | + BIT_SLV3_DLY_EN); + break; + default: + return -EINVAL; + } + d = max(d, st->slave_compass->rate_scale); + } + if (st->sensor[SENSOR_PRESSURE].on) { + /* read fake data when compass is disabled for DMP read */ + if ((!st->sensor[SENSOR_COMPASS].on) && st->chip_config.dmp_on) + delay |= BIT_SLV0_DLY_EN; + switch (st->plat_data.aux_slave_id) { + case PRESSURE_ID_BMP280: + delay |= (BIT_SLV2_DLY_EN | BIT_SLV3_DLY_EN); + break; + default: + return -EINVAL; + } + d = max(d, st->slave_pressure->rate_scale); + } + + d = d * st->chip_config.fifo_rate / ONE_K_HZ; + if (st->chip_config.dmp_on) { + rate = 0; + if (st->sensor[SENSOR_PRESSURE].on) + rate = max(rate, st->sensor[SENSOR_PRESSURE].rate); + if (st->sensor[SENSOR_COMPASS].on) + rate = max(rate, st->sensor[SENSOR_COMPASS].rate); + if (rate == 0) + return -EINVAL; + d = max(d, st->chip_config.fifo_rate / rate); + } + + if (d > 0) + d -= 1; + if (d > 0x1F) + d = 0x1F; + + /* I2C_MST_DLY is set to slow down secondary I2C */ + if (0 == d) + delay = 0; + if (delay) { + result = inv_saturate_secondary_counter(st); + if (result) + return result; + } + result = inv_i2c_single_write(st, REG_I2C_MST_DELAY_CTRL, delay); + if (result) + return result; + + return inv_i2c_single_write(st, REG_I2C_SLV4_CTRL, d); +} + +/* + * reset_fifo_itg() - Reset FIFO related registers. + */ +static int reset_fifo_itg(struct iio_dev *indio_dev) +{ + struct inv_reg_map_s *reg; + int result, i; + u8 val, int_word; + struct inv_mpu_state *st = iio_priv(indio_dev); + + reg = &st->reg; + if (st->chip_config.lpa_mode) { + result = inv_lpa_mode(st, 0); + if (result) { + pr_err("reset lpa mode failed\n"); + return result; + } + } + /* disable interrupt */ + result = inv_i2c_single_write(st, reg->int_enable, 0); + if (result) { + pr_err("int_enable write failed\n"); + return result; + } + /* disable the sensor output to FIFO */ + result = inv_i2c_single_write(st, reg->fifo_en, 0); + if (result) + goto reset_fifo_fail; + /* disable fifo reading */ + result = inv_i2c_single_write(st, reg->user_ctrl, 0); + if (result) + goto reset_fifo_fail; + int_word = 0; + + /* MPU6500's BIT_6500_WOM_EN is the same as BIT_MOT_EN */ + if (st->mot_int.mot_on) + int_word |= BIT_MOT_EN; + + if (st->chip_config.dmp_on) { + val = (BIT_FIFO_RST | BIT_DMP_RST); + result = inv_i2c_single_write(st, reg->user_ctrl, val); + if (result) + goto reset_fifo_fail; + if (st->chip_config.dmp_int_on) { + int_word |= BIT_DMP_INT_EN; + result = inv_i2c_single_write(st, reg->int_enable, + int_word); + if (result) + return result; + } + val = (BIT_DMP_EN | BIT_FIFO_EN); + if ((st->sensor[SENSOR_COMPASS].on || + st->sensor[SENSOR_PRESSURE].on) && + (!st->chip_config.dmp_event_int_on)) + val |= BIT_I2C_MST_EN; + result = inv_i2c_single_write(st, reg->user_ctrl, val); + if (result) + goto reset_fifo_fail; + } else { + /* reset FIFO and possibly reset I2C*/ + val = BIT_FIFO_RST; + result = inv_i2c_single_write(st, reg->user_ctrl, val); + if (result) + goto reset_fifo_fail; + /* enable interrupt */ + if (st->sensor[SENSOR_ACCEL].on || + st->sensor[SENSOR_GYRO].on || + st->sensor[SENSOR_COMPASS].on || + st->sensor[SENSOR_PRESSURE].on) + int_word |= BIT_DATA_RDY_EN; + + result = inv_i2c_single_write(st, reg->int_enable, int_word); + if (result) + return result; + /* enable FIFO reading and I2C master interface*/ + val = BIT_FIFO_EN; + if (st->sensor[SENSOR_COMPASS].on || + st->sensor[SENSOR_PRESSURE].on) + val |= BIT_I2C_MST_EN; + result = inv_i2c_single_write(st, reg->user_ctrl, val); + if (result) + goto reset_fifo_fail; + /* enable sensor output to FIFO */ + val = 0; + if (st->sensor[SENSOR_GYRO].on) + val |= BITS_GYRO_OUT; + if (st->sensor[SENSOR_ACCEL].on) + val |= BIT_ACCEL_OUT; + result = inv_i2c_single_write(st, reg->fifo_en, val); + if (result) + goto reset_fifo_fail; + } + st->last_ts = get_time_ns(); + st->prev_ts = st->last_ts; + st->last_run_time = st->last_ts; + if (st->sensor[SENSOR_COMPASS].on) + st->slave_compass->prev_ts = st->last_ts; + if (st->sensor[SENSOR_PRESSURE].on) + st->slave_pressure->prev_ts = st->last_ts; + + st->dmp_interval = DMP_INTERVAL_INIT; + st->ts_counter = 0; + st->diff_accumulater = 0; + st->dmp_interval_accum = 0; + st->step_detector_base_ts = st->last_ts; + st->chip_config.normal_compass_measure = 0; + st->chip_config.normal_pressure_measure = 0; + st->left_over_size = 0; + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].ts = st->last_ts; + + result = inv_lpa_mode(st, st->chip_config.lpa_mode); + if (result) + goto reset_fifo_fail; + + return 0; + +reset_fifo_fail: + if (st->chip_config.dmp_on) + val = BIT_DMP_INT_EN; + else + val = BIT_DATA_RDY_EN; + inv_i2c_single_write(st, reg->int_enable, val); + pr_err("reset fifo failed\n"); + + return result; +} + +/** + * inv_clear_kfifo() - clear time stamp fifo + * @st: Device driver instance. + */ +static void inv_clear_kfifo(struct inv_mpu_state *st) +{ + unsigned long flags; + + spin_lock_irqsave(&st->time_stamp_lock, flags); + kfifo_reset(&st->timestamps); + spin_unlock_irqrestore(&st->time_stamp_lock, flags); +} + +/* + * inv_reset_fifo() - Reset FIFO related registers. + */ +int inv_reset_fifo(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + + inv_clear_kfifo(st); + if (INV_MPU3050 == st->chip_type) + return reset_fifo_mpu3050(indio_dev); + else + return reset_fifo_itg(indio_dev); +} + +static int inv_send_gyro_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x12}; + int result; + u8 *r; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_GYRO, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_accel_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x12}; + int result; + u8 *r; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_ACCL, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_three_q_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x13}; + int result; + u8 *r; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_3QUAT, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_six_q_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x13}; + int result; + u8 *r; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_6QUAT, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_ped_q_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x12}; + u8 *r; + int result; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_PQUAT, ARRAY_SIZE(rn), r); + + return result; +} + +static int inv_add_step_indicator(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xf3, 0xf3}; + u8 rf[] = {0xf4, 0x03}; + int result; + u8 *r; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_PEDSTEP_DET, ARRAY_SIZE(rn), r); + + return result; +} + +static int inv_send_compass_dmp_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x12}; + u8 *r; + int result; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_CPASS, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_pressure_dmp_data(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x12}; + u8 *r; + int result; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_PRESS, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_send_step_detector(struct inv_mpu_state *st, bool on) +{ + u8 rn[] = {0xa3, 0xa3}; + u8 rf[] = {0xf4, 0x0e}; + u8 *r; + int result; + + if (on) + r = rn; + else + r = rf; + result = mem_w_key(KEY_CFG_OUT_STEPDET, ARRAY_SIZE(rf), r); + + return result; +} + +static int inv_set_rate(struct inv_mpu_state *st, int k, int k_ct, int rate) +{ + int v, result; + + v = MPU_DEFAULT_DMP_FREQ / rate - 1; + result = inv_write_2bytes(st, k, v); + if (result) + return result; + result = inv_write_2bytes(st, k_ct, 0); + + return result; +} + +static int inv_set_gyro_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_GYRO_ODR, KEY_ODR_CNTR_GYRO, + st->sensor[SENSOR_GYRO].rate); + + return result; +} + +static int inv_set_accel_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_ACCL_ODR, KEY_ODR_CNTR_ACCL, + st->sensor[SENSOR_ACCEL].rate); + + return result; +} + +static int inv_set_compass_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_CPASS_ODR, KEY_ODR_CNTR_CPASS, + st->sensor[SENSOR_COMPASS].rate); + + return result; +} + +static int inv_set_pressure_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_PRESS_ODR, KEY_ODR_CNTR_PRESS, + st->sensor[SENSOR_PRESSURE].rate); + + return result; +} + +static int inv_set_step_detector(struct inv_mpu_state *st) +{ + return 0; +} + + +static int inv_set_lpq_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_3QUAT_ODR, KEY_ODR_CNTR_3QUAT, + st->sensor[SENSOR_LPQ].rate); + + return result; +} + +static int inv_set_sixq_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_6QUAT_ODR, KEY_ODR_CNTR_6QUAT, + st->sensor[SENSOR_SIXQ].rate); + + return result; +} + +static int inv_set_pedq_rate(struct inv_mpu_state *st) +{ + int result; + + result = inv_set_rate(st, KEY_CFG_PQUAT6_ODR, KEY_ODR_CNTR_PQUAT, + st->sensor[SENSOR_PEDQ].rate); + + return result; +} + + +static int inv_set_dmp_sysfs(struct inv_mpu_state *st) +{ + int result, i, s; + u8 d[] = {0, 0, 0, 0}; + + result = inv_set_interrupt_on_gesture_event(st, + st->chip_config.dmp_event_int_on); + if (result) + return result; + + if (st->chip_config.dmp_event_int_on) { + for (i = 0; i < SENSOR_NUM_MAX; i++) { + result = st->sensor[i].send_data(st, false); + if (result) + return result; + } + } else { + s = 0; + st->batch.min_rate = MAX_DMP_OUTPUT_RATE; + for (i = 0; i < SENSOR_NUM_MAX; i++) { + result = st->sensor[i].send_data(st, st->sensor[i].on); + if (result) + return result; + if (st->sensor[i].on) { + if (0 == st->sensor[i].rate) + return -EINVAL; + if (st->sensor[i].rate < st->batch.min_rate) + st->batch.min_rate = st->sensor[i].rate; + s += st->sensor[i].rate * + st->sensor[i].sample_size; + + result = st->sensor[i].set_rate(st); + if (result) + return result; + st->sensor[i].counter = MPU_DEFAULT_DMP_FREQ / + st->sensor[i].rate; + } + } + st->bytes_per_sec = s; + if (st->sensor[SENSOR_STEP].on) + result = inv_add_step_indicator(st, true); + else + result = inv_add_step_indicator(st, + st->chip_config.step_indicator_on); + if (result) + return result; + } + result = inv_batchmode_setup(st); + if (result) + return result; + + st->dmp_counter = 0; + result = mem_w_key(KEY_DMP_RUN_CNTR, ARRAY_SIZE(d), d); + if (result) + return result; + result = mem_w_key(KEY_D_STPDET_TIMESTAMP, ARRAY_SIZE(d), d); + + return result; +} + +static void inv_get_data_count(struct inv_mpu_state *st) +{ + struct inv_chip_config_s *c; + int b, i; + + c = &st->chip_config; + b = 0; + if (st->chip_config.dmp_on) { + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (st->sensor[i].on) + b += st->sensor[i].sample_size; + } + } else { + if (st->sensor[SENSOR_ACCEL].on) + b += BYTES_PER_SENSOR; + if (st->sensor[SENSOR_GYRO].on) + b += BYTES_PER_SENSOR; + } + c->bytes_per_datum = b; + + return; +} +/* + * set_inv_enable() - main enable/disable function. + */ +int set_inv_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + struct inv_reg_map_s *reg; + u8 data[2]; + int result; + + reg = &st->reg; + if (enable) { + if (st->chip_config.dmp_on && + (!st->chip_config.firmware_loaded)) + return -EINVAL; + st->batch.on = false; + + inv_get_data_count(st); + result = inv_set_master_delay(st); + if (result) + return result; + + result = set_fifo_rate_reg(st); + if (result) + return result; + if (st->chip_config.dmp_on) { + result = inv_set_dmp_sysfs(st); + if (result) + return result; + } + + if (st->chip_config.gyro_enable) { + result = st->switch_gyro_engine(st, true); + if (result) + return result; + } + if (st->chip_config.accel_enable) { + result = st->switch_accel_engine(st, true); + if (result) + return result; + } + if (st->sensor[SENSOR_COMPASS].on) { + result = st->slave_compass->resume(st); + if (result) + return result; + } + if (st->sensor[SENSOR_PRESSURE].on) { + result = st->slave_pressure->resume(st); + if (result) + return result; + } + result = inv_reset_fifo(indio_dev); + if (result) + return result; + } else { + if ((INV_MPU3050 != st->chip_type) + && st->chip_config.lpa_mode) { + /* if the chip is in low power mode, + register write/read could fail */ + result = inv_lpa_mode(st, 0); + if (result) + return result; + } + result = inv_i2c_single_write(st, reg->fifo_en, 0); + if (result) + return result; + if (st->chip_config.dmp_on) { + result = inv_read_time_and_ticks(st, false); + if (result) + return result; + result = inv_i2c_read(st, reg->fifo_count_h, + FIFO_COUNT_BYTE, data); + if (result) + return result; + st->fifo_count = be16_to_cpup((__be16 *)(data)); + if (st->fifo_count) { + result = inv_process_batchmode(st); + if (result) + return result; + } + } + inv_push_marker_to_buffer(st, END_MARKER); + /* disable fifo reading */ + if (INV_MPU3050 != st->chip_type) { + result = inv_i2c_single_write(st, reg->int_enable, 0); + if (result) + return result; + result = inv_i2c_single_write(st, reg->user_ctrl, 0); + } else { + result = inv_i2c_single_write(st, reg->int_enable, + st->plat_data.int_config); + } + if (result) + return result; + /* turn off the gyro/accel engine during disable phase */ + result = st->switch_gyro_engine(st, false); + if (result) + return result; + result = st->switch_accel_engine(st, false); + if (result) + return result; + if (st->sensor[SENSOR_COMPASS].on) { + result = st->slave_compass->suspend(st); + if (result) + return result; + } + if (st->sensor[SENSOR_PRESSURE].on) { + result = st->slave_pressure->suspend(st); + if (result) + return result; + } + } + st->chip_config.enable = enable; + + return 0; +} + +/* + * inv_irq_handler() - Cache a timestamp at each data ready interrupt. + */ +static irqreturn_t inv_irq_handler(int irq, void *dev_id) +{ + struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; + u64 ts; + + if (!st->chip_config.dmp_on) { + ts = get_time_ns(); + kfifo_in_spinlocked(&st->timestamps, &ts, 1, + &st->time_stamp_lock); + } + + return IRQ_WAKE_THREAD; +} + +static void inv_report_data_3050(struct iio_dev *indio_dev, s64 t, + int has_footer, u8 *data) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + int ind, i; + short s[THREE_AXIS]; + + ind = 0; + if (has_footer) + ind += 2; + + if (st->sensor[SENSOR_GYRO].on) { + for (i = 0; i < 3; i++) + s[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2])); + + inv_push_8bytes_buffer(st, GYRO_HDR, t, s); + ind += BYTES_PER_SENSOR; + } + if (st->sensor[SENSOR_ACCEL].on) { + st->slave_accel->combine_data(&data[ind], s); + inv_push_8bytes_buffer(st, ACCEL_HDR, t, s); + } +} + +/* + * inv_read_fifo_mpu3050() - Transfer data from FIFO to ring buffer for + * mpu3050. + */ +irqreturn_t inv_read_fifo_mpu3050(int irq, void *dev_id) +{ + + struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; + struct iio_dev *indio_dev = iio_priv_to_dev(st); + int bytes_per_datum; + u8 data[64]; + int result; + short fifo_count, byte_read; + s64 timestamp; + struct inv_reg_map_s *reg; + + reg = &st->reg; + mutex_lock(&st->suspend_resume_lock); + mutex_lock(&indio_dev->mlock); + if (st->chip_config.dmp_on) + bytes_per_datum = HEADERED_NORMAL_BYTES; + else + bytes_per_datum = (st->sensor[SENSOR_ACCEL].on + + st->sensor[SENSOR_GYRO].on) * BYTES_PER_SENSOR; + if (st->chip_config.has_footer) + byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE; + else + byte_read = bytes_per_datum; + + fifo_count = 0; + if (byte_read != 0) { + result = inv_i2c_read(st, reg->fifo_count_h, + FIFO_COUNT_BYTE, data); + if (result) + goto end_session; + fifo_count = be16_to_cpup((__be16 *)(&data[0])); + if (fifo_count < byte_read) + goto end_session; + if (fifo_count & 1) + goto flush_fifo; + if (fifo_count > FIFO_THRESHOLD) + goto flush_fifo; + /* Timestamp mismatch. */ + if (kfifo_len(&st->timestamps) < + fifo_count / byte_read) + goto flush_fifo; + if (kfifo_len(&st->timestamps) > + fifo_count / byte_read + TIME_STAMP_TOR) { + if (st->chip_config.dmp_on) { + result = kfifo_out(&st->timestamps, + ×tamp, 1); + if (result != 1) + goto flush_fifo; + } else { + goto flush_fifo; + } + } + } + while ((bytes_per_datum != 0) && (fifo_count >= byte_read)) { + result = inv_i2c_read(st, reg->fifo_r_w, byte_read, data); + if (result) + goto flush_fifo; + + result = kfifo_out(&st->timestamps, ×tamp, 1); + if (result != 1) + goto flush_fifo; + inv_report_data_3050(indio_dev, timestamp, + st->chip_config.has_footer, data); + fifo_count -= byte_read; + if (st->chip_config.has_footer == 0) { + st->chip_config.has_footer = 1; + byte_read = bytes_per_datum + MPU3050_FOOTER_SIZE; + } + } + +end_session: + mutex_unlock(&indio_dev->mlock); + mutex_unlock(&st->suspend_resume_lock); + + return IRQ_HANDLED; + +flush_fifo: + /* Flush HW and SW FIFOs. */ + inv_reset_fifo(indio_dev); + inv_clear_kfifo(st); + mutex_unlock(&indio_dev->mlock); + mutex_unlock(&st->suspend_resume_lock); + + return IRQ_HANDLED; +} + +static int inv_report_gyro_accel(struct iio_dev *indio_dev, + u8 *data, s64 t) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + short s[THREE_AXIS]; + int ind; + int i; + + ind = 0; + if (st->sensor[SENSOR_ACCEL].on) { + for (i = 0; i < 3; i++) + s[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2])); + inv_push_8bytes_buffer(st, ACCEL_HDR, t, s); + ind += BYTES_PER_SENSOR; + } + + if (st->sensor[SENSOR_GYRO].on) { + for (i = 0; i < 3; i++) + s[i] = be16_to_cpup((__be16 *)(&data[ind + i * 2])); + inv_push_8bytes_buffer(st, GYRO_HDR, t, s); + ind += BYTES_PER_SENSOR; + } + + return 0; +} + +static void inv_process_motion(struct inv_mpu_state *st) +{ + struct iio_dev *indio_dev = iio_priv_to_dev(st); + int result; + u8 data[1]; + + /* motion interrupt */ + result = inv_i2c_read(st, REG_INT_STATUS, 1, data); + if (result) + return; + + if (data[0] & BIT_MOT_INT) + sysfs_notify(&indio_dev->dev.kobj, NULL, "event_accel_motion"); +} + +static int inv_get_timestamp(struct inv_mpu_state *st, int count) +{ + u32 *dur; + u32 thresh; + s32 diff, result, counter; + u64 ts; + + /* goal of algorithm is to estimate the true frequency of the chip */ + if (st->chip_config.dmp_on && st->chip_config.dmp_event_int_on) + return 0; + dur = &st->irq_dur_ns; + counter = 1; + thresh = min((u32)((*dur) >> 2), (u32)(10 * NSEC_PER_MSEC)); + while (kfifo_len(&st->timestamps) >= count) { + result = kfifo_out(&st->timestamps, &ts, 1); + if (result != 1) + return -EINVAL; + /* first time since reset fifo, just take it */ + if (!st->ts_counter) { + st->last_ts = ts; + st->prev_ts = ts; + st->ts_counter++; + return 0; + } + diff = (s32)(ts - st->prev_ts); + st->prev_ts = ts; + if (abs(diff - (*dur)) < thresh) { + st->diff_accumulater >>= 1; + if (*dur > diff) + st->diff_accumulater -= (((*dur) - diff) >> 7); + else + st->diff_accumulater += ((diff - (*dur)) >> 7); + *dur += st->diff_accumulater; + } + } + ts = *dur; + ts *= counter; + st->last_ts += ts; + + return 0; +} + +static int inv_process_dmp_interrupt(struct inv_mpu_state *st) +{ + int r; + u8 d[1]; + struct iio_dev *indio_dev = iio_priv_to_dev(st); + +#define DMP_INT_SMD 0x04 +#define DMP_INT_PED 0x08 + + if ((!st->chip_config.smd_enable) && + (!st->ped.int_on) && + (!st->tap.on)) + return 0; + + r = inv_i2c_read(st, REG_DMP_INT_STATUS, 1, d); + if (r) + return r; + if (d[0] & DMP_INT_SMD) { + sysfs_notify(&indio_dev->dev.kobj, NULL, "event_smd"); + st->chip_config.smd_enable = false; + st->chip_config.smd_triggered = true; + } + if (d[0] & DMP_INT_PED) + sysfs_notify(&indio_dev->dev.kobj, NULL, "event_pedometer"); + + return 0; +} + +static int inv_get_shift2(int count) +{ + int i; + + if (1 == count) + return 13; + if (count > 2000) + return 2; + i = 13; + while (count > 0) { + count >>= 1; + i--; + } + + return i; +} + +static void inv_adjust_sensor_ts(struct inv_mpu_state *st, int sensor_ind) +{ + s64 diff; + int i, rate_adj, s3, delta, total_count; + + if (!st->chip_config.adjust_time) + return; +#define MAX_DIFF 0x7fffffff + total_count = st->dmp_ticks; + if (0 == total_count) + total_count = 1; + + diff = (st->last_ts - st->prev_ts) - (u64)(st->dmp_interval) * + total_count; + if (diff > MAX_DIFF) + diff = MAX_DIFF; + if (diff < -MAX_DIFF) + diff = -MAX_DIFF; + s3 = 4; + rate_adj = (int)diff; + rate_adj /= total_count; + delta = min(abs(rate_adj) >> inv_get_shift2(total_count), + DMP_INTERVAL_MIN_ADJ); + if (rate_adj < 0) + delta = -delta; + st->dmp_interval_accum >>= 1; + st->dmp_interval_accum += delta; + st->dmp_interval += st->dmp_interval_accum; + for (i = 0; i < SENSOR_NUM_MAX; i++) + if (st->sensor[i].on) + st->sensor[i].dur = st->dmp_interval * + st->sensor[i].counter; + + st->prev_ts = st->last_ts; +} + +static void inv_reset_ts(struct inv_mpu_state *st, u64 curr_ts) +{ + u32 dur, i; + + dur = USEC_PER_SEC / st->bytes_per_sec; + dur *= 1024; + curr_ts -= ((u64)dur * NSEC_PER_USEC); + for (i = 0; i < SENSOR_NUM_MAX; i++) + st->sensor[i].ts = curr_ts; +} + +static void inv_push_step_indicator(struct inv_mpu_state *st, int sensor_ind, + int steps) +{ + int dur, i; + s16 sen[3]; + u64 base; +#define STEP_INDICATOR_HEADER 0x0001 + + dur = st->sensor[sensor_ind].dur / steps; + base = st->sensor[sensor_ind].ts; + + for (i = 1; i < steps; i++) + inv_push_8bytes_buffer(st, STEP_INDICATOR_HEADER, + base + i * dur, sen); +} + +static int inv_parse_header(u16 hdr) +{ + int sensor_ind; + + switch (hdr) { + case ACCEL_HDR: + sensor_ind = SENSOR_ACCEL; + break; + case GYRO_HDR: + sensor_ind = SENSOR_GYRO; + break; + case PEDQUAT_HDR: + sensor_ind = SENSOR_PEDQ; + break; + case LPQUAT_HDR: + sensor_ind = SENSOR_LPQ; + break; + case SIXQUAT_HDR: + sensor_ind = SENSOR_SIXQ; + break; + case COMPASS_HDR: + sensor_ind = SENSOR_COMPASS; + break; + case PRESSURE_HDR: + sensor_ind = SENSOR_PRESSURE; + break; + case STEP_DETECTOR_HDR: + sensor_ind = SENSOR_STEP; + break; + default: + sensor_ind = SENSOR_INVALID; + break; + } + + return sensor_ind; +} + +static int inv_process_batchmode(struct inv_mpu_state *st) +{ + int i, target_bytes, tmp, res, counter; + int sensor_ind, q[3]; + u8 *dptr, *d; + u16 hdr, steps; + s16 sen[3]; + u64 t; + bool done_flag; + + if (1024 == st->fifo_count) { + inv_reset_ts(st, st->last_ts); + st->left_over_size = 0; + } + d = fifo_data; + if (st->left_over_size > 0) { + dptr = d + st->left_over_size; + memcpy(d, st->left_over, st->left_over_size); + } else { + dptr = d; + } + target_bytes = st->fifo_count; + while (target_bytes > 0) { + if (target_bytes < MAX_READ_SIZE) + tmp = target_bytes; + else + tmp = MAX_READ_SIZE; + res = inv_i2c_read(st, st->reg.fifo_r_w, tmp, dptr); + if (res < 0) + return res; + dptr += tmp; + target_bytes -= tmp; + } + dptr = d; + done_flag = false; + target_bytes = st->fifo_count + st->left_over_size; + counter = 0; + while ((dptr - d <= target_bytes - HEADERED_NORMAL_BYTES) && + (!done_flag)) { + hdr = (u16)be16_to_cpup((__be16 *)(dptr)); + steps = (hdr & STEP_INDICATOR_MASK); + hdr &= (~STEP_INDICATOR_MASK); + sensor_ind = inv_parse_header(hdr); + /* incomplete packet */ + if (target_bytes - (dptr - d) < + st->sensor[sensor_ind].sample_size) { + done_flag = true; + continue; + } + /* error packet */ + if ((sensor_ind == SENSOR_INVALID) || + (!st->sensor[sensor_ind].on)) { + dptr += HEADERED_NORMAL_BYTES; + continue; + } + if (sensor_ind == SENSOR_STEP) { + tmp = (int)be32_to_cpup((__be32 *)(dptr + 4)); + t = st->step_detector_base_ts + + (u64)tmp * 5 * NSEC_PER_MSEC; + inv_push_8bytes_buffer(st, hdr, t, sen); + dptr += HEADERED_NORMAL_BYTES; + continue; + } + if (steps > 1) + inv_push_step_indicator(st, sensor_ind, steps); + st->sensor[sensor_ind].ts += (u64)st->sensor[sensor_ind].dur; + t = st->sensor[sensor_ind].ts; + if (sensor_ind == SENSOR_COMPASS) { + if (!st->chip_config.normal_compass_measure) { + st->chip_config.normal_compass_measure = 1; + dptr += HEADERED_NORMAL_BYTES; + continue; + } + for (i = 0; i < 6; i++) + st->fifo_data[i] = dptr[i + 2]; + res = st->slave_compass->read_data(st, sen); + if (!res) + inv_push_8bytes_buffer(st, hdr | + (!!steps), t, sen); + + dptr += HEADERED_NORMAL_BYTES; + continue; + } + if (sensor_ind == SENSOR_PRESSURE) { + if (!st->chip_config.normal_pressure_measure) { + st->chip_config.normal_pressure_measure = 1; + dptr += HEADERED_NORMAL_BYTES; + continue; + } + for (i = 0; i < 6; i++) + st->fifo_data[i] = dptr[i + 2]; + res = st->slave_pressure->read_data(st, sen); + if (!res) + inv_push_8bytes_buffer(st, hdr | + (!!steps), t, sen); + + dptr += HEADERED_NORMAL_BYTES; + continue; + } + if (st->sensor[sensor_ind].sample_size == HEADERED_Q_BYTES) { + for (i = 0; i < 3; i++) + q[i] = (int)be32_to_cpup((__be32 *)(dptr + 4 + + i * 4)); + inv_push_16bytes_buffer(st, hdr | (!!steps), t, q); + } else { + for (i = 0; i < 3; i++) + sen[i] = (short)be16_to_cpup((__be16 *)(dptr + + 2 + i * 2)); + inv_push_8bytes_buffer(st, hdr | (!!steps), t, sen); + } + dptr += st->sensor[sensor_ind].sample_size; + } + inv_adjust_sensor_ts(st, sensor_ind); + st->left_over_size = target_bytes - (dptr - d); + + if (st->left_over_size) + memcpy(st->left_over, dptr, st->left_over_size); + + return 0; +} + +int inv_read_time_and_ticks(struct inv_mpu_state *st, bool resume) +{ + int result; + u32 counter; + u8 data[4]; + +#define MIN_TICK_READING_TIME NSEC_PER_SEC + st->last_ts = get_time_ns(); + if ((st->last_ts - st->prev_ts < MIN_TICK_READING_TIME) && + (!resume)) { + st->chip_config.adjust_time = false; + return 0; + } + result = mpu_memory_read(st, st->i2c_addr, + inv_dmp_get_address(KEY_DMP_RUN_CNTR), 4, data); + if (result) + return result; + + counter = be32_to_cpup((__be32 *)(data)); + if (resume) { + st->dmp_counter = counter; + st->prev_ts = st->last_ts; + + return 0; + } + if (counter > st->dmp_counter) + st->dmp_ticks = counter - st->dmp_counter; + else + st->dmp_ticks = 0xffffffff - st->dmp_counter + counter + 1; + st->dmp_counter = counter; + st->chip_config.adjust_time = true; + + return 0; +} + +/* + * inv_read_fifo() - Transfer data from FIFO to ring buffer. + */ +irqreturn_t inv_read_fifo(int irq, void *dev_id) +{ + + struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; + struct iio_dev *indio_dev = iio_priv_to_dev(st); + int result, bpm; + u8 data[MAX_HW_FIFO_BYTES]; + u16 fifo_count; + struct inv_reg_map_s *reg; + u64 pts1; + +#define DMP_MIN_RUN_TIME (37 * NSEC_PER_MSEC) + mutex_lock(&st->suspend_resume_lock); + mutex_lock(&indio_dev->mlock); + if (st->chip_config.dmp_on) { + pts1 = get_time_ns(); + result = inv_process_dmp_interrupt(st); + if (result || st->chip_config.dmp_event_int_on) + goto end_session; + if (!st->chip_config.smd_triggered) { + if (pts1 - st->last_run_time < DMP_MIN_RUN_TIME) + goto end_session; + else + st->last_run_time = pts1; + } else { + st->chip_config.smd_triggered = false; + } + } + + if (!(iio_buffer_enabled(indio_dev)) || (!st->chip_config.enable)) + goto end_session; + + reg = &st->reg; + if (!(st->sensor[SENSOR_ACCEL].on | + st->sensor[SENSOR_GYRO].on | + st->sensor[SENSOR_COMPASS].on | + st->sensor[SENSOR_PRESSURE].on | + st->chip_config.dmp_on | + st->mot_int.mot_on)) + goto end_session; + if (st->chip_config.lpa_mode) { + result = inv_i2c_read(st, reg->raw_accel, + BYTES_PER_SENSOR, data); + if (result) + goto end_session; + inv_report_gyro_accel(indio_dev, data, get_time_ns()); + if (st->mot_int.mot_on) + inv_process_motion(st); + + goto end_session; + } + + if (st->chip_config.dmp_on) { + result = inv_read_time_and_ticks(st, false); + if (result) + goto end_session; + } + bpm = st->chip_config.bytes_per_datum; + fifo_count = 0; + if (bpm) { + result = inv_i2c_read(st, reg->fifo_count_h, FIFO_COUNT_BYTE, + data); + if (result) + goto end_session; + fifo_count = be16_to_cpup((__be16 *)(data)); + /* fifo count can't be odd number */ + if (fifo_count & 1) + goto flush_fifo; + if (fifo_count == 0) + goto end_session; + st->fifo_count = fifo_count; + } + + if (st->chip_config.dmp_on) { + result = inv_process_batchmode(st); + } else { + if (fifo_count > FIFO_THRESHOLD) + goto flush_fifo; + if (bpm) { + while (fifo_count >= bpm) { + result = inv_i2c_read(st, reg->fifo_r_w, bpm, + data); + if (result) + goto flush_fifo; + result = inv_get_timestamp(st, + fifo_count / bpm); + if (result) + goto flush_fifo; + inv_report_gyro_accel(indio_dev, data, + st->last_ts); + fifo_count -= bpm; + } + } else { + result = inv_get_timestamp(st, 1); + if (result) + goto flush_fifo; + } + inv_send_compass_data(st); + inv_send_pressure_data(st); + } +end_session: + mutex_unlock(&indio_dev->mlock); + mutex_unlock(&st->suspend_resume_lock); + + return IRQ_HANDLED; +flush_fifo: + /* Flush HW and SW FIFOs. */ + inv_reset_fifo(indio_dev); + inv_clear_kfifo(st); + mutex_unlock(&indio_dev->mlock); + mutex_unlock(&st->suspend_resume_lock); + + return IRQ_HANDLED; +} + +void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + free_irq(st->client->irq, st); + iio_kfifo_free(indio_dev->buffer); +}; + +static int inv_predisable(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + int result; + + if (st->chip_config.enable) { + result = set_inv_enable(indio_dev, false); + if (result) + return result; + result = st->set_power_state(st, false); + if (result) + return result; + } + + return 0; +} + +static int inv_check_conflict_sysfs(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + + if (st->chip_config.lpa_mode) { + /* dmp cannot run with low power mode on */ + st->chip_config.dmp_on = 0; + st->chip_config.gyro_enable = false; + st->sensor[SENSOR_GYRO].on = false; + st->sensor[SENSOR_COMPASS].on = false; + } + if (st->sensor[SENSOR_GYRO].on && + (!st->chip_config.gyro_enable)) { + st->chip_config.gyro_enable = true; + } + if (st->sensor[SENSOR_ACCEL].on && + (!st->chip_config.accel_enable)) { + st->chip_config.accel_enable = true; + } + + return 0; +} + +static int inv_preenable(struct iio_dev *indio_dev) +{ + int result; + + result = inv_check_conflict_sysfs(indio_dev); + if (result) + return result; + result = iio_sw_buffer_preenable(indio_dev); + + return result; +} + +void inv_init_sensor_struct(struct inv_mpu_state *st) +{ + int i; + + for (i = 0; i < SENSOR_NUM_MAX; i++) { + if (i < SENSOR_SIXQ) + st->sensor[i].sample_size = + HEADERED_NORMAL_BYTES; + else + st->sensor[i].sample_size = HEADERED_Q_BYTES; + if (i == SENSOR_STEP) { + st->sensor[i].rate = 1; + st->sensor[i].dur = NSEC_PER_SEC; + } else { + st->sensor[i].rate = INIT_DMP_OUTPUT_RATE; + st->sensor[i].dur = NSEC_PER_SEC / + INIT_DMP_OUTPUT_RATE; + } + } + + st->sensor[SENSOR_ACCEL].send_data = inv_send_accel_data; + st->sensor[SENSOR_GYRO].send_data = inv_send_gyro_data; + st->sensor[SENSOR_COMPASS].send_data = inv_send_compass_dmp_data; + st->sensor[SENSOR_PRESSURE].send_data = inv_send_pressure_dmp_data; + st->sensor[SENSOR_STEP].send_data = inv_send_step_detector; + st->sensor[SENSOR_PEDQ].send_data = inv_send_ped_q_data; + st->sensor[SENSOR_SIXQ].send_data = inv_send_six_q_data; + st->sensor[SENSOR_LPQ].send_data = inv_send_three_q_data; + + st->sensor[SENSOR_ACCEL].set_rate = inv_set_accel_rate; + st->sensor[SENSOR_GYRO].set_rate = inv_set_gyro_rate; + st->sensor[SENSOR_COMPASS].set_rate = inv_set_compass_rate; + st->sensor[SENSOR_PRESSURE].set_rate = inv_set_pressure_rate; + st->sensor[SENSOR_STEP].set_rate = inv_set_step_detector; + st->sensor[SENSOR_PEDQ].set_rate = inv_set_pedq_rate; + st->sensor[SENSOR_SIXQ].set_rate = inv_set_sixq_rate; + st->sensor[SENSOR_LPQ].set_rate = inv_set_lpq_rate; +} + +int inv_flush_batch_data(struct iio_dev *indio_dev, bool *has_data) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + struct inv_reg_map_s *reg; + u8 data[4]; + int result; + + reg = &st->reg; + if (!(iio_buffer_enabled(indio_dev)) || (!st->chip_config.enable)) + return -EINVAL; + + if (st->batch.on) { + result = inv_read_time_and_ticks(st, false); + if (result) + return result; + result = inv_i2c_read(st, reg->fifo_count_h, + FIFO_COUNT_BYTE, data); + if (result) + return result; + st->fifo_count = be16_to_cpup((__be16 *)(data)); + if (st->fifo_count) { + result = inv_process_batchmode(st); + if (result) + return result; + *has_data = !!st->fifo_count; + inv_push_marker_to_buffer(st, END_MARKER); + result = write_be32_key_to_mem(st, 0, + KEY_BM_BATCH_CNTR); + return result; + } + } + inv_push_marker_to_buffer(st, EMPTY_MARKER); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_mpu_ring_setup_ops = { + .preenable = &inv_preenable, + .predisable = &inv_predisable, +}; + +int inv_mpu_configure_ring(struct iio_dev *indio_dev) +{ + int ret; + struct inv_mpu_state *st = iio_priv(indio_dev); + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) + return -ENOMEM; + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_mpu_ring_setup_ops; + /*scan count double count timestamp. should subtract 1. but + number of channels still includes timestamp*/ + if (INV_MPU3050 == st->chip_type) + ret = request_threaded_irq(st->client->irq, inv_irq_handler, + inv_read_fifo_mpu3050, + IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st); + else + ret = request_threaded_irq(st->client->irq, inv_irq_handler, + inv_read_fifo, + IRQF_TRIGGER_RISING | IRQF_SHARED, "inv_irq", st); + if (ret) + goto error_iio_sw_rb_free; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + + return 0; +error_iio_sw_rb_free: + iio_kfifo_free(indio_dev->buffer); + + return ret; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu/inv_mpu_trigger.c new file mode 100644 index 000000000000..6db275acd507 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_mpu_trigger.c @@ -0,0 +1,78 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> +#include <linux/iio/iio.h> +#include <linux/iio/sysfs.h> +#include <linux/iio/trigger.h> + +#include "inv_mpu_iio.h" + +/* + * inv_mpu_data_rdy_trigger_set_state() set data ready interrupt state + */ +static int inv_mpu_data_rdy_trigger_set_state(struct iio_trigger *trig, + bool state) +{ + return 0; +} + +static const struct iio_trigger_ops inv_mpu_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = &inv_mpu_data_rdy_trigger_set_state, +}; + +int inv_mpu_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_mpu_state *st = iio_priv(indio_dev); + + st->trig = iio_trigger_alloc("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) + return -ENOMEM; + st->trig->dev.parent = &st->client->dev; + st->trig->ops = &inv_mpu_trigger_ops; + ret = iio_trigger_register(st->trig); + + if (ret) { + iio_trigger_free(st->trig); + return -EPERM; + } + indio_dev->trig = st->trig; + + return 0; +} + +void inv_mpu_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_mpu_state *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_trigger_free(st->trig); +} + diff --git a/drivers/iio/imu/inv_mpu/inv_slave_bma250.c b/drivers/iio/imu/inv_mpu/inv_slave_bma250.c new file mode 100644 index 000000000000..99ae702e67ad --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_slave_bma250.c @@ -0,0 +1,315 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_mpu_iio.h" +#define BMA250_CHIP_ID 3 +#define BMA250_RANGE_SET 0 +#define BMA250_BW_SET 4 + +/* range and bandwidth */ +#define BMA250_RANGE_2G 3 +#define BMA250_RANGE_4G 5 +#define BMA250_RANGE_8G 8 +#define BMA250_RANGE_16G 12 +#define BMA250_RANGE_MAX 4 +#define BMA250_RANGE_MASK 0xF0 + +#define BMA250_BW_7_81HZ 0x08 +#define BMA250_BW_15_63HZ 0x09 +#define BMA250_BW_31_25HZ 0x0A +#define BMA250_BW_62_50HZ 0x0B +#define BMA250_BW_125HZ 0x0C +#define BMA250_BW_250HZ 0x0D +#define BMA250_BW_500HZ 0x0E +#define BMA250_BW_1000HZ 0x0F +#define BMA250_MAX_BW_SIZE 8 +#define BMA250_BW_REG_MASK 0xE0 + +/* register definitions */ +#define BMA250_X_AXIS_LSB_REG 0x02 +#define BMA250_RANGE_SEL_REG 0x0F +#define BMA250_BW_SEL_REG 0x10 +#define BMA250_MODE_CTRL_REG 0x11 + +/* mode settings */ +#define BMA250_MODE_NORMAL 0 +#define BMA250_MODE_LOWPOWER 1 +#define BMA250_MODE_SUSPEND 2 +#define BMA250_MODE_MAX 3 +#define BMA250_MODE_MASK 0x3F +#define BMA250_BIT_SUSPEND 0x80 +#define BMA250_BIT_LP 0x40 + +struct bma_property { + int range; + int bandwidth; + int mode; +}; + +static struct bma_property bma_static_property = { + .range = BMA250_RANGE_SET, + .bandwidth = BMA250_BW_SET, + .mode = BMA250_MODE_SUSPEND +}; + +static int bma250_set_bandwidth(struct inv_mpu_state *st, u8 bw) +{ + int res; + u8 data; + int bandwidth; + switch (bw) { + case 0: + bandwidth = BMA250_BW_7_81HZ; + break; + case 1: + bandwidth = BMA250_BW_15_63HZ; + break; + case 2: + bandwidth = BMA250_BW_31_25HZ; + break; + case 3: + bandwidth = BMA250_BW_62_50HZ; + break; + case 4: + bandwidth = BMA250_BW_125HZ; + break; + case 5: + bandwidth = BMA250_BW_250HZ; + break; + case 6: + bandwidth = BMA250_BW_500HZ; + break; + case 7: + bandwidth = BMA250_BW_1000HZ; + break; + default: + return -EINVAL; + } + res = inv_secondary_read(BMA250_BW_SEL_REG, 1, &data); + if (res) + return res; + data &= BMA250_BW_REG_MASK; + data |= bandwidth; + res = inv_secondary_write(BMA250_BW_SEL_REG, data); + return res; +} + +static int bma250_set_range(struct inv_mpu_state *st, u8 range) +{ + int res; + u8 orig, data; + switch (range) { + case 0: + data = BMA250_RANGE_2G; + break; + case 1: + data = BMA250_RANGE_4G; + break; + case 2: + data = BMA250_RANGE_8G; + break; + case 3: + data = BMA250_RANGE_16G; + break; + default: + return -EINVAL; + } + res = inv_secondary_read(BMA250_RANGE_SEL_REG, 1, &orig); + if (res) + return res; + orig &= BMA250_RANGE_MASK; + data |= orig; + res = inv_secondary_write(BMA250_RANGE_SEL_REG, data); + if (res) + return res; + bma_static_property.range = range; + + return 0; +} + +static int setup_slave_bma250(struct inv_mpu_state *st) +{ + int result; + u8 data[2]; + result = set_3050_bypass(st, true); + if (result) + return result; + /*read secondary i2c ID register */ + result = inv_secondary_read(0, 1, data); + if (result) + return result; + if (BMA250_CHIP_ID != data[0]) + return -EINVAL; + result = set_3050_bypass(st, false); + if (result) + return result; + /*AUX(accel), slave address is set inside set_3050_bypass*/ + /* bma250 x axis LSB register address is 2 */ + result = inv_i2c_single_write(st, REG_3050_AUX_BST_ADDR, + BMA250_X_AXIS_LSB_REG); + + return result; +} + +static int bma250_set_mode(struct inv_mpu_state *st, u8 mode) +{ + int res; + u8 data; + + res = inv_secondary_read(BMA250_MODE_CTRL_REG, 1, &data); + if (res) + return res; + data &= BMA250_MODE_MASK; + switch (mode) { + case BMA250_MODE_NORMAL: + break; + case BMA250_MODE_LOWPOWER: + data |= BMA250_BIT_LP; + break; + case BMA250_MODE_SUSPEND: + data |= BMA250_BIT_SUSPEND; + break; + default: + return -EINVAL; + } + res = inv_secondary_write(BMA250_MODE_CTRL_REG, data); + if (res) + return res; + bma_static_property.mode = mode; + + return 0; +} + +static int suspend_slave_bma250(struct inv_mpu_state *st) +{ + int result; + if (bma_static_property.mode == BMA250_MODE_SUSPEND) + return 0; + /*set to bypass mode */ + result = set_3050_bypass(st, true); + if (result) + return result; + bma250_set_mode(st, BMA250_MODE_SUSPEND); + /* no need to recover to non-bypass mode because we need it now */ + + return 0; +} + +static int resume_slave_bma250(struct inv_mpu_state *st) +{ + int result; + if (bma_static_property.mode == BMA250_MODE_NORMAL) + return 0; + /*set to bypass mode */ + result = set_3050_bypass(st, true); + if (result) + return result; + result = bma250_set_mode(st, BMA250_MODE_NORMAL); + /* recover bypass mode */ + result |= set_3050_bypass(st, false); + + return result ? (-EINVAL) : 0; +} + +static int combine_data_slave_bma250(u8 *in, short *out) +{ + out[0] = le16_to_cpup((__le16 *)(&in[0])); + out[1] = le16_to_cpup((__le16 *)(&in[2])); + out[2] = le16_to_cpup((__le16 *)(&in[4])); + + return 0; +} + +static int get_mode_slave_bma250(void) +{ + switch (bma_static_property.mode) { + case BMA250_MODE_SUSPEND: + return INV_MODE_SUSPEND; + case BMA250_MODE_NORMAL: + return INV_MODE_NORMAL; + default: + return -EINVAL; + } +} + +/** + * set_lpf_bma250() - set lpf value + */ + +static int set_lpf_bma250(struct inv_mpu_state *st, int rate) +{ + const short hz[] = {1000, 500, 250, 125, 62, 31, 15, 7}; + const int d[] = {7, 6, 5, 4, 3, 2, 1, 0}; + int i, h, data, result; + h = (rate >> 1); + i = 0; + while ((h < hz[i]) && (i < ARRAY_SIZE(hz) - 1)) + i++; + data = d[i]; + + result = set_3050_bypass(st, true); + if (result) + return result; + result = bma250_set_bandwidth(st, (u8) data); + result |= set_3050_bypass(st, false); + + return result ? (-EINVAL) : 0; +} +/** + * set_fs_bma250() - set range value + */ + +static int set_fs_bma250(struct inv_mpu_state *st, int fs) +{ + int result; + result = set_3050_bypass(st, true); + if (result) + return result; + result = bma250_set_range(st, (u8) fs); + result |= set_3050_bypass(st, false); + + return result ? (-EINVAL) : 0; +} + +static struct inv_mpu_slave slave_bma250 = { + .suspend = suspend_slave_bma250, + .resume = resume_slave_bma250, + .setup = setup_slave_bma250, + .combine_data = combine_data_slave_bma250, + .get_mode = get_mode_slave_bma250, + .set_lpf = set_lpf_bma250, + .set_fs = set_fs_bma250 +}; + +int inv_register_mpu3050_slave(struct inv_mpu_state *st) +{ + st->slave_accel = &slave_bma250; + + return 0; +} + diff --git a/drivers/iio/imu/inv_mpu/inv_slave_compass.c b/drivers/iio/imu/inv_mpu/inv_slave_compass.c new file mode 100644 index 000000000000..f7d7ff20b173 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_slave_compass.c @@ -0,0 +1,852 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> + +#include "inv_mpu_iio.h" +/* AKM definitions */ +#define REG_AKM_ID 0x00 +#define REG_AKM_INFO 0x01 +#define REG_AKM_STATUS 0x02 +#define REG_AKM_MEASURE_DATA 0x03 +#define REG_AKM_MODE 0x0A +#define REG_AKM_ST_CTRL 0x0C +#define REG_AKM_SENSITIVITY 0x10 +#define REG_AKM8963_CNTL1 0x0A + +/* AK09911 register definition */ +#define REG_AK09911_DMP_READ 0x10 +#define REG_AK09911_STATUS1 0x10 +#define REG_AK09911_CNTL2 0x31 +#define REG_AK09911_SENSITIVITY 0x60 + +#define DATA_AKM_ID 0x48 +#define DATA_AKM_MODE_PD 0x00 +#define DATA_AKM_MODE_SM 0x01 +#define DATA_AKM_MODE_ST 0x08 +#define DATA_AKM_MODE_FR 0x0F +#define DATA_AK09911_MODE_FR 0x1F +#define DATA_AKM_SELF_TEST 0x40 +#define DATA_AKM_DRDY 0x01 +#define DATA_AKM8963_BIT 0x10 +#define DATA_AKM_STAT_MASK 0x0C + +#define DATA_AKM8975_SCALE (9830 * (1L << 15)) +#define DATA_AKM8972_SCALE (19661 * (1L << 15)) +#define DATA_AKM8963_SCALE0 (19661 * (1L << 15)) +#define DATA_AKM8963_SCALE1 (4915 * (1L << 15)) +#define DATA_AK09911_SCALE (19661 * (1L << 15)) +#define DATA_MLX_SCALE (4915 * (1L << 15)) +#define DATA_MLX_SCALE_EMPIRICAL (26214 * (1L << 15)) + +#define DATA_AKM8963_SCALE_SHIFT 4 +#define DATA_AKM_BYTES_DMP 10 +#define DATA_AKM_BYTES 8 +#define DATA_AKM_MIN_READ_TIME (9 * NSEC_PER_MSEC) + +#define DEF_ST_COMPASS_WAIT_MIN (10 * 1000) +#define DEF_ST_COMPASS_WAIT_MAX (15 * 1000) +#define DEF_ST_COMPASS_TRY_TIMES 10 +#define DEF_ST_COMPASS_8963_SHIFT 2 +#define X 0 +#define Y 1 +#define Z 2 + +/* milliseconds between each access */ +#define AKM_RATE_SCALE 10 +#define MLX_RATE_SCALE 50 + +/* MLX90399 compass definition */ +#define DATA_MLX_CMD_READ_MEASURE 0x4F +#define DATA_MLX_CMD_SINGLE_MEASURE 0x3F +#define DATA_MLX_READ_DATA_BYTES 9 +#define DATA_MLX_STATUS_DATA 3 +#define DATA_MLX_MIN_READ_TIME (95 * NSEC_PER_MSEC) + +static const short AKM8975_ST_Lower[3] = {-100, -100, -1000}; +static const short AKM8975_ST_Upper[3] = {100, 100, -300}; + +static const short AKM8972_ST_Lower[3] = {-50, -50, -500}; +static const short AKM8972_ST_Upper[3] = {50, 50, -100}; + +static const short AKM8963_ST_Lower[3] = {-200, -200, -3200}; +static const short AKM8963_ST_Upper[3] = {200, 200, -800}; + +/* + * inv_setup_compass_akm() - Configure akm series compass. + */ +static int inv_setup_compass_akm(struct inv_mpu_state *st) +{ + int result; + u8 data[4]; + u8 sens, mode, cmd; + + /* set to bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config | BIT_BYPASS_EN); + if (result) + return result; + /* read secondary i2c ID register */ + result = inv_secondary_read(REG_AKM_ID, 1, data); + if (result) + return result; + if (data[0] != DATA_AKM_ID) + return -ENXIO; + /* set AKM to Fuse ROM access mode */ + if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) { + mode = REG_AK09911_CNTL2; + sens = REG_AK09911_SENSITIVITY; + cmd = DATA_AK09911_MODE_FR; + } else { + mode = REG_AKM_MODE; + sens = REG_AKM_SENSITIVITY; + cmd = DATA_AKM_MODE_FR; + } + + result = inv_secondary_write(mode, cmd); + if (result) + return result; + result = inv_secondary_read(sens, THREE_AXIS, + st->chip_info.compass_sens); + if (result) + return result; + /* revert to power down mode */ + result = inv_secondary_write(mode, DATA_AKM_MODE_PD); + if (result) + return result; + pr_debug("%s senx=%d, seny=%d, senz=%d\n", + st->hw->name, + st->chip_info.compass_sens[0], + st->chip_info.compass_sens[1], + st->chip_info.compass_sens[2]); + /* restore to non-bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + if (result) + return result; + + /* setup master mode and master clock and ES bit */ + result = inv_i2c_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES); + if (result) + return result; + /* slave 1 is used for AKM mode change only */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_ADDR, + st->plat_data.secondary_i2c_addr); + if (result) + return result; + /* AKM mode register address */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_REG, mode); + if (result) + return result; + /* output data for slave 1 is fixed, single measure mode */ + st->slave_compass->scale = 1; + if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id) { + st->slave_compass->st_upper = AKM8975_ST_Upper; + st->slave_compass->st_lower = AKM8975_ST_Lower; + data[0] = DATA_AKM_MODE_SM; + } else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id) { + st->slave_compass->st_upper = AKM8972_ST_Upper; + st->slave_compass->st_lower = AKM8972_ST_Lower; + data[0] = DATA_AKM_MODE_SM; + } else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) { + st->slave_compass->st_upper = AKM8963_ST_Upper; + st->slave_compass->st_lower = AKM8963_ST_Lower; + data[0] = DATA_AKM_MODE_SM | + (st->slave_compass->scale << DATA_AKM8963_SCALE_SHIFT); + } else if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) { + st->slave_compass->st_upper = AKM8963_ST_Upper; + st->slave_compass->st_lower = AKM8963_ST_Lower; + data[0] = DATA_AKM_MODE_SM; + } else { + return -EINVAL; + } + + result = inv_i2c_single_write(st, INV_MPU_REG_I2C_SLV1_DO, data[0]); + + return result; +} + +static int inv_akm_read_data(struct inv_mpu_state *st, short *o) +{ + int result, shift; + int i; + u8 d[DATA_AKM_BYTES]; + u8 *sens; + + sens = st->chip_info.compass_sens; + result = 0; + if (st->chip_config.dmp_on && + (COMPASS_ID_AK09911 != st->plat_data.sec_slave_id)) { + for (i = 0; i < 6; i++) + d[1 + i] = st->fifo_data[i]; + } else { + result = inv_i2c_read(st, REG_EXT_SENS_DATA_00, + DATA_AKM_BYTES, d); + if ((DATA_AKM_DRDY != d[0]) || result) + result = -EINVAL; + } + if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) + shift = 7; + else + shift = 8; + for (i = 0; i < 3; i++) { + o[i] = (short)((d[i * 2 + 2] << 8) | d[i * 2 + 1]); + o[i] = (short)(((int)o[i] * (sens[i] + 128)) >> shift); + } + + return result; +} + +static int inv_mlx_read_data(struct inv_mpu_state *st, short *o) +{ + int result; + int i, z; + u8 d[DATA_MLX_READ_DATA_BYTES]; + + result = inv_i2c_read(st, REG_EXT_SENS_DATA_00, + DATA_MLX_READ_DATA_BYTES, d); + if ((!(d[0] & ~DATA_MLX_STATUS_DATA)) && (!result)) { + for (i = 0; i < 3; i++) + o[i] = (short)((d[i * 2 + 3] << 8) + d[i * 2 + 4]); + } else { + for (i = 0; i < 3; i++) + o[i] = 0; + } + z = o[2]; + /* axis sensitivity conversion. Z axis has different sensitiviy from + x and y */ + z *= 26; + z /= 15; + o[2] = z; + + return 0; +} + +static int inv_check_akm_self_test(struct inv_mpu_state *st) +{ + int result; + u8 data[6], mode; + u8 counter, cntl; + short x, y, z; + u8 *sens; + sens = st->chip_info.compass_sens; + + /* set to bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config | BIT_BYPASS_EN); + if (result) { + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + return result; + } + if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) + mode = REG_AK09911_CNTL2; + else + mode = REG_AKM_MODE; + /* set to power down mode */ + result = inv_secondary_write(mode, DATA_AKM_MODE_PD); + if (result) + goto AKM_fail; + + /* write 1 to ASTC register */ + result = inv_secondary_write(REG_AKM_ST_CTRL, DATA_AKM_SELF_TEST); + if (result) + goto AKM_fail; + /* set self test mode */ + result = inv_secondary_write(mode, DATA_AKM_MODE_ST); + if (result) + goto AKM_fail; + counter = DEF_ST_COMPASS_TRY_TIMES; + while (counter > 0) { + usleep_range(DEF_ST_COMPASS_WAIT_MIN, DEF_ST_COMPASS_WAIT_MAX); + result = inv_secondary_read(REG_AKM_STATUS, 1, data); + if (result) + goto AKM_fail; + if ((data[0] & DATA_AKM_DRDY) == 0) + counter--; + else + counter = 0; + } + if ((data[0] & DATA_AKM_DRDY) == 0) { + result = -EINVAL; + goto AKM_fail; + } + result = inv_secondary_read(REG_AKM_MEASURE_DATA, + BYTES_PER_SENSOR, data); + if (result) + goto AKM_fail; + + x = le16_to_cpup((__le16 *)(&data[0])); + y = le16_to_cpup((__le16 *)(&data[2])); + z = le16_to_cpup((__le16 *)(&data[4])); + x = ((x * (sens[0] + 128)) >> 8); + y = ((y * (sens[1] + 128)) >> 8); + z = ((z * (sens[2] + 128)) >> 8); + if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) { + result = inv_secondary_read(REG_AKM8963_CNTL1, 1, &cntl); + if (result) + goto AKM_fail; + if (0 == (cntl & DATA_AKM8963_BIT)) { + x <<= DEF_ST_COMPASS_8963_SHIFT; + y <<= DEF_ST_COMPASS_8963_SHIFT; + z <<= DEF_ST_COMPASS_8963_SHIFT; + } + } + result = -EINVAL; + if (x > st->slave_compass->st_upper[X] || + x < st->slave_compass->st_lower[X]) + goto AKM_fail; + if (y > st->slave_compass->st_upper[Y] || + y < st->slave_compass->st_lower[Y]) + goto AKM_fail; + if (z > st->slave_compass->st_upper[Z] || + z < st->slave_compass->st_lower[Z]) + goto AKM_fail; + result = 0; +AKM_fail: + /*write 0 to ASTC register */ + result |= inv_secondary_write(REG_AKM_ST_CTRL, 0); + /*set to power down mode */ + result |= inv_secondary_write(mode, DATA_AKM_MODE_PD); + /*restore to non-bypass mode */ + result |= inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + return result; +} + +/* + * inv_write_akm_scale() - Configure the akm scale range. + */ +static int inv_write_akm_scale(struct inv_mpu_state *st, int data) +{ + char d, en; + int result; + + if (COMPASS_ID_AK8963 != st->plat_data.sec_slave_id) + return 0; + en = !!data; + if (st->slave_compass->scale == en) + return 0; + d = (DATA_AKM_MODE_SM | (en << DATA_AKM8963_SCALE_SHIFT)); + result = inv_i2c_single_write(st, INV_MPU_REG_I2C_SLV1_DO, d); + if (result) + return result; + st->slave_compass->scale = en; + + return 0; +} + +/* + * inv_read_akm_scale() - show AKM scale. + */ +static int inv_read_akm_scale(struct inv_mpu_state *st, int *scale) +{ + if (COMPASS_ID_AK8975 == st->plat_data.sec_slave_id) + *scale = DATA_AKM8975_SCALE; + else if (COMPASS_ID_AK8972 == st->plat_data.sec_slave_id) + *scale = DATA_AKM8972_SCALE; + else if (COMPASS_ID_AK8963 == st->plat_data.sec_slave_id) + if (st->slave_compass->scale) + *scale = DATA_AKM8963_SCALE1; + else + *scale = DATA_AKM8963_SCALE0; + else if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) + *scale = DATA_AK09911_SCALE; + else + return -EINVAL; + + return IIO_VAL_INT; +} + +static int inv_suspend_akm(struct inv_mpu_state *st) +{ + int result; + + /* slave 0 is disabled */ + result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, 0); + if (result) + return result; + /* slave 1 is disabled */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, 0); + + return result; +} + +static int inv_resume_akm(struct inv_mpu_state *st) +{ + int result; + u8 reg_addr, bytes; + + /* slave 0 is used to read data from compass */ + /*read mode */ + result = inv_i2c_single_write(st, REG_I2C_SLV0_ADDR, + INV_MPU_BIT_I2C_READ | + st->plat_data.secondary_i2c_addr); + if (result) + return result; + /* AKM status register address is 1 */ + if (COMPASS_ID_AK09911 == st->plat_data.sec_slave_id) { + if (st->chip_config.dmp_on) { + reg_addr = REG_AK09911_DMP_READ; + bytes = DATA_AKM_BYTES_DMP; + } else { + reg_addr = REG_AK09911_STATUS1; + bytes = DATA_AKM_BYTES; + } + } else { + if (st->chip_config.dmp_on) { + reg_addr = REG_AKM_INFO; + bytes = DATA_AKM_BYTES_DMP; + } else { + reg_addr = REG_AKM_STATUS; + bytes = DATA_AKM_BYTES; + } + } + result = inv_i2c_single_write(st, REG_I2C_SLV0_REG, reg_addr); + if (result) + return result; + + /* slave 0 is enabled, read 10 or 8 bytes from here */ + result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, + INV_MPU_BIT_SLV_EN | bytes); + if (result) + return result; + /* slave 1 is enabled, write byte length is 1 */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, + INV_MPU_BIT_SLV_EN | 1); + + return result; +} + +/* + * inv_write_mlx_scale() - Configure the mlx90399 scale range. + */ +static int inv_write_mlx_scale(struct inv_mpu_state *st, int data) +{ + st->slave_compass->scale = data; + return 0; +} + +/* + * inv_read_mlx_scale() - show mlx90399 scale. + */ +static int inv_read_mlx_scale(struct inv_mpu_state *st, int *scale) +{ + *scale = st->slave_compass->scale; + return IIO_VAL_INT; +} + +static int inv_i2c_read_mlx(struct inv_mpu_state *st, u16 i2c_addr, + u16 length, u8 *data) +{ + struct i2c_msg msgs[1]; + int res; + + if (!data) + return -EINVAL; + + msgs[0].addr = i2c_addr; + msgs[0].flags = I2C_M_RD; + msgs[0].buf = data; + msgs[0].len = length; + + res = i2c_transfer(st->sl_handle, msgs, 1); + + if (res < 1) { + if (res >= 0) + res = -EIO; + } else + res = 0; + + return res; +} + +static int inv_i2c_write_mlx(struct inv_mpu_state *st, + u16 i2c_addr, u8 data) +{ + u8 tmp[1]; + struct i2c_msg msg; + int res; + + tmp[0] = data; + msg.addr = i2c_addr; + msg.flags = 0; /* write */ + msg.buf = tmp; + msg.len = 1; + + res = i2c_transfer(st->sl_handle, &msg, 1); + if (res < 1) { + if (res == 0) + res = -EIO; + return res; + } else + return 0; +} + +static int inv_i2c_read_reg_mlx(struct inv_mpu_state *st, + u16 i2c_addr, u8 reg, u16 *val) +{ + u8 tmp[10]; + struct i2c_msg msg; + int res; + + tmp[0] = 0x50; + tmp[1] = (reg << 2); + msg.addr = i2c_addr; + msg.flags = 0; /* write */ + msg.buf = tmp; + msg.len = 2; + + res = i2c_transfer(st->sl_handle, &msg, 1); + if (res < 1) { + if (res == 0) + res = -EIO; + return res; + } + res = inv_i2c_read_mlx(st, i2c_addr, 10, tmp); + if (res) + return res; + *val = ((tmp[1] << 8) | tmp[2]); + + return res; +} + +static int inv_i2c_write_mlx_reg(struct inv_mpu_state *st, + u16 i2c_addr, int reg, u16 d) +{ + u8 tmp[10]; + struct i2c_msg msg; + int res; + + /* write register command, writing volatile memory */ + tmp[0] = 0x60; + tmp[1] = ((d >> 8) & 0xff); + tmp[2] = (d & 0xff); + tmp[3] = (reg << 2); + msg.addr = i2c_addr; + msg.flags = 0; /* write */ + msg.buf = tmp; + msg.len = 4; + + res = i2c_transfer(st->sl_handle, &msg, 1); + if (res < 1) { + if (res == 0) + res = -EIO; + return res; + } + /* read status */ + res = inv_i2c_read_mlx(st, i2c_addr, 10, tmp); + + return res; +} + +static int inv_write_mlx_cmd(struct inv_mpu_state *st, u8 cmd) +{ + int result; + u8 d[10]; + int addr; + + addr = st->plat_data.secondary_i2c_addr; + result = inv_i2c_write_mlx(st, addr, cmd); + if (result) + return result; + /* read back status byte */ + result = inv_i2c_read_mlx(st, addr, 10, d); + + return result; +} + +static int inv_read_mlx_z_axis(struct inv_mpu_state *st, s16 *z) +{ + int result; + u8 d[10]; + int addr; + + addr = st->plat_data.secondary_i2c_addr; + + /* measure z axis */ + result = inv_write_mlx_cmd(st, 0x39); + if (result) + return result; + msleep(100); + /* read z axis */ + result = inv_i2c_write_mlx(st, addr, 0x49); + if (result) + return result; + /* read back status byte */ + result = inv_i2c_read_mlx(st, addr, 10, d); + if (result) + return result; + if ((d[0] & 0x3) == 1) + *z = (short)((d[3] << 8) + d[4]); + else + return -EINVAL; + + return 0; +} + +static int inv_write_mlx_reg(struct inv_mpu_state *st) +{ + int result; + int addr; + u16 r_val; + + addr = st->plat_data.secondary_i2c_addr; + + /* write register 0. + set GAIN_SEL as 7; + set HALL_CONF as 0xC. */ + result = inv_i2c_write_mlx_reg(st, addr, 0, 0x7c); + if (result) + return result; + /* write register 2. + set resolution is zero for all axes; + set DIGI filter as 6. + set OSR as 0. + set OSR2 as 0. */ + result = inv_i2c_write_mlx_reg(st, addr, 2, 0x18); + if (result) + return result; + /* read register 1 */ + result = inv_i2c_read_reg_mlx(st, addr, 1, &r_val); + if (result) + return result; + /* enable temp comp */ + r_val |= 0x400; + result = inv_i2c_write_mlx_reg(st, addr, 1, r_val); + /* the value should be kept in the volatile memory */ + + return result; +} + +static int inv_check_mlx_self_test(struct inv_mpu_state *st) +{ + int result; + int addr; + s16 meas_ref, meas_coil; + u16 diff, r_val; + + /* set to bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config | BIT_BYPASS_EN); + if (result) { + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + return result; + } + + addr = st->plat_data.secondary_i2c_addr; + + /* fake read to flush the previous data */ + result = inv_read_mlx_z_axis(st, &meas_ref); + + result = inv_read_mlx_z_axis(st, &meas_ref); + if (result) + return result; + + /* read register 1 */ + result = inv_i2c_read_reg_mlx(st, addr, 0, &r_val); + if (result) + return result; + /* enable self test */ + r_val |= 0x100; + result = inv_i2c_write_mlx_reg(st, addr, 0, r_val); + if (result) + return result; + msleep(200); + result = inv_read_mlx_z_axis(st, &meas_coil); + if (result) + return result; + result = inv_write_mlx_cmd(st, 0xD0); + if (result) + return result; + result = inv_write_mlx_reg(st); + if (result) + return result; + diff = abs(meas_ref - meas_coil); + if (diff < 25 || diff > 300) + result = 1; + + /*restore to non-bypass mode */ + result |= inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + + return result; +} + +/* + * inv_setup_compass_mlx() - Configure akm series compass. + */ +static int inv_setup_compass_mlx(struct inv_mpu_state *st) +{ + int result; + int addr; + + addr = st->plat_data.secondary_i2c_addr; + /* set to bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config | BIT_BYPASS_EN); + if (result) + return result; + result = inv_write_mlx_reg(st); + if (result) + return result; + + /*restore to non-bypass mode */ + result = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + if (result) + return result; + + /*setup master mode and master clock and ES bit*/ + result = inv_i2c_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES); + if (result) + return result; + + /* slave 0 used to write read measurement command, write mode */ + result = inv_i2c_single_write(st, REG_I2C_SLV0_ADDR, addr); + if (result) + return result; + /* ignore the register address, send out data only */ + result = inv_i2c_single_write(st, INV_MPU_REG_I2C_SLV0_DO, + DATA_MLX_CMD_READ_MEASURE); + if (result) + return result; + + /* slave 1 used to read status bytes and data of read measurement */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_ADDR, + INV_MPU_BIT_I2C_READ | addr); + if (result) + return result; + /* slave 2 used to write single measurement command, write mode */ + result = inv_i2c_single_write(st, REG_I2C_SLV2_ADDR, addr); + if (result) + return result; + /* ignore the register address, send out data only */ + result = inv_i2c_single_write(st, INV_MPU_REG_I2C_SLV2_DO, + DATA_MLX_CMD_SINGLE_MEASURE); + if (result) + return result; + /* slave 3 used to read status bytes and data of read measurement */ + result = inv_i2c_single_write(st, REG_I2C_SLV3_ADDR, + INV_MPU_BIT_I2C_READ | addr); + + st->slave_compass->scale = DATA_MLX_SCALE; + + return result; +} + +static int inv_suspend_mlx(struct inv_mpu_state *st) +{ + int result; + + result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, 0); + if (result) + return result; + result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, 0); + if (result) + return result; + result = inv_i2c_single_write(st, REG_I2C_SLV2_CTRL, 0); + if (result) + return result; + result = inv_i2c_single_write(st, REG_I2C_SLV3_CTRL, 0); + + return result; +} + +static int inv_resume_mlx(struct inv_mpu_state *st) +{ + int result; + + /* enable, ignore register, write 1 bytes */ + result = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, + INV_MPU_BIT_SLV_EN | + INV_MPU_BIT_REG_DIS | + 1); + if (result) + return result; + + /* enable, ignore register, read 9 bytes */ + result = inv_i2c_single_write(st, REG_I2C_SLV1_CTRL, + INV_MPU_BIT_SLV_EN | + INV_MPU_BIT_REG_DIS | + DATA_MLX_READ_DATA_BYTES); + if (result) + return result; + /* enable, ignore register, write 1 bytes */ + result = inv_i2c_single_write(st, REG_I2C_SLV2_CTRL, + INV_MPU_BIT_SLV_EN | + INV_MPU_BIT_REG_DIS | + 1); + if (result) + return result; + + /* enable, ignore register, read 1 bytes */ + result = inv_i2c_single_write(st, REG_I2C_SLV3_CTRL, + INV_MPU_BIT_SLV_EN | + INV_MPU_BIT_REG_DIS | + 1); + + return result; +} + +static struct inv_mpu_slave slave_akm = { + .suspend = inv_suspend_akm, + .resume = inv_resume_akm, + .get_scale = inv_read_akm_scale, + .set_scale = inv_write_akm_scale, + .self_test = inv_check_akm_self_test, + .setup = inv_setup_compass_akm, + .read_data = inv_akm_read_data, + .rate_scale = AKM_RATE_SCALE, + .min_read_time = DATA_AKM_MIN_READ_TIME, +}; + +static struct inv_mpu_slave slave_mlx90399 = { + .suspend = inv_suspend_mlx, + .resume = inv_resume_mlx, + .get_scale = inv_read_mlx_scale, + .set_scale = inv_write_mlx_scale, + .self_test = inv_check_mlx_self_test, + .setup = inv_setup_compass_mlx, + .read_data = inv_mlx_read_data, + .rate_scale = MLX_RATE_SCALE, + .min_read_time = DATA_MLX_MIN_READ_TIME, +}; + +int inv_mpu_setup_compass_slave(struct inv_mpu_state *st) +{ + switch (st->plat_data.sec_slave_id) { + case COMPASS_ID_AK8975: + case COMPASS_ID_AK8972: + case COMPASS_ID_AK8963: + case COMPASS_ID_AK09911: + st->slave_compass = &slave_akm; + break; + case COMPASS_ID_MLX90399: + st->slave_compass = &slave_mlx90399; + break; + default: + return -EINVAL; + } + + return st->slave_compass->setup(st); +} + diff --git a/drivers/iio/imu/inv_mpu/inv_slave_pressure.c b/drivers/iio/imu/inv_mpu/inv_slave_pressure.c new file mode 100644 index 000000000000..7843556c7325 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_slave_pressure.c @@ -0,0 +1,510 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> + +#include "inv_mpu_iio.h" + +/* Constants */ +#define SHIFT_RIGHT_4_POSITION 4 +#define SHIFT_LEFT_2_POSITION 2 +#define SHIFT_LEFT_4_POSITION 4 +#define SHIFT_LEFT_5_POSITION 5 +#define SHIFT_LEFT_8_POSITION 8 +#define SHIFT_LEFT_12_POSITION 12 +#define SHIFT_LEFT_16_POSITION 16 + +/* Sensor Specific constants */ +#define BMP280_SLEEP_MODE 0x00 +#define BMP280_FORCED_MODE 0x01 +#define BMP280_NORMAL_MODE 0x03 +#define BMP280_SOFT_RESET 0xB6 + +#define BMP280_DELAYTIME_MS_NONE 0 +#define BMP280_DELAYTIME_MS_5 5 +#define BMP280_DELAYTIME_MS_6 6 +#define BMP280_DELAYTIME_MS_8 8 +#define BMP280_DELAYTIME_MS_12 12 +#define BMP280_DELAYTIME_MS_22 22 +#define BMP280_DELAYTIME_MS_38 38 + +#define BMP280_OVERSAMPLING_SKIPPED 0x00 +#define BMP280_OVERSAMPLING_1X 0x01 +#define BMP280_OVERSAMPLING_2X 0x02 +#define BMP280_OVERSAMPLING_4X 0x03 +#define BMP280_OVERSAMPLING_8X 0x04 +#define BMP280_OVERSAMPLING_16X 0x05 + +#define BMP280_ULTRALOWPOWER_MODE 0x00 +#define BMP280_LOWPOWER_MODE 0x01 +#define BMP280_STANDARDRESOLUTION_MODE 0x02 +#define BMP280_HIGHRESOLUTION_MODE 0x03 +#define BMP280_ULTRAHIGHRESOLUTION_MODE 0x04 + +#define BMP280_ULTRALOWPOWER_OSRS_P BMP280_OVERSAMPLING_1X +#define BMP280_ULTRALOWPOWER_OSRS_T BMP280_OVERSAMPLING_1X + +#define BMP280_LOWPOWER_OSRS_P BMP280_OVERSAMPLING_2X +#define BMP280_LOWPOWER_OSRS_T BMP280_OVERSAMPLING_1X + +#define BMP280_STANDARDRESOLUTION_OSRS_P BMP280_OVERSAMPLING_4X +#define BMP280_STANDARDRESOLUTION_OSRS_T BMP280_OVERSAMPLING_1X + +#define BMP280_HIGHRESOLUTION_OSRS_P BMP280_OVERSAMPLING_8X +#define BMP280_HIGHRESOLUTION_OSRS_T BMP280_OVERSAMPLING_1X + +#define BMP280_ULTRAHIGHRESOLUTION_OSRS_P BMP280_OVERSAMPLING_16X +#define BMP280_ULTRAHIGHRESOLUTION_OSRS_T BMP280_OVERSAMPLING_2X + +#define BMP280_FILTERCOEFF_OFF 0x00 +#define BMP280_FILTERCOEFF_2 0x01 +#define BMP280_FILTERCOEFF_4 0x02 +#define BMP280_FILTERCOEFF_8 0x03 +#define BMP280_FILTERCOEFF_16 0x04 + +/*calibration parameters */ +#define BMP280_DIG_T1_LSB_REG 0x88 +#define BMP280_DIG_T1_MSB_REG 0x89 +#define BMP280_DIG_T2_LSB_REG 0x8A +#define BMP280_DIG_T2_MSB_REG 0x8B +#define BMP280_DIG_T3_LSB_REG 0x8C +#define BMP280_DIG_T3_MSB_REG 0x8D +#define BMP280_DIG_P1_LSB_REG 0x8E +#define BMP280_DIG_P1_MSB_REG 0x8F +#define BMP280_DIG_P2_LSB_REG 0x90 +#define BMP280_DIG_P2_MSB_REG 0x91 +#define BMP280_DIG_P3_LSB_REG 0x92 +#define BMP280_DIG_P3_MSB_REG 0x93 +#define BMP280_DIG_P4_LSB_REG 0x94 +#define BMP280_DIG_P4_MSB_REG 0x95 +#define BMP280_DIG_P5_LSB_REG 0x96 +#define BMP280_DIG_P5_MSB_REG 0x97 +#define BMP280_DIG_P6_LSB_REG 0x98 +#define BMP280_DIG_P6_MSB_REG 0x99 +#define BMP280_DIG_P7_LSB_REG 0x9A +#define BMP280_DIG_P7_MSB_REG 0x9B +#define BMP280_DIG_P8_LSB_REG 0x9C +#define BMP280_DIG_P8_MSB_REG 0x9D +#define BMP280_DIG_P9_LSB_REG 0x9E +#define BMP280_DIG_P9_MSB_REG 0x9F + +#define BMP280_CHIPID_REG 0xD0 /*Chip ID Register */ +#define BMP280_RESET_REG 0xE0 /*Softreset Register */ +#define BMP280_STATUS_REG 0xF3 /*Status Register */ +#define BMP280_CTRLMEAS_REG 0xF4 /*Ctrl Measure Register */ +#define BMP280_CONFIG_REG 0xF5 /*Configuration Register */ +#define BMP280_PRESSURE_MSB_REG 0xF7 /*Pressure MSB Register */ +#define BMP280_PRESSURE_LSB_REG 0xF8 /*Pressure LSB Register */ +#define BMP280_PRESSURE_XLSB_REG 0xF9 /*Pressure XLSB Register */ +#define BMP280_TEMPERATURE_MSB_REG 0xFA /*Temperature MSB Reg */ +#define BMP280_TEMPERATURE_LSB_REG 0xFB /*Temperature LSB Reg */ +#define BMP280_TEMPERATURE_XLSB_REG 0xFC /*Temperature XLSB Reg */ + +/* Status Register */ +#define BMP280_STATUS_REG_MEASURING__POS 3 +#define BMP280_STATUS_REG_MEASURING__MSK 0x08 +#define BMP280_STATUS_REG_MEASURING__LEN 1 +#define BMP280_STATUS_REG_MEASURING__REG BMP280_STATUS_REG + +#define BMP280_STATUS_REG_IMUPDATE__POS 0 +#define BMP280_STATUS_REG_IMUPDATE__MSK 0x01 +#define BMP280_STATUS_REG_IMUPDATE__LEN 1 +#define BMP280_STATUS_REG_IMUPDATE__REG BMP280_STATUS_REG + +/* Control Measurement Register */ +#define BMP280_CTRLMEAS_REG_OSRST__POS 5 +#define BMP280_CTRLMEAS_REG_OSRST__MSK 0xE0 +#define BMP280_CTRLMEAS_REG_OSRST__LEN 3 +#define BMP280_CTRLMEAS_REG_OSRST__REG BMP280_CTRLMEAS_REG + +#define BMP280_CTRLMEAS_REG_OSRSP__POS 2 +#define BMP280_CTRLMEAS_REG_OSRSP__MSK 0x1C +#define BMP280_CTRLMEAS_REG_OSRSP__LEN 3 +#define BMP280_CTRLMEAS_REG_OSRSP__REG BMP280_CTRLMEAS_REG + +#define BMP280_CTRLMEAS_REG_MODE__POS 0 +#define BMP280_CTRLMEAS_REG_MODE__MSK 0x03 +#define BMP280_CTRLMEAS_REG_MODE__LEN 2 +#define BMP280_CTRLMEAS_REG_MODE__REG BMP280_CTRLMEAS_REG + +/* Configuation Register */ +#define BMP280_CONFIG_REG_TSB__POS 5 +#define BMP280_CONFIG_REG_TSB__MSK 0xE0 +#define BMP280_CONFIG_REG_TSB__LEN 3 +#define BMP280_CONFIG_REG_TSB__REG BMP280_CONFIG_REG + +#define BMP280_CONFIG_REG_FILTER__POS 2 +#define BMP280_CONFIG_REG_FILTER__MSK 0x1C +#define BMP280_CONFIG_REG_FILTER__LEN 3 +#define BMP280_CONFIG_REG_FILTER__REG BMP280_CONFIG_REG + +#define BMP280_CONFIG_REG_SPI3WEN__POS 0 +#define BMP280_CONFIG_REG_SPI3WEN__MSK 0x01 +#define BMP280_CONFIG_REG_SPI3WEN__LEN 1 +#define BMP280_CONFIG_REG_SPI3WEN__REG BMP280_CONFIG_REG + +/* Data Register */ +#define BMP280_PRESSURE_XLSB_REG_DATA__POS 4 +#define BMP280_PRESSURE_XLSB_REG_DATA__MSK 0xF0 +#define BMP280_PRESSURE_XLSB_REG_DATA__LEN 4 +#define BMP280_PRESSURE_XLSB_REG_DATA__REG BMP280_PRESSURE_XLSB_REG + +#define BMP280_TEMPERATURE_XLSB_REG_DATA__POS 4 +#define BMP280_TEMPERATURE_XLSB_REG_DATA__MSK 0xF0 +#define BMP280_TEMPERATURE_XLSB_REG_DATA__LEN 4 +#define BMP280_TEMPERATURE_XLSB_REG_DATA__REG BMP280_TEMPERATURE_XLSB_REG + +#define BMP280_RATE_SCALE 34 +#define DATA_BMP280_MIN_READ_TIME (32 * NSEC_PER_MSEC) +#define BMP280_DATA_BYTES 6 +#define FAKE_DATA_NUM_BYTES 10 + +/** this structure holds all device specific calibration parameters */ +struct bmp280_calibration_param_t { + u32 dig_T1; + s32 dig_T2; + s32 dig_T3; + u32 dig_P1; + s32 dig_P2; + s32 dig_P3; + s32 dig_P4; + s32 dig_P5; + s32 dig_P6; + s32 dig_P7; + s32 dig_P8; + s32 dig_P9; + + s32 t_fine; +}; +/** BMP280 image registers data structure */ +struct bmp280_t { + struct bmp280_calibration_param_t cal_param; + + u8 chip_id; + u8 dev_addr; + + u8 waittime; + + u8 osrs_t; + u8 osrs_p; +}; +static struct bmp280_t bmp280; + +static int bmp280_get_calib_param(struct inv_mpu_state *st) +{ + u8 d[24]; + int r; + + r = inv_aux_read(BMP280_DIG_T1_LSB_REG, 24, d); + if (r) + return r; + + bmp280.cal_param.dig_T1 = (u16)((((u16)((u8)d[1])) << + SHIFT_LEFT_8_POSITION) | d[0]); + bmp280.cal_param.dig_T2 = (s16)((((s16)((s8)d[3])) << + SHIFT_LEFT_8_POSITION) | d[2]); + bmp280.cal_param.dig_T3 = (s16)((((s16)((s8)d[5])) << + SHIFT_LEFT_8_POSITION) | d[4]); + bmp280.cal_param.dig_P1 = (u16)((((u16)((u8)d[7])) << + SHIFT_LEFT_8_POSITION) | d[6]); + bmp280.cal_param.dig_P2 = (s16)((((s16)((s8)d[9])) << + SHIFT_LEFT_8_POSITION) | d[8]); + bmp280.cal_param.dig_P3 = (s16)((((s16)((s8)d[11])) << + SHIFT_LEFT_8_POSITION) | d[10]); + bmp280.cal_param.dig_P4 = (s16)((((s16)((s8)d[13])) << + SHIFT_LEFT_8_POSITION) | d[12]); + bmp280.cal_param.dig_P5 = (s16)((((s16)((s8)d[15])) << + SHIFT_LEFT_8_POSITION) | d[14]); + bmp280.cal_param.dig_P6 = (s16)((((s16)((s8)d[17])) << + SHIFT_LEFT_8_POSITION) | d[16]); + bmp280.cal_param.dig_P7 = (s16)((((s16)((s8)d[19])) << + SHIFT_LEFT_8_POSITION) | d[18]); + bmp280.cal_param.dig_P8 = (s16)((((s16)((s8)d[21])) << + SHIFT_LEFT_8_POSITION) | d[20]); + bmp280.cal_param.dig_P9 = (s16)((((s16)((s8)d[23])) << + SHIFT_LEFT_8_POSITION) | d[22]); + + return 0; +} + +static int inv_setup_bmp280(struct inv_mpu_state *st) +{ + int r; + u8 d[10]; + + /* set to bypass mode */ + r = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config | BIT_BYPASS_EN); + if (r) + return r; + /* issue soft reset */ + r = inv_aux_write(BMP280_RESET_REG, BMP280_SOFT_RESET); + if (r) + return r; + msleep(100); + r = inv_aux_read(BMP280_CHIPID_REG, 1, d); + if (r) + return r; + /* set pressure as ultra high resolution */ + bmp280.osrs_t = BMP280_ULTRAHIGHRESOLUTION_OSRS_T; + bmp280.osrs_p = BMP280_ULTRAHIGHRESOLUTION_OSRS_P; + + /* set IIR filter as 4 */ + r = inv_aux_write(BMP280_CONFIG_REG_FILTER__REG, + BMP280_FILTERCOEFF_16 << SHIFT_LEFT_2_POSITION); + if (r) + return r; + r = bmp280_get_calib_param(st); + if (r) + return r; + + /*restore to non-bypass mode */ + r = inv_i2c_single_write(st, REG_INT_PIN_CFG, + st->plat_data.int_config); + if (r) + return r; + + /* setup master mode and master clock and ES bit */ + r = inv_i2c_single_write(st, REG_I2C_MST_CTRL, BIT_WAIT_FOR_ES); + if (r) + return r; + /*slave 3 is used for pressure mode change only*/ + r = inv_i2c_single_write(st, REG_I2C_SLV3_ADDR, + st->plat_data.aux_i2c_addr); + if (r) + return r; + /* pressure sensor mode register address */ + r = inv_i2c_single_write(st, REG_I2C_SLV3_REG, BMP280_CTRLMEAS_REG); + if (r) + return r; + d[0] = (bmp280.osrs_t << SHIFT_LEFT_5_POSITION) + + (bmp280.osrs_p << SHIFT_LEFT_2_POSITION) + + BMP280_FORCED_MODE; + r = inv_i2c_single_write(st, INV_MPU_REG_I2C_SLV3_DO, d[0]); + + return r; +} + +static int inv_check_bmp280_self_test(struct inv_mpu_state *st) +{ + return 0; +} +static int inv_write_bmp280_scale(struct inv_mpu_state *st, int data) +{ + return 0; +} +static int inv_read_bmp280_scale(struct inv_mpu_state *st, int *scale) +{ + return 0; +} + +static int inv_resume_bmp280(struct inv_mpu_state *st) +{ + int r; + + if ((!st->sensor[SENSOR_COMPASS].on) && st->chip_config.dmp_on) { + /* if compass is disabled, read fake data for DMP */ + /*read mode */ + r = inv_i2c_single_write(st, REG_I2C_SLV0_ADDR, + INV_MPU_BIT_I2C_READ | + st->plat_data.aux_i2c_addr); + if (r) + return r; + /* read calibration data as the fake data */ + r = inv_i2c_single_write(st, REG_I2C_SLV0_REG, + BMP280_DIG_T1_LSB_REG); + if (r) + return r; + /* slave 0 is enabled, read 10 bytes from here */ + r = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, + INV_MPU_BIT_SLV_EN | + FAKE_DATA_NUM_BYTES); + } + + /* slave 2 is used to read data from pressure sensor */ + /*read mode */ + r = inv_i2c_single_write(st, REG_I2C_SLV2_ADDR, + INV_MPU_BIT_I2C_READ | + st->plat_data.aux_i2c_addr); + if (r) + return r; + /* start from pressure sensor */ + r = inv_i2c_single_write(st, REG_I2C_SLV2_REG, + BMP280_PRESSURE_MSB_REG); + if (r) + return r; + + /* slave 2 is enabled, read 6 bytes from here */ + r = inv_i2c_single_write(st, REG_I2C_SLV2_CTRL, + INV_MPU_BIT_SLV_EN | BMP280_DATA_BYTES); + if (r) + return r; + /* slave 3 is enabled, write byte length is 1 */ + r = inv_i2c_single_write(st, REG_I2C_SLV3_CTRL, + INV_MPU_BIT_SLV_EN | 1); + + return r; +} + +static int inv_suspend_bmp280(struct inv_mpu_state *st) +{ + int r; + + if ((!st->sensor[SENSOR_COMPASS].on) && st->chip_config.dmp_on) { + /* slave 0 is disabled */ + r = inv_i2c_single_write(st, REG_I2C_SLV0_CTRL, 0); + if (r) + return r; + } + + /* slave 2 is disabled */ + r = inv_i2c_single_write(st, REG_I2C_SLV2_CTRL, 0); + if (r) + return r; + /* slave 3 is disabled */ + r = inv_i2c_single_write(st, REG_I2C_SLV3_CTRL, 0); + + return r; +} + +static s32 bmp280_compensate_T_int32(s32 adc_t) +{ + s32 v_x1_u32r = 0; + s32 v_x2_u32r = 0; + s32 temperature = 0; + + v_x1_u32r = ((((adc_t >> 3) - ((s32) + bmp280.cal_param.dig_T1 << 1))) * + ((s32)bmp280.cal_param.dig_T2)) >> 11; + v_x2_u32r = (((((adc_t >> 4) - + ((s32)bmp280.cal_param.dig_T1)) * ((adc_t >> 4) - + ((s32)bmp280.cal_param.dig_T1))) >> 12) * + ((s32)bmp280.cal_param.dig_T3)) >> 14; + bmp280.cal_param.t_fine = v_x1_u32r + v_x2_u32r; + temperature = (bmp280.cal_param.t_fine * 5 + 128) >> 8; + + return temperature; +} + + +static u32 bmp280_compensate_P_int32(s32 adc_p) +{ + s32 v_x1_u32r = 0; + s32 v_x2_u32r = 0; + u32 pressure = 0; + + v_x1_u32r = (((s32)bmp280.cal_param.t_fine) >> 1) - + (s32)64000; + v_x2_u32r = (((v_x1_u32r >> 2) * (v_x1_u32r >> 2)) >> 11) * + ((s32)bmp280.cal_param.dig_P6); + v_x2_u32r = v_x2_u32r + ((v_x1_u32r * + ((s32)bmp280.cal_param.dig_P5)) << 1); + v_x2_u32r = (v_x2_u32r >> 2) + + (((s32)bmp280.cal_param.dig_P4) << 16); + v_x1_u32r = (((bmp280.cal_param.dig_P3 * (((v_x1_u32r >> 2) * + (v_x1_u32r >> 2)) >> 13)) >> 3) + + ((((s32)bmp280.cal_param.dig_P2) * + v_x1_u32r) >> 1)) >> 18; + v_x1_u32r = ((((32768+v_x1_u32r)) * + ((s32)bmp280.cal_param.dig_P1)) >> 15); + /* Avoid exception caused by division by zero */ + if (v_x1_u32r == 0) + return 0; + pressure = (((u32)(((s32)1048576) - adc_p) - + (v_x2_u32r >> 12))) * 3125; + if (pressure < 0x80000000) + pressure = (pressure << 1) / ((u32)v_x1_u32r); + else + pressure = (pressure / (u32)v_x1_u32r) * 2; + v_x1_u32r = (((s32)bmp280.cal_param.dig_P9) * + ((s32)(((pressure >> 3) * (pressure >> 3)) >> 13))) + >> 12; + v_x2_u32r = (((s32)(pressure >> 2)) * + ((s32)bmp280.cal_param.dig_P8)) >> 13; + pressure = (u32)((s32)pressure + + ((v_x1_u32r + v_x2_u32r + bmp280.cal_param.dig_P7) >> 4)); + + return pressure; +} + +static int inv_bmp280_read_data(struct inv_mpu_state *st, short *o) +{ + int r, i; + u8 d[BMP280_DATA_BYTES], reg_addr; + s32 upressure, utemperature; + + if (st->chip_config.dmp_on) { + for (i = 0; i < 6; i++) + d[i] = st->fifo_data[i]; + } else { + if (st->sensor[SENSOR_COMPASS].on) + reg_addr = REG_EXT_SENS_DATA_08; + else + reg_addr = REG_EXT_SENS_DATA_00; + r = inv_i2c_read(st, reg_addr, BMP280_DATA_BYTES, d); + if (r) + return r; + } + /* pressure */ + upressure = (s32)((((s32)(d[0])) + << SHIFT_LEFT_12_POSITION) | (((u32)(d[1])) + << SHIFT_LEFT_4_POSITION) | ((u32)d[2] >> + SHIFT_RIGHT_4_POSITION)); + + /* Temperature */ + utemperature = (s32)((( + (s32) (d[3])) << SHIFT_LEFT_12_POSITION) | + (((u32)(d[4])) << SHIFT_LEFT_4_POSITION) + | ((u32)d[5] >> SHIFT_RIGHT_4_POSITION)); + + bmp280_compensate_T_int32(utemperature); + r = bmp280_compensate_P_int32(upressure); + o[0] = 0; + o[1] = (r >> 16); + o[2] = (r & 0xffff); + + return 0; +} + +static struct inv_mpu_slave slave_bmp280 = { + .suspend = inv_suspend_bmp280, + .resume = inv_resume_bmp280, + .get_scale = inv_read_bmp280_scale, + .set_scale = inv_write_bmp280_scale, + .self_test = inv_check_bmp280_self_test, + .setup = inv_setup_bmp280, + .read_data = inv_bmp280_read_data, + .rate_scale = BMP280_RATE_SCALE, + .min_read_time = DATA_BMP280_MIN_READ_TIME, +}; + +int inv_mpu_setup_pressure_slave(struct inv_mpu_state *st) +{ + switch (st->plat_data.aux_slave_id) { + case PRESSURE_ID_BMP280: + st->slave_pressure = &slave_bmp280; + break; + default: + return -EINVAL; + } + + return st->slave_pressure->setup(st); +} + diff --git a/drivers/iio/imu/inv_mpu/inv_test/Kconfig b/drivers/iio/imu/inv_mpu/inv_test/Kconfig new file mode 100644 index 000000000000..86c30bd8a636 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/Kconfig @@ -0,0 +1,13 @@ +# +# Kconfig for Invensense IIO testing hooks +# + +config INV_TESTING + boolean "Invensense IIO testing hooks" + depends on INV_MPU_IIO || INV_AMI306_IIO || INV_YAS530 || INV_HUB_IIO + default n + help + This flag enables display of additional testing information from the + Invensense IIO drivers. + It also enables the I2C counters facility to perform IO profiling. + Some additional sysfs entries will appear when this flag is enabled. diff --git a/drivers/iio/imu/inv_mpu/inv_test/Makefile b/drivers/iio/imu/inv_mpu/inv_test/Makefile new file mode 100644 index 000000000000..4f0edd3de901 --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/Makefile @@ -0,0 +1,6 @@ +# +# Makefile for Invensense IIO testing hooks. +# + +obj-$(CONFIG_INV_TESTING) += inv_counters.o + diff --git a/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c new file mode 100644 index 000000000000..3b26ca97284f --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.c @@ -0,0 +1,154 @@ +/* + * @file inv_counters.c + * @brief Exports i2c read write counts through sysfs + * + * @version 0.1 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/device.h> +#include <linux/miscdevice.h> +#include <linux/err.h> +#include <linux/sysfs.h> +#include <linux/kdev_t.h> +#include <linux/string.h> +#include <linux/jiffies.h> +#include <linux/spinlock.h> +#include <linux/kernel_stat.h> + +#include "inv_counters.h" + +static int mpu_irq; +static int accel_irq; +static int compass_irq; + +struct inv_counters { + uint32_t i2c_tempreads; + uint32_t i2c_mpureads; + uint32_t i2c_mpuwrites; + uint32_t i2c_accelreads; + uint32_t i2c_accelwrites; + uint32_t i2c_compassreads; + uint32_t i2c_compasswrites; + uint32_t i2c_compassirq; + uint32_t i2c_accelirq; +}; + +static struct inv_counters Counters; + +static ssize_t i2c_counters_show(struct class *cls, + struct class_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%ld.%03ld %u %u %u %u %u %u %u %u %u %u\n", + jiffies / HZ, ((jiffies % HZ) * (1024 / HZ)), + mpu_irq ? kstat_irqs(mpu_irq) : 0, + Counters.i2c_tempreads, + Counters.i2c_mpureads, Counters.i2c_mpuwrites, + accel_irq ? kstat_irqs(accel_irq) : Counters.i2c_accelirq, + Counters.i2c_accelreads, Counters.i2c_accelwrites, + compass_irq ? kstat_irqs(compass_irq) : Counters.i2c_compassirq, + Counters.i2c_compassreads, Counters.i2c_compasswrites); +} + +void inv_iio_counters_set_i2cirq(enum irqtype type, int irq) +{ + switch (type) { + case IRQ_MPU: + mpu_irq = irq; + break; + case IRQ_ACCEL: + accel_irq = irq; + break; + case IRQ_COMPASS: + compass_irq = irq; + break; + } +} +EXPORT_SYMBOL_GPL(inv_iio_counters_set_i2cirq); + +void inv_iio_counters_tempread(int count) +{ + Counters.i2c_tempreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_tempread); + +void inv_iio_counters_mpuread(int count) +{ + Counters.i2c_mpureads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_mpuread); + +void inv_iio_counters_mpuwrite(int count) +{ + Counters.i2c_mpuwrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_mpuwrite); + +void inv_iio_counters_accelread(int count) +{ + Counters.i2c_accelreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelread); + +void inv_iio_counters_accelwrite(int count) +{ + Counters.i2c_accelwrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelwrite); + +void inv_iio_counters_compassread(int count) +{ + Counters.i2c_compassreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compassread); + +void inv_iio_counters_compasswrite(int count) +{ + Counters.i2c_compasswrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compasswrite); + +void inv_iio_counters_compassirq(void) +{ + Counters.i2c_compassirq++; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compassirq); + +void inv_iio_counters_accelirq(void) +{ + Counters.i2c_accelirq++; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelirq); + +static struct class_attribute inv_class_attr[] = { + __ATTR(i2c_counter, S_IRUGO, i2c_counters_show, NULL), + __ATTR_NULL +}; + +static struct class inv_counters_class = { + .name = "inv_counters", + .owner = THIS_MODULE, + .class_attrs = (struct class_attribute *) &inv_class_attr +}; + +static int __init inv_counters_init(void) +{ + memset(&Counters, 0, sizeof(Counters)); + + return class_register(&inv_counters_class); +} + +static void __exit inv_counters_exit(void) +{ + class_unregister(&inv_counters_class); +} + +module_init(inv_counters_init); +module_exit(inv_counters_exit); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("GESL"); +MODULE_DESCRIPTION("inv_counters debug support"); + diff --git a/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h new file mode 100644 index 000000000000..d60dac9d97bd --- /dev/null +++ b/drivers/iio/imu/inv_mpu/inv_test/inv_counters.h @@ -0,0 +1,72 @@ +/* + * @file inv_counters.h + * @brief Debug file to keep track of various counters for the InvenSense + * sensor drivers. + * + * @version 0.1 + */ + +#ifndef _INV_COUNTERS_H_ +#define _INV_COUNTERS_H_ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/err.h> +#include <linux/sysfs.h> +#include <linux/string.h> +#include <linux/jiffies.h> +#include <linux/spinlock.h> + +#ifdef CONFIG_INV_TESTING + +enum irqtype { + IRQ_MPU, + IRQ_ACCEL, + IRQ_COMPASS +}; + +#define INV_I2C_INC_MPUREAD(x) inv_iio_counters_mpuread(x) +#define INV_I2C_INC_MPUWRITE(x) inv_iio_counters_mpuwrite(x) +#define INV_I2C_INC_ACCELREAD(x) inv_iio_counters_accelread(x) +#define INV_I2C_INC_ACCELWRITE(x) inv_iio_counters_accelwrite(x) +#define INV_I2C_INC_COMPASSREAD(x) inv_iio_counters_compassread(x) +#define INV_I2C_INC_COMPASSWRITE(x) inv_iio_counters_compasswrite(x) + +#define INV_I2C_INC_TEMPREAD(x) inv_iio_counters_tempread(x) + +#define INV_I2C_SETIRQ(type, irq) inv_iio_counters_set_i2cirq(type, irq) +#define INV_I2C_INC_COMPASSIRQ() inv_iio_counters_compassirq() +#define INV_I2C_INC_ACCELIRQ() inv_iio_counters_accelirq() + +void inv_iio_counters_mpuread(int count); +void inv_iio_counters_mpuwrite(int count); +void inv_iio_counters_accelread(int count); +void inv_iio_counters_accelwrite(int count); +void inv_iio_counters_compassread(int count); +void inv_iio_counters_compasswrite(int count); + +void inv_iio_counters_tempread(int count); + +void inv_iio_counters_set_i2cirq(enum irqtype type, int irq); +void inv_iio_counters_compassirq(void); +void inv_iio_counters_accelirq(void); + +#else + +#define INV_I2C_INC_MPUREAD(x) +#define INV_I2C_INC_MPUWRITE(x) +#define INV_I2C_INC_ACCELREAD(x) +#define INV_I2C_INC_ACCELWRITE(x) +#define INV_I2C_INC_COMPASSREAD(x) +#define INV_I2C_INC_COMPASSWRITE(x) + +#define INV_I2C_INC_TEMPREAD(x) + +#define INV_I2C_SETIRQ(type, irq) +#define INV_I2C_INC_COMPASSIRQ() +#define INV_I2C_INC_ACCELIRQ() + +#endif /* CONFIG_INV_TESTING */ + +#endif /* _INV_COUNTERS_H_ */ + diff --git a/drivers/iio/inv_test/Kconfig b/drivers/iio/inv_test/Kconfig new file mode 100644 index 000000000000..86c30bd8a636 --- /dev/null +++ b/drivers/iio/inv_test/Kconfig @@ -0,0 +1,13 @@ +# +# Kconfig for Invensense IIO testing hooks +# + +config INV_TESTING + boolean "Invensense IIO testing hooks" + depends on INV_MPU_IIO || INV_AMI306_IIO || INV_YAS530 || INV_HUB_IIO + default n + help + This flag enables display of additional testing information from the + Invensense IIO drivers. + It also enables the I2C counters facility to perform IO profiling. + Some additional sysfs entries will appear when this flag is enabled. diff --git a/drivers/iio/inv_test/Makefile b/drivers/iio/inv_test/Makefile new file mode 100644 index 000000000000..4f0edd3de901 --- /dev/null +++ b/drivers/iio/inv_test/Makefile @@ -0,0 +1,6 @@ +# +# Makefile for Invensense IIO testing hooks. +# + +obj-$(CONFIG_INV_TESTING) += inv_counters.o + diff --git a/drivers/iio/inv_test/inv_counters.c b/drivers/iio/inv_test/inv_counters.c new file mode 100644 index 000000000000..3b26ca97284f --- /dev/null +++ b/drivers/iio/inv_test/inv_counters.c @@ -0,0 +1,154 @@ +/* + * @file inv_counters.c + * @brief Exports i2c read write counts through sysfs + * + * @version 0.1 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/device.h> +#include <linux/miscdevice.h> +#include <linux/err.h> +#include <linux/sysfs.h> +#include <linux/kdev_t.h> +#include <linux/string.h> +#include <linux/jiffies.h> +#include <linux/spinlock.h> +#include <linux/kernel_stat.h> + +#include "inv_counters.h" + +static int mpu_irq; +static int accel_irq; +static int compass_irq; + +struct inv_counters { + uint32_t i2c_tempreads; + uint32_t i2c_mpureads; + uint32_t i2c_mpuwrites; + uint32_t i2c_accelreads; + uint32_t i2c_accelwrites; + uint32_t i2c_compassreads; + uint32_t i2c_compasswrites; + uint32_t i2c_compassirq; + uint32_t i2c_accelirq; +}; + +static struct inv_counters Counters; + +static ssize_t i2c_counters_show(struct class *cls, + struct class_attribute *attr, char *buf) +{ + return scnprintf(buf, PAGE_SIZE, + "%ld.%03ld %u %u %u %u %u %u %u %u %u %u\n", + jiffies / HZ, ((jiffies % HZ) * (1024 / HZ)), + mpu_irq ? kstat_irqs(mpu_irq) : 0, + Counters.i2c_tempreads, + Counters.i2c_mpureads, Counters.i2c_mpuwrites, + accel_irq ? kstat_irqs(accel_irq) : Counters.i2c_accelirq, + Counters.i2c_accelreads, Counters.i2c_accelwrites, + compass_irq ? kstat_irqs(compass_irq) : Counters.i2c_compassirq, + Counters.i2c_compassreads, Counters.i2c_compasswrites); +} + +void inv_iio_counters_set_i2cirq(enum irqtype type, int irq) +{ + switch (type) { + case IRQ_MPU: + mpu_irq = irq; + break; + case IRQ_ACCEL: + accel_irq = irq; + break; + case IRQ_COMPASS: + compass_irq = irq; + break; + } +} +EXPORT_SYMBOL_GPL(inv_iio_counters_set_i2cirq); + +void inv_iio_counters_tempread(int count) +{ + Counters.i2c_tempreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_tempread); + +void inv_iio_counters_mpuread(int count) +{ + Counters.i2c_mpureads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_mpuread); + +void inv_iio_counters_mpuwrite(int count) +{ + Counters.i2c_mpuwrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_mpuwrite); + +void inv_iio_counters_accelread(int count) +{ + Counters.i2c_accelreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelread); + +void inv_iio_counters_accelwrite(int count) +{ + Counters.i2c_accelwrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelwrite); + +void inv_iio_counters_compassread(int count) +{ + Counters.i2c_compassreads += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compassread); + +void inv_iio_counters_compasswrite(int count) +{ + Counters.i2c_compasswrites += count; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compasswrite); + +void inv_iio_counters_compassirq(void) +{ + Counters.i2c_compassirq++; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_compassirq); + +void inv_iio_counters_accelirq(void) +{ + Counters.i2c_accelirq++; +} +EXPORT_SYMBOL_GPL(inv_iio_counters_accelirq); + +static struct class_attribute inv_class_attr[] = { + __ATTR(i2c_counter, S_IRUGO, i2c_counters_show, NULL), + __ATTR_NULL +}; + +static struct class inv_counters_class = { + .name = "inv_counters", + .owner = THIS_MODULE, + .class_attrs = (struct class_attribute *) &inv_class_attr +}; + +static int __init inv_counters_init(void) +{ + memset(&Counters, 0, sizeof(Counters)); + + return class_register(&inv_counters_class); +} + +static void __exit inv_counters_exit(void) +{ + class_unregister(&inv_counters_class); +} + +module_init(inv_counters_init); +module_exit(inv_counters_exit); + +MODULE_LICENSE("GPL"); +MODULE_AUTHOR("GESL"); +MODULE_DESCRIPTION("inv_counters debug support"); + diff --git a/drivers/iio/inv_test/inv_counters.h b/drivers/iio/inv_test/inv_counters.h new file mode 100644 index 000000000000..d60dac9d97bd --- /dev/null +++ b/drivers/iio/inv_test/inv_counters.h @@ -0,0 +1,72 @@ +/* + * @file inv_counters.h + * @brief Debug file to keep track of various counters for the InvenSense + * sensor drivers. + * + * @version 0.1 + */ + +#ifndef _INV_COUNTERS_H_ +#define _INV_COUNTERS_H_ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/err.h> +#include <linux/sysfs.h> +#include <linux/string.h> +#include <linux/jiffies.h> +#include <linux/spinlock.h> + +#ifdef CONFIG_INV_TESTING + +enum irqtype { + IRQ_MPU, + IRQ_ACCEL, + IRQ_COMPASS +}; + +#define INV_I2C_INC_MPUREAD(x) inv_iio_counters_mpuread(x) +#define INV_I2C_INC_MPUWRITE(x) inv_iio_counters_mpuwrite(x) +#define INV_I2C_INC_ACCELREAD(x) inv_iio_counters_accelread(x) +#define INV_I2C_INC_ACCELWRITE(x) inv_iio_counters_accelwrite(x) +#define INV_I2C_INC_COMPASSREAD(x) inv_iio_counters_compassread(x) +#define INV_I2C_INC_COMPASSWRITE(x) inv_iio_counters_compasswrite(x) + +#define INV_I2C_INC_TEMPREAD(x) inv_iio_counters_tempread(x) + +#define INV_I2C_SETIRQ(type, irq) inv_iio_counters_set_i2cirq(type, irq) +#define INV_I2C_INC_COMPASSIRQ() inv_iio_counters_compassirq() +#define INV_I2C_INC_ACCELIRQ() inv_iio_counters_accelirq() + +void inv_iio_counters_mpuread(int count); +void inv_iio_counters_mpuwrite(int count); +void inv_iio_counters_accelread(int count); +void inv_iio_counters_accelwrite(int count); +void inv_iio_counters_compassread(int count); +void inv_iio_counters_compasswrite(int count); + +void inv_iio_counters_tempread(int count); + +void inv_iio_counters_set_i2cirq(enum irqtype type, int irq); +void inv_iio_counters_compassirq(void); +void inv_iio_counters_accelirq(void); + +#else + +#define INV_I2C_INC_MPUREAD(x) +#define INV_I2C_INC_MPUWRITE(x) +#define INV_I2C_INC_ACCELREAD(x) +#define INV_I2C_INC_ACCELWRITE(x) +#define INV_I2C_INC_COMPASSREAD(x) +#define INV_I2C_INC_COMPASSWRITE(x) + +#define INV_I2C_INC_TEMPREAD(x) + +#define INV_I2C_SETIRQ(type, irq) +#define INV_I2C_INC_COMPASSIRQ() +#define INV_I2C_INC_ACCELIRQ() + +#endif /* CONFIG_INV_TESTING */ + +#endif /* _INV_COUNTERS_H_ */ + diff --git a/drivers/iio/magnetometer/inv_compass/Kconfig b/drivers/iio/magnetometer/inv_compass/Kconfig new file mode 100644 index 000000000000..b5c8c44fd7ed --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/Kconfig @@ -0,0 +1,36 @@ +# +# Kconfig for Invensense IIO compass drivers of 3rd party compass devices. +# + +# Yamaha YAS530/YAS532/YAS533 +config INV_YAS53X_IIO + tristate "Invensense IIO driver for Yamaha YAS530/YAS532/YAS533 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Yamaha YAS530/YAS532/YAS533. It is the Invensense + implementation of YAS53x series compass devices. + This driver can be built as a module. The module will be called + inv_yas53x_iio. + +# Aichi AMI306 +config INV_AMI306_IIO + tristate "Invensense IIO driver for Aichi AMI306 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Aichi AMI306 compass. It is the Invensense + IIO implementation for the AMI306 compass device. + This driver can be built as a module. The module will be called + inv-ami306-iio. + +# Asahi Kasei AK8975/AK8972/AK8963 +config INV_AK89XX_IIO + tristate "Invensense IIO driver for Asahi Kasei AK8975/AK8972/AK8963 compass" + depends on I2C && SYSFS && IIO && IIO_KFIFO_BUF + default n + help + This driver supports the Asahi Kasei AK8975/AK8972/AK8963 compasses. It is the Invensense + IIO implementation of AK89xx series compass devices. + This driver can be built as a module. The module will be called + inv-ak89xx-iio.
\ No newline at end of file diff --git a/drivers/iio/magnetometer/inv_compass/Makefile b/drivers/iio/magnetometer/inv_compass/Makefile new file mode 100644 index 000000000000..47c4271d3744 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/Makefile @@ -0,0 +1,36 @@ +# +# Makefile for Invensense IIO compass drivers of 3rd party compass devices. +# + +# Yamaha YAS530/YAS532/YAS533 +obj-$(CONFIG_INV_YAS53X_IIO) += inv_yas53x.o + +inv_yas53x-objs := inv_yas53x_core.o +inv_yas53x-objs += inv_yas53x_ring.o +inv_yas53x-objs += inv_yas53x_trigger.o + +CFLAGS_inv_yas53x_core.o += -Idrivers/staging/iio +CFLAGS_inv_yas53x_ring.o += -Idrivers/staging/iio +CFLAGS_inv_yas53x_trigger.o += -Idrivers/staging/iio + +# Aichi AMI306 +obj-$(CONFIG_INV_AMI306_IIO) += inv-ami306-iio.o + +inv-ami306-iio-objs := inv_ami306_core.o +inv-ami306-iio-objs += inv_ami306_ring.o +inv-ami306-iio-objs += inv_ami306_trigger.o + +CFLAGS_inv_ami306_core.o += -Idrivers/staging/iio +CFLAGS_inv_ami306_ring.o += -Idrivers/staging/iio +CFLAGS_inv_ami306_trigger.o += -Idrivers/staging/iio + +# Asahi Kasei AK8975/AK8972/AK8963 +obj-$(CONFIG_INV_AK89XX_IIO) += inv-ak89xx-iio.o + +inv-ak89xx-iio-objs := inv_ak89xx_core.o +inv-ak89xx-iio-objs += inv_ak89xx_ring.o +inv-ak89xx-iio-objs += inv_ak89xx_trigger.o + +CFLAGS_inv_ak89xx_core.o += -Idrivers/staging/iio +CFLAGS_inv_ak89xx_ring.o += -Idrivers/staging/iio +CFLAGS_inv_ak89xx_trigger.o += -Idrivers/staging/iio
\ No newline at end of file diff --git a/drivers/iio/magnetometer/inv_compass/README b/drivers/iio/magnetometer/inv_compass/README new file mode 100644 index 000000000000..54f2bb8ded2e --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/README @@ -0,0 +1,176 @@ +Kernel driver +Author: Invensense <http://invensense.com> + +Table of Contents: +================== +- Description +- Integrating the Driver in the Linux Kernel +- Board and Platform Data + > Platform Data +- Board File Modifications for compass + > AMI306 + > YAS530/532/533 +- IIO Subsystem + > Communicating with the Driver in Userspace +- Streaming Data to an Userspace Application +- Test Applications + > Running Test Applications with AMI306 or YAS53x + +Description +=========== +This document describes how to install the Invensense device driver for AMI306 +and YAS53x series compass chip into a Linux kernel. The Invensense driver +currently supports the following sensors: +- AMI306 +- YAS530 +- YAS532 +- YAS533 + +Please refer to the appropriate product specification +document for further information regarding the slave address. + +The following files are included in this package: +- Kconfig +- Makefile +- inv_ami306_core.c +- inv_ami306_ring.c +- inv_ami306_trigger.c +- inv_ami306_iio.h +- inv_yas53x_core.c +- inv_yas53x_ring.c +- inv_yas53x_trigger.c +- inv_yas53x_iio.h + +Integrating the Driver in the Linux Kernel +========================================== +Please add the files as follows: +- Add all above files to drivers/staging/iio/magnetometer/inv_compass +(another directory is acceptable, but this is the recommended destination) + +In order to see the driver in menuconfig when building the kernel, please +make modifications as shown below: + + modify "drivers/staging/iio/magnetometer/Kconfig" with: + >> source "drivers/staging/iio/magnetometer/inv_compass/Kconfig" + + modify "drivers/staging/iio/magnetometer/Makefile" with: + >> obj-y += inv_compass/ + + +Board and Platform Data +======================= +In order to recognize the Invensense device on the I2C bus, the board file must +be modified. +The i2c_board_info instance must be defined as shown below. + +Platform Data +------------- +The platform data (orientation matrix and secondary bus configurations) must be +modified as show below, according to your particular platform configuration. + +Board File Modifications for Secondary I2C Configuration +======================================================== +For the Panda Board, the board file can be found at +arch/arm/mach-omap2/board-omap4panda.c. +Please modify the pertinent baord file in your system according to the examples +shown below: + +AMI306 +------------------------------------------------- +static struct mpu_platform_data compass_data = { + .orientation = { 0, 0, 1, + 0, 1, 0, + 1, 0, 0 }, +}; + +static struct i2c_board_info __initdata chip_board_info[] = { + { + I2C_BOARD_INFO("ami306", 0x0E), + .platform_data = &compass_data, + }, +}; + +YAS53x(Use YAS532 as an example) +------------------------------------------------- +static struct mpu_platform_data compass_data = { + .orientation = { 0, -1, 0, + 1, 0, 0, + 0, 0, 1 }, +}; + +static struct i2c_board_info __initdata compass_board_info[] = { + { + I2C_BOARD_INFO("yas532", 0x2E), + .platform_data = &compass_data, + }, +}; + +IIO subsystem +============= +A successful installation will create the following two new directories under +/sys/bus/iio/devices: + - iio:device0 + - trigger0 + +Also, a new file, "iio:device0", will be created in the /dev/ diretory. +(if you have more than one IIO device, the file will be named "iio:deviceX", +where X is a number) + + +Communicating with the Driver in Userspace +------------------------------------------ +The driver generates several files in sysfs upon installation. +These files are used to communicate with the driver. The files can be found +at /sys/bus/iio/devices/iio:device0 (or ../iio:deviceX as shown above). + +A brief description of the pertinent files for each Invensense device is shown +below: + +AMI306 +-------- +compass_matrix (read-only) +--show the orientation matrix obtained from the board file. + +sampling_frequency(read and write) +--show and change the sampling rate of the sensor. + +YAS53x +--------------------- +YAS53x has all the attributes AMI306 has. It has one more additional attribute: + +overunderflow(read-only) +--value 1 shows an overflow or underflow happens. Need to write into it to make + it zero. + +Streaming Data to an Userspace Application +========================================== +When streaming data to an userspace application, we recommend that you access +compass data via /dev/iio:device0. + +Please follow the steps below to read data at a constant rate from the driver: + +1. Write the desired output rate to sampling_frequency. +2. Write 1 to enable to turn on the event. +3. Read /dev/iio:device0 to get a string of gyro/accel/compass data. +4. Parse this string to obtain each compass element. + +Test Applications +================= +A test application is located under software/simple_apps/mpu_iio. +This application is stand-alone in that it cannot be run concurrently with other +entities trying to access the device node(s) or sysfs entries; in particular, +the + +Running Test Applications +--------------------------------------------------------- +To run test applications with AMI306 or YAS53x devices, +please use the following commands: + +1. for ami306: + mpu_iio -n ami306 -c 10 -l 3 + +2. for yas532: + mpu_iio -n yas532 -c 10 -l 3 + +Please use mpu_iio.c and iio_utils.h as example code for your development +purposes. diff --git a/drivers/iio/magnetometer/inv_compass/inv_ak89xx_core.c b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_core.c new file mode 100644 index 000000000000..a781fd398225 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_core.c @@ -0,0 +1,590 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_ak89xx_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +static s64 get_time_ns(void) +{ + struct timespec ts; + ktime_get_ts(&ts); + return timespec_to_ns(&ts); +} + +/** + * inv_serial_read() - Read one or more bytes from the device registers. + * @st: Device driver instance. + * @reg: First device register to be read from. + * @length: Number of bytes to read. + * @data: Data read from device. + * NOTE: The slave register will not increment when reading from the FIFO. + */ +int inv_serial_read(struct inv_ak89xx_state_s *st, u8 reg, u16 length, u8 *data) +{ + int result; + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + result = i2c_smbus_read_i2c_block_data(st->i2c, reg, length, data); + if (result != length) { + if (result < 0) + return result; + else + return -EINVAL; + } else { + return 0; + } +} + +/** + * inv_serial_single_write() - Write a byte to a device register. + * @st: Device driver instance. + * @reg: Device register to be written to. + * @data: Byte to write to device. + */ +int inv_serial_single_write(struct inv_ak89xx_state_s *st, u8 reg, u8 data) +{ + u8 d[1]; + d[0] = data; + INV_I2C_INC_COMPASSWRITE(3); + + return i2c_smbus_write_i2c_block_data(st->i2c, reg, 1, d); +} + +static int ak89xx_init(struct inv_ak89xx_state_s *st) +{ + int result = 0; + unsigned char serial_data[3]; + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_POWER_DOWN); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + /* Wait at least 100us */ + udelay(100); + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_FUSE_ACCESS); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + /* Wait at least 200us */ + udelay(200); + + result = inv_serial_read(st, AK89XX_FUSE_ASAX, 3, serial_data); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + st->asa[0] = serial_data[0]; + st->asa[1] = serial_data[1]; + st->asa[2] = serial_data[2]; + + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + AK89XX_CNTL_MODE_POWER_DOWN); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + udelay(100); + + return result; +} + +int ak89xx_read(struct inv_ak89xx_state_s *st, short rawfixed[3]) +{ + unsigned char regs[8]; + unsigned char *stat = ®s[0]; + unsigned char *stat2 = ®s[7]; + int result = 0; + int status = 0; + + result = inv_serial_read(st, AK89XX_REG_ST1, 8, regs); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + + rawfixed[0] = (short)((regs[2]<<8) | regs[1]); + rawfixed[1] = (short)((regs[4]<<8) | regs[3]); + rawfixed[2] = (short)((regs[6]<<8) | regs[5]); + + /* + * ST : data ready - + * Measurement has been completed and data is ready to be read. + */ + if (*stat & 0x01) + status = 0; + + /* + * ST2 : data error - + * occurs when data read is started outside of a readable period; + * data read would not be correct. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour but we + * stil account for it and return an error, since the data would be + * corrupted. + * DERR bit is self-clearing when ST2 register is read. + */ + if (*stat2 & 0x04) + status = 0x04; + /* + * ST2 : overflow - + * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. + * This is likely to happen in presence of an external magnetic + * disturbance; it indicates, the sensor data is incorrect and should + * be ignored. + * An error is returned. + * HOFL bit clears when a new measurement starts. + */ + if (*stat2 & 0x08) + status = 0x08; + /* + * ST : overrun - + * the previous sample was not fetched and lost. + * Valid in continuous measurement mode only. + * In single measurement mode this error should not occour and we + * don't consider this condition an error. + * DOR bit is self-clearing when ST2 or any meas. data register is + * read. + */ + if (*stat & 0x02) { + /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */ + status = 0; + } + + /* + * trigger next measurement if: + * - stat is non zero; + * - if stat is zero and stat2 is non zero. + * Won't trigger if data is not ready and there was no error. + */ + if (1) { + unsigned char scale = 0; + if (st->compass_id == COMPASS_ID_AK8963) + scale = st->compass_scale; + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE); + if (result) { + pr_err("%s, line=%d\n", __func__, __LINE__); + return result; + } + } else + pr_err("%s, no next measure(0x%x,0x%x)\n", __func__, + *stat, *stat2); + + if (status) + pr_err("%s, line=%d, status=%d\n", __func__, __LINE__, status); + + return status; +} + +/** + * ak89xx_read_raw() - read raw method. + */ +static int ak89xx_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int scale = 0; + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + if (st->compass_id == COMPASS_ID_AK8975) + scale = 9830; + else if (st->compass_id == COMPASS_ID_AK8972) + scale = 19661; + else if (st->compass_id == COMPASS_ID_AK8963) { + if (st->compass_scale) + scale = 4915; /* 16 bit */ + else + scale = 19661; /* 14 bit */ + } + scale *= (1L << 15); + *val = scale; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +static ssize_t ak89xx_value_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + short c[3]; + + mutex_lock(&indio_dev->mlock); + c[0] = st->compass_data[0]; + c[1] = st->compass_data[1]; + c[2] = st->compass_data[2]; + mutex_unlock(&indio_dev->mlock); + return sprintf(buf, "%d, %d, %d\n", c[0], c[1], c[2]); +} + +static ssize_t ak89xx_scale_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int scale = 0; + + if (st->compass_id == COMPASS_ID_AK8975) + scale = 9830; + else if (st->compass_id == COMPASS_ID_AK8972) + scale = 19661; + else if (st->compass_id == COMPASS_ID_AK8963) { + if (st->compass_scale) + scale = 4915; /* 16 bit */ + else + scale = 19661; /* 14 bit */ + } + scale *= (1L << 15); + return sprintf(buf, "%d\n", scale); +} + +static ssize_t ak89xx_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", (1000 / st->delay)); +} + +/** + * ak89xx_matrix_show() - show orientation matrix + */ +static ssize_t ak89xx_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + int result = 0; + unsigned char scale = 0; + + if (st->compass_id == COMPASS_ID_AK8963) + scale = st->compass_scale; + + if (enable) { + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE); + if (result) + pr_err("%s, line=%d\n", __func__, __LINE__); + schedule_delayed_work(&st->work, + msecs_to_jiffies(st->delay)); + } else { + cancel_delayed_work_sync(&st->work); + result = inv_serial_single_write(st, AK89XX_REG_CNTL, + (scale << 4) | AK89XX_CNTL_MODE_POWER_DOWN); + if (result) + pr_err("%s, line=%d\n", __func__, __LINE__); + mdelay(1); /* wait at least 100us */ + } +} + +static ssize_t ak89xx_scale_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + unsigned long data, result; + + result = kstrtoul(buf, 10, &data); + if (result) + return result; + if (st->compass_id == COMPASS_ID_AK8963) + st->compass_scale = !!data; + return count; +} + +static ssize_t ak89xx_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + error = kstrtoul(buf, 10, &data); + if (error) + return error; + /* transform rate to delay in ms */ + data = 1000 / data; + if (data > AK89XX_MAX_DELAY) + data = AK89XX_MAX_DELAY; + if (data < AK89XX_MIN_DELAY) + data = AK89XX_MIN_DELAY; + st->delay = (unsigned int) data; + return count; +} + +static void ak89xx_work_func(struct work_struct *work) +{ + struct inv_ak89xx_state_s *st = + container_of((struct delayed_work *)work, + struct inv_ak89xx_state_s, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + unsigned long delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + st->timestamp = get_time_ns(); + schedule_delayed_work(&st->work, delay); + inv_read_ak89xx_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AK89XX_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_AK89XX_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(value, S_IRUGO, ak89xx_value_show, NULL); +static DEVICE_ATTR(scale, S_IRUGO | S_IWUSR, ak89xx_scale_show, + ak89xx_scale_store); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, ak89xx_rate_show, + ak89xx_rate_store); +static DEVICE_ATTR(compass_matrix, S_IRUGO, ak89xx_matrix_show, NULL); + +static struct attribute *inv_ak89xx_attributes[] = { + &dev_attr_value.attr, + &dev_attr_scale.attr, + &dev_attr_sampling_frequency.attr, + &dev_attr_compass_matrix.attr, + NULL, +}; + +static const struct attribute_group inv_attribute_group = { + .name = "ak89xx", + .attrs = inv_ak89xx_attributes +}; + +static const struct iio_info ak89xx_info = { + .driver_module = THIS_MODULE, + .read_raw = &ak89xx_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_ak89xx_probe() - probe function. + */ +static int inv_ak89xx_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_ak89xx_state_s *st; + struct iio_dev *indio_dev; + int result; + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->i2c = client; + st->sl_handle = client->adapter; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->i2c_addr = client->addr; + st->delay = AK89XX_DEFAULT_DELAY; + st->compass_id = id->driver_data; + st->compass_scale = 0; + + i2c_set_clientdata(client, indio_dev); + result = ak89xx_init(st); + if (result) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &ak89xx_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_ak89xx_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_ak89xx_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, ak89xx_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_ak89xx_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_ak89xx_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_ak89xx_remove() - remove function. + */ +static int inv_ak89xx_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_ak89xx_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_ak89xx_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv-ak89xx-iio module removed.\n"); + return 0; +} + +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; + +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_ak89xx_id[] = { + {"akm8975", COMPASS_ID_AK8975}, + {"akm8972", COMPASS_ID_AK8972}, + {"akm8963", COMPASS_ID_AK8963}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_ak89xx_id); + +static struct i2c_driver inv_ak89xx_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_ak89xx_probe, + .remove = inv_ak89xx_remove, + .id_table = inv_ak89xx_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-ak89xx-iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_ak89xx_init(void) +{ + int result = i2c_add_driver(&inv_ak89xx_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_ak89xx_exit(void) +{ + i2c_del_driver(&inv_ak89xx_driver); +} + +module_init(inv_ak89xx_init); +module_exit(inv_ak89xx_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv-ak89xx-iio"); + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ak89xx_iio.h b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_iio.h new file mode 100644 index 000000000000..9a9f14a73ec5 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_iio.h @@ -0,0 +1,144 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#ifndef _INV_AK89XX_IIO_H_ +#define _INV_AK89XX_IIO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +/** + * struct inv_ak89xx_state_s - Driver state variables. + * @plat_data: board file platform data. + * @i2c: i2c client handle. + * @trig: not used. for compatibility. + * @work: work data structure. + * @delay: delay between each scheduled work. + * @dev: Represents read-only node for accessing buffered data. + * @inv_dev: Handle to input device. + * @sl_handle: Handle to I2C port. + */ +struct inv_ak89xx_state_s { + struct mpu_platform_data plat_data; + struct i2c_client *i2c; + struct iio_trigger *trig; + struct delayed_work work; + int delay; /* msec */ + unsigned char compass_id; + int compass_scale; /* for ak8963, 1:16-bit, 0:14-bit */ + short compass_data[3]; + u8 asa[3]; /* axis sensitivity adjustment */ + s64 timestamp; + short i2c_addr; + void *sl_handle; + struct device *inv_dev; + struct input_dev *idev; +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_AK89XX_SCAN_MAGN_X, + INV_AK89XX_SCAN_MAGN_Y, + INV_AK89XX_SCAN_MAGN_Z, + INV_AK89XX_SCAN_TIMESTAMP, +}; + +#define AK89XX_I2C_NAME "ak89xx" + +#define SENSOR_DATA_SIZE 8 +#define YPR_DATA_SIZE 12 +#define RWBUF_SIZE 16 + +#define ACC_DATA_FLAG 0 +#define MAG_DATA_FLAG 1 +#define ORI_DATA_FLAG 2 +#define AKM_NUM_SENSORS 3 + +#define ACC_DATA_READY (1 << (ACC_DATA_FLAG)) +#define MAG_DATA_READY (1 << (MAG_DATA_FLAG)) +#define ORI_DATA_READY (1 << (ORI_DATA_FLAG)) + +#define AKM_MINOR_NUMBER 254 + +/*! \name AK89XX constant definition + \anchor AK89XX_Def + Constant definitions of the AK89XX.*/ +#define AK89XX_MEASUREMENT_TIME_US 10000 + +/*! \name AK89XX operation mode + \anchor AK89XX_Mode + Defines an operation mode of the AK89XX.*/ +/*! @{*/ +#define AK89XX_CNTL_MODE_SNG_MEASURE 0x01 +#define AK89XX_CNTL_MODE_SELF_TEST 0x08 +#define AK89XX_CNTL_MODE_FUSE_ACCESS 0x0F +#define AK89XX_CNTL_MODE_POWER_DOWN 0x00 +/*! @}*/ + +/*! \name AK89XX register address +\anchor AK89XX_REG +Defines a register address of the AK89XX.*/ +/*! @{*/ +#define AK89XX_REG_WIA 0x00 +#define AK89XX_REG_INFO 0x01 +#define AK89XX_REG_ST1 0x02 +#define AK89XX_REG_HXL 0x03 +#define AK89XX_REG_HXH 0x04 +#define AK89XX_REG_HYL 0x05 +#define AK89XX_REG_HYH 0x06 +#define AK89XX_REG_HZL 0x07 +#define AK89XX_REG_HZH 0x08 +#define AK89XX_REG_ST2 0x09 +#define AK89XX_REG_CNTL 0x0A +#define AK89XX_REG_RSV 0x0B +#define AK89XX_REG_ASTC 0x0C +#define AK89XX_REG_TS1 0x0D +#define AK89XX_REG_TS2 0x0E +#define AK89XX_REG_I2CDIS 0x0F +/*! @}*/ + +/*! \name AK89XX fuse-rom address +\anchor AK89XX_FUSE +Defines a read-only address of the fuse ROM of the AK89XX.*/ +/*! @{*/ +#define AK89XX_FUSE_ASAX 0x10 +#define AK89XX_FUSE_ASAY 0x11 +#define AK89XX_FUSE_ASAZ 0x12 +/*! @}*/ + +#define AK89XX_MAX_DELAY (200) +#define AK89XX_MIN_DELAY (10) +#define AK89XX_DEFAULT_DELAY (100) + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + +int inv_ak89xx_configure_ring(struct iio_dev *indio_dev); +void inv_ak89xx_unconfigure_ring(struct iio_dev *indio_dev); +int inv_ak89xx_probe_trigger(struct iio_dev *indio_dev); +void inv_ak89xx_remove_trigger(struct iio_dev *indio_dev); +void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable); +int ak89xx_read_raw_data(struct inv_ak89xx_state_s *st, + short dat[3]); +void inv_read_ak89xx_fifo(struct iio_dev *indio_dev); +int ak89xx_read(struct inv_ak89xx_state_s *st, short rawfixed[3]); + +#endif + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ak89xx_ring.c b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_ring.c new file mode 100644 index 000000000000..a8c1090bed5b --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_ring.c @@ -0,0 +1,138 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_ak89xx_iio.h" + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) +{ + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + + return d_ind; +} + +/** + * inv_read_ak89xx_fifo() - Transfer data from FIFO to ring buffer. + */ +void inv_read_ak89xx_fifo(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int d_ind; + s8 *tmp; + s64 tmp_buf[2]; + + if (!ak89xx_read(st, st->compass_data)) { + st->compass_data[0] = (short)(((int)st->compass_data[0] * (st->asa[0] + 128)) >> 8); + st->compass_data[1] = (short)(((int)st->compass_data[1] * (st->asa[1] + 128)) >> 8); + st->compass_data[2] = (short)(((int)st->compass_data[2] * (st->asa[2] + 128)) >> 8); + tmp = (u8 *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_AK89XX_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7)/8] = st->timestamp; + ring->access->store_to(indio_dev->buffer, tmp, st->timestamp); + } +} + +void inv_ak89xx_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; + +static int inv_ak89xx_postenable(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_AK89XX_SCAN_MAGN_Z))) + return 0; + + set_ak89xx_enable(indio_dev, true); + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_ak89xx_predisable(struct iio_dev *indio_dev) +{ + struct iio_buffer *ring = indio_dev->buffer; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_AK89XX_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_AK89XX_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_AK89XX_SCAN_MAGN_Z, ring->scan_mask); + set_ak89xx_enable(indio_dev, false); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_ak89xx_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_ak89xx_postenable, + .predisable = &inv_ak89xx_predisable, +}; + +int inv_ak89xx_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_ak89xx_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ak89xx_trigger.c b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_trigger.c new file mode 100644 index 000000000000..04c77ab5c79d --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ak89xx_trigger.c @@ -0,0 +1,75 @@ +/* +* Copyright (C) 2013 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +*/ +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" +#include "inv_ak89xx_iio.h" + +static const struct iio_trigger_ops inv_ak89xx_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_ak89xx_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->i2c->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_ak89xx_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_ak89xx_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_ak89xx_state_s *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ami306_core.c b/drivers/iio/magnetometer/inv_compass/inv_ami306_core.c new file mode 100644 index 000000000000..612ba72b59e9 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ami306_core.c @@ -0,0 +1,570 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_core.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_ami306_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +static unsigned char late_initialize = true; + +s32 i2c_write(const struct i2c_client *client, + u8 command, u8 length, const u8 *values) +{ + INV_I2C_INC_COMPASSWRITE(3); + return i2c_smbus_write_i2c_block_data(client, command, length, values); +} + +s32 i2c_read(const struct i2c_client *client, + u8 command, u8 length, u8 *values) +{ + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + return i2c_smbus_read_i2c_block_data(client, command, length, values); +} + +static int ami306_read_param(struct inv_ami306_state_s *st) +{ + int result = 0; + unsigned char regs[AMI_PARAM_LEN]; + struct ami_sensor_parametor *param = &st->param; + + result = i2c_read(st->i2c, REG_AMI_SENX, + AMI_PARAM_LEN, regs); + if (result < 0) + return result; + + /* Little endian 16 bit registers */ + param->m_gain.x = le16_to_cpup((__le16 *)(®s[0])); + param->m_gain.y = le16_to_cpup((__le16 *)(®s[2])); + param->m_gain.z = le16_to_cpup((__le16 *)(®s[4])); + + param->m_interference.xy = regs[7]; + param->m_interference.xz = regs[6]; + param->m_interference.yx = regs[9]; + param->m_interference.yz = regs[8]; + param->m_interference.zx = regs[11]; + param->m_interference.zy = regs[10]; + + param->m_offset.x = AMI_STANDARD_OFFSET; + param->m_offset.y = AMI_STANDARD_OFFSET; + param->m_offset.z = AMI_STANDARD_OFFSET; + + param->m_gain_cor.x = AMI_GAIN_COR_DEFAULT; + param->m_gain_cor.y = AMI_GAIN_COR_DEFAULT; + param->m_gain_cor.z = AMI_GAIN_COR_DEFAULT; + + return 0; +} + +static int ami306_write_offset(const struct i2c_client *client, + unsigned char *fine) +{ + int result = 0; + unsigned char dat[3]; + dat[0] = (0x7f & fine[0]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFX, 2, dat); + dat[0] = (0x7f & fine[1]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFY, 2, dat); + dat[0] = (0x7f & fine[2]); + dat[1] = 0; + result = i2c_write(client, REG_AMI_OFFZ, 2, dat); + + return result; +} + +static int ami306_wait_data_ready(struct inv_ami306_state_s *st, + unsigned long usecs, unsigned long times) +{ + int result = 0; + unsigned char buf; + + for (; 0 < times; --times) { + udelay(usecs); + result = i2c_read(st->i2c, REG_AMI_STA1, 1, &buf); + if (result < 0) + return INV_ERROR_COMPASS_DATA_NOT_READY; + if (buf & AMI_STA1_DRDY_BIT) + return 0; + else if (buf & AMI_STA1_DOR_BIT) + return INV_ERROR_COMPASS_DATA_OVERFLOW; + } + + return INV_ERROR_COMPASS_DATA_NOT_READY; +} +int ami306_read_raw_data(struct inv_ami306_state_s *st, + short dat[3]) +{ + int result; + unsigned char buf[6]; + result = i2c_read(st->i2c, REG_AMI_DATAX, sizeof(buf), buf); + if (result < 0) + return result; + dat[0] = le16_to_cpup((__le16 *)(&buf[0])); + dat[1] = le16_to_cpup((__le16 *)(&buf[2])); + dat[2] = le16_to_cpup((__le16 *)(&buf[4])); + + return 0; +} + +#define AMI_WAIT_DATAREADY_RETRY 3 /* retry times */ +#define AMI_DRDYWAIT 800 /* u(micro) sec */ +static int ami306_force_measurement(struct inv_ami306_state_s *st, + short ver[3]) +{ + int result; + int status; + char buf; + buf = AMI_CTRL3_FORCE_BIT; + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, &buf); + if (result < 0) + return result; + + result = ami306_wait_data_ready(st, + AMI_DRDYWAIT, AMI_WAIT_DATAREADY_RETRY); + if (result && result != INV_ERROR_COMPASS_DATA_OVERFLOW) + return result; + /* READ DATA X,Y,Z */ + status = ami306_read_raw_data(st, ver); + if (status) + return status; + + return result; +} + +static int ami306_initial_b0_adjust(struct inv_ami306_state_s *st) +{ + int result; + unsigned char fine[3] = { 0 }; + short data[3]; + int diff[3] = { 0x7fff, 0x7fff, 0x7fff }; + int fn = 0; + int ax = 0; + unsigned char buf[3]; + + buf[0] = AMI_CTRL2_DREN; + result = i2c_write(st->i2c, REG_AMI_CTRL2, 1, buf); + if (result) + return result; + + buf[0] = AMI_CTRL4_HS & 0xFF; + buf[1] = (AMI_CTRL4_HS >> 8) & 0xFF; + result = i2c_write(st->i2c, REG_AMI_CTRL4, 2, buf); + if (result < 0) + return result; + + for (fn = 0; fn < AMI_FINE_MAX; ++fn) { /* fine 0 -> 95 */ + fine[0] = fine[1] = fine[2] = fn; + result = ami306_write_offset(st->i2c, fine); + if (result) + return result; + + result = ami306_force_measurement(st, data); + if (result) + return result; + + for (ax = 0; ax < 3; ax++) { + /* search point most close to zero. */ + if (diff[ax] > abs(data[ax])) { + st->fine[ax] = fn; + diff[ax] = abs(data[ax]); + } + } + } + result = ami306_write_offset(st->i2c, st->fine); + if (result) + return result; + + /* Software Reset */ + buf[0] = AMI_CTRL3_SRST_BIT; + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, buf); + if (result < 0) + return result; + else + return 0; +} + +static int ami306_start_sensor(struct inv_ami306_state_s *st) +{ + int result = 0; + unsigned char buf[2]; + + /* Step 1 */ + buf[0] = (AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE); + result = i2c_write(st->i2c, REG_AMI_CTRL1, 1, buf); + if (result < 0) + return result; + /* Step 2 */ + buf[0] = AMI_CTRL2_DREN; + result = i2c_write(st->i2c, REG_AMI_CTRL2, 1, buf); + if (result < 0) + return result; + /* Step 3 */ + buf[0] = (AMI_CTRL4_HS & 0xFF); + buf[1] = (AMI_CTRL4_HS >> 8) & 0xFF; + + result = i2c_write(st->i2c, REG_AMI_CTRL4, 2, buf); + if (result < 0) + return result; + + /* Step 4 */ + result = ami306_write_offset(st->i2c, st->fine); + + return result; +} + +int set_ami306_enable(struct iio_dev *indio_dev, int state) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + int result; + char buf; + + buf = (AMI_CTRL1_PC1 | AMI_CTRL1_FS1_FORCE); + result = i2c_write(st->i2c, REG_AMI_CTRL1, 1, &buf); + if (result < 0) + return result; + + result = ami306_read_param(st); + if (result) + return result; + if (late_initialize) { + result = ami306_initial_b0_adjust(st); + if (result) + return result; + late_initialize = false; + } + result = ami306_start_sensor(st); + if (result) + return result; + buf = AMI_CTRL3_FORCE_BIT; + st->timestamp = iio_get_time_ns(); + result = i2c_write(st->i2c, REG_AMI_CTRL3, 1, &buf); + if (result) + return result; + + return 0; +} + +/** + * ami306_read_raw() - read raw method. + */ +static int ami306_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + *val = AMI_SCALE; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +/** + * inv_compass_matrix_show() - show orientation matrix + */ +static ssize_t inv_compass_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +static ssize_t ami306_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + unsigned long data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + error = kstrtoul(buf, 10, &data); + if (error) + return error; + if (0 == data) + return -EINVAL; + /* transform rate to delay in ms */ + data = 1000 / data; + if (data > AMI_MAX_DELAY) + data = AMI_MAX_DELAY; + if (data < AMI_MIN_DELAY) + data = AMI_MIN_DELAY; + st->delay = data; + return count; +} + +static ssize_t ami306_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", 1000 / st->delay); +} + + +static void ami306_work_func(struct work_struct *work) +{ + struct inv_ami306_state_s *st = + container_of((struct delayed_work *)work, + struct inv_ami306_state_s, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + unsigned long delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + st->timestamp = iio_get_time_ns(); + schedule_delayed_work(&st->work, delay); + inv_read_ami306_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_AMI306_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_AMI306_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(compass_matrix, S_IRUGO, inv_compass_matrix_show, NULL); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, ami306_rate_show, + ami306_rate_store); + +static struct attribute *inv_ami306_attributes[] = { + &dev_attr_compass_matrix.attr, + &dev_attr_sampling_frequency.attr, + NULL, +}; +static const struct attribute_group inv_attribute_group = { + .name = "ami306", + .attrs = inv_ami306_attributes +}; + +static const struct iio_info ami306_info = { + .driver_module = THIS_MODULE, + .read_raw = &ami306_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_ami306_probe() - probe function. + */ +static int inv_ami306_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_ami306_state_s *st; + struct iio_dev *indio_dev; + int result; + char data; + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->i2c = client; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->delay = 10; + + /* Make state variables available to all _show and _store functions. */ + i2c_set_clientdata(client, indio_dev); + result = i2c_read(st->i2c, REG_AMI_WIA, 1, &data); + if (result < 0) + goto out_free; + if (data != DATA_WIA) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &ami306_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_ami306_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_ami306_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, ami306_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_ami306_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_ami306_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_ami306_remove() - remove function. + */ +static int inv_ami306_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_ami306_state_s *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_ami306_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_ami306_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv-ami306-iio module removed.\n"); + return 0; +} +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_ami306_id[] = { + {"ami306", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_ami306_id); + +static struct i2c_driver inv_ami306_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_ami306_probe, + .remove = inv_ami306_remove, + .id_table = inv_ami306_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv-ami306-iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_ami306_init(void) +{ + int result = i2c_add_driver(&inv_ami306_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_ami306_exit(void) +{ + i2c_del_driver(&inv_ami306_driver); +} + +module_init(inv_ami306_init); +module_exit(inv_ami306_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv-ami306-iio"); +/** + * @} + */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ami306_iio.h b/drivers/iio/magnetometer/inv_compass/inv_ami306_iio.h new file mode 100644 index 000000000000..fa4f4ee1e5da --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ami306_iio.h @@ -0,0 +1,159 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_iio.h + * @brief Struct definitions for the Invensense implementation + * of ami306 driver. + */ + +#ifndef _INV_GYRO_H_ +#define _INV_GYRO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +/** axis sensitivity(gain) calibration parameter information */ +struct ami_vector3d { + signed short x; /**< X-axis */ + signed short y; /**< Y-axis */ + signed short z; /**< Z-axis */ +}; + +/** axis interference information */ +struct ami_interference { + /**< Y-axis magnetic field for X-axis correction value */ + signed short xy; + /**< Z-axis magnetic field for X-axis correction value */ + signed short xz; + /**< X-axis magnetic field for Y-axis correction value */ + signed short yx; + /**< Z-axis magnetic field for Y-axis correction value */ + signed short yz; + /**< X-axis magnetic field for Z-axis correction value */ + signed short zx; + /**< Y-axis magnetic field for Z-axis correction value */ + signed short zy; +}; + +/** sensor calibration Parameter information */ +struct ami_sensor_parametor { + /**< geomagnetic field sensor gain */ + struct ami_vector3d m_gain; + /**< geomagnetic field sensor gain correction parameter */ + struct ami_vector3d m_gain_cor; + /**< geomagnetic field sensor offset */ + struct ami_vector3d m_offset; + /**< geomagnetic field sensor axis interference parameter */ + struct ami_interference m_interference; +}; + +/** + * struct inv_ami306_state_s - Driver state variables. + * @plat_data: board file platform data. + * @i2c: i2c client handle. + * @trig: not used. for compatibility. + * @param: ami specific sensor data. + * @work: work data structure. + * @delay: delay between each scheduled work. + * @fine: fine tunign parameters. + * @compass_data: compass data store. + * @timestamp: time stamp. + */ +struct inv_ami306_state_s { + struct mpu_platform_data plat_data; + struct i2c_client *i2c; + struct iio_trigger *trig; + struct ami_sensor_parametor param; + struct delayed_work work; + int delay; + s8 fine[3]; + short compass_data[3]; + s64 timestamp; +}; +/* scan element definition */ +enum inv_mpu_scan { + INV_AMI306_SCAN_MAGN_X, + INV_AMI306_SCAN_MAGN_Y, + INV_AMI306_SCAN_MAGN_Z, + INV_AMI306_SCAN_TIMESTAMP, +}; + +#define REG_AMI_WIA 0x0f +#define REG_AMI_DATAX 0x10 +#define REG_AMI_STA1 0x18 +#define REG_AMI_CTRL1 0x1b +#define REG_AMI_CTRL2 0x1c +#define REG_AMI_CTRL3 0x1d +#define REG_AMI_B0X 0x20 +#define REG_AMI_B0Y 0x22 +#define REG_AMI_B0Z 0x24 +#define REG_AMI_CTRL5 0x40 +#define REG_AMI_CTRL4 0x5c +#define REG_AMI_TEMP 0x60 +#define REG_AMI_SENX 0x96 +#define REG_AMI_OFFX 0x6c +#define REG_AMI_OFFY 0x72 +#define REG_AMI_OFFZ 0x78 + + +#define DATA_WIA 0x46 +#define AMI_CTRL1_PC1 0x80 +#define AMI_CTRL1_FS1_FORCE 0x02 +#define AMI_CTRL1_ODR1 0x10 +#define AMI_CTRL2_DREN 0x08 +#define AMI_CTRL2_DRP 0x04 +#define AMI_CTRL3_FORCE_BIT 0x40 +#define AMI_CTRL3_B0_LO_BIT 0x10 +#define AMI_CTRL3_SRST_BIT 0x80 +#define AMI_CTRL4_HS 0xa07e +#define AMI_CTRL4_AB 0x0001 +#define AMI_STA1_DRDY_BIT 0x40 +#define AMI_STA1_DOR_BIT 0x20 + +#define AMI_PARAM_LEN 12 +#define AMI_STANDARD_OFFSET 0x800 +#define AMI_GAIN_COR_DEFAULT 1000 +#define AMI_FINE_MAX 96 +#define AMI_MAX_DELAY 1000 +#define AMI_MIN_DELAY 10 +#define AMI_SCALE (5461 * (1<<15)) + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + + +int inv_ami306_configure_ring(struct iio_dev *indio_dev); +void inv_ami306_unconfigure_ring(struct iio_dev *indio_dev); +int inv_ami306_probe_trigger(struct iio_dev *indio_dev); +void inv_ami306_remove_trigger(struct iio_dev *indio_dev); +int set_ami306_enable(struct iio_dev *indio_dev, int state); +int ami306_read_raw_data(struct inv_ami306_state_s *st, + short dat[3]); +int inv_read_ami306_fifo(struct iio_dev *indio_dev); + +#endif /* #ifndef _INV_GYRO_H_ */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ami306_ring.c b/drivers/iio/magnetometer/inv_compass/inv_ami306_ring.c new file mode 100644 index 000000000000..ed91cf49516f --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ami306_ring.c @@ -0,0 +1,163 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_ring.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_ami306_iio.h" + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) { + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + return d_ind; +} + +/** + * inv_read_fifo() - Transfer data from FIFO to ring buffer. + */ +int inv_read_ami306_fifo(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int result, status, d_ind; + char b; + char *tmp; + s64 tmp_buf[2]; + + result = i2c_smbus_read_i2c_block_data(st->i2c, REG_AMI_STA1, 1, &b); + if (result < 0) + goto end_session; + if (b & AMI_STA1_DRDY_BIT) { + status = ami306_read_raw_data(st, st->compass_data); + if (status) { + pr_err("error reading raw\n"); + goto end_session; + } + tmp = (unsigned char *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_AMI306_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7)/8] = st->timestamp; + ring->access->store_to(indio_dev->buffer, tmp, st->timestamp); + } else if (b & AMI_STA1_DOR_BIT) + pr_err("not ready\n"); +end_session: + b = AMI_CTRL3_FORCE_BIT; + result = i2c_smbus_write_i2c_block_data(st->i2c, REG_AMI_CTRL3, 1, &b); + + return IRQ_HANDLED; +} + +void inv_ami306_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; +static int inv_ami306_postenable(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int result; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_AMI306_SCAN_MAGN_Z))) + return 0; + + result = set_ami306_enable(indio_dev, true); + if (result) + return result; + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_ami306_predisable(struct iio_dev *indio_dev) +{ + struct iio_buffer *ring = indio_dev->buffer; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_AMI306_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_AMI306_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_AMI306_SCAN_MAGN_Z, ring->scan_mask); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_ami306_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_ami306_postenable, + .predisable = &inv_ami306_predisable, +}; + +int inv_ami306_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_ami306_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} +/** + * @} + */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_ami306_trigger.c b/drivers/iio/magnetometer/inv_compass/inv_ami306_trigger.c new file mode 100644 index 000000000000..f7fe59ef5dff --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_ami306_trigger.c @@ -0,0 +1,90 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_ami306_trigger.c + * @brief Invensense implementation for AMI306 + * @details This driver currently works for the AMI306 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" +#include "inv_ami306_iio.h" + +static const struct iio_trigger_ops inv_ami306_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_ami306_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->i2c->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_ami306_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_ami306_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_ami306_state_s *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} +/** + * @} + */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_yas53x_core.c b/drivers/iio/magnetometer/inv_compass/inv_yas53x_core.c new file mode 100644 index 000000000000..6af420bb5cf1 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_yas53x_core.c @@ -0,0 +1,969 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_core.c + * @brief Invensense implementation for yas530/yas532/yas533. + * @details This driver currently works for yas530/yas532/yas533. + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "inv_yas53x_iio.h" +#include "sysfs.h" +#include "inv_test/inv_counters.h" + +/* -------------------------------------------------------------------------- */ +static int Cx, Cy1, Cy2; +static int /*a1, */ a2, a3, a4, a5, a6, a7, a8, a9; +static int k; + +static u8 dx, dy1, dy2; +static u8 d2, d3, d4, d5, d6, d7, d8, d9, d0; +static u8 dck, ver; + +/** + * inv_serial_read() - Read one or more bytes from the device registers. + * @st: Device driver instance. + * @reg: First device register to be read from. + * @length: Number of bytes to read. + * @data: Data read from device. + * NOTE: The slave register will not increment when reading from the FIFO. + */ +int inv_serial_read(struct inv_compass_state *st, u8 reg, u16 length, u8 *data) +{ + int result; + INV_I2C_INC_COMPASSWRITE(3); + INV_I2C_INC_COMPASSREAD(length); + result = i2c_smbus_read_i2c_block_data(st->client, reg, length, data); + if (result != length) { + if (result < 0) + return result; + else + return -EINVAL; + } else { + return 0; + } +} + +/** + * inv_serial_single_write() - Write a byte to a device register. + * @st: Device driver instance. + * @reg: Device register to be written to. + * @data: Byte to write to device. + */ +int inv_serial_single_write(struct inv_compass_state *st, u8 reg, u8 data) +{ + u8 d[1]; + d[0] = data; + INV_I2C_INC_COMPASSWRITE(3); + + return i2c_smbus_write_i2c_block_data(st->client, reg, 1, d); +} + +static int set_hardware_offset(struct inv_compass_state *st, + char offset_x, char offset_y1, char offset_y2) +{ + char data; + int result = 0; + + data = offset_x & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_X, data); + if (result) + return result; + + data = offset_y1 & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_Y1, data); + if (result) + return result; + + data = offset_y2 & 0x3f; + result = inv_serial_single_write(st, YAS530_REGADDR_OFFSET_Y2, data); + return result; +} + +static int set_measure_command(struct inv_compass_state *st) +{ + int result = 0; + result = inv_serial_single_write(st, + YAS530_REGADDR_MEASURE_COMMAND, 0x01); + return result; +} + +static int measure_normal(struct inv_compass_state *st, + int *busy, unsigned short *t, + unsigned short *x, unsigned short *y1, + unsigned short *y2) +{ + int result; + ktime_t sleeptime; + result = set_measure_command(st); + sleeptime = ktime_set(0, 2 * NSEC_PER_MSEC); + set_current_state(TASK_UNINTERRUPTIBLE); + schedule_hrtimeout(&sleeptime, HRTIMER_MODE_REL); + + result = st->read_data(st, busy, t, x, y1, y2); + + return result; +} + +static int measure_int(struct inv_compass_state *st, + int *busy, unsigned short *t, + unsigned short *x, unsigned short *y1, + unsigned short *y2) +{ + int result; + if (st->first_read_after_reset) { + st->first_read_after_reset = 0; + result = 1; + } else { + result = st->read_data(st, busy, t, x, y1, y2); + } + result |= set_measure_command(st); + + return result; +} + +static int yas530_read_data(struct inv_compass_state *st, + int *busy, u16 *t, u16 *x, u16 *y1, u16 *y2) +{ + u8 data[8]; + u16 b, to, xo, y1o, y2o; + int result; + + result = inv_serial_read(st, + YAS530_REGADDR_MEASURE_DATA, 8, data); + if (result) + return result; + + b = (data[0] >> 7) & 0x01; + to = (s16)(((data[0] << 2) & 0x1fc) | ((data[1] >> 6) & 0x03)); + xo = (s16)(((data[2] << 5) & 0xfe0) | ((data[3] >> 3) & 0x1f)); + y1o = (s16)(((data[4] << 5) & 0xfe0) | ((data[5] >> 3) & 0x1f)); + y2o = (s16)(((data[6] << 5) & 0xfe0) | ((data[7] >> 3) & 0x1f)); + + *busy = b; + *t = to; + *x = xo; + *y1 = y1o; + *y2 = y2o; + + return 0; +} + +static int yas532_533_read_data(struct inv_compass_state *st, + int *busy, u16 *t, u16 *x, u16 *y1, u16 *y2) +{ + u8 data[8]; + u16 b, to, xo, y1o, y2o; + int result; + + result = inv_serial_read(st, + YAS530_REGADDR_MEASURE_DATA, 8, data); + if (result) + return result; + + b = (data[0] >> 7) & 0x01; + to = (s16)((((s32)data[0] << 3) & 0x3f8) | ((data[1] >> 5) & 0x07)); + xo = (s16)((((s32)data[2] << 6) & 0x1fc0) | ((data[3] >> 2) & 0x3f)); + y1o = (s16)((((s32)data[4] << 6) & 0x1fc0) | ((data[5] >> 2) & 0x3f)); + y2o = (s16)((((s32)data[6] << 6) & 0x1fc0) | ((data[7] >> 2) & 0x3f)); + + *busy = b; + *t = to; + *x = xo; + *y1 = y1o; + *y2 = y2o; + + return 0; +} + +static int check_offset(struct inv_compass_state *st, + char offset_x, char offset_y1, char offset_y2, + int *flag_x, int *flag_y1, int *flag_y2) +{ + int result; + int busy; + short t, x, y1, y2; + + result = set_hardware_offset(st, offset_x, offset_y1, offset_y2); + if (result) + return result; + result = measure_normal(st, &busy, &t, &x, &y1, &y2); + if (result) + return result; + *flag_x = 0; + *flag_y1 = 0; + *flag_y2 = 0; + + if (x > st->center) + *flag_x = 1; + if (y1 > st->center) + *flag_y1 = 1; + if (y2 > st->center) + *flag_y2 = 1; + if (x < st->center) + *flag_x = -1; + if (y1 < st->center) + *flag_y1 = -1; + if (y2 < st->center) + *flag_y2 = -1; + + return result; +} + +static int measure_and_set_offset(struct inv_compass_state *st, + char *offset) +{ + int i; + int result = 0; + char offset_x = 0, offset_y1 = 0, offset_y2 = 0; + int flag_x = 0, flag_y1 = 0, flag_y2 = 0; + static const int correct[5] = {16, 8, 4, 2, 1}; + + for (i = 0; i < 5; i++) { + result = check_offset(st, + offset_x, offset_y1, offset_y2, + &flag_x, &flag_y1, &flag_y2); + if (result) + return result; + if (flag_x) + offset_x += flag_x * correct[i]; + if (flag_y1) + offset_y1 += flag_y1 * correct[i]; + if (flag_y2) + offset_y2 += flag_y2 * correct[i]; + } + + result = set_hardware_offset(st, offset_x, offset_y1, offset_y2); + if (result) + return result; + offset[0] = offset_x; + offset[1] = offset_y1; + offset[2] = offset_y2; + + return result; +} + +static void coordinate_conversion(short x, short y1, short y2, short t, + int *xo, int *yo, int *zo) +{ + int sx, sy1, sy2, sy, sz; + int hx, hy, hz; + + sx = x - (Cx * t) / 100; + sy1 = y1 - (Cy1 * t) / 100; + sy2 = y2 - (Cy2 * t) / 100; + + sy = sy1 - sy2; + sz = -sy1 - sy2; + + hx = k * ((100 * sx + a2 * sy + a3 * sz) / 10); + hy = k * ((a4 * sx + a5 * sy + a6 * sz) / 10); + hz = k * ((a7 * sx + a8 * sy + a9 * sz) / 10); + + *xo = hx; + *yo = hy; + *zo = hz; +} + +static int get_cal_data_yas532_533(struct inv_compass_state *st) +{ + u8 data[YAS_YAS532_533_CAL_DATA_SIZE]; + int result; + + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS532_533_CAL_DATA_SIZE, data); + if (result) + return result; + /* CAL data Second Read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS532_533_CAL_DATA_SIZE, data); + if (result) + return result; + + dx = data[0]; + dy1 = data[1]; + dy2 = data[2]; + d2 = (data[3] >> 2) & 0x03f; + d3 = (u8)(((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03)); + d4 = (u8)(data[4] & 0x3f); + d5 = (data[5] >> 2) & 0x3f; + d6 = (u8)(((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f)); + d7 = (u8)(((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07)); + d8 = (u8)(((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01)); + d9 = (u8)(((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01)); + d0 = (u8)((data[9] >> 2) & 0x1f); + dck = (u8)(((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01)); + ver = (u8)((data[13]) & 0x01); + + Cx = dx * 10 - 1280; + Cy1 = dy1 * 10 - 1280; + Cy2 = dy2 * 10 - 1280; + a2 = d2 - 32; + a3 = d3 - 8; + a4 = d4 - 32; + a5 = d5 + 38; + a6 = d6 - 32; + a7 = d7 - 64; + a8 = d8 - 32; + a9 = d9; + k = d0; + + return 0; +} + +static int get_cal_data_yas530(struct inv_compass_state *st) +{ + u8 data[YAS_YAS530_CAL_DATA_SIZE]; + int result; + /* CAL data read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS530_CAL_DATA_SIZE, data); + if (result) + return result; + /* CAL data Second Read */ + result = inv_serial_read(st, YAS530_REGADDR_CAL, + YAS_YAS530_CAL_DATA_SIZE, data); + if (result) + return result; + /*Cal data */ + dx = data[0]; + dy1 = data[1]; + dy2 = data[2]; + d2 = (data[3] >> 2) & 0x03f; + d3 = ((data[3] << 2) & 0x0c) | ((data[4] >> 6) & 0x03); + d4 = data[4] & 0x3f; + d5 = (data[5] >> 2) & 0x3f; + d6 = ((data[5] << 4) & 0x30) | ((data[6] >> 4) & 0x0f); + d7 = ((data[6] << 3) & 0x78) | ((data[7] >> 5) & 0x07); + d8 = ((data[7] << 1) & 0x3e) | ((data[8] >> 7) & 0x01); + d9 = ((data[8] << 1) & 0xfe) | ((data[9] >> 7) & 0x01); + d0 = (data[9] >> 2) & 0x1f; + dck = ((data[9] << 1) & 0x06) | ((data[10] >> 7) & 0x01); + ver = (u8)((data[15]) & 0x03); + + /*Correction Data */ + Cx = (int)dx * 6 - 768; + Cy1 = (int)dy1 * 6 - 768; + Cy2 = (int)dy2 * 6 - 768; + a2 = (int)d2 - 32; + a3 = (int)d3 - 8; + a4 = (int)d4 - 32; + a5 = (int)d5 + 38; + a6 = (int)d6 - 32; + a7 = (int)d7 - 64; + a8 = (int)d8 - 32; + a9 = (int)d9; + k = (int)d0 + 10; + + return 0; +} + + +static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, + int threshold) +{ + thresh_filter->threshold = threshold; + thresh_filter->last = 0; +} + +static void +adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, + int noise) +{ + int i; + + adap_filter->num = 0; + adap_filter->index = 0; + adap_filter->filter_noise = noise; + adap_filter->filter_len = len; + + for (i = 0; i < adap_filter->filter_len; ++i) + adap_filter->sequence[i] = 0; +} + +static void yas_init_adap_filter(struct inv_compass_state *st) +{ + struct yas_filter *f; + int i; + int noise[] = {YAS_MAG_DEFAULT_FILTER_NOISE_X, + YAS_MAG_DEFAULT_FILTER_NOISE_Y, + YAS_MAG_DEFAULT_FILTER_NOISE_Z}; + + f = &st->filter; + f->filter_len = YAS_MAG_DEFAULT_FILTER_LEN; + for (i = 0; i < 3; i++) + f->filter_noise[i] = noise[i]; + + for (i = 0; i < 3; i++) { + adaptive_filter_init(&f->adap_filter[i], f->filter_len, + f->filter_noise[i]); + thresh_filter_init(&f->thresh_filter[i], f->filter_thresh); + } +} + +int yas53x_resume(struct inv_compass_state *st) +{ + int result = 0; + + unsigned char dummyData = 0x00; + unsigned char read_reg[1]; + + /* =============================================== */ + + /* Step 1 - Test register initialization */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_TEST1, dummyData); + if (result) + return result; + result = + inv_serial_single_write(st, + YAS530_REGADDR_TEST2, dummyData); + if (result) + return result; + /* Device ID read */ + result = inv_serial_read(st, + YAS530_REGADDR_DEVICE_ID, 1, read_reg); + + /*Step 2 Read the CAL register */ + st->get_cal_data(st); + + /*Obtain the [49:47] bits */ + dck &= 0x07; + + /*Step 3 : Storing the CONFIG with the CLK value */ + dummyData = 0x00 | (dck << 2); + result = inv_serial_single_write(st, + YAS530_REGADDR_CONFIG, dummyData); + if (result) + return result; + /*Step 4 : Set Acquisition Interval Register */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_MEASURE_INTERVAL, + dummyData); + if (result) + return result; + + /*Step 5 : Reset Coil */ + dummyData = 0x00; + result = inv_serial_single_write(st, + YAS530_REGADDR_ACTUATE_INIT_COIL, + dummyData); + if (result) + return result; + /* Offset Measurement and Set */ + result = measure_and_set_offset(st, st->offset); + if (result) + return result; + st->first_measure_after_reset = 1; + st->first_read_after_reset = 1; + st->reset_timer = 0; + + yas_init_adap_filter(st); + + return result; +} + +static int inv_check_range(struct inv_compass_state *st, s16 x, s16 y1, s16 y2) +{ + int result = 0; + + if (x == 0) + result |= 0x01; + if (x == st->overflow_bound) + result |= 0x02; + if (y1 == 0) + result |= 0x04; + if (y1 == st->overflow_bound) + result |= 0x08; + if (y2 == 0) + result |= 0x10; + if (y2 == st->overflow_bound) + result |= 0x20; + + return result; +} +static int square(int data) +{ + return data * data; +} + +static int +adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, int in) +{ + int avg, sum; + int i; + + if (adap_filter->filter_len == 0) + return in; + if (adap_filter->num < adap_filter->filter_len) { + adap_filter->sequence[adap_filter->index++] = in / 100; + adap_filter->num++; + return in; + } + if (adap_filter->filter_len <= adap_filter->index) + adap_filter->index = 0; + adap_filter->sequence[adap_filter->index++] = in / 100; + + avg = 0; + for (i = 0; i < adap_filter->filter_len; i++) + avg += adap_filter->sequence[i]; + avg /= adap_filter->filter_len; + + sum = 0; + for (i = 0; i < adap_filter->filter_len; i++) + sum += square(avg - adap_filter->sequence[i]); + sum /= adap_filter->filter_len; + + if (sum <= adap_filter->filter_noise) + return avg * 100; + + return ((in/100 - avg) * (sum - adap_filter->filter_noise) / sum + avg) + * 100; +} + +static int +thresh_filter_filter(struct yas_thresh_filter *thresh_filter, int in) +{ + if (in < thresh_filter->last - thresh_filter->threshold + || thresh_filter->last + + thresh_filter->threshold < in) { + thresh_filter->last = in; + return in; + } else { + return thresh_filter->last; + } +} + +static void +filter_filter(struct yas_filter *d, int *orig, int *filtered) +{ + int i; + + for (i = 0; i < 3; i++) { + filtered[i] = adaptive_filter_filter(&d->adap_filter[i], + orig[i]); + filtered[i] = thresh_filter_filter(&d->thresh_filter[i], + filtered[i]); + } +} + +int yas53x_read(struct inv_compass_state *st, short rawfixed[3], + int *overunderflow) +{ + int result = 0; + + int busy, i, ov; + short t, x, y1, y2; + s32 xyz[3], disturb[3]; + + result = measure_int(st, &busy, &t, &x, &y1, &y2); + if (result) + return result; + if (busy) + return -1; + coordinate_conversion(x, y1, y2, t, &xyz[0], &xyz[1], &xyz[2]); + filter_filter(&st->filter, xyz, xyz); + for (i = 0; i < 3; i++) + rawfixed[i] = (short)(xyz[i] / 100); + + if (st->first_measure_after_reset) { + for (i = 0; i < 3; i++) + st->base_compass_data[i] = rawfixed[i]; + st->first_measure_after_reset = 0; + } + ov = 0; + for (i = 0; i < 3; i++) { + disturb[i] = abs(st->base_compass_data[i] - rawfixed[i]); + if (disturb[i] > YAS_MAG_DISTURBURNCE_THRESHOLD) + ov = 1; + } + if (ov) + st->reset_timer += st->delay; + else + st->reset_timer = 0; + + if (st->reset_timer > YAS_RESET_COIL_TIME_THRESHOLD) + *overunderflow = (1<<8); + else + *overunderflow = 0; + *overunderflow |= inv_check_range(st, x, y1, y2); + + return 0; +} + +/** + * yas53x_read_raw() - read raw method. + */ +static int yas53x_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) { + struct inv_compass_state *st = iio_priv(indio_dev); + + switch (mask) { + case 0: + if (!(iio_buffer_enabled(indio_dev))) + return -EINVAL; + if (chan->type == IIO_MAGN) { + *val = st->compass_data[chan->channel2 - IIO_MOD_X]; + return IIO_VAL_INT; + } + + return -EINVAL; + case IIO_CHAN_INFO_SCALE: + if (chan->type == IIO_MAGN) { + *val = YAS530_SCALE; + return IIO_VAL_INT; + } + return -EINVAL; + default: + return -EINVAL; + } +} + +/** + * inv_compass_matrix_show() - show orientation matrix + */ +static ssize_t inv_compass_matrix_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + signed char *m; + struct inv_compass_state *st = iio_priv(indio_dev); + m = st->plat_data.orientation; + return sprintf(buf, + "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]); +} + +static ssize_t yas53x_rate_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + u32 data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + error = kstrtoint(buf, 10, &data); + if (error) + return error; + if (0 == data) + return -EINVAL; + /* transform rate to delay in ms */ + data = MSEC_PER_SEC / data; + + if (data > YAS530_MAX_DELAY) + data = YAS530_MAX_DELAY; + if (data < YAS530_MIN_DELAY) + data = YAS530_MIN_DELAY; + st->delay = data; + + return count; +} + +static ssize_t yas53x_rate_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + /* transform delay in ms to rate */ + return sprintf(buf, "%d\n", (int)MSEC_PER_SEC / st->delay); +} + +static ssize_t yas53x_overunderflow_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + u32 data; + int error; + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + error = kstrtoint(buf, 10, &data); + if (error) + return error; + if (data) + return -EINVAL; + st->overunderflow = data; + + return count; +} + +static ssize_t yas53x_overunderflow_show(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_compass_state *st = iio_priv(indio_dev); + + return sprintf(buf, "%d\n", st->overunderflow); +} + +void set_yas53x_enable(struct iio_dev *indio_dev, bool enable) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + + yas_init_adap_filter(st); + st->first_measure_after_reset = 1; + st->first_read_after_reset = 1; + schedule_delayed_work(&st->work, msecs_to_jiffies(st->delay)); +} + +static void yas53x_work_func(struct work_struct *work) +{ + struct inv_compass_state *st = + container_of((struct delayed_work *)work, + struct inv_compass_state, work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + u32 delay = msecs_to_jiffies(st->delay); + + mutex_lock(&indio_dev->mlock); + if (!(iio_buffer_enabled(indio_dev))) + goto error_ret; + + schedule_delayed_work(&st->work, delay); + inv_read_yas53x_fifo(indio_dev); + INV_I2C_INC_COMPASSIRQ(); + +error_ret: + mutex_unlock(&indio_dev->mlock); +} + +static const struct iio_chan_spec compass_channels[] = { + { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_X, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_Y, + .scan_type = IIO_ST('s', 16, 16, 0) + }, { + .type = IIO_MAGN, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask = IIO_CHAN_INFO_SCALE_SHARED_BIT, + .scan_index = INV_YAS53X_SCAN_MAGN_Z, + .scan_type = IIO_ST('s', 16, 16, 0) + }, + IIO_CHAN_SOFT_TIMESTAMP(INV_YAS53X_SCAN_TIMESTAMP) +}; + +static DEVICE_ATTR(compass_matrix, S_IRUGO, inv_compass_matrix_show, NULL); +static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR, yas53x_rate_show, + yas53x_rate_store); +static DEVICE_ATTR(overunderflow, S_IRUGO | S_IWUSR, + yas53x_overunderflow_show, yas53x_overunderflow_store); + +static struct attribute *inv_yas53x_attributes[] = { + &dev_attr_compass_matrix.attr, + &dev_attr_sampling_frequency.attr, + &dev_attr_overunderflow.attr, + NULL, +}; +static const struct attribute_group inv_attribute_group = { + .name = "yas53x", + .attrs = inv_yas53x_attributes +}; + +static const struct iio_info yas53x_info = { + .driver_module = THIS_MODULE, + .read_raw = &yas53x_read_raw, + .attrs = &inv_attribute_group, +}; + +/*constant IIO attribute */ +/** + * inv_yas53x_probe() - probe function. + */ +static int inv_yas53x_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct inv_compass_state *st; + struct iio_dev *indio_dev; + int result; + + if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { + result = -ENODEV; + goto out_no_free; + } + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + result = -ENOMEM; + goto out_no_free; + } + st = iio_priv(indio_dev); + st->client = client; + st->plat_data = + *(struct mpu_platform_data *)dev_get_platdata(&client->dev); + st->delay = 10; + + i2c_set_clientdata(client, indio_dev); + + if (!strcmp(id->name, "yas530")) { + st->read_data = yas530_read_data; + st->get_cal_data = get_cal_data_yas530; + st->overflow_bound = YAS_YAS530_DATA_OVERFLOW; + st->center = YAS_YAS530_DATA_CENTER; + st->filter.filter_thresh = YAS530_MAG_DEFAULT_FILTER_THRESH; + } else { + st->read_data = yas532_533_read_data; + st->get_cal_data = get_cal_data_yas532_533; + st->overflow_bound = YAS_YAS532_533_DATA_OVERFLOW; + st->center = YAS_YAS532_533_DATA_CENTER; + st->filter.filter_thresh = YAS532_MAG_DEFAULT_FILTER_THRESH; + } + st->upper_bound = st->center + (st->center >> 1); + st->lower_bound = (st->center >> 1); + + result = yas53x_resume(st); + if (result) + goto out_free; + + indio_dev->dev.parent = &client->dev; + indio_dev->name = id->name; + indio_dev->channels = compass_channels; + indio_dev->num_channels = ARRAY_SIZE(compass_channels); + indio_dev->info = &yas53x_info; + indio_dev->modes = INDIO_DIRECT_MODE; + indio_dev->currentmode = INDIO_DIRECT_MODE; + + result = inv_yas53x_configure_ring(indio_dev); + if (result) + goto out_free; + result = iio_buffer_register(indio_dev, indio_dev->channels, + indio_dev->num_channels); + if (result) + goto out_unreg_ring; + result = inv_yas53x_probe_trigger(indio_dev); + if (result) + goto out_remove_ring; + + result = iio_device_register(indio_dev); + if (result) + goto out_remove_trigger; + INIT_DELAYED_WORK(&st->work, yas53x_work_func); + pr_info("%s: Probe name %s\n", __func__, id->name); + + return 0; +out_remove_trigger: + if (indio_dev->modes & INDIO_BUFFER_TRIGGERED) + inv_yas53x_remove_trigger(indio_dev); +out_remove_ring: + iio_buffer_unregister(indio_dev); +out_unreg_ring: + inv_yas53x_unconfigure_ring(indio_dev); +out_free: + iio_free_device(indio_dev); +out_no_free: + dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); + return -EIO; +} + +/** + * inv_yas53x_remove() - remove function. + */ +static int inv_yas53x_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + struct inv_compass_state *st = iio_priv(indio_dev); + cancel_delayed_work_sync(&st->work); + iio_device_unregister(indio_dev); + inv_yas53x_remove_trigger(indio_dev); + iio_buffer_unregister(indio_dev); + inv_yas53x_unconfigure_ring(indio_dev); + iio_free_device(indio_dev); + + dev_info(&client->adapter->dev, "inv_yas53x_iio module removed.\n"); + return 0; +} +static const unsigned short normal_i2c[] = { I2C_CLIENT_END }; +/* device id table is used to identify what device can be + * supported by this driver + */ +static const struct i2c_device_id inv_yas53x_id[] = { + {"yas530", 0}, + {"yas532", 0}, + {"yas533", 0}, + {} +}; + +MODULE_DEVICE_TABLE(i2c, inv_yas53x_id); + +static struct i2c_driver inv_yas53x_driver = { + .class = I2C_CLASS_HWMON, + .probe = inv_yas53x_probe, + .remove = inv_yas53x_remove, + .id_table = inv_yas53x_id, + .driver = { + .owner = THIS_MODULE, + .name = "inv_yas53x_iio", + }, + .address_list = normal_i2c, +}; + +static int __init inv_yas53x_init(void) +{ + int result = i2c_add_driver(&inv_yas53x_driver); + if (result) { + pr_err("%s failed\n", __func__); + return result; + } + return 0; +} + +static void __exit inv_yas53x_exit(void) +{ + i2c_del_driver(&inv_yas53x_driver); +} + +module_init(inv_yas53x_init); +module_exit(inv_yas53x_exit); + +MODULE_AUTHOR("Invensense Corporation"); +MODULE_DESCRIPTION("Invensense device driver"); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("inv_yas53x_iio"); +/** + * @} + */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_yas53x_iio.h b/drivers/iio/magnetometer/inv_compass/inv_yas53x_iio.h new file mode 100644 index 000000000000..92bf0af7ec7e --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_yas53x_iio.h @@ -0,0 +1,172 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_iio.h + * @brief Struct definitions for the Invensense implementation + * of yas53x driver. + */ + +#ifndef _INV_GYRO_H_ +#define _INV_GYRO_H_ + +#include <linux/i2c.h> +#include <linux/kfifo.h> +#include <linux/miscdevice.h> +#include <linux/input.h> +#include <linux/spinlock.h> +#include <linux/mpu.h> + +#include "iio.h" +#include "buffer.h" +#include "trigger.h" + +#define YAS_MAG_MAX_FILTER_LEN 30 +struct yas_adaptive_filter { + int num; + int index; + int filter_len; + int filter_noise; + int sequence[YAS_MAG_MAX_FILTER_LEN]; +}; + +struct yas_thresh_filter { + int threshold; + int last; +}; + +struct yas_filter { + int filter_len; + int filter_thresh; + int filter_noise[3]; + struct yas_adaptive_filter adap_filter[3]; + struct yas_thresh_filter thresh_filter[3]; +}; + +/** + * struct inv_compass_state - Driver state variables. + * @plat_data: mpu platform data from board file. + * @client: i2c client handle. + * @chan_info: channel information. + * @trig: IIO trigger. + * @work: work structure. + * @delay: delay to schedule the next work. + * @overflow_bound: bound to determine overflow. + * @center: center of the measurement. + * @compass_data[3]: compass data store. + * @offset[3]: yas530 specific data. + * @base_compass_data[3]: first measure data after reset. + * @first_measure_after_reset:1: flag for first measurement after reset. + * @first_read_after_reset:1: flag for first read after reset. + * @reset_timer: timer to accumulate overflow conditions. + * @overunderflow:1: overflow and underflow flag. + * @filter: filter data structure. + * @read_data: function pointer of reading data from device. + * @get_cal_data: function pointer of reading cal data. + */ +struct inv_compass_state { + struct mpu_platform_data plat_data; + struct i2c_client *client; + struct iio_trigger *trig; + struct delayed_work work; + s16 delay; + s16 overflow_bound; + s16 upper_bound; + s16 lower_bound; + s16 center; + s16 compass_data[3]; + s8 offset[3]; + s16 base_compass_data[3]; + u8 first_measure_after_reset:1; + u8 first_read_after_reset:1; + u8 overunderflow:1; + s32 reset_timer; + struct yas_filter filter; + int (*read_data)(struct inv_compass_state *st, + int *, u16 *, u16 *, u16 *, u16 *); + int (*get_cal_data)(struct inv_compass_state *); +}; + +/* scan element definition */ +enum inv_mpu_scan { + INV_YAS53X_SCAN_MAGN_X, + INV_YAS53X_SCAN_MAGN_Y, + INV_YAS53X_SCAN_MAGN_Z, + INV_YAS53X_SCAN_TIMESTAMP, +}; + +#define YAS530_REGADDR_DEVICE_ID 0x80 +#define YAS530_REGADDR_ACTUATE_INIT_COIL 0x81 +#define YAS530_REGADDR_MEASURE_COMMAND 0x82 +#define YAS530_REGADDR_CONFIG 0x83 +#define YAS530_REGADDR_MEASURE_INTERVAL 0x84 +#define YAS530_REGADDR_OFFSET_X 0x85 +#define YAS530_REGADDR_OFFSET_Y1 0x86 +#define YAS530_REGADDR_OFFSET_Y2 0x87 +#define YAS530_REGADDR_TEST1 0x88 +#define YAS530_REGADDR_TEST2 0x89 +#define YAS530_REGADDR_CAL 0x90 +#define YAS530_REGADDR_MEASURE_DATA 0xb0 + +#define YAS530_MAX_DELAY 200 +#define YAS530_MIN_DELAY 5 +#define YAS530_SCALE 107374182L + +#define YAS_YAS530_VERSION_A 0 /* YAS530 (MS-3E Aver) */ +#define YAS_YAS530_VERSION_B 1 /* YAS530B (MS-3E Bver) */ +#define YAS_YAS530_VERSION_A_COEF 380 +#define YAS_YAS530_VERSION_B_COEF 550 +#define YAS_YAS530_DATA_CENTER 2048 +#define YAS_YAS530_DATA_OVERFLOW 4095 +#define YAS_YAS530_CAL_DATA_SIZE 16 + +/*filter related defines */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_X 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_Y 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_NOISE_Z 144 /* sd: 1200 nT */ +#define YAS_MAG_DEFAULT_FILTER_LEN 20 + +#define YAS530_MAG_DEFAULT_FILTER_THRESH 100 +#define YAS532_MAG_DEFAULT_FILTER_THRESH 300 + +#define YAS_YAS532_533_VERSION_AB 0 /* YAS532_533AB (MS-3R/3F ABver) */ +#define YAS_YAS532_533_VERSION_AC 1 /* YAS532_533AC (MS-3R/3F ACver) */ +#define YAS_YAS532_533_VERSION_AB_COEF 1800 +#define YAS_YAS532_533_VERSION_AC_COEF 900 +#define YAS_YAS532_533_DATA_CENTER 4096 +#define YAS_YAS532_533_DATA_OVERFLOW 8190 +#define YAS_YAS532_533_CAL_DATA_SIZE 14 + +#define YAS_MAG_DISTURBURNCE_THRESHOLD 1600 +#define YAS_RESET_COIL_TIME_THRESHOLD 3000 + +#define INV_ERROR_COMPASS_DATA_OVERFLOW (-1) +#define INV_ERROR_COMPASS_DATA_NOT_READY (-2) + +int inv_yas53x_configure_ring(struct iio_dev *indio_dev); +void inv_yas53x_unconfigure_ring(struct iio_dev *indio_dev); +int inv_yas53x_probe_trigger(struct iio_dev *indio_dev); +void inv_yas53x_remove_trigger(struct iio_dev *indio_dev); +void set_yas53x_enable(struct iio_dev *indio_dev, bool enable); +void inv_read_yas53x_fifo(struct iio_dev *indio_dev); +int yas53x_read(struct inv_compass_state *st, short rawfixed[3], + s32 *overunderflow); +int yas53x_resume(struct inv_compass_state *st); + +#endif /* #ifndef _INV_GYRO_H_ */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_yas53x_ring.c b/drivers/iio/magnetometer/inv_compass/inv_yas53x_ring.c new file mode 100644 index 000000000000..efcf49c68393 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_yas53x_ring.c @@ -0,0 +1,165 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_ring.c + * @brief Invensense implementation for yas530/yas532/yas533. + * @details This driver currently works for the yas530/yas532/yas533. + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> + +#include "iio.h" +#include "kfifo_buf.h" +#include "trigger_consumer.h" +#include "sysfs.h" + +#include "inv_yas53x_iio.h" + +static s64 get_time_ns(void) +{ + struct timespec ts; + ktime_get_ts(&ts); + + return timespec_to_ns(&ts); +} + +static int put_scan_to_buf(struct iio_dev *indio_dev, unsigned char *d, + short *s, int scan_index) +{ + struct iio_buffer *ring = indio_dev->buffer; + int st; + int i, d_ind; + + d_ind = 0; + for (i = 0; i < 3; i++) { + st = iio_scan_mask_query(indio_dev, ring, scan_index + i); + if (st) { + memcpy(&d[d_ind], &s[i], sizeof(s[i])); + d_ind += sizeof(s[i]); + } + } + + return d_ind; +} + +/** + * inv_read_yas53x_fifo() - Transfer data from FIFO to ring buffer. + */ +void inv_read_yas53x_fifo(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + int d_ind; + s32 overunderflow; + s8 *tmp; + s64 tmp_buf[2]; + + if (!yas53x_read(st, st->compass_data, &overunderflow)) { + tmp = (u8 *)tmp_buf; + d_ind = put_scan_to_buf(indio_dev, tmp, st->compass_data, + INV_YAS53X_SCAN_MAGN_X); + if (ring->scan_timestamp) + tmp_buf[(d_ind + 7) / 8] = get_time_ns(); + ring->access->store_to(indio_dev->buffer, tmp, 0); + + if (overunderflow) { + yas53x_resume(st); + if (!st->overunderflow) + st->overunderflow = 1; + } + } +} + +void inv_yas53x_unconfigure_ring(struct iio_dev *indio_dev) +{ + iio_kfifo_free(indio_dev->buffer); +}; + +static int inv_yas53x_postenable(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + /* when all the outputs are disabled, even though buffer/enable is on, + do nothing */ + if (!(iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_X) || + iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_Y) || + iio_scan_mask_query(indio_dev, ring, INV_YAS53X_SCAN_MAGN_Z))) + return 0; + + set_yas53x_enable(indio_dev, true); + schedule_delayed_work(&st->work, + msecs_to_jiffies(st->delay)); + + return 0; +} + +static int inv_yas53x_predisable(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + struct iio_buffer *ring = indio_dev->buffer; + + cancel_delayed_work_sync(&st->work); + clear_bit(INV_YAS53X_SCAN_MAGN_X, ring->scan_mask); + clear_bit(INV_YAS53X_SCAN_MAGN_Y, ring->scan_mask); + clear_bit(INV_YAS53X_SCAN_MAGN_Z, ring->scan_mask); + + return 0; +} + +static const struct iio_buffer_setup_ops inv_yas53x_ring_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &inv_yas53x_postenable, + .predisable = &inv_yas53x_predisable, +}; + +int inv_yas53x_configure_ring(struct iio_dev *indio_dev) +{ + int ret = 0; + struct iio_buffer *ring; + + ring = iio_kfifo_allocate(indio_dev); + if (!ring) { + ret = -ENOMEM; + return ret; + } + indio_dev->buffer = ring; + /* setup ring buffer */ + ring->scan_timestamp = true; + indio_dev->setup_ops = &inv_yas53x_ring_setup_ops; + + indio_dev->modes |= INDIO_BUFFER_TRIGGERED; + return 0; +} +/** + * @} + */ + diff --git a/drivers/iio/magnetometer/inv_compass/inv_yas53x_trigger.c b/drivers/iio/magnetometer/inv_compass/inv_yas53x_trigger.c new file mode 100644 index 000000000000..a20ce2baa7e0 --- /dev/null +++ b/drivers/iio/magnetometer/inv_compass/inv_yas53x_trigger.c @@ -0,0 +1,91 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* This software is licensed under the terms of the GNU General Public +* License version 2, as published by the Free Software Foundation, and +* may be copied, distributed, and modified under those terms. +* +* 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. +* +*/ + +/** + * @addtogroup DRIVERS + * @brief Hardware drivers. + * + * @{ + * @file inv_yas53x_trigger.c + * @brief Invensense implementation for yas530/yas532/yas533 + * @details This driver currently works for the yas530/yas532/yas533 + */ + +#include <linux/module.h> +#include <linux/init.h> +#include <linux/slab.h> +#include <linux/i2c.h> +#include <linux/err.h> +#include <linux/delay.h> +#include <linux/sysfs.h> +#include <linux/jiffies.h> +#include <linux/irq.h> +#include <linux/interrupt.h> +#include <linux/kfifo.h> +#include <linux/poll.h> +#include <linux/miscdevice.h> +#include <linux/spinlock.h> + +#include "iio.h" +#include "sysfs.h" +#include "trigger.h" + +#include "inv_yas53x_iio.h" + +static const struct iio_trigger_ops inv_yas53x_trigger_ops = { + .owner = THIS_MODULE, +}; + +int inv_yas53x_probe_trigger(struct iio_dev *indio_dev) +{ + int ret; + struct inv_compass_state *st = iio_priv(indio_dev); + + st->trig = iio_allocate_trigger("%s-dev%d", + indio_dev->name, + indio_dev->id); + if (st->trig == NULL) { + ret = -ENOMEM; + goto error_ret; + } + /* select default trigger */ + st->trig->dev.parent = &st->client->dev; + st->trig->private_data = indio_dev; + st->trig->ops = &inv_yas53x_trigger_ops; + ret = iio_trigger_register(st->trig); + + /* select default trigger */ + indio_dev->trig = st->trig; + if (ret) + goto error_free_trig; + + return 0; + +error_free_trig: + iio_free_trigger(st->trig); +error_ret: + return ret; +} + +void inv_yas53x_remove_trigger(struct iio_dev *indio_dev) +{ + struct inv_compass_state *st = iio_priv(indio_dev); + + iio_trigger_unregister(st->trig); + iio_free_trigger(st->trig); +} +/** + * @} + */ + |