Merge "driver: iio: imu: asm330 early buffer implemented"
diff --git a/drivers/iio/imu/st_asm330lhh/Kconfig b/drivers/iio/imu/st_asm330lhh/Kconfig
index 092cc48..ebdba32 100644
--- a/drivers/iio/imu/st_asm330lhh/Kconfig
+++ b/drivers/iio/imu/st_asm330lhh/Kconfig
@@ -21,3 +21,9 @@
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
diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h b/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
index 52b293f..9605ab2 100644
--- a/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
+++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh.h
@@ -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>
#define ST_ASM330LHH_REVISION "2.0.1"
#define ST_ASM330LHH_PATCH "1"
@@ -82,6 +85,16 @@
#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;
@@ -165,6 +178,17 @@
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
};
/**
@@ -235,4 +259,9 @@
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 */
diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
index af8c5ba..64b89da 100644
--- a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
+++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_buffer.c
@@ -121,7 +121,7 @@
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 @@
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] = y;
+ sensor->asm_samplist[sensor->bufsample_cnt]->xyz[2] = z;
+ 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 %d buffering %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 @@
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 @@
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 @@
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,12 +503,24 @@
static int st_asm330lhh_buffer_preenable(struct iio_dev *iio_dev)
{
- return st_asm330lhh_update_fifo(iio_dev, true);
+ 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)
{
- return st_asm330lhh_update_fifo(iio_dev, false);
+ 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);
}
static const struct iio_buffer_setup_ops st_asm330lhh_buffer_ops = {
diff --git a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
index 9d9ee20..112b7c8 100644
--- a/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
+++ b/drivers/iio/imu/st_asm330lhh/st_asm330lhh_core.c
@@ -432,6 +432,9 @@
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 @@
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) {
+ 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) {
+ 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 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 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,181 @@
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 = 30;
+ 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;
+}
+#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;
+}
+#endif
+
int st_asm330lhh_probe(struct device *dev, int irq,
const struct st_asm330lhh_transfer_function *tf_ops)
@@ -756,10 +1059,17 @@
return err;
}
+ 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);
static int __maybe_unused st_asm330lhh_suspend(struct device *dev)