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 Additionally, timestamp management is fixed --- drivers/iio/gyro/hid-sensor-gyro-3d.c | 28 ++++++++++++++++++++------- drivers/iio/gyro/hid-sensor-accel-3d.c | 28 ++++++++++++++++++++------- 2 file changed, 56 insertions(+), 14 deletions(-) diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c index ab1e238..28262c4 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)) - hid_sensor_push_data(indio_dev, - accel_state->accel_val, - sizeof(accel_state->accel_val)); + if (atomic_read(&accel_state->common_attributes.data_ready)) { + if (!accel_state->timestamp) + accel_state->timestamp = iio_get_time_ns(); + + hid_sensor_push_data(indio_dev, + 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..f45c5a2 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(); + hid_sensor_push_data(indio_dev, - gyro_state->gyro_val, - sizeof(gyro_state->gyro_val)); + 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..9a3a9db 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