From b1c8f25f65b5465eb7fb7efa10692fa7e17ffc44 Mon Sep 17 00:00:00 2001 From: Eiji Iwatsuki Date: Tue, 1 May 2018 10:26:25 -0700 Subject: [PATCH] MA Lite 8.1.5 Module: * 8.1.2-simple-test1 Supported Kernel Versions: * 3.18 * 4.4 * 4.9 Supported TDK-InvenSense Sensors: * IAM20680 Change log: * Timer based batch mode support for IAM20680 --- .../inv_mpu/iam20680/inv_mpu_iio_reg_20680.h | 15 ++++-- .../imu/inv_mpu/iam20680/inv_mpu_init_20680.c | 6 ++- .../inv_mpu/iam20680/inv_mpu_parsing_20680.c | 51 +++++++++++++++---- .../inv_mpu/iam20680/inv_mpu_setup_20680.c | 43 ++++++++++++++++ drivers/iio/imu/inv_mpu/inv_mpu_i2c.c | 5 +- drivers/iio/imu/inv_mpu/inv_mpu_iio.h | 14 +++-- drivers/iio/imu/inv_mpu/inv_mpu_ring.c | 23 +++++++++ drivers/iio/imu/inv_mpu/inv_mpu_spi.c | 5 +- 8 files changed, 143 insertions(+), 19 deletions(-) diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h index 83c5cc7af653..3f8ce71be024 100644 --- a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_iio_reg_20680.h @@ -19,10 +19,17 @@ * sysfs entries */ #define SUPPORT_ONLY_BASIC_FEATURES -/* Uncomment to read data registers for sensor data - * instead of FIFO */ +/* Uncomment to read data registers for sensor data instead of FIFO */ //#define SENSOR_DATA_FROM_REGISTERS +/* Uncomment to enable timer based batching */ +#define TIMER_BASED_BATCHING + +/* Polling (batch mode) can be enabled only when FIFO read */ +#if defined(SENSOR_DATA_FROM_REGISTERS) +#undef TIMER_BASED_BATCHING +#endif + /*register and associated bit definition*/ #define REG_XA_OFFS_H 0x77 #define REG_YA_OFFS_H 0x7A @@ -140,7 +147,7 @@ #define BYTES_FOR_TEMP 2 #define FIFO_COUNT_BYTE 2 #define HARDWARE_FIFO_SIZE 512 -#define FIFO_SIZE (HARDWARE_FIFO_SIZE * 7 / 8) +#define FIFO_SIZE (HARDWARE_FIFO_SIZE * 7 / 10) #define POWER_UP_TIME 100 #define REG_UP_TIME_USEC 100 #define LEFT_OVER_BYTES 128 @@ -190,7 +197,7 @@ enum inv_filter_e { #define GESTURE_ACCEL_RATE 50 #define ESI_GYRO_RATE 1000 #define MAX_FIFO_PACKET_READ 6 -#define MAX_BATCH_FIFO_SIZE 896 +#define MAX_BATCH_FIFO_SIZE FIFO_SIZE #define MIN_MST_ODR_CONFIG 4 #define MAX_MST_ODR_CONFIG 5 diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c index 1f76d3b00745..58bd8d073890 100644 --- a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_init_20680.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2017-2017 InvenSense, Inc. + * Copyright (C) 2017-2018 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 @@ -172,6 +172,10 @@ static int inv_init_config(struct inv_mpu_state *st) st->eis.count_precision = NSEC_PER_MSEC; st->firmware = 0; st->fifo_count_mode = BYTE_MODE; +#ifdef TIMER_BASED_BATCHING + st->batch_timeout = 0; + st->is_batch_timer_running = false; +#endif st->eng_info[ENGINE_GYRO].base_time = NSEC_PER_SEC; st->eng_info[ENGINE_ACCEL].base_time = NSEC_PER_SEC; diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c index 5e20f0086e9b..0f17b6dd2c52 100644 --- a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_parsing_20680.c @@ -193,6 +193,11 @@ static int inv_process_20680_data(struct inv_mpu_state *st) if (!pk_size) return -EINVAL; + if (fifo_count >= (HARDWARE_FIFO_SIZE / st->batch.pk_size)) { + pr_warn("fifo overflow pkt count=%d pkt sz=%d\n", fifo_count, st->batch.pk_size); + return -EOVERFLOW; + } + fifo_count *= st->batch.pk_size; st->fifo_count = fifo_count; d = st->fifo_data_store; @@ -312,20 +317,24 @@ static int inv_process_20680_data(struct inv_mpu_state *st) } /* - * inv_read_fifo() - Transfer data from FIFO to ring buffer. + * _inv_read_fifo() - Transfer data from FIFO to ring buffer. */ -irqreturn_t inv_read_fifo(int irq, void *dev_id) +static void _inv_read_fifo(struct inv_mpu_state *st) { - - struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; struct iio_dev *indio_dev = iio_priv_to_dev(st); int result; result = wait_event_interruptible_timeout(st->wait_queue, st->resume_state, msecs_to_jiffies(300)); if (result <= 0) - return IRQ_HANDLED; + return; mutex_lock(&indio_dev->mlock); +#ifdef TIMER_BASED_BATCHING + if (st->batch_timeout) { + if (inv_plat_single_write(st, REG_INT_ENABLE, st->int_en)) + pr_err("REG_INT_ENABLE write error\n"); + } +#endif st->wake_sensor_received = false; result = inv_process_20680_data(st); if (result) @@ -338,7 +347,7 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id) #else __pm_wakeup_event(&st->wake_lock, 200); /* 200 msecs */ #endif - return IRQ_HANDLED; + return; err_reset_fifo: if ((!st->chip_config.gyro_enable) && @@ -348,7 +357,7 @@ err_reset_fifo: inv_switch_power_in_lp(st, false); mutex_unlock(&indio_dev->mlock); - return IRQ_HANDLED; + return; } pr_err("error to reset fifo\n"); @@ -357,10 +366,34 @@ err_reset_fifo: inv_switch_power_in_lp(st, false); mutex_unlock(&indio_dev->mlock); - return IRQ_HANDLED; - + return; } +irqreturn_t inv_read_fifo(int irq, void *dev_id) +{ + struct inv_mpu_state *st = (struct inv_mpu_state *)dev_id; + + _inv_read_fifo(st); + + return IRQ_HANDLED; +} + +#ifdef TIMER_BASED_BATCHING +void inv_batch_work(struct work_struct *work) +{ + struct inv_mpu_state *st = + container_of(work, struct inv_mpu_state, batch_work); + struct iio_dev *indio_dev = iio_priv_to_dev(st); + + mutex_lock(&indio_dev->mlock); + if (inv_plat_single_write(st, REG_INT_ENABLE, st->int_en | BIT_DATA_RDY_EN)) + pr_err("REG_INT_ENABLE write error\n"); + mutex_unlock(&indio_dev->mlock); + + return; +} +#endif + int inv_flush_batch_data(struct iio_dev *indio_dev, int data) { struct inv_mpu_state *st = iio_priv(indio_dev); diff --git a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c index d922ce5ba6f0..5e9cf8906d79 100644 --- a/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c +++ b/drivers/iio/imu/inv_mpu/iam20680/inv_mpu_setup_20680.c @@ -44,6 +44,13 @@ static int inv_turn_on_fifo(struct inv_mpu_state *st) st->gesture_int_count = WOM_DELAY_THRESHOLD; int_en |= BIT_WOM_ALL_INT_EN; } +#ifdef TIMER_BASED_BATCHING + if (st->chip_config.eis_enable) + int_en |= BIT_FSYNC_INT_EN; + if (!st->batch_timeout) { + int_en |= BIT_DATA_RDY_EN; + } +#else if (st->batch.timeout) { if(!st->batch.fifo_wm_th) int_en = BIT_DATA_RDY_EN; @@ -52,6 +59,7 @@ static int inv_turn_on_fifo(struct inv_mpu_state *st) if (st->chip_config.eis_enable) int_en |= BIT_FSYNC_INT_EN; } +#endif if (st->sensor[SENSOR_GYRO].on) fifo_en |= BITS_GYRO_FIFO_EN; @@ -60,6 +68,7 @@ static int inv_turn_on_fifo(struct inv_mpu_state *st) r = inv_plat_single_write(st, REG_FIFO_EN, fifo_en); if (r) return r; + st->int_en = int_en; r = inv_plat_single_write(st, REG_INT_ENABLE, int_en); if (r) return r; @@ -75,6 +84,19 @@ static int inv_turn_on_fifo(struct inv_mpu_state *st) user = BIT_FIFO_EN; #endif r = inv_plat_single_write(st, REG_USER_CTRL, user | st->i2c_dis); +#ifdef TIMER_BASED_BATCHING + if (fifo_en && st->batch_timeout) { + if (st->is_batch_timer_running) + hrtimer_cancel(&st ->hr_batch_timer); + st->is_batch_timer_running = true; + hrtimer_start(&st ->hr_batch_timer, + ns_to_ktime(st->batch_timeout), HRTIMER_MODE_REL); + } else { + if (st->is_batch_timer_running) + hrtimer_cancel(&st ->hr_batch_timer); + st->is_batch_timer_running = false; + } +#endif return r; } @@ -245,6 +267,27 @@ static int inv_set_div(struct inv_mpu_state *st, int a_d, int g_d) // 20680 does not support batching static int inv_set_batch(struct inv_mpu_state *st) { +#ifdef TIMER_BASED_BATCHING + u64 timeout; + int required_fifo_size; + + if (st->batch.timeout) { + required_fifo_size = st->batch.timeout * st->eng_info[ENGINE_GYRO].running_rate + * st->batch.pk_size / 1000; + if (required_fifo_size > MAX_BATCH_FIFO_SIZE) { + required_fifo_size = MAX_BATCH_FIFO_SIZE; + timeout = (required_fifo_size / st->batch.pk_size) * (1000 / st->eng_info[ENGINE_GYRO].running_rate); + } else { + timeout = st->batch.timeout; + } + } else { + timeout = 1000 / st->eng_info[ENGINE_GYRO].running_rate; + } + if (timeout <= 1000 / st->eng_info[ENGINE_GYRO].running_rate) + st->batch_timeout = 0; + else + st->batch_timeout = timeout * 1000000; // ms to ns +#endif st->batch.fifo_wm_th = 0; return 0; diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c index 0c2eaf3b8cd7..e7838fce84a8 100644 --- a/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c +++ b/drivers/iio/imu/inv_mpu/inv_mpu_i2c.c @@ -1,5 +1,5 @@ /* -* Copyright (C) 2012-2017 InvenSense, Inc. +* Copyright (C) 2012-2018 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 @@ -417,6 +417,9 @@ static int inv_mpu_probe(struct i2c_client *client, #else pr_info("Data read from FIFO\n"); #endif +#ifdef TIMER_BASED_BATCHING + pr_info("Timer based batching\n"); +#endif return 0; #ifdef KERNEL_VERSION_4_X diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h index 426757c75d96..9e7316558eae 100644 --- a/drivers/iio/imu/inv_mpu/inv_mpu_iio.h +++ b/drivers/iio/imu/inv_mpu/inv_mpu_iio.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2012-2017 InvenSense, Inc. + * Copyright (C) 2012-2018 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 @@ -59,7 +59,7 @@ #include "iam20680/inv_mpu_iio_reg_20680.h" #endif -#define INVENSENSE_DRIVER_VERSION "8.1.1-simple-test1a" +#define INVENSENSE_DRIVER_VERSION "8.1.2-simple-test1" /* #define DEBUG */ @@ -722,6 +722,12 @@ struct inv_mpu_state { struct wake_lock wake_lock; #else struct wakeup_source wake_lock; +#endif +#ifdef TIMER_BASED_BATCHING + struct hrtimer hr_batch_timer; + u64 batch_timeout; + bool is_batch_timer_running; + struct work_struct batch_work; #endif struct i2c_client *client; struct mpu_platform_data plat_data; @@ -1018,7 +1024,9 @@ int inv_create_dmp_sysfs(struct iio_dev *ind); int inv_check_chip_type(struct iio_dev *indio_dev, const char *name); int inv_write_compass_matrix(struct inv_mpu_state *st, int *adj); irqreturn_t inv_read_fifo(int irq, void *dev_id); - +#ifdef TIMER_BASED_BATCHING +void inv_batch_work(struct work_struct *work); +#endif int inv_flush_batch_data(struct iio_dev *indio_dev, int data); static inline int mpu_memory_write(struct inv_mpu_state *st, u8 mpu_addr, u16 mem_addr, u32 len, u8 const *data) diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c index bbd808f09177..3e5bccbea0df 100644 --- a/drivers/iio/imu/inv_mpu/inv_mpu_ring.c +++ b/drivers/iio/imu/inv_mpu/inv_mpu_ring.c @@ -533,6 +533,23 @@ static irqreturn_t inv_irq_handler(int irq, void *dev_id) return IRQ_WAKE_THREAD; } +#ifdef TIMER_BASED_BATCHING +static enum hrtimer_restart inv_batch_timer_handler(struct hrtimer *timer) +{ + struct inv_mpu_state *st = + container_of(timer, struct inv_mpu_state, hr_batch_timer); + + if (st->chip_config.gyro_enable || st->chip_config.accel_enable) { + hrtimer_forward_now(&st->hr_batch_timer, + ns_to_ktime(st->batch_timeout)); + schedule_work(&st->batch_work); + return HRTIMER_RESTART; + } + st->is_batch_timer_running = 0; + return HRTIMER_NORESTART; +} +#endif + void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev) { struct inv_mpu_state *st = iio_priv(indio_dev); @@ -569,6 +586,12 @@ int inv_mpu_configure_ring(struct iio_dev *indio_dev) struct inv_mpu_state *st = iio_priv(indio_dev); struct iio_buffer *ring; +#ifdef TIMER_BASED_BATCHING + /* configure hrtimer */ + hrtimer_init(&st->hr_batch_timer, CLOCK_BOOTTIME, HRTIMER_MODE_REL); + st->hr_batch_timer.function = inv_batch_timer_handler; + INIT_WORK(&st->batch_work, inv_batch_work); +#endif #ifdef KERNEL_VERSION_4_X ring = devm_iio_kfifo_allocate(st->dev); if (!ring) diff --git a/drivers/iio/imu/inv_mpu/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu/inv_mpu_spi.c index 4762ce614b40..fb916788a9df 100644 --- a/drivers/iio/imu/inv_mpu/inv_mpu_spi.c +++ b/drivers/iio/imu/inv_mpu/inv_mpu_spi.c @@ -1,5 +1,5 @@ /* -* Copyright (C) 2012-2017 InvenSense, Inc. +* Copyright (C) 2012-2018 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 @@ -271,6 +271,9 @@ static int inv_mpu_probe(struct spi_device *spi) #else pr_info("Data read from FIFO\n"); #endif +#ifdef TIMER_BASED_BATCHING + pr_info("Timer based batching\n"); +#endif return 0; #ifdef KERNEL_VERSION_4_X