text: /*
   * 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
   * 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) "inv_mpu: " fmt
  
  #include <linux/module.h>
  #include <linux/init.h>
  #include <linux/slab.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/math64.h>
  
  #include "../inv_mpu_iio.h"
  
  static char iden[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
  
  static int inv_process_gyro(struct inv_mpu_state *st, u8 *d, u64 t)
  {
  	s16 raw[3];
  	s32 calib[3];
  	int i;
  #define BIAS_UNIT 2859
  
  	for (i = 0; i < 3; i++)
  		raw[i] = be16_to_cpup((__be16 *) (d + i * 2));
  
  	for (i = 0; i < 3; i++)
  		calib[i] = (raw[i] << 15);
  
  
  	inv_push_gyro_data(st, raw, calib, t);
  
  	return 0;
  }
  
  static int inv_check_fsync(struct inv_mpu_state *st, u8 fsync_status)
  {
  	u8 data[1];
  
  	if (!st->chip_config.eis_enable)
  		return 0;
  	inv_plat_read(st, REG_FSYNC_INT, 1, data);
  	if (data[0] & BIT_FSYNC_INT) {
  		pr_debug("fsync\n");
  		st->eis.eis_triggered = true;
  		st->eis.fsync_delay = 1;
  		st->eis.prev_state = 1;
  		st->eis.frame_count++;
  		st->eis.eis_frame = true;
  	}
  	st->header_count--;
  
  	return 0;
  }
  
  static int inv_push_sensor(struct inv_mpu_state *st, int ind, u64 t, u8 *d)
  {
  #ifdef ACCEL_BIAS_TEST
  	s16 acc[3], avg[3];
  #endif
  
  	switch (ind) {
  	case SENSOR_ACCEL:
  		inv_convert_and_push_8bytes(st, ind, d, t, iden);
  #ifdef ACCEL_BIAS_TEST
  		acc[0] = be16_to_cpup((__be16 *) (d));
  		acc[1] = be16_to_cpup((__be16 *) (d + 2));
  		acc[2] = be16_to_cpup((__be16 *) (d + 4));
  		if(inv_get_3axis_average(acc, avg, 0)){
  			pr_debug("accel 200 samples average = %5d, %5d, %5d\n", avg[0], avg[1], avg[2]);
  		}
  #endif
  		break;
  	case SENSOR_TEMP:
  		inv_check_fsync(st, d[1]);
  		break;
  	case SENSOR_GYRO:
  		inv_process_gyro(st, d, t);
  		break;
  	default:
  		break;
  	}
  
  	return 0;
  }
  
  static int inv_push_20680_data(struct inv_mpu_state *st, u8 *d)
  {
  	u8 *dptr;
  	int i;
  
  	dptr = d;
  
  	for (i = 0; i < SENSOR_NUM_MAX; i++) {
  		if (st->sensor[i].on) {
  			inv_get_dmp_ts(st, i);
  			if (st->sensor[i].send && (!st->ts_algo.first_sample)) {
  				st->sensor[i].sample_calib++;
  				inv_push_sensor(st, i, st->sensor[i].ts, dptr);
  			}
  			dptr += st->sensor[i].sample_size;
  		}
  	}
  	if (st->ts_algo.first_sample)
  		st->ts_algo.first_sample--;
  	st->header_count--;
  
  	return 0;
  }
  
  static int inv_process_20680_data(struct inv_mpu_state *st)
  {
  	int total_bytes, tmp, res, fifo_count, pk_size, i;
  	u8 *dptr, *d;
  	u8 data[14];
  	bool done_flag;
  	u8 v;
  #ifdef SENSOR_DATA_FROM_REGISTERS
  	u8 reg;
  	int len;
  #endif
  
  	if(st->gesture_only_on && (!st->batch.timeout)) {
  		res = inv_plat_read(st, REG_INT_STATUS, 1, data);
  		if (res)
  			return res;
  		pr_debug("ges cnt=%d, statu=%x\n",
  						st->gesture_int_count, data[0]);
  		if (data[0] & (BIT_WOM_ALL_INT_EN)) {
  			if (!st->gesture_int_count) {
  				inv_switch_power_in_lp(st, true);
  				res = inv_plat_single_write(st, REG_INT_ENABLE,
  					BIT_WOM_ALL_INT_EN | BIT_DATA_RDY_EN);
  				if (res)
  					return res;
  				v = 0;
  				if (st->chip_config.gyro_enable)
  					v |= BITS_GYRO_FIFO_EN;
  
  				if (st->chip_config.accel_enable)
  					v |= BIT_ACCEL_FIFO_EN;
  				res = inv_plat_single_write(st, REG_FIFO_EN, v);
  				if (res)
  					return res;
  				/* First time wake up from WOM.
  					We don't need data in the FIFO */
  				res = inv_reset_fifo(st, true);
  				if (res)
  					return res;
  				res = inv_switch_power_in_lp(st, false);
  				st->gesture_int_count = WOM_DELAY_THRESHOLD;
  
  				return res;
  			}
  			st->gesture_int_count = WOM_DELAY_THRESHOLD;
  		} else {
  			if (!st->gesture_int_count) {
  				inv_switch_power_in_lp(st, true);
  				res = inv_plat_single_write(st, REG_FIFO_EN, 0);
  				res = inv_plat_single_write(st, REG_INT_ENABLE,
  					BIT_WOM_ALL_INT_EN);
  				inv_switch_power_in_lp(st, false);
  
  				return res;
  			}
  			st->gesture_int_count--;
  		}
  	}
  
  	fifo_count = inv_get_last_run_time_non_dmp_record_mode(st);
  	pr_debug("fifc= %d\n", fifo_count);
  	if (!fifo_count) {
  		pr_debug("REG_FIFO_COUNT_H size is 0\n");
  		return 0;
  	}
  	pk_size = st->batch.pk_size;
  	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;
  	dptr = d;
  	total_bytes = fifo_count;
  
  #ifdef SENSOR_DATA_FROM_REGISTERS
  	len = 0;
  	if (st->sensor[SENSOR_GYRO].on) {
  		reg = REG_RAW_GYRO;
  		len += BYTES_PER_SENSOR;
  		if (st->sensor[SENSOR_ACCEL].on && !st->sensor[SENSOR_TEMP].on)
  			len += BYTES_FOR_TEMP;
  	}
  	if (st->sensor[SENSOR_TEMP].on) {
  		reg = REG_RAW_TEMP;
  		len += BYTES_FOR_TEMP;
  	}
  	if (st->sensor[SENSOR_ACCEL].on) {
  		reg = REG_RAW_ACCEL;
  		len += BYTES_PER_SENSOR;
  	}
  
  	if (len == 0) {
  		pr_debug("No sensor is enabled\n");
  		return 0;
  	}
  
  	/* read data registers */
  	res = inv_plat_read(st, reg, len, data);
  	if (res < 0) {
  		pr_err("read data registers is failed\n");
  		return res;
  	}
  
  	/* copy sensor data to buffer as FIFO data format */
  	tmp = 0;
  	if (st->sensor[SENSOR_ACCEL].on) {
  		for (i = 0; i < BYTES_PER_SENSOR; i++)
  			dptr[i] = data[tmp + i];
  		dptr += BYTES_PER_SENSOR;
  		tmp += BYTES_PER_SENSOR;
  	}
  
  	if (st->sensor[SENSOR_TEMP].on) {
  		for (i = 0; i < BYTES_FOR_TEMP; i++)
  			dptr[i] = data[tmp + i];
  		dptr += BYTES_FOR_TEMP;
  		tmp += BYTES_FOR_TEMP;
  	}
  
  	if (st->sensor[SENSOR_GYRO].on) {
  		if (st->sensor[SENSOR_ACCEL].on && !st->sensor[SENSOR_TEMP].on)
  			tmp += BYTES_FOR_TEMP;
  		for (i = 0; i < BYTES_PER_SENSOR; i++)
  			dptr[i] = data[tmp + i];
  	}
  #else
  	while (total_bytes > 0) {
  		if (total_bytes < pk_size * MAX_FIFO_PACKET_READ)
  			tmp = total_bytes;
  		else
  			tmp = pk_size * MAX_FIFO_PACKET_READ;
  		res = inv_plat_read(st, REG_FIFO_R_W, tmp, dptr);
  		if (res < 0) {
  			pr_err("read REG_FIFO_R_W is failed\n");
  			return res;
  		}
  		pr_debug("inside: %x, %x, %x, %x, %x, %x, %x, %x\n", dptr[0], dptr[1], dptr[2],
  						dptr[3], dptr[4], dptr[5], dptr[6], dptr[7]);
  		pr_debug("insid2: %x, %x, %x, %x, %x, %x, %x, %x\n", dptr[8], dptr[9], dptr[10],
  						dptr[11], dptr[12], dptr[13], dptr[14], dptr[15]);
  
  		dptr += tmp;
  		total_bytes -= tmp;
  	}
  #endif /* SENSOR_DATA_FROM_REGISTERS */
  	dptr = d;
  	pr_debug("dd: %x, %x, %x, %x, %x, %x, %x, %x\n", d[0], d[1], d[2],
  						d[3], d[4], d[5], d[6], d[7]);
  	pr_debug("dd2: %x, %x, %x, %x, %x, %x, %x, %x\n", d[8], d[9], d[10],
  					d[11], d[12], d[13], d[14], d[15]);
  	total_bytes = fifo_count;
  
  	for (i = 0; i < SENSOR_NUM_MAX; i++) {
  		if (st->sensor[i].on) {
  			st->sensor[i].count =  total_bytes / pk_size;
  		}
  	}
  	st->header_count = 0;
  	for (i = 0; i < SENSOR_NUM_MAX; i++) {
  		if (st->sensor[i].on)
  			st->header_count = max(st->header_count,
  							st->sensor[i].count);
  	}
  
  	st->ts_algo.calib_counter++;
  	inv_bound_timestamp(st);
  
  	dptr = d;
  	done_flag = false;
  
  	while (!done_flag) {
  		pr_debug("total%d, pk=%d\n", total_bytes, pk_size);
  		if (total_bytes >= pk_size) {
  			res = inv_push_20680_data(st, dptr);
  			if (res)
  				return res;
  			total_bytes -= pk_size;
  			dptr += pk_size;
  		} else {
  			done_flag = true;
  		}
  	}
  
  	return 0;
  }
  
  /*
   *  _inv_read_fifo() - Transfer data from FIFO to ring buffer.
   */
  static void _inv_read_fifo(struct inv_mpu_state *st)
  {
  	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;
  	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)
  		goto err_reset_fifo;
  	mutex_unlock(&indio_dev->mlock);
  
  	if (st->wake_sensor_received)
  #ifdef CONFIG_HAS_WAKELOCK
  		wake_lock_timeout(&st->wake_lock, msecs_to_jiffies(200));
  #else
  		__pm_wakeup_event(&st->wake_lock, 200); /* 200 msecs */
  #endif
  	return;
  
  err_reset_fifo:
  	if ((!st->chip_config.gyro_enable) &&
  		(!st->chip_config.accel_enable) &&
  		(!st->chip_config.slave_enable) &&
  		(!st->chip_config.pressure_enable)) {
  		inv_switch_power_in_lp(st, false);
  		mutex_unlock(&indio_dev->mlock);
  
  		return;
  	}
  
  	pr_err("error to reset fifo\n");
  	inv_switch_power_in_lp(st, true);
  	inv_reset_fifo(st, true);
  	inv_switch_power_in_lp(st, false);
  	mutex_unlock(&indio_dev->mlock);
  
  	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);
  
  #ifndef SENSOR_DATA_FROM_REGISTERS
  	if (st->chip_config.gyro_enable ||
  		st->chip_config.accel_enable ||
  		st->chip_config.slave_enable ||
  		st->chip_config.pressure_enable) {
  		st->wake_sensor_received = 0;
  		inv_process_20680_data(st);
  		if (st->wake_sensor_received)
  #ifdef CONFIG_HAS_WAKELOCK
  			wake_lock_timeout(&st->wake_lock, msecs_to_jiffies(200));
  #else
  			__pm_wakeup_event(&st->wake_lock, 200); /* 200 msecs */
  #endif
  		inv_switch_power_in_lp(st, false);
  	}
  #endif /* SENSOR_DATA_FROM_REGISTERS */
  	inv_push_marker_to_buffer(st, END_MARKER, data);
  
  	return 0;
  }
  
  