hid-sensor-rotation.c (9972B)
1// SPDX-License-Identifier: GPL-2.0-only 2/* 3 * HID Sensors Driver 4 * Copyright (c) 2014, Intel Corporation. 5 */ 6 7#include <linux/device.h> 8#include <linux/platform_device.h> 9#include <linux/module.h> 10#include <linux/mod_devicetable.h> 11#include <linux/hid-sensor-hub.h> 12#include <linux/iio/iio.h> 13#include <linux/iio/sysfs.h> 14#include <linux/iio/buffer.h> 15#include "../common/hid-sensors/hid-sensor-trigger.h" 16 17struct dev_rot_state { 18 struct hid_sensor_hub_callbacks callbacks; 19 struct hid_sensor_common common_attributes; 20 struct hid_sensor_hub_attribute_info quaternion; 21 struct { 22 s32 sampled_vals[4] __aligned(16); 23 u64 timestamp __aligned(8); 24 } scan; 25 int scale_pre_decml; 26 int scale_post_decml; 27 int scale_precision; 28 int value_offset; 29 s64 timestamp; 30}; 31 32static const u32 rotation_sensitivity_addresses[] = { 33 HID_USAGE_SENSOR_DATA_ORIENTATION, 34 HID_USAGE_SENSOR_ORIENT_QUATERNION, 35}; 36 37/* Channel definitions */ 38static const struct iio_chan_spec dev_rot_channels[] = { 39 { 40 .type = IIO_ROT, 41 .modified = 1, 42 .channel2 = IIO_MOD_QUATERNION, 43 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), 44 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | 45 BIT(IIO_CHAN_INFO_OFFSET) | 46 BIT(IIO_CHAN_INFO_SCALE) | 47 BIT(IIO_CHAN_INFO_HYSTERESIS), 48 .scan_index = 0 49 }, 50 IIO_CHAN_SOFT_TIMESTAMP(1) 51}; 52 53/* Adjust channel real bits based on report descriptor */ 54static void dev_rot_adjust_channel_bit_mask(struct iio_chan_spec *chan, 55 int size) 56{ 57 chan->scan_type.sign = 's'; 58 /* Real storage bits will change based on the report desc. */ 59 chan->scan_type.realbits = size * 8; 60 /* Maximum size of a sample to capture is u32 */ 61 chan->scan_type.storagebits = sizeof(u32) * 8; 62 chan->scan_type.repeat = 4; 63} 64 65/* Channel read_raw handler */ 66static int dev_rot_read_raw(struct iio_dev *indio_dev, 67 struct iio_chan_spec const *chan, 68 int size, int *vals, int *val_len, 69 long mask) 70{ 71 struct dev_rot_state *rot_state = iio_priv(indio_dev); 72 int ret_type; 73 int i; 74 75 vals[0] = 0; 76 vals[1] = 0; 77 78 switch (mask) { 79 case IIO_CHAN_INFO_RAW: 80 if (size >= 4) { 81 for (i = 0; i < 4; ++i) 82 vals[i] = rot_state->scan.sampled_vals[i]; 83 ret_type = IIO_VAL_INT_MULTIPLE; 84 *val_len = 4; 85 } else 86 ret_type = -EINVAL; 87 break; 88 case IIO_CHAN_INFO_SCALE: 89 vals[0] = rot_state->scale_pre_decml; 90 vals[1] = rot_state->scale_post_decml; 91 return rot_state->scale_precision; 92 93 case IIO_CHAN_INFO_OFFSET: 94 *vals = rot_state->value_offset; 95 return IIO_VAL_INT; 96 97 case IIO_CHAN_INFO_SAMP_FREQ: 98 ret_type = hid_sensor_read_samp_freq_value( 99 &rot_state->common_attributes, &vals[0], &vals[1]); 100 break; 101 case IIO_CHAN_INFO_HYSTERESIS: 102 ret_type = hid_sensor_read_raw_hyst_value( 103 &rot_state->common_attributes, &vals[0], &vals[1]); 104 break; 105 default: 106 ret_type = -EINVAL; 107 break; 108 } 109 110 return ret_type; 111} 112 113/* Channel write_raw handler */ 114static int dev_rot_write_raw(struct iio_dev *indio_dev, 115 struct iio_chan_spec const *chan, 116 int val, 117 int val2, 118 long mask) 119{ 120 struct dev_rot_state *rot_state = iio_priv(indio_dev); 121 int ret; 122 123 switch (mask) { 124 case IIO_CHAN_INFO_SAMP_FREQ: 125 ret = hid_sensor_write_samp_freq_value( 126 &rot_state->common_attributes, val, val2); 127 break; 128 case IIO_CHAN_INFO_HYSTERESIS: 129 ret = hid_sensor_write_raw_hyst_value( 130 &rot_state->common_attributes, val, val2); 131 break; 132 default: 133 ret = -EINVAL; 134 } 135 136 return ret; 137} 138 139static const struct iio_info dev_rot_info = { 140 .read_raw_multi = &dev_rot_read_raw, 141 .write_raw = &dev_rot_write_raw, 142}; 143 144/* Callback handler to send event after all samples are received and captured */ 145static int dev_rot_proc_event(struct hid_sensor_hub_device *hsdev, 146 unsigned usage_id, 147 void *priv) 148{ 149 struct iio_dev *indio_dev = platform_get_drvdata(priv); 150 struct dev_rot_state *rot_state = iio_priv(indio_dev); 151 152 dev_dbg(&indio_dev->dev, "dev_rot_proc_event\n"); 153 if (atomic_read(&rot_state->common_attributes.data_ready)) { 154 if (!rot_state->timestamp) 155 rot_state->timestamp = iio_get_time_ns(indio_dev); 156 157 iio_push_to_buffers_with_timestamp(indio_dev, &rot_state->scan, 158 rot_state->timestamp); 159 160 rot_state->timestamp = 0; 161 } 162 163 return 0; 164} 165 166/* Capture samples in local storage */ 167static int dev_rot_capture_sample(struct hid_sensor_hub_device *hsdev, 168 unsigned usage_id, 169 size_t raw_len, char *raw_data, 170 void *priv) 171{ 172 struct iio_dev *indio_dev = platform_get_drvdata(priv); 173 struct dev_rot_state *rot_state = iio_priv(indio_dev); 174 175 if (usage_id == HID_USAGE_SENSOR_ORIENT_QUATERNION) { 176 if (raw_len / 4 == sizeof(s16)) { 177 rot_state->scan.sampled_vals[0] = ((s16 *)raw_data)[0]; 178 rot_state->scan.sampled_vals[1] = ((s16 *)raw_data)[1]; 179 rot_state->scan.sampled_vals[2] = ((s16 *)raw_data)[2]; 180 rot_state->scan.sampled_vals[3] = ((s16 *)raw_data)[3]; 181 } else { 182 memcpy(&rot_state->scan.sampled_vals, raw_data, 183 sizeof(rot_state->scan.sampled_vals)); 184 } 185 186 dev_dbg(&indio_dev->dev, "Recd Quat len:%zu::%zu\n", raw_len, 187 sizeof(rot_state->scan.sampled_vals)); 188 } else if (usage_id == HID_USAGE_SENSOR_TIME_TIMESTAMP) { 189 rot_state->timestamp = hid_sensor_convert_timestamp(&rot_state->common_attributes, 190 *(s64 *)raw_data); 191 } 192 193 return 0; 194} 195 196/* Parse report which is specific to an usage id*/ 197static int dev_rot_parse_report(struct platform_device *pdev, 198 struct hid_sensor_hub_device *hsdev, 199 struct iio_chan_spec *channels, 200 unsigned usage_id, 201 struct dev_rot_state *st) 202{ 203 int ret; 204 205 ret = sensor_hub_input_get_attribute_info(hsdev, 206 HID_INPUT_REPORT, 207 usage_id, 208 HID_USAGE_SENSOR_ORIENT_QUATERNION, 209 &st->quaternion); 210 if (ret) 211 return ret; 212 213 dev_rot_adjust_channel_bit_mask(&channels[0], 214 st->quaternion.size / 4); 215 216 dev_dbg(&pdev->dev, "dev_rot %x:%x\n", st->quaternion.index, 217 st->quaternion.report_id); 218 219 dev_dbg(&pdev->dev, "dev_rot: attrib size %d\n", 220 st->quaternion.size); 221 222 st->scale_precision = hid_sensor_format_scale( 223 hsdev->usage, 224 &st->quaternion, 225 &st->scale_pre_decml, &st->scale_post_decml); 226 227 return 0; 228} 229 230/* Function to initialize the processing for usage id */ 231static int hid_dev_rot_probe(struct platform_device *pdev) 232{ 233 int ret; 234 char *name; 235 struct iio_dev *indio_dev; 236 struct dev_rot_state *rot_state; 237 struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; 238 239 indio_dev = devm_iio_device_alloc(&pdev->dev, 240 sizeof(struct dev_rot_state)); 241 if (indio_dev == NULL) 242 return -ENOMEM; 243 244 platform_set_drvdata(pdev, indio_dev); 245 246 rot_state = iio_priv(indio_dev); 247 rot_state->common_attributes.hsdev = hsdev; 248 rot_state->common_attributes.pdev = pdev; 249 250 switch (hsdev->usage) { 251 case HID_USAGE_SENSOR_DEVICE_ORIENTATION: 252 name = "dev_rotation"; 253 break; 254 case HID_USAGE_SENSOR_RELATIVE_ORIENTATION: 255 name = "relative_orientation"; 256 break; 257 case HID_USAGE_SENSOR_GEOMAGNETIC_ORIENTATION: 258 name = "geomagnetic_orientation"; 259 break; 260 default: 261 return -EINVAL; 262 } 263 264 ret = hid_sensor_parse_common_attributes(hsdev, 265 hsdev->usage, 266 &rot_state->common_attributes, 267 rotation_sensitivity_addresses, 268 ARRAY_SIZE(rotation_sensitivity_addresses)); 269 if (ret) { 270 dev_err(&pdev->dev, "failed to setup common attributes\n"); 271 return ret; 272 } 273 274 indio_dev->channels = devm_kmemdup(&pdev->dev, dev_rot_channels, 275 sizeof(dev_rot_channels), 276 GFP_KERNEL); 277 if (!indio_dev->channels) { 278 dev_err(&pdev->dev, "failed to duplicate channels\n"); 279 return -ENOMEM; 280 } 281 282 ret = dev_rot_parse_report(pdev, hsdev, 283 (struct iio_chan_spec *)indio_dev->channels, 284 hsdev->usage, rot_state); 285 if (ret) { 286 dev_err(&pdev->dev, "failed to setup attributes\n"); 287 return ret; 288 } 289 290 indio_dev->num_channels = ARRAY_SIZE(dev_rot_channels); 291 indio_dev->info = &dev_rot_info; 292 indio_dev->name = name; 293 indio_dev->modes = INDIO_DIRECT_MODE; 294 295 atomic_set(&rot_state->common_attributes.data_ready, 0); 296 297 ret = hid_sensor_setup_trigger(indio_dev, name, 298 &rot_state->common_attributes); 299 if (ret) { 300 dev_err(&pdev->dev, "trigger setup failed\n"); 301 return ret; 302 } 303 304 ret = iio_device_register(indio_dev); 305 if (ret) { 306 dev_err(&pdev->dev, "device register failed\n"); 307 goto error_remove_trigger; 308 } 309 310 rot_state->callbacks.send_event = dev_rot_proc_event; 311 rot_state->callbacks.capture_sample = dev_rot_capture_sample; 312 rot_state->callbacks.pdev = pdev; 313 ret = sensor_hub_register_callback(hsdev, hsdev->usage, 314 &rot_state->callbacks); 315 if (ret) { 316 dev_err(&pdev->dev, "callback reg failed\n"); 317 goto error_iio_unreg; 318 } 319 320 return 0; 321 322error_iio_unreg: 323 iio_device_unregister(indio_dev); 324error_remove_trigger: 325 hid_sensor_remove_trigger(indio_dev, &rot_state->common_attributes); 326 return ret; 327} 328 329/* Function to deinitialize the processing for usage id */ 330static int hid_dev_rot_remove(struct platform_device *pdev) 331{ 332 struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data; 333 struct iio_dev *indio_dev = platform_get_drvdata(pdev); 334 struct dev_rot_state *rot_state = iio_priv(indio_dev); 335 336 sensor_hub_remove_callback(hsdev, hsdev->usage); 337 iio_device_unregister(indio_dev); 338 hid_sensor_remove_trigger(indio_dev, &rot_state->common_attributes); 339 340 return 0; 341} 342 343static const struct platform_device_id hid_dev_rot_ids[] = { 344 { 345 /* Format: HID-SENSOR-usage_id_in_hex_lowercase */ 346 .name = "HID-SENSOR-20008a", 347 }, 348 { 349 /* Relative orientation(AG) sensor */ 350 .name = "HID-SENSOR-20008e", 351 }, 352 { 353 /* Geomagnetic orientation(AM) sensor */ 354 .name = "HID-SENSOR-2000c1", 355 }, 356 { /* sentinel */ } 357}; 358MODULE_DEVICE_TABLE(platform, hid_dev_rot_ids); 359 360static struct platform_driver hid_dev_rot_platform_driver = { 361 .id_table = hid_dev_rot_ids, 362 .driver = { 363 .name = KBUILD_MODNAME, 364 .pm = &hid_sensor_pm_ops, 365 }, 366 .probe = hid_dev_rot_probe, 367 .remove = hid_dev_rot_remove, 368}; 369module_platform_driver(hid_dev_rot_platform_driver); 370 371MODULE_DESCRIPTION("HID Sensor Device Rotation"); 372MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>"); 373MODULE_LICENSE("GPL"); 374MODULE_IMPORT_NS(IIO_HID);