From 5ef00171f95a150a912f1d14645bcae25df203c9 Mon Sep 17 00:00:00 2001 From: Srinivas Pandruvada From: Evgeni Raikhel Date: Tue, 13 Feb 2018 12:06:44 +0200 Subject: [PATCH] Adding missing HID timestamp patch for Gyro sensor. A symmetric patch for Accelerator was already upstreamed, this patch was skipped from upstream The patch was written by Srinivas Pandruvada The version tag has been added to accel module so it will be also replaced regardless of the target kernel version --- drivers/iio/accel/hid-sensor-accel-3d.c | 30 ++++++++++++++++++++++-------- drivers/iio/gyro/hid-sensor-gyro-3d.c | 28 +++++++++++++++++++++------- include/linux/hid-sensor-ids.h | 1 + 3 files changed, 44 insertions(+), 15 deletions(-) diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c index ab1e238..15b55a3 100644 --- a/drivers/iio/accel/hid-sensor-accel-3d.c +++ b/drivers/iio/accel/hid-sensor-accel-3d.c @@ -42,11 +42,13 @@ struct accel_3d_state { struct hid_sensor_hub_callbacks callbacks; struct hid_sensor_common common_attributes; struct hid_sensor_hub_attribute_info accel[ACCEL_3D_CHANNEL_MAX]; - u32 accel_val[ACCEL_3D_CHANNEL_MAX]; + /* Reserve for 3 channels + padding + timestamp */ + u32 accel_val[ACCEL_3D_CHANNEL_MAX + 3]; int scale_pre_decml; int scale_post_decml; int scale_precision; int value_offset; + int64_t timestamp; }; static const u32 accel_3d_addresses[ACCEL_3D_CHANNEL_MAX] = { @@ -87,7 +89,8 @@ static const struct iio_chan_spec accel_3d_channels[] = { BIT(IIO_CHAN_INFO_SAMP_FREQ) | BIT(IIO_CHAN_INFO_HYSTERESIS), .scan_index = CHANNEL_SCAN_INDEX_Z, - } + }, + IIO_CHAN_SOFT_TIMESTAMP(3) }; /* Adjust channel real bits based on report descriptor */ @@ -192,11 +195,11 @@ static const struct iio_info accel_3d_info = { }; /* Function to push data to buffer */ -static void hid_sensor_push_data(struct iio_dev *indio_dev, const void *data, - int len) +static void hid_sensor_push_data(struct iio_dev *indio_dev, void *data, + int len, int64_t timestamp) { dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n"); - iio_push_to_buffers(indio_dev, data); + iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); } /* Callback handler to send event after all samples are received and captured */ @@ -208,10 +211,17 @@ static int accel_3d_proc_event(struct hid_sensor_hub_device *hsdev, struct accel_3d_state *accel_state = iio_priv(indio_dev); dev_dbg(&indio_dev->dev, "accel_3d_proc_event\n"); - if (atomic_read(&accel_state->common_attributes.data_ready)) + if (atomic_read(&accel_state->common_attributes.data_ready)) { + if (!accel_state->timestamp) + accel_state->timestamp = iio_get_time_ns(indio_dev); + hid_sensor_push_data(indio_dev, - accel_state->accel_val, - sizeof(accel_state->accel_val)); + accel_state->accel_val, + sizeof(accel_state->accel_val), + accel_state->timestamp); + + accel_state->timestamp = 0; + } return 0; } @@ -236,6 +246,10 @@ static int accel_3d_capture_sample(struct hid_sensor_hub_device *hsdev, *(u32 *)raw_data; ret = 0; break; + case HID_USAGE_SENSOR_TIME_TIMESTAMP: // usec->nsec + accel_state->timestamp = (*(int64_t *)raw_data)*1000; + ret = 0; + break; default: break; } diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c index c67ce2a..859cdde 100644 --- a/drivers/iio/gyro/hid-sensor-gyro-3d.c +++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c @@ -42,11 +42,13 @@ struct gyro_3d_state { struct hid_sensor_hub_callbacks callbacks; struct hid_sensor_common common_attributes; struct hid_sensor_hub_attribute_info gyro[GYRO_3D_CHANNEL_MAX]; - u32 gyro_val[GYRO_3D_CHANNEL_MAX]; + /* Reserve for 3 channels + padding + timestamp */ + u32 gyro_val[GYRO_3D_CHANNEL_MAX + 3]; int scale_pre_decml; int scale_post_decml; int scale_precision; int value_offset; + int64_t timestamp; }; static const u32 gyro_3d_addresses[GYRO_3D_CHANNEL_MAX] = { @@ -87,7 +89,8 @@ static const struct iio_chan_spec gyro_3d_channels[] = { BIT(IIO_CHAN_INFO_SAMP_FREQ) | BIT(IIO_CHAN_INFO_HYSTERESIS), .scan_index = CHANNEL_SCAN_INDEX_Z, - } + }, + IIO_CHAN_SOFT_TIMESTAMP(3) }; /* Adjust channel real bits based on report descriptor */ @@ -192,11 +195,11 @@ static const struct iio_info gyro_3d_info = { }; /* Function to push data to buffer */ -static void hid_sensor_push_data(struct iio_dev *indio_dev, const void *data, - int len) +static void hid_sensor_push_data(struct iio_dev *indio_dev, void *data, + int len, int64_t timestamp) { dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n"); - iio_push_to_buffers(indio_dev, data); + iio_push_to_buffers_with_timestamp(indio_dev, data, timestamp); } /* Callback handler to send event after all samples are received and captured */ @@ -208,10 +211,17 @@ static int gyro_3d_proc_event(struct hid_sensor_hub_device *hsdev, struct gyro_3d_state *gyro_state = iio_priv(indio_dev); dev_dbg(&indio_dev->dev, "gyro_3d_proc_event\n"); - if (atomic_read(&gyro_state->common_attributes.data_ready)) + if (atomic_read(&gyro_state->common_attributes.data_ready)) { + if (!gyro_state->timestamp) + gyro_state->timestamp = iio_get_time_ns(indio_dev); + hid_sensor_push_data(indio_dev, gyro_state->gyro_val, - sizeof(gyro_state->gyro_val)); + sizeof(gyro_state->gyro_val), + gyro_state->timestamp); + + gyro_state->timestamp = 0; + } return 0; } @@ -236,6 +246,10 @@ static int gyro_3d_capture_sample(struct hid_sensor_hub_device *hsdev, *(u32 *)raw_data; ret = 0; break; + case HID_USAGE_SENSOR_TIME_TIMESTAMP: // usec->nsec + gyro_state->timestamp = (*(int64_t *)raw_data)*1000; + ret = 0; + break; default: break; } diff --git a/include/linux/hid-sensor-ids.h b/include/linux/hid-sensor-ids.h index f2ee90a..063aa1f 100644 --- a/include/linux/hid-sensor-ids.h +++ b/include/linux/hid-sensor-ids.h @@ -95,6 +95,7 @@ #define HID_USAGE_SENSOR_TIME_HOUR 0x200525 #define HID_USAGE_SENSOR_TIME_MINUTE 0x200526 #define HID_USAGE_SENSOR_TIME_SECOND 0x200527 +#define HID_USAGE_SENSOR_TIME_TIMESTAMP 0x200529 /* Units */ #define HID_USAGE_SENSOR_UNITS_NOT_SPECIFIED 0x00 -- 2.7.4