From 76cb841cb886eef6b3bee341a2266c76578724ad Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Mon, 6 May 2024 03:02:30 +0200 Subject: Adding upstream version 4.19.249. Signed-off-by: Daniel Baumann --- drivers/iio/orientation/Kconfig | 31 ++ drivers/iio/orientation/Makefile | 7 + drivers/iio/orientation/hid-sensor-incl-3d.c | 443 ++++++++++++++++++++++++++ drivers/iio/orientation/hid-sensor-rotation.c | 385 ++++++++++++++++++++++ 4 files changed, 866 insertions(+) create mode 100644 drivers/iio/orientation/Kconfig create mode 100644 drivers/iio/orientation/Makefile create mode 100644 drivers/iio/orientation/hid-sensor-incl-3d.c create mode 100644 drivers/iio/orientation/hid-sensor-rotation.c (limited to 'drivers/iio/orientation') diff --git a/drivers/iio/orientation/Kconfig b/drivers/iio/orientation/Kconfig new file mode 100644 index 000000000..e3aa1e58d --- /dev/null +++ b/drivers/iio/orientation/Kconfig @@ -0,0 +1,31 @@ +# +# Inclinometer sensors +# +# When adding new entries keep the list in alphabetical order + +menu "Inclinometer sensors" + +config HID_SENSOR_INCLINOMETER_3D + depends on HID_SENSOR_HUB + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select HID_SENSOR_IIO_COMMON + select HID_SENSOR_IIO_TRIGGER + tristate "HID Inclinometer 3D" + help + Say yes here to build support for the HID SENSOR + Inclinometer 3D. + +config HID_SENSOR_DEVICE_ROTATION + depends on HID_SENSOR_HUB + select IIO_BUFFER + select IIO_TRIGGERED_BUFFER + select HID_SENSOR_IIO_COMMON + select HID_SENSOR_IIO_TRIGGER + tristate "HID Device Rotation" + help + Say yes here to build support for the HID SENSOR + device rotation. The output of a device rotation sensor + is presented using quaternion format. + +endmenu diff --git a/drivers/iio/orientation/Makefile b/drivers/iio/orientation/Makefile new file mode 100644 index 000000000..4734dabbd --- /dev/null +++ b/drivers/iio/orientation/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for industrial I/O Inclinometer sensor drivers +# + +# When adding new entries keep the list in alphabetical order +obj-$(CONFIG_HID_SENSOR_INCLINOMETER_3D) += hid-sensor-incl-3d.o +obj-$(CONFIG_HID_SENSOR_DEVICE_ROTATION) += hid-sensor-rotation.o diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c new file mode 100644 index 000000000..bdc5e4554 --- /dev/null +++ b/drivers/iio/orientation/hid-sensor-incl-3d.c @@ -0,0 +1,443 @@ +/* + * HID Sensors Driver + * Copyright (c) 2013, Intel Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope 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. + * + * You should have received a copy of the GNU General Public License along with + * this program; if not, write to the Free Software Foundation, Inc. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../common/hid-sensors/hid-sensor-trigger.h" + +enum incl_3d_channel { + CHANNEL_SCAN_INDEX_X, + CHANNEL_SCAN_INDEX_Y, + CHANNEL_SCAN_INDEX_Z, + INCLI_3D_CHANNEL_MAX, +}; + +struct incl_3d_state { + struct hid_sensor_hub_callbacks callbacks; + struct hid_sensor_common common_attributes; + struct hid_sensor_hub_attribute_info incl[INCLI_3D_CHANNEL_MAX]; + u32 incl_val[INCLI_3D_CHANNEL_MAX]; + int scale_pre_decml; + int scale_post_decml; + int scale_precision; + int value_offset; +}; + +static const u32 incl_3d_addresses[INCLI_3D_CHANNEL_MAX] = { + HID_USAGE_SENSOR_ORIENT_TILT_X, + HID_USAGE_SENSOR_ORIENT_TILT_Y, + HID_USAGE_SENSOR_ORIENT_TILT_Z +}; + +/* Channel definitions */ +static const struct iio_chan_spec incl_3d_channels[] = { + { + .type = IIO_INCLI, + .modified = 1, + .channel2 = IIO_MOD_X, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) | + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_SAMP_FREQ) | + BIT(IIO_CHAN_INFO_HYSTERESIS), + .scan_index = CHANNEL_SCAN_INDEX_X, + }, { + .type = IIO_INCLI, + .modified = 1, + .channel2 = IIO_MOD_Y, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) | + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_SAMP_FREQ) | + BIT(IIO_CHAN_INFO_HYSTERESIS), + .scan_index = CHANNEL_SCAN_INDEX_Y, + }, { + .type = IIO_INCLI, + .modified = 1, + .channel2 = IIO_MOD_Z, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_OFFSET) | + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_SAMP_FREQ) | + BIT(IIO_CHAN_INFO_HYSTERESIS), + .scan_index = CHANNEL_SCAN_INDEX_Z, + } +}; + +/* Adjust channel real bits based on report descriptor */ +static void incl_3d_adjust_channel_bit_mask(struct iio_chan_spec *chan, + int size) +{ + chan->scan_type.sign = 's'; + /* Real storage bits will change based on the report desc. */ + chan->scan_type.realbits = size * 8; + /* Maximum size of a sample to capture is u32 */ + chan->scan_type.storagebits = sizeof(u32) * 8; +} + +/* Channel read_raw handler */ +static int incl_3d_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, + long mask) +{ + struct incl_3d_state *incl_state = iio_priv(indio_dev); + int report_id = -1; + u32 address; + int ret_type; + s32 min; + + *val = 0; + *val2 = 0; + switch (mask) { + case IIO_CHAN_INFO_RAW: + hid_sensor_power_state(&incl_state->common_attributes, true); + report_id = incl_state->incl[chan->scan_index].report_id; + min = incl_state->incl[chan->scan_index].logical_minimum; + address = incl_3d_addresses[chan->scan_index]; + if (report_id >= 0) + *val = sensor_hub_input_attr_get_raw_value( + incl_state->common_attributes.hsdev, + HID_USAGE_SENSOR_INCLINOMETER_3D, address, + report_id, + SENSOR_HUB_SYNC, + min < 0); + else { + hid_sensor_power_state(&incl_state->common_attributes, + false); + return -EINVAL; + } + hid_sensor_power_state(&incl_state->common_attributes, false); + ret_type = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SCALE: + *val = incl_state->scale_pre_decml; + *val2 = incl_state->scale_post_decml; + ret_type = incl_state->scale_precision; + break; + case IIO_CHAN_INFO_OFFSET: + *val = incl_state->value_offset; + ret_type = IIO_VAL_INT; + break; + case IIO_CHAN_INFO_SAMP_FREQ: + ret_type = hid_sensor_read_samp_freq_value( + &incl_state->common_attributes, val, val2); + break; + case IIO_CHAN_INFO_HYSTERESIS: + ret_type = hid_sensor_read_raw_hyst_value( + &incl_state->common_attributes, val, val2); + break; + default: + ret_type = -EINVAL; + break; + } + + return ret_type; +} + +/* Channel write_raw handler */ +static int incl_3d_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, + int val2, + long mask) +{ + struct incl_3d_state *incl_state = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = hid_sensor_write_samp_freq_value( + &incl_state->common_attributes, val, val2); + break; + case IIO_CHAN_INFO_HYSTERESIS: + ret = hid_sensor_write_raw_hyst_value( + &incl_state->common_attributes, val, val2); + break; + default: + ret = -EINVAL; + } + + return ret; +} + +static const struct iio_info incl_3d_info = { + .read_raw = &incl_3d_read_raw, + .write_raw = &incl_3d_write_raw, +}; + +/* Function to push data to buffer */ +static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len) +{ + dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n"); + iio_push_to_buffers(indio_dev, (u8 *)data); +} + +/* Callback handler to send event after all samples are received and captured */ +static int incl_3d_proc_event(struct hid_sensor_hub_device *hsdev, + unsigned usage_id, + void *priv) +{ + struct iio_dev *indio_dev = platform_get_drvdata(priv); + struct incl_3d_state *incl_state = iio_priv(indio_dev); + + dev_dbg(&indio_dev->dev, "incl_3d_proc_event\n"); + if (atomic_read(&incl_state->common_attributes.data_ready)) + hid_sensor_push_data(indio_dev, + (u8 *)incl_state->incl_val, + sizeof(incl_state->incl_val)); + + return 0; +} + +/* Capture samples in local storage */ +static int incl_3d_capture_sample(struct hid_sensor_hub_device *hsdev, + unsigned usage_id, + size_t raw_len, char *raw_data, + void *priv) +{ + struct iio_dev *indio_dev = platform_get_drvdata(priv); + struct incl_3d_state *incl_state = iio_priv(indio_dev); + int ret = 0; + + switch (usage_id) { + case HID_USAGE_SENSOR_ORIENT_TILT_X: + incl_state->incl_val[CHANNEL_SCAN_INDEX_X] = *(u32 *)raw_data; + break; + case HID_USAGE_SENSOR_ORIENT_TILT_Y: + incl_state->incl_val[CHANNEL_SCAN_INDEX_Y] = *(u32 *)raw_data; + break; + case HID_USAGE_SENSOR_ORIENT_TILT_Z: + incl_state->incl_val[CHANNEL_SCAN_INDEX_Z] = *(u32 *)raw_data; + break; + default: + ret = -EINVAL; + break; + } + + return ret; +} + +/* Parse report which is specific to an usage id*/ +static int incl_3d_parse_report(struct platform_device *pdev, + struct hid_sensor_hub_device *hsdev, + struct iio_chan_spec *channels, + unsigned usage_id, + struct incl_3d_state *st) +{ + int ret; + + ret = sensor_hub_input_get_attribute_info(hsdev, + HID_INPUT_REPORT, + usage_id, + HID_USAGE_SENSOR_ORIENT_TILT_X, + &st->incl[CHANNEL_SCAN_INDEX_X]); + if (ret) + return ret; + incl_3d_adjust_channel_bit_mask(&channels[CHANNEL_SCAN_INDEX_X], + st->incl[CHANNEL_SCAN_INDEX_X].size); + + ret = sensor_hub_input_get_attribute_info(hsdev, + HID_INPUT_REPORT, + usage_id, + HID_USAGE_SENSOR_ORIENT_TILT_Y, + &st->incl[CHANNEL_SCAN_INDEX_Y]); + if (ret) + return ret; + incl_3d_adjust_channel_bit_mask(&channels[CHANNEL_SCAN_INDEX_Y], + st->incl[CHANNEL_SCAN_INDEX_Y].size); + + ret = sensor_hub_input_get_attribute_info(hsdev, + HID_INPUT_REPORT, + usage_id, + HID_USAGE_SENSOR_ORIENT_TILT_Z, + &st->incl[CHANNEL_SCAN_INDEX_Z]); + if (ret) + return ret; + incl_3d_adjust_channel_bit_mask(&channels[CHANNEL_SCAN_INDEX_Z], + st->incl[CHANNEL_SCAN_INDEX_Z].size); + + dev_dbg(&pdev->dev, "incl_3d %x:%x, %x:%x, %x:%x\n", + st->incl[0].index, + st->incl[0].report_id, + st->incl[1].index, st->incl[1].report_id, + st->incl[2].index, st->incl[2].report_id); + + st->scale_precision = hid_sensor_format_scale( + HID_USAGE_SENSOR_INCLINOMETER_3D, + &st->incl[CHANNEL_SCAN_INDEX_X], + &st->scale_pre_decml, &st->scale_post_decml); + + /* Set Sensitivity field ids, when there is no individual modifier */ + if (st->common_attributes.sensitivity.index < 0) { + sensor_hub_input_get_attribute_info(hsdev, + HID_FEATURE_REPORT, usage_id, + HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS | + HID_USAGE_SENSOR_DATA_ORIENTATION, + &st->common_attributes.sensitivity); + dev_dbg(&pdev->dev, "Sensitivity index:report %d:%d\n", + st->common_attributes.sensitivity.index, + st->common_attributes.sensitivity.report_id); + } + return ret; +} + +/* Function to initialize the processing for usage id */ +static int hid_incl_3d_probe(struct platform_device *pdev) +{ + int ret; + static char *name = "incli_3d"; + struct iio_dev *indio_dev; + struct incl_3d_state *incl_state; + struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; + + indio_dev = devm_iio_device_alloc(&pdev->dev, + sizeof(struct incl_3d_state)); + if (indio_dev == NULL) + return -ENOMEM; + + platform_set_drvdata(pdev, indio_dev); + + incl_state = iio_priv(indio_dev); + incl_state->common_attributes.hsdev = hsdev; + incl_state->common_attributes.pdev = pdev; + + ret = hid_sensor_parse_common_attributes(hsdev, + HID_USAGE_SENSOR_INCLINOMETER_3D, + &incl_state->common_attributes); + if (ret) { + dev_err(&pdev->dev, "failed to setup common attributes\n"); + return ret; + } + + indio_dev->channels = kmemdup(incl_3d_channels, + sizeof(incl_3d_channels), GFP_KERNEL); + if (!indio_dev->channels) { + dev_err(&pdev->dev, "failed to duplicate channels\n"); + return -ENOMEM; + } + + ret = incl_3d_parse_report(pdev, hsdev, + (struct iio_chan_spec *)indio_dev->channels, + HID_USAGE_SENSOR_INCLINOMETER_3D, + incl_state); + if (ret) { + dev_err(&pdev->dev, "failed to setup attributes\n"); + goto error_free_dev_mem; + } + + indio_dev->num_channels = ARRAY_SIZE(incl_3d_channels); + indio_dev->dev.parent = &pdev->dev; + indio_dev->info = &incl_3d_info; + indio_dev->name = name; + indio_dev->modes = INDIO_DIRECT_MODE; + + ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time, + NULL, NULL); + if (ret) { + dev_err(&pdev->dev, "failed to initialize trigger buffer\n"); + goto error_free_dev_mem; + } + atomic_set(&incl_state->common_attributes.data_ready, 0); + ret = hid_sensor_setup_trigger(indio_dev, name, + &incl_state->common_attributes); + if (ret) { + dev_err(&pdev->dev, "trigger setup failed\n"); + goto error_unreg_buffer_funcs; + } + + ret = iio_device_register(indio_dev); + if (ret) { + dev_err(&pdev->dev, "device register failed\n"); + goto error_remove_trigger; + } + + incl_state->callbacks.send_event = incl_3d_proc_event; + incl_state->callbacks.capture_sample = incl_3d_capture_sample; + incl_state->callbacks.pdev = pdev; + ret = sensor_hub_register_callback(hsdev, + HID_USAGE_SENSOR_INCLINOMETER_3D, + &incl_state->callbacks); + if (ret) { + dev_err(&pdev->dev, "callback reg failed\n"); + goto error_iio_unreg; + } + + return 0; + +error_iio_unreg: + iio_device_unregister(indio_dev); +error_remove_trigger: + hid_sensor_remove_trigger(&incl_state->common_attributes); +error_unreg_buffer_funcs: + iio_triggered_buffer_cleanup(indio_dev); +error_free_dev_mem: + kfree(indio_dev->channels); + return ret; +} + +/* Function to deinitialize the processing for usage id */ +static int hid_incl_3d_remove(struct platform_device *pdev) +{ + struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; + struct iio_dev *indio_dev = platform_get_drvdata(pdev); + struct incl_3d_state *incl_state = iio_priv(indio_dev); + + sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_INCLINOMETER_3D); + iio_device_unregister(indio_dev); + hid_sensor_remove_trigger(&incl_state->common_attributes); + iio_triggered_buffer_cleanup(indio_dev); + kfree(indio_dev->channels); + + return 0; +} + +static const struct platform_device_id hid_incl_3d_ids[] = { + { + /* Format: HID-SENSOR-usage_id_in_hex_lowercase */ + .name = "HID-SENSOR-200086", + }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(platform, hid_incl_3d_ids); + +static struct platform_driver hid_incl_3d_platform_driver = { + .id_table = hid_incl_3d_ids, + .driver = { + .name = KBUILD_MODNAME, + .pm = &hid_sensor_pm_ops, + }, + .probe = hid_incl_3d_probe, + .remove = hid_incl_3d_remove, +}; +module_platform_driver(hid_incl_3d_platform_driver); + +MODULE_DESCRIPTION("HID Sensor Inclinometer 3D"); +MODULE_AUTHOR("Srinivas Pandruvada "); +MODULE_LICENSE("GPL"); diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c new file mode 100644 index 000000000..a69db2002 --- /dev/null +++ b/drivers/iio/orientation/hid-sensor-rotation.c @@ -0,0 +1,385 @@ +/* + * HID Sensors Driver + * Copyright (c) 2014, Intel Corporation. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope 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. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../common/hid-sensors/hid-sensor-trigger.h" + +struct dev_rot_state { + struct hid_sensor_hub_callbacks callbacks; + struct hid_sensor_common common_attributes; + struct hid_sensor_hub_attribute_info quaternion; + u32 sampled_vals[4]; + int scale_pre_decml; + int scale_post_decml; + int scale_precision; + int value_offset; +}; + +/* Channel definitions */ +static const struct iio_chan_spec dev_rot_channels[] = { + { + .type = IIO_ROT, + .modified = 1, + .channel2 = IIO_MOD_QUATERNION, + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | + BIT(IIO_CHAN_INFO_OFFSET) | + BIT(IIO_CHAN_INFO_SCALE) | + BIT(IIO_CHAN_INFO_HYSTERESIS) + } +}; + +/* Adjust channel real bits based on report descriptor */ +static void dev_rot_adjust_channel_bit_mask(struct iio_chan_spec *chan, + int size) +{ + chan->scan_type.sign = 's'; + /* Real storage bits will change based on the report desc. */ + chan->scan_type.realbits = size * 8; + /* Maximum size of a sample to capture is u32 */ + chan->scan_type.storagebits = sizeof(u32) * 8; + chan->scan_type.repeat = 4; +} + +/* Channel read_raw handler */ +static int dev_rot_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int size, int *vals, int *val_len, + long mask) +{ + struct dev_rot_state *rot_state = iio_priv(indio_dev); + int ret_type; + int i; + + vals[0] = 0; + vals[1] = 0; + + switch (mask) { + case IIO_CHAN_INFO_RAW: + if (size >= 4) { + for (i = 0; i < 4; ++i) + vals[i] = rot_state->sampled_vals[i]; + ret_type = IIO_VAL_INT_MULTIPLE; + *val_len = 4; + } else + ret_type = -EINVAL; + break; + case IIO_CHAN_INFO_SCALE: + vals[0] = rot_state->scale_pre_decml; + vals[1] = rot_state->scale_post_decml; + return rot_state->scale_precision; + + case IIO_CHAN_INFO_OFFSET: + *vals = rot_state->value_offset; + return IIO_VAL_INT; + + case IIO_CHAN_INFO_SAMP_FREQ: + ret_type = hid_sensor_read_samp_freq_value( + &rot_state->common_attributes, &vals[0], &vals[1]); + break; + case IIO_CHAN_INFO_HYSTERESIS: + ret_type = hid_sensor_read_raw_hyst_value( + &rot_state->common_attributes, &vals[0], &vals[1]); + break; + default: + ret_type = -EINVAL; + break; + } + + return ret_type; +} + +/* Channel write_raw handler */ +static int dev_rot_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, + int val2, + long mask) +{ + struct dev_rot_state *rot_state = iio_priv(indio_dev); + int ret; + + switch (mask) { + case IIO_CHAN_INFO_SAMP_FREQ: + ret = hid_sensor_write_samp_freq_value( + &rot_state->common_attributes, val, val2); + break; + case IIO_CHAN_INFO_HYSTERESIS: + ret = hid_sensor_write_raw_hyst_value( + &rot_state->common_attributes, val, val2); + break; + default: + ret = -EINVAL; + } + + return ret; +} + +static const struct iio_info dev_rot_info = { + .read_raw_multi = &dev_rot_read_raw, + .write_raw = &dev_rot_write_raw, +}; + +/* Function to push data to buffer */ +static void hid_sensor_push_data(struct iio_dev *indio_dev, u8 *data, int len) +{ + dev_dbg(&indio_dev->dev, "hid_sensor_push_data >>\n"); + iio_push_to_buffers(indio_dev, (u8 *)data); + dev_dbg(&indio_dev->dev, "hid_sensor_push_data <<\n"); + +} + +/* Callback handler to send event after all samples are received and captured */ +static int dev_rot_proc_event(struct hid_sensor_hub_device *hsdev, + unsigned usage_id, + void *priv) +{ + struct iio_dev *indio_dev = platform_get_drvdata(priv); + struct dev_rot_state *rot_state = iio_priv(indio_dev); + + dev_dbg(&indio_dev->dev, "dev_rot_proc_event\n"); + if (atomic_read(&rot_state->common_attributes.data_ready)) + hid_sensor_push_data(indio_dev, + (u8 *)rot_state->sampled_vals, + sizeof(rot_state->sampled_vals)); + + return 0; +} + +/* Capture samples in local storage */ +static int dev_rot_capture_sample(struct hid_sensor_hub_device *hsdev, + unsigned usage_id, + size_t raw_len, char *raw_data, + void *priv) +{ + struct iio_dev *indio_dev = platform_get_drvdata(priv); + struct dev_rot_state *rot_state = iio_priv(indio_dev); + + if (usage_id == HID_USAGE_SENSOR_ORIENT_QUATERNION) { + memcpy(rot_state->sampled_vals, raw_data, + sizeof(rot_state->sampled_vals)); + dev_dbg(&indio_dev->dev, "Recd Quat len:%zu::%zu\n", raw_len, + sizeof(rot_state->sampled_vals)); + } + + return 0; +} + +/* Parse report which is specific to an usage id*/ +static int dev_rot_parse_report(struct platform_device *pdev, + struct hid_sensor_hub_device *hsdev, + struct iio_chan_spec *channels, + unsigned usage_id, + struct dev_rot_state *st) +{ + int ret; + + ret = sensor_hub_input_get_attribute_info(hsdev, + HID_INPUT_REPORT, + usage_id, + HID_USAGE_SENSOR_ORIENT_QUATERNION, + &st->quaternion); + if (ret) + return ret; + + dev_rot_adjust_channel_bit_mask(&channels[0], + st->quaternion.size / 4); + + dev_dbg(&pdev->dev, "dev_rot %x:%x\n", st->quaternion.index, + st->quaternion.report_id); + + dev_dbg(&pdev->dev, "dev_rot: attrib size %d\n", + st->quaternion.size); + + st->scale_precision = hid_sensor_format_scale( + hsdev->usage, + &st->quaternion, + &st->scale_pre_decml, &st->scale_post_decml); + + /* Set Sensitivity field ids, when there is no individual modifier */ + if (st->common_attributes.sensitivity.index < 0) { + sensor_hub_input_get_attribute_info(hsdev, + HID_FEATURE_REPORT, usage_id, + HID_USAGE_SENSOR_DATA_MOD_CHANGE_SENSITIVITY_ABS | + HID_USAGE_SENSOR_DATA_ORIENTATION, + &st->common_attributes.sensitivity); + dev_dbg(&pdev->dev, "Sensitivity index:report %d:%d\n", + st->common_attributes.sensitivity.index, + st->common_attributes.sensitivity.report_id); + } + + return 0; +} + +/* Function to initialize the processing for usage id */ +static int hid_dev_rot_probe(struct platform_device *pdev) +{ + int ret; + char *name; + struct iio_dev *indio_dev; + struct dev_rot_state *rot_state; + struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; + + indio_dev = devm_iio_device_alloc(&pdev->dev, + sizeof(struct dev_rot_state)); + if (indio_dev == NULL) + return -ENOMEM; + + platform_set_drvdata(pdev, indio_dev); + + rot_state = iio_priv(indio_dev); + rot_state->common_attributes.hsdev = hsdev; + rot_state->common_attributes.pdev = pdev; + + switch (hsdev->usage) { + case HID_USAGE_SENSOR_DEVICE_ORIENTATION: + name = "dev_rotation"; + break; + case HID_USAGE_SENSOR_RELATIVE_ORIENTATION: + name = "relative_orientation"; + break; + case HID_USAGE_SENSOR_GEOMAGNETIC_ORIENTATION: + name = "geomagnetic_orientation"; + break; + default: + return -EINVAL; + } + + ret = hid_sensor_parse_common_attributes(hsdev, hsdev->usage, + &rot_state->common_attributes); + if (ret) { + dev_err(&pdev->dev, "failed to setup common attributes\n"); + return ret; + } + + indio_dev->channels = devm_kmemdup(&pdev->dev, dev_rot_channels, + sizeof(dev_rot_channels), + GFP_KERNEL); + if (!indio_dev->channels) { + dev_err(&pdev->dev, "failed to duplicate channels\n"); + return -ENOMEM; + } + + ret = dev_rot_parse_report(pdev, hsdev, + (struct iio_chan_spec *)indio_dev->channels, + hsdev->usage, rot_state); + if (ret) { + dev_err(&pdev->dev, "failed to setup attributes\n"); + return ret; + } + + indio_dev->num_channels = ARRAY_SIZE(dev_rot_channels); + indio_dev->dev.parent = &pdev->dev; + indio_dev->info = &dev_rot_info; + indio_dev->name = name; + indio_dev->modes = INDIO_DIRECT_MODE; + + ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time, + NULL, NULL); + if (ret) { + dev_err(&pdev->dev, "failed to initialize trigger buffer\n"); + return ret; + } + atomic_set(&rot_state->common_attributes.data_ready, 0); + ret = hid_sensor_setup_trigger(indio_dev, name, + &rot_state->common_attributes); + if (ret) { + dev_err(&pdev->dev, "trigger setup failed\n"); + goto error_unreg_buffer_funcs; + } + + ret = iio_device_register(indio_dev); + if (ret) { + dev_err(&pdev->dev, "device register failed\n"); + goto error_remove_trigger; + } + + rot_state->callbacks.send_event = dev_rot_proc_event; + rot_state->callbacks.capture_sample = dev_rot_capture_sample; + rot_state->callbacks.pdev = pdev; + ret = sensor_hub_register_callback(hsdev, hsdev->usage, + &rot_state->callbacks); + if (ret) { + dev_err(&pdev->dev, "callback reg failed\n"); + goto error_iio_unreg; + } + + return 0; + +error_iio_unreg: + iio_device_unregister(indio_dev); +error_remove_trigger: + hid_sensor_remove_trigger(&rot_state->common_attributes); +error_unreg_buffer_funcs: + iio_triggered_buffer_cleanup(indio_dev); + return ret; +} + +/* Function to deinitialize the processing for usage id */ +static int hid_dev_rot_remove(struct platform_device *pdev) +{ + struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; + struct iio_dev *indio_dev = platform_get_drvdata(pdev); + struct dev_rot_state *rot_state = iio_priv(indio_dev); + + sensor_hub_remove_callback(hsdev, hsdev->usage); + iio_device_unregister(indio_dev); + hid_sensor_remove_trigger(&rot_state->common_attributes); + iio_triggered_buffer_cleanup(indio_dev); + + return 0; +} + +static const struct platform_device_id hid_dev_rot_ids[] = { + { + /* Format: HID-SENSOR-usage_id_in_hex_lowercase */ + .name = "HID-SENSOR-20008a", + }, + { + /* Relative orientation(AG) sensor */ + .name = "HID-SENSOR-20008e", + }, + { + /* Geomagnetic orientation(AM) sensor */ + .name = "HID-SENSOR-2000c1", + }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(platform, hid_dev_rot_ids); + +static struct platform_driver hid_dev_rot_platform_driver = { + .id_table = hid_dev_rot_ids, + .driver = { + .name = KBUILD_MODNAME, + .pm = &hid_sensor_pm_ops, + }, + .probe = hid_dev_rot_probe, + .remove = hid_dev_rot_remove, +}; +module_platform_driver(hid_dev_rot_platform_driver); + +MODULE_DESCRIPTION("HID Sensor Device Rotation"); +MODULE_AUTHOR("Srinivas Pandruvada "); +MODULE_LICENSE("GPL"); -- cgit v1.2.3