Merge "driver: iio: imu: asm330 early buffer implemented" (2e003568) · Commits · e / devices / android_kernel_xiaomi_markw

drivers/iio/imu/st_asm330lhh/Kconfig

+6 −0

Original line number	Diff line number	Diff line
		@@ -21,3 +21,9 @@ config IIO_ST_ASM330LHH_SPI
		tristate
		depends on IIO_ST_ASM330LHH

		config ENABLE_ASM_ACC_GYRO_BUFFERING
		bool "Enable accel & gyro boot time sensor sample buffering"
		depends on IIO_ST_ASM330LHH
		help
		Say Y here if you want to buffer boot time sensor
		samples for ASM330 accelerometer and gyroscope

drivers/iio/imu/st_asm330lhh/st_asm330lhh.h

+29 −0

Original line number	Diff line number	Diff line
		@@ -13,6 +13,9 @@

		#include <linux/device.h>
		#include <linux/iio/iio.h>
		#include <linux/input.h>
		#include <linux/ktime.h>
		#include <linux/slab.h>

		/*
		* Module version:
		@@ -87,6 +90,16 @@ static const struct iio_event_spec st_asm330lhh_flush_event = {
		#define ST_ASM330LHH_RX_MAX_LENGTH 8
		#define ST_ASM330LHH_TX_MAX_LENGTH 8

		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		#define ASM_MAXSAMPLE 4000
		#define G_MAX 23920640
		struct asm_sample {
		int xyz[3];
		unsigned int tsec;
		unsigned long long tnsec;
		};
		#endif

		struct st_asm330lhh_transfer_buffer {
		u8 rx_buf[ST_ASM330LHH_RX_MAX_LENGTH];
		u8 tx_buf[ST_ASM330LHH_TX_MAX_LENGTH] ____cacheline_aligned;
		@@ -170,6 +183,17 @@ struct st_asm330lhh_sensor {
		u16 watermark;
		u8 batch_mask;
		u8 batch_addr;
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		bool read_boot_sample;
		int bufsample_cnt;
		bool buffer_asm_samples;
		struct kmem_cache *asm_cachepool;
		struct asm_sample *asm_samplist[ASM_MAXSAMPLE];
		ktime_t timestamp;
		int max_buffer_time;
		struct input_dev *buf_dev;
		int report_evt_cnt;
		#endif
		};

		/**
		@@ -242,4 +266,9 @@ ssize_t st_asm330lhh_set_watermark(struct device *dev,
		int st_asm330lhh_set_fifo_mode(struct st_asm330lhh_hw *hw,
		enum st_asm330lhh_fifo_mode fifo_mode);
		int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw);
		int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,
		u16 watermark);
		int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable);
		int asm330_check_acc_gyro_early_buff_enable_flag(
		struct st_asm330lhh_sensor *sensor);
		#endif /* ST_ASM330LHH_H */

drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c

+76 −4

Original line number	Diff line number	Diff line
		@@ -121,7 +121,7 @@ static u16 st_asm330lhh_ts_odr(struct st_asm330lhh_hw *hw)
		return odr;
		}

		static int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,
		int st_asm330lhh_update_watermark(struct st_asm330lhh_sensor *sensor,
		u16 watermark)
		{
		u16 fifo_watermark = ST_ASM330LHH_MAX_FIFO_DEPTH, cur_watermark = 0;
		@@ -190,6 +190,61 @@ static struct iio_dev st_asm330lhh_get_iiodev_from_tag(struct st_asm330lhh_hw

		return iio_dev;
		}
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		int asm330_check_acc_gyro_early_buff_enable_flag(
		struct st_asm330lhh_sensor *sensor)
		{
		if (sensor->buffer_asm_samples == true)
		return 1;
		else
		return 0;
		}
		#else
		int asm330_check_acc_gyro_early_buff_enable_flag(
		struct st_asm330lhh_sensor *sensor)
		{
		return 0;
		}
		#endif

		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		static void store_acc_gyro_boot_sample(struct st_asm330lhh_sensor *sensor,
		u8 *iio_buf, s64 tsample)
		{
		int x, y, z;

		if (false == sensor->buffer_asm_samples)
		return;

		sensor->timestamp = (ktime_t)tsample;
		x = iio_buf[1]<<8\|iio_buf[0];
		y = iio_buf[3]<<8\|iio_buf[2];
		z = iio_buf[5]<<8\|iio_buf[4];

		if (ktime_to_timespec(sensor->timestamp).tv_sec
		< sensor->max_buffer_time) {
		if (sensor->bufsample_cnt < ASM_MAXSAMPLE) {
		sensor->asm_samplist[sensor->bufsample_cnt]->xyz[0] = x;
		sensor->asm_samplist[sensor->bufsample_cnt]->xyz[1] = x;
		sensor->asm_samplist[sensor->bufsample_cnt]->xyz[2] = x;
		sensor->asm_samplist[sensor->bufsample_cnt]->tsec =
		ktime_to_timespec(sensor->timestamp).tv_sec;
		sensor->asm_samplist[sensor->bufsample_cnt]->tnsec =
		ktime_to_timespec(sensor->timestamp).tv_nsec;
		sensor->bufsample_cnt++;
		}
		} else {
		dev_info(sensor->hw->dev, "End of sensor %duffering %d\n",
		sensor->id, sensor->bufsample_cnt);
		sensor->buffer_asm_samples = false;
		}
		}
		#else
		static void store_acc_gyro_boot_sample(struct st_asm330lhh_sensor *sensor,
		u8 *iio_buf, s64 tsample)
		{
		}
		#endif

		static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)
		{
		@@ -278,6 +333,8 @@ static int st_asm330lhh_read_fifo(struct st_asm330lhh_hw *hw)
		iio_push_to_buffers_with_timestamp(iio_dev,
		iio_buf,
		hw->tsample);
		store_acc_gyro_boot_sample(sensor,
		iio_buf, hw->tsample);
		}
		}
		read_len += word_len;
		@@ -316,6 +373,9 @@ ssize_t st_asm330lhh_set_watermark(struct device *dev,
		struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
		int err, val;

		if (asm330_check_acc_gyro_early_buff_enable_flag(sensor))
		return 0;

		mutex_lock(&iio_dev->mlock);
		if (iio_buffer_enabled(iio_dev)) {
		err = -EBUSY;
		@@ -381,7 +441,7 @@ int st_asm330lhh_suspend_fifo(struct st_asm330lhh_hw *hw)
		return err;
		}

		static int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable)
		int st_asm330lhh_update_fifo(struct iio_dev *iio_dev, bool enable)
		{
		struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
		struct st_asm330lhh_hw *hw = sensor->hw;
		@@ -443,11 +503,23 @@ static irqreturn_t st_asm330lhh_handler_thread(int irq, void *private)

		static int st_asm330lhh_buffer_preenable(struct iio_dev *iio_dev)
		{
		struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
		int err = -1;

		if (asm330_check_acc_gyro_early_buff_enable_flag(sensor))
		return err;
		else
		return st_asm330lhh_update_fifo(iio_dev, true);
		}

		static int st_asm330lhh_buffer_postdisable(struct iio_dev *iio_dev)
		{
		struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
		int err = -1;

		if (asm330_check_acc_gyro_early_buff_enable_flag(sensor))
		return err;
		else
		return st_asm330lhh_update_fifo(iio_dev, false);
		}

drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c

+337 −0

Original line number	Diff line number	Diff line
		@@ -432,6 +432,9 @@ static int st_asm330lhh_write_raw(struct iio_dev *iio_dev,
		struct st_asm330lhh_sensor *sensor = iio_priv(iio_dev);
		int err;

		if (asm330_check_acc_gyro_early_buff_enable_flag(sensor))
		return 0;

		mutex_lock(&iio_dev->mlock);

		switch (mask) {
		@@ -494,10 +497,129 @@ static ssize_t st_asm330lhh_sysfs_scale_avail(struct device *dev,

		return len;
		}
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		static int asm_read_bootsampl(struct st_asm330lhh_sensor *sensor,
		unsigned long enable_read)
		{
		int i = 0;

		if (enable_read) {
		sensor->buffer_asm_samples = false;
		for (i = 0; i < sensor->bufsample_cnt; i++) {
		dev_dbg(sensor->hw->dev,
		"sensor:%d count:%d x=%d,y=%d,z=%d,tsec=%d,nsec=%lld\n",
		sensor->id, i, sensor->asm_samplist[i]->xyz[0],
		sensor->asm_samplist[i]->xyz[1],
		sensor->asm_samplist[i]->xyz[2],
		sensor->asm_samplist[i]->tsec,
		sensor->asm_samplist[i]->tnsec);
		input_report_abs(sensor->buf_dev, ABS_X,
		sensor->asm_samplist[i]->xyz[0]);
		input_report_abs(sensor->buf_dev, ABS_Y,
		sensor->asm_samplist[i]->xyz[1]);
		input_report_abs(sensor->buf_dev, ABS_Z,
		sensor->asm_samplist[i]->xyz[2]);
		input_report_abs(sensor->buf_dev, ABS_RX,
		sensor->asm_samplist[i]->tsec);
		input_report_abs(sensor->buf_dev, ABS_RY,
		sensor->asm_samplist[i]->tnsec);
		input_sync(sensor->buf_dev);
		}
		} else {
		/* clean up */
		if (sensor->bufsample_cnt != 0) {
		for (i = 0; i < ASM_MAXSAMPLE; i++)
		kmem_cache_free(sensor->asm_cachepool,
		sensor->asm_samplist[i]);
		kmem_cache_destroy(sensor->asm_cachepool);
		sensor->bufsample_cnt = 0;
		}

		}
		/SYN_CONFIG indicates end of data/
		input_event(sensor->buf_dev, EV_SYN, SYN_CONFIG, 0xFFFFFFFF);
		input_sync(sensor->buf_dev);
		dev_dbg(sensor->hw->dev, "End of gyro samples bufsample_cnt=%d\n",
		sensor->bufsample_cnt);
		return 0;
		}
		static ssize_t read_gyro_boot_sample_show(struct device *dev,
		struct device_attribute *attr,
		char *buf)
		{
		struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));

		return snprintf(buf, PAGE_SIZE, "%d\n",
		sensor->read_boot_sample);
		}
		static ssize_t read_gyro_boot_sample_store(struct device *dev,
		struct device_attribute *attr,
		const char *buf, size_t count)
		{
		int err;
		struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));
		unsigned long enable = 0;

		err = kstrtoul(buf, 10, &enable);
		if (err)
		return err;
		if (enable > 1) {
		err = dev_err(sensor->hw->dev,
		"Invalid value of input, input=%ld\n", enable);
		return -EINVAL;
		}
		err = asm_read_bootsampl(sensor, enable);
		if (err)
		return err;
		sensor->read_boot_sample = enable;
		return count;

		}

		static ssize_t read_acc_boot_sample_show(struct device *dev,
		struct device_attribute *attr,
		char *buf)
		{
		struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));

		return snprintf(buf, PAGE_SIZE, "%d\n",
		sensor->read_boot_sample);
		}

		static ssize_t read_acc_boot_sample_store(struct device *dev,
		struct device_attribute *attr,
		const char *buf, size_t count)
		{
		int err;
		struct st_asm330lhh_sensor *sensor = iio_priv(dev_get_drvdata(dev));

		unsigned long enable = 0;

		err = kstrtoul(buf, 10, &enable);
		if (err)
		return err;
		if (enable > 1) {
		err = dev_err(sensor->hw->dev,
		"Invalid value of input, input=%ld\n", enable);
		return -EINVAL;
		}
		err = asm_read_bootsampl(sensor, enable);
		if (err)
		return err;
		sensor->read_boot_sample = enable;
		return count;
		}
		#endif

		static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_asm330lhh_sysfs_sampling_frequency_avail);
		static IIO_DEVICE_ATTR(in_accel_scale_available, 0444,
		st_asm330lhh_sysfs_scale_avail, NULL, 0);
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		static IIO_DEVICE_ATTR(read_acc_boot_sample, 0444,
		read_acc_boot_sample_show, read_acc_boot_sample_store, 0);
		static IIO_DEVICE_ATTR(read_gyro_boot_sample, 0444,
		read_gyro_boot_sample_show, read_gyro_boot_sample_store, 0);
		#endif
		static IIO_DEVICE_ATTR(in_anglvel_scale_available, 0444,
		st_asm330lhh_sysfs_scale_avail, NULL, 0);
		static IIO_DEVICE_ATTR(in_temp_scale_available, 0444,
		@@ -510,6 +632,9 @@ static IIO_DEVICE_ATTR(hwfifo_watermark, 0644, st_asm330lhh_get_watermark,

		static struct attribute *st_asm330lhh_acc_attributes[] = {
		&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		&iio_dev_attr_read_acc_boot_sample.dev_attr.attr,
		#endif
		&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
		&iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
		&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
		@@ -530,6 +655,9 @@ static const struct iio_info st_asm330lhh_acc_info = {

		static struct attribute *st_asm330lhh_gyro_attributes[] = {
		&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		&iio_dev_attr_read_gyro_boot_sample.dev_attr.attr,
		#endif
		&iio_dev_attr_in_anglvel_scale_available.dev_attr.attr,
		&iio_dev_attr_hwfifo_watermark_max.dev_attr.attr,
		&iio_dev_attr_hwfifo_watermark.dev_attr.attr,
		@@ -706,6 +834,206 @@ static struct iio_dev st_asm330lhh_alloc_iiodev(struct st_asm330lhh_hw hw,

		return iio_dev;
		}
		#ifdef CONFIG_ENABLE_ASM_ACC_GYRO_BUFFERING
		static void st_asm330lhh_enable_acc_gyro(struct st_asm330lhh_hw *hw)
		{
		int i = 0;
		struct st_asm330lhh_sensor *sensor;
		int acc_gain = ST_ASM330LHH_ACC_FS_2G_GAIN;
		int gyro_gain = ST_ASM330LHH_GYRO_FS_125_GAIN;

		for (i = 0; i < ST_ASM330LHH_ID_MAX; i++) {
		if (!hw->iio_devs[i])
		continue;
		sensor = iio_priv(hw->iio_devs[i]);
		sensor->odr = 104;
		sensor->watermark = 3;
		st_asm330lhh_update_fifo(hw->iio_devs[i], false);

		if (sensor->id == ST_ASM330LHH_ID_ACC)
		st_asm330lhh_set_full_scale(sensor, acc_gain);
		else if (sensor->id == ST_ASM330LHH_ID_GYRO)
		st_asm330lhh_set_full_scale(sensor, gyro_gain);

		st_asm330lhh_update_watermark(sensor, sensor->watermark);

		st_asm330lhh_update_fifo(hw->iio_devs[i], true);
		}
		}

		static int asm330_acc_gyro_early_buff_init(struct st_asm330lhh_hw *hw)
		{
		int i = 0, err = 0;
		struct st_asm330lhh_sensor *acc;
		struct st_asm330lhh_sensor *gyro;

		acc = iio_priv(hw->iio_devs[ST_ASM330LHH_ID_ACC]);
		gyro = iio_priv(hw->iio_devs[ST_ASM330LHH_ID_GYRO]);

		acc->bufsample_cnt = 0;
		gyro->bufsample_cnt = 0;
		acc->report_evt_cnt = 5;
		gyro->report_evt_cnt = 5;
		acc->max_buffer_time = 40;
		gyro->max_buffer_time = 40;

		acc->asm_cachepool = kmem_cache_create("acc_sensor_sample",
		sizeof(struct asm_sample),
		0,
		SLAB_HWCACHE_ALIGN, NULL);
		if (!acc->asm_cachepool) {
		dev_err(hw->dev,
		"asm_acc_cachepool cache create failed\n");
		err = -ENOMEM;
		goto clean_exit1;
		}

		for (i = 0; i < ASM_MAXSAMPLE; i++) {
		acc->asm_samplist[i] =
		kmem_cache_alloc(acc->asm_cachepool,
		GFP_KERNEL);
		if (!acc->asm_samplist[i]) {
		err = -ENOMEM;
		goto clean_exit2;
		}
		}

		gyro->asm_cachepool = kmem_cache_create("gyro_sensor_sample"
		, sizeof(struct asm_sample), 0,
		SLAB_HWCACHE_ALIGN, NULL);
		if (!gyro->asm_cachepool) {
		dev_err(hw->dev,
		"asm_gyro_cachepool cache create failed\n");
		err = -ENOMEM;
		goto clean_exit3;
		}

		for (i = 0; i < ASM_MAXSAMPLE; i++) {
		gyro->asm_samplist[i] =
		kmem_cache_alloc(gyro->asm_cachepool,
		GFP_KERNEL);
		if (!gyro->asm_samplist[i]) {
		err = -ENOMEM;
		goto clean_exit4;
		}
		}

		acc->buf_dev = input_allocate_device();
		if (!acc->buf_dev) {
		err = -ENOMEM;
		dev_err(hw->dev, "input device allocation failed\n");
		goto clean_exit5;
		}
		acc->buf_dev->name = "asm_accbuf";
		acc->buf_dev->id.bustype = BUS_I2C;
		input_set_events_per_packet(acc->buf_dev,
		acc->report_evt_cnt * ASM_MAXSAMPLE);
		set_bit(EV_ABS, acc->buf_dev->evbit);
		input_set_abs_params(acc->buf_dev, ABS_X,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(acc->buf_dev, ABS_Y,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(acc->buf_dev, ABS_Z,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(acc->buf_dev, ABS_RX,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(acc->buf_dev, ABS_RY,
		-G_MAX, G_MAX, 0, 0);
		err = input_register_device(acc->buf_dev);
		if (err) {
		dev_err(hw->dev,
		"unable to register input device %s\n",
		acc->buf_dev->name);
		goto clean_exit5;
		}

		gyro->buf_dev = input_allocate_device();
		if (!gyro->buf_dev) {
		err = -ENOMEM;
		dev_err(hw->dev, "input device allocation failed\n");
		goto clean_exit6;
		}
		gyro->buf_dev->name = "asm_gyrobuf";
		gyro->buf_dev->id.bustype = BUS_I2C;
		input_set_events_per_packet(gyro->buf_dev,
		gyro->report_evt_cnt * ASM_MAXSAMPLE);
		set_bit(EV_ABS, gyro->buf_dev->evbit);
		input_set_abs_params(gyro->buf_dev, ABS_X,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(gyro->buf_dev, ABS_Y,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(gyro->buf_dev, ABS_Z,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(gyro->buf_dev, ABS_RX,
		-G_MAX, G_MAX, 0, 0);
		input_set_abs_params(gyro->buf_dev, ABS_RY,
		-G_MAX, G_MAX, 0, 0);
		err = input_register_device(gyro->buf_dev);
		if (err) {
		dev_err(hw->dev,
		"unable to register input device %s\n",
		gyro->buf_dev->name);
		goto clean_exit6;
		}

		acc->buffer_asm_samples = true;
		gyro->buffer_asm_samples = true;

		return 1;
		clean_exit6:
		input_free_device(gyro->buf_dev);
		input_unregister_device(acc->buf_dev);
		clean_exit5:
		input_free_device(acc->buf_dev);
		clean_exit4:
		for (i = 0; i < ASM_MAXSAMPLE; i++)
		kmem_cache_free(gyro->asm_cachepool,
		gyro->asm_samplist[i]);
		clean_exit3:
		kmem_cache_destroy(gyro->asm_cachepool);
		clean_exit2:
		for (i = 0; i < ASM_MAXSAMPLE; i++)
		kmem_cache_free(acc->asm_cachepool,
		acc->asm_samplist[i]);
		clean_exit1:
		kmem_cache_destroy(acc->asm_cachepool);
		return 0;
		}
		static void asm330_acc_gyro_input_cleanup(
		struct st_asm330lhh_hw *hw)
		{
		int i = 0;
		struct st_asm330lhh_sensor *acc;
		struct st_asm330lhh_sensor *gyro;

		acc = iio_priv(hw->iio_devs[ST_ASM330LHH_ID_ACC]);
		gyro = iio_priv(hw->iio_devs[ST_ASM330LHH_ID_GYRO]);

		input_free_device(acc->buf_dev);
		input_unregister_device(gyro->buf_dev);
		input_free_device(gyro->buf_dev);
		for (i = 0; i < ASM_MAXSAMPLE; i++)
		kmem_cache_free(gyro->asm_cachepool,
		gyro->asm_samplist[i]);
		kmem_cache_destroy(gyro->asm_cachepool);
		for (i = 0; i < ASM_MAXSAMPLE; i++)
		kmem_cache_free(acc->asm_cachepool,
		acc->asm_samplist[i]);
		kmem_cache_destroy(acc->asm_cachepool);
		}
		#else
		static void st_asm330lhh_enable_acc_gyro(struct st_asm330lhh_hw *hw)
		{
		}
		static int asm330_acc_gyro_early_buff_init(struct st_asm330lhh_hw *hw)
		{
		return 1;
		}
		static void asm330_acc_gyro_input_cleanup(struct st_asm330lhh_hw *hw)
		{
		}
		#endif


		int st_asm330lhh_probe(struct device *dev, int irq,
		const struct st_asm330lhh_transfer_function *tf_ops)
		@@ -759,16 +1087,25 @@ int st_asm330lhh_probe(struct device *dev, int irq,
		}
		}

		err = asm330_acc_gyro_early_buff_init(hw);
		if (!err)
		return err;

		st_asm330lhh_enable_acc_gyro(hw);

		dev_info(hw->dev, "probe ok\n");

		return 0;
		}

		EXPORT_SYMBOL(st_asm330lhh_probe);

		int st_asm330lhh_remove(struct device *dev)
		{
		struct st_asm330lhh_hw *hw = dev_get_drvdata(dev);

		asm330_acc_gyro_input_cleanup(hw);

		return st_asm330lhh_deallocate_fifo(hw);
		}
		EXPORT_SYMBOL(st_asm330lhh_remove);