drivers/hid/hid-sensor-custom.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * hid-sensor-custom.c
   4  * Copyright (c) 2015, Intel Corporation.
   5  */
   6
   7 #include <linux/ctype.h>
   8 #include <linux/kernel.h>
   9 #include <linux/module.h>
  10 #include <linux/init.h>
  11 #include <linux/miscdevice.h>
  12 #include <linux/kfifo.h>
  13 #include <linux/sched.h>
  14 #include <linux/wait.h>
  15 #include <linux/poll.h>
  16 #include <linux/bsearch.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/hid-sensor-hub.h>
  19
  20 #define HID_CUSTOM_NAME_LENGTH          64
  21 #define HID_CUSTOM_MAX_CORE_ATTRS       10
  22 #define HID_CUSTOM_TOTAL_ATTRS          (HID_CUSTOM_MAX_CORE_ATTRS + 1)
  23 #define HID_CUSTOM_FIFO_SIZE            4096
  24 #define HID_CUSTOM_MAX_FEATURE_BYTES    64
  25 #define HID_SENSOR_USAGE_LENGTH (4 + 1)
  26
  27 struct hid_sensor_custom_field {
  28         int report_id;
  29         char group_name[HID_CUSTOM_NAME_LENGTH];
  30         struct hid_sensor_hub_attribute_info attribute;
  31         struct device_attribute sd_attrs[HID_CUSTOM_MAX_CORE_ATTRS];
  32         char attr_name[HID_CUSTOM_TOTAL_ATTRS][HID_CUSTOM_NAME_LENGTH];
  33         struct attribute *attrs[HID_CUSTOM_TOTAL_ATTRS];
  34         struct attribute_group hid_custom_attribute_group;
  35 };
  36
  37 struct hid_sensor_custom {
  38         struct mutex mutex;
  39         struct platform_device *pdev;
  40         struct hid_sensor_hub_device *hsdev;
  41         struct hid_sensor_hub_callbacks callbacks;
  42         int sensor_field_count;
  43         struct hid_sensor_custom_field *fields;
  44         int input_field_count;
  45         int input_report_size;
  46         int input_report_recd_size;
  47         bool input_skip_sample;
  48         bool enable;
  49         struct hid_sensor_custom_field *power_state;
  50         struct hid_sensor_custom_field *report_state;
  51         struct miscdevice custom_dev;
  52         struct kfifo data_fifo;
  53         unsigned long misc_opened;
  54         wait_queue_head_t wait;
  55         struct platform_device *custom_pdev;
  56 };
  57
  58 /* Header for each sample to user space via dev interface */
  59 struct hid_sensor_sample {
  60         u32 usage_id;
  61         u64 timestamp;
  62         u32 raw_len;
  63 } __packed;
  64
  65 static struct attribute hid_custom_attrs[] = {
  66         {.name = "name", .mode = S_IRUGO},
  67         {.name = "units", .mode = S_IRUGO},
  68         {.name = "unit-expo", .mode = S_IRUGO},
  69         {.name = "minimum", .mode = S_IRUGO},
  70         {.name = "maximum", .mode = S_IRUGO},
  71         {.name = "size", .mode = S_IRUGO},
  72         {.name = "value", .mode = S_IWUSR | S_IRUGO},
  73         {.name = NULL}
  74 };
  75
  76 static const struct hid_custom_usage_desc {
  77         int usage_id;
  78         char *desc;
  79 } hid_custom_usage_desc_table[] = {
  80         {0x200201,      "event-sensor-state"},
  81         {0x200202,      "event-sensor-event"},
  82         {0x200301,      "property-friendly-name"},
  83         {0x200302,      "property-persistent-unique-id"},
  84         {0x200303,      "property-sensor-status"},
  85         {0x200304,      "property-min-report-interval"},
  86         {0x200305,      "property-sensor-manufacturer"},
  87         {0x200306,      "property-sensor-model"},
  88         {0x200307,      "property-sensor-serial-number"},
  89         {0x200308,      "property-sensor-description"},
  90         {0x200309,      "property-sensor-connection-type"},
  91         {0x20030A,      "property-sensor-device-path"},
  92         {0x20030B,      "property-hardware-revision"},
  93         {0x20030C,      "property-firmware-version"},
  94         {0x20030D,      "property-release-date"},
  95         {0x20030E,      "property-report-interval"},
  96         {0x20030F,      "property-change-sensitivity-absolute"},
  97         {0x200310,      "property-change-sensitivity-percent-range"},
  98         {0x200311,      "property-change-sensitivity-percent-relative"},
  99         {0x200312,      "property-accuracy"},
 100         {0x200313,      "property-resolution"},
 101         {0x200314,      "property-maximum"},
 102         {0x200315,      "property-minimum"},
 103         {0x200316,      "property-reporting-state"},
 104         {0x200317,      "property-sampling-rate"},
 105         {0x200318,      "property-response-curve"},
 106         {0x200319,      "property-power-state"},
 107         {0x200540,      "data-field-custom"},
 108         {0x200541,      "data-field-custom-usage"},
 109         {0x200542,      "data-field-custom-boolean-array"},
 110         {0x200543,      "data-field-custom-value"},
 111         {0x200544,      "data-field-custom-value_1"},
 112         {0x200545,      "data-field-custom-value_2"},
 113         {0x200546,      "data-field-custom-value_3"},
 114         {0x200547,      "data-field-custom-value_4"},
 115         {0x200548,      "data-field-custom-value_5"},
 116         {0x200549,      "data-field-custom-value_6"},
 117         {0x20054A,      "data-field-custom-value_7"},
 118         {0x20054B,      "data-field-custom-value_8"},
 119         {0x20054C,      "data-field-custom-value_9"},
 120         {0x20054D,      "data-field-custom-value_10"},
 121         {0x20054E,      "data-field-custom-value_11"},
 122         {0x20054F,      "data-field-custom-value_12"},
 123         {0x200550,      "data-field-custom-value_13"},
 124         {0x200551,      "data-field-custom-value_14"},
 125         {0x200552,      "data-field-custom-value_15"},
 126         {0x200553,      "data-field-custom-value_16"},
 127         {0x200554,      "data-field-custom-value_17"},
 128         {0x200555,      "data-field-custom-value_18"},
 129         {0x200556,      "data-field-custom-value_19"},
 130         {0x200557,      "data-field-custom-value_20"},
 131         {0x200558,      "data-field-custom-value_21"},
 132         {0x200559,      "data-field-custom-value_22"},
 133         {0x20055A,      "data-field-custom-value_23"},
 134         {0x20055B,      "data-field-custom-value_24"},
 135         {0x20055C,      "data-field-custom-value_25"},
 136         {0x20055D,      "data-field-custom-value_26"},
 137         {0x20055E,      "data-field-custom-value_27"},
 138         {0x20055F,      "data-field-custom-value_28"},
 139 };
 140
 141 static int usage_id_cmp(const void *p1, const void *p2)
 142 {
 143         if (*(int *)p1 < *(int *)p2)
 144                 return -1;
 145
 146         if (*(int *)p1 > *(int *)p2)
 147                 return 1;
 148
 149         return 0;
 150 }
 151
 152 static ssize_t enable_sensor_show(struct device *dev,
 153                                   struct device_attribute *attr, char *buf)
 154 {
 155         struct hid_sensor_custom *sensor_inst = dev_get_drvdata(dev);
 156
 157         return sprintf(buf, "%d\n", sensor_inst->enable);
 158 }
 159
 160 static int set_power_report_state(struct hid_sensor_custom *sensor_inst,
 161                                   bool state)
 162 {
 163         int power_val = -1;
 164         int report_val = -1;
 165         u32 power_state_usage_id;
 166         u32 report_state_usage_id;
 167         int ret;
 168
 169         /*
 170          * It is possible that the power/report state ids are not present.
 171          * In this case this function will return success. But if the
 172          * ids are present, then it will return error if set fails.
 173          */
 174         if (state) {
 175                 power_state_usage_id =
 176                         HID_USAGE_SENSOR_PROP_POWER_STATE_D0_FULL_POWER_ENUM;
 177                 report_state_usage_id =
 178                         HID_USAGE_SENSOR_PROP_REPORTING_STATE_ALL_EVENTS_ENUM;
 179         } else {
 180                 power_state_usage_id =
 181                         HID_USAGE_SENSOR_PROP_POWER_STATE_D4_POWER_OFF_ENUM;
 182                 report_state_usage_id =
 183                         HID_USAGE_SENSOR_PROP_REPORTING_STATE_NO_EVENTS_ENUM;
 184         }
 185
 186         if (sensor_inst->power_state)
 187                 power_val = hid_sensor_get_usage_index(sensor_inst->hsdev,
 188                                 sensor_inst->power_state->attribute.report_id,
 189                                 sensor_inst->power_state->attribute.index,
 190                                 power_state_usage_id);
 191         if (sensor_inst->report_state)
 192                 report_val = hid_sensor_get_usage_index(sensor_inst->hsdev,
 193                                 sensor_inst->report_state->attribute.report_id,
 194                                 sensor_inst->report_state->attribute.index,
 195                                 report_state_usage_id);
 196
 197         if (power_val >= 0) {
 198                 power_val +=
 199                         sensor_inst->power_state->attribute.logical_minimum;
 200                 ret = sensor_hub_set_feature(sensor_inst->hsdev,
 201                                 sensor_inst->power_state->attribute.report_id,
 202                                 sensor_inst->power_state->attribute.index,
 203                                 sizeof(power_val),
 204                                 &power_val);
 205                 if (ret) {
 206                         hid_err(sensor_inst->hsdev->hdev,
 207                                 "Set power state failed\n");
 208                         return ret;
 209                 }
 210         }
 211
 212         if (report_val >= 0) {
 213                 report_val +=
 214                         sensor_inst->report_state->attribute.logical_minimum;
 215                 ret = sensor_hub_set_feature(sensor_inst->hsdev,
 216                                 sensor_inst->report_state->attribute.report_id,
 217                                 sensor_inst->report_state->attribute.index,
 218                                 sizeof(report_val),
 219                                 &report_val);
 220                 if (ret) {
 221                         hid_err(sensor_inst->hsdev->hdev,
 222                                 "Set report state failed\n");
 223                         return ret;
 224                 }
 225         }
 226
 227         return 0;
 228 }
 229
 230 static ssize_t enable_sensor_store(struct device *dev,
 231                                    struct device_attribute *attr,
 232                                    const char *buf, size_t count)
 233 {
 234         struct hid_sensor_custom *sensor_inst = dev_get_drvdata(dev);
 235         int value;
 236         int ret = -EINVAL;
 237
 238         if (kstrtoint(buf, 0, &value) != 0)
 239                 return -EINVAL;
 240
 241         mutex_lock(&sensor_inst->mutex);
 242         if (value && !sensor_inst->enable) {
 243                 ret = sensor_hub_device_open(sensor_inst->hsdev);
 244                 if (ret)
 245                         goto unlock_state;
 246
 247                 ret = set_power_report_state(sensor_inst, true);
 248                 if (ret) {
 249                         sensor_hub_device_close(sensor_inst->hsdev);
 250                         goto unlock_state;
 251                 }
 252                 sensor_inst->enable = true;
 253         } else if (!value && sensor_inst->enable) {
 254                 ret = set_power_report_state(sensor_inst, false);
 255                 sensor_hub_device_close(sensor_inst->hsdev);
 256                 sensor_inst->enable = false;
 257         }
 258 unlock_state:
 259         mutex_unlock(&sensor_inst->mutex);
 260         if (ret < 0)
 261                 return ret;
 262
 263         return count;
 264 }
 265 static DEVICE_ATTR_RW(enable_sensor);
 266
 267 static struct attribute *enable_sensor_attrs[] = {
 268         &dev_attr_enable_sensor.attr,
 269         NULL,
 270 };
 271
 272 static const struct attribute_group enable_sensor_attr_group = {
 273         .attrs = enable_sensor_attrs,
 274 };
 275
 276 static ssize_t show_value(struct device *dev, struct device_attribute *attr,
 277                           char *buf)
 278 {
 279         struct hid_sensor_custom *sensor_inst = dev_get_drvdata(dev);
 280         struct hid_sensor_hub_attribute_info *attribute;
 281         int index, usage, field_index;
 282         char name[HID_CUSTOM_NAME_LENGTH];
 283         bool feature = false;
 284         bool input = false;
 285         int value = 0;
 286
 287         if (sscanf(attr->attr.name, "feature-%x-%x-%s", &index, &usage,
 288                    name) == 3) {
 289                 feature = true;
 290                 field_index = index + sensor_inst->input_field_count;
 291         } else if (sscanf(attr->attr.name, "input-%x-%x-%s", &index, &usage,
 292                    name) == 3) {
 293                 input = true;
 294                 field_index = index;
 295         } else
 296                 return -EINVAL;
 297
 298         if (!strncmp(name, "value", strlen("value"))) {
 299                 u32 report_id;
 300                 int ret;
 301
 302                 attribute = &sensor_inst->fields[field_index].attribute;
 303                 report_id = attribute->report_id;
 304                 if (feature) {
 305                         u8 values[HID_CUSTOM_MAX_FEATURE_BYTES];
 306                         int len = 0;
 307                         u64 value = 0;
 308                         int i = 0;
 309
 310                         ret = sensor_hub_get_feature(sensor_inst->hsdev,
 311                                                      report_id,
 312                                                      index,
 313                                                      sizeof(values), values);
 314                         if (ret < 0)
 315                                 return ret;
 316
 317                         while (i < ret) {
 318                                 if (i + attribute->size > ret) {
 319                                         len += scnprintf(&buf[len],
 320                                                         PAGE_SIZE - len,
 321                                                         "%d ", values[i]);
 322                                         break;
 323                                 }
 324                                 switch (attribute->size) {
 325                                 case 2:
 326                                         value = (u64) *(u16 *)&values[i];
 327                                         i += attribute->size;
 328                                         break;
 329                                 case 4:
 330                                         value = (u64) *(u32 *)&values[i];
 331                                         i += attribute->size;
 332                                         break;
 333                                 case 8:
 334                                         value = *(u64 *)&values[i];
 335                                         i += attribute->size;
 336                                         break;
 337                                 default:
 338                                         value = (u64) values[i];
 339                                         ++i;
 340                                         break;
 341                                 }
 342                                 len += scnprintf(&buf[len], PAGE_SIZE - len,
 343                                                 "%lld ", value);
 344                         }
 345                         len += scnprintf(&buf[len], PAGE_SIZE - len, "\n");
 346
 347                         return len;
 348                 } else if (input)
 349                         value = sensor_hub_input_attr_get_raw_value(
 350                                                 sensor_inst->hsdev,
 351                                                 sensor_inst->hsdev->usage,
 352                                                 usage, report_id,
 353                                                 SENSOR_HUB_SYNC, false);
 354         } else if (!strncmp(name, "units", strlen("units")))
 355                 value = sensor_inst->fields[field_index].attribute.units;
 356         else if (!strncmp(name, "unit-expo", strlen("unit-expo")))
 357                 value = sensor_inst->fields[field_index].attribute.unit_expo;
 358         else if (!strncmp(name, "size", strlen("size")))
 359                 value = sensor_inst->fields[field_index].attribute.size;
 360         else if (!strncmp(name, "minimum", strlen("minimum")))
 361                 value = sensor_inst->fields[field_index].attribute.
 362                                                         logical_minimum;
 363         else if (!strncmp(name, "maximum", strlen("maximum")))
 364                 value = sensor_inst->fields[field_index].attribute.
 365                                                         logical_maximum;
 366         else if (!strncmp(name, "name", strlen("name"))) {
 367                 struct hid_custom_usage_desc *usage_desc;
 368
 369                 usage_desc = bsearch(&usage, hid_custom_usage_desc_table,
 370                                      ARRAY_SIZE(hid_custom_usage_desc_table),
 371                                      sizeof(struct hid_custom_usage_desc),
 372                                      usage_id_cmp);
 373                 if (usage_desc)
 374                         return snprintf(buf, PAGE_SIZE, "%s\n",
 375                                         usage_desc->desc);
 376                 else
 377                         return sprintf(buf, "not-specified\n");
 378          } else
 379                 return -EINVAL;
 380
 381         return sprintf(buf, "%d\n", value);
 382 }
 383
 384 static ssize_t store_value(struct device *dev, struct device_attribute *attr,
 385                            const char *buf, size_t count)
 386 {
 387         struct hid_sensor_custom *sensor_inst = dev_get_drvdata(dev);
 388         int index, field_index, usage;
 389         char name[HID_CUSTOM_NAME_LENGTH];
 390         int value;
 391
 392         if (sscanf(attr->attr.name, "feature-%x-%x-%s", &index, &usage,
 393                    name) == 3) {
 394                 field_index = index + sensor_inst->input_field_count;
 395         } else
 396                 return -EINVAL;
 397
 398         if (!strncmp(name, "value", strlen("value"))) {
 399                 u32 report_id;
 400                 int ret;
 401
 402                 if (kstrtoint(buf, 0, &value) != 0)
 403                         return -EINVAL;
 404
 405                 report_id = sensor_inst->fields[field_index].attribute.
 406                                                                 report_id;
 407                 ret = sensor_hub_set_feature(sensor_inst->hsdev, report_id,
 408                                              index, sizeof(value), &value);
 409         } else
 410                 return -EINVAL;
 411
 412         return count;
 413 }
 414
 415 static int hid_sensor_capture_sample(struct hid_sensor_hub_device *hsdev,
 416                                   unsigned usage_id, size_t raw_len,
 417                                   char *raw_data, void *priv)
 418 {
 419         struct hid_sensor_custom *sensor_inst = platform_get_drvdata(priv);
 420         struct hid_sensor_sample header;
 421
 422         /* If any error occurs in a sample, rest of the fields are ignored */
 423         if (sensor_inst->input_skip_sample) {
 424                 hid_err(sensor_inst->hsdev->hdev, "Skipped remaining data\n");
 425                 return 0;
 426         }
 427
 428         hid_dbg(sensor_inst->hsdev->hdev, "%s received %d of %d\n", __func__,
 429                 (int) (sensor_inst->input_report_recd_size + raw_len),
 430                 sensor_inst->input_report_size);
 431
 432         if (!test_bit(0, &sensor_inst->misc_opened))
 433                 return 0;
 434
 435         if (!sensor_inst->input_report_recd_size) {
 436                 int required_size = sizeof(struct hid_sensor_sample) +
 437                                                 sensor_inst->input_report_size;
 438                 header.usage_id = hsdev->usage;
 439                 header.raw_len = sensor_inst->input_report_size;
 440                 header.timestamp = ktime_get_real_ns();
 441                 if (kfifo_avail(&sensor_inst->data_fifo) >= required_size) {
 442                         kfifo_in(&sensor_inst->data_fifo,
 443                                  (unsigned char *)&header,
 444                                  sizeof(header));
 445                 } else
 446                         sensor_inst->input_skip_sample = true;
 447         }
 448         if (kfifo_avail(&sensor_inst->data_fifo) >= raw_len)
 449                 kfifo_in(&sensor_inst->data_fifo, (unsigned char *)raw_data,
 450                          raw_len);
 451
 452         sensor_inst->input_report_recd_size += raw_len;
 453
 454         return 0;
 455 }
 456
 457 static int hid_sensor_send_event(struct hid_sensor_hub_device *hsdev,
 458                                  unsigned usage_id, void *priv)
 459 {
 460         struct hid_sensor_custom *sensor_inst = platform_get_drvdata(priv);
 461
 462         if (!test_bit(0, &sensor_inst->misc_opened))
 463                 return 0;
 464
 465         sensor_inst->input_report_recd_size = 0;
 466         sensor_inst->input_skip_sample = false;
 467
 468         wake_up(&sensor_inst->wait);
 469
 470         return 0;
 471 }
 472
 473 static int hid_sensor_custom_add_field(struct hid_sensor_custom *sensor_inst,
 474                                        int index, int report_type,
 475                                        struct hid_report *report,
 476                                        struct hid_field *field)
 477 {
 478         struct hid_sensor_custom_field *sensor_field;
 479         void *fields;
 480
 481         fields = krealloc(sensor_inst->fields,
 482                           (sensor_inst->sensor_field_count + 1) *
 483                            sizeof(struct hid_sensor_custom_field), GFP_KERNEL);
 484         if (!fields) {
 485                 kfree(sensor_inst->fields);
 486                 return -ENOMEM;
 487         }
 488         sensor_inst->fields = fields;
 489         sensor_field = &sensor_inst->fields[sensor_inst->sensor_field_count];
 490         sensor_field->attribute.usage_id = sensor_inst->hsdev->usage;
 491         if (field->logical)
 492                 sensor_field->attribute.attrib_id = field->logical;
 493         else
 494                 sensor_field->attribute.attrib_id = field->usage[0].hid;
 495
 496         sensor_field->attribute.index = index;
 497         sensor_field->attribute.report_id = report->id;
 498         sensor_field->attribute.units = field->unit;
 499         sensor_field->attribute.unit_expo = field->unit_exponent;
 500         sensor_field->attribute.size = (field->report_size / 8);
 501         sensor_field->attribute.logical_minimum = field->logical_minimum;
 502         sensor_field->attribute.logical_maximum = field->logical_maximum;
 503
 504         if (report_type == HID_FEATURE_REPORT)
 505                 snprintf(sensor_field->group_name,
 506                          sizeof(sensor_field->group_name), "feature-%x-%x",
 507                          sensor_field->attribute.index,
 508                          sensor_field->attribute.attrib_id);
 509         else if (report_type == HID_INPUT_REPORT) {
 510                 snprintf(sensor_field->group_name,
 511                          sizeof(sensor_field->group_name),
 512                          "input-%x-%x", sensor_field->attribute.index,
 513                          sensor_field->attribute.attrib_id);
 514                 sensor_inst->input_field_count++;
 515                 sensor_inst->input_report_size += (field->report_size *
 516                                                    field->report_count) / 8;
 517         }
 518
 519         memset(&sensor_field->hid_custom_attribute_group, 0,
 520                sizeof(struct attribute_group));
 521         sensor_inst->sensor_field_count++;
 522
 523         return 0;
 524 }
 525
 526 static int hid_sensor_custom_add_fields(struct hid_sensor_custom *sensor_inst,
 527                                         struct hid_report_enum *report_enum,
 528                                         int report_type)
 529 {
 530         int i;
 531         int ret;
 532         struct hid_report *report;
 533         struct hid_field *field;
 534         struct hid_sensor_hub_device *hsdev = sensor_inst->hsdev;
 535
 536         list_for_each_entry(report, &report_enum->report_list, list) {
 537                 for (i = 0; i < report->maxfield; ++i) {
 538                         field = report->field[i];
 539                         if (field->maxusage &&
 540                             ((field->usage[0].collection_index >=
 541                               hsdev->start_collection_index) &&
 542                               (field->usage[0].collection_index <
 543                                hsdev->end_collection_index))) {
 544
 545                                 ret = hid_sensor_custom_add_field(sensor_inst,
 546                                                                   i,
 547                                                                   report_type,
 548                                                                   report,
 549                                                                   field);
 550                                 if (ret)
 551                                         return ret;
 552
 553                         }
 554                 }
 555         }
 556
 557         return 0;
 558 }
 559
 560 static int hid_sensor_custom_add_attributes(struct hid_sensor_custom
 561                                                                 *sensor_inst)
 562 {
 563         struct hid_sensor_hub_device *hsdev = sensor_inst->hsdev;
 564         struct hid_device *hdev = hsdev->hdev;
 565         int ret = -1;
 566         int i, j;
 567
 568         for (j = 0; j < HID_REPORT_TYPES; ++j) {
 569                 if (j == HID_OUTPUT_REPORT)
 570                         continue;
 571
 572                 ret = hid_sensor_custom_add_fields(sensor_inst,
 573                                                    &hdev->report_enum[j], j);
 574                 if (ret)
 575                         return ret;
 576
 577         }
 578
 579         /* Create sysfs attributes */
 580         for (i = 0; i < sensor_inst->sensor_field_count; ++i) {
 581                 j = 0;
 582                 while (j < HID_CUSTOM_TOTAL_ATTRS &&
 583                        hid_custom_attrs[j].name) {
 584                         struct device_attribute *device_attr;
 585
 586                         device_attr = &sensor_inst->fields[i].sd_attrs[j];
 587
 588                         snprintf((char *)&sensor_inst->fields[i].attr_name[j],
 589                                  HID_CUSTOM_NAME_LENGTH, "%s-%s",
 590                                  sensor_inst->fields[i].group_name,
 591                                  hid_custom_attrs[j].name);
 592                         sysfs_attr_init(&device_attr->attr);
 593                         device_attr->attr.name =
 594                                 (char *)&sensor_inst->fields[i].attr_name[j];
 595                         device_attr->attr.mode = hid_custom_attrs[j].mode;
 596                         device_attr->show = show_value;
 597                         if (hid_custom_attrs[j].mode & S_IWUSR)
 598                                 device_attr->store = store_value;
 599                         sensor_inst->fields[i].attrs[j] = &device_attr->attr;
 600                         ++j;
 601                 }
 602                 sensor_inst->fields[i].attrs[j] = NULL;
 603                 sensor_inst->fields[i].hid_custom_attribute_group.attrs =
 604                                                 sensor_inst->fields[i].attrs;
 605                 sensor_inst->fields[i].hid_custom_attribute_group.name =
 606                                         sensor_inst->fields[i].group_name;
 607                 ret = sysfs_create_group(&sensor_inst->pdev->dev.kobj,
 608                                          &sensor_inst->fields[i].
 609                                          hid_custom_attribute_group);
 610                 if (ret)
 611                         break;
 612
 613                 /* For power or report field store indexes */
 614                 if (sensor_inst->fields[i].attribute.attrib_id ==
 615                                         HID_USAGE_SENSOR_PROY_POWER_STATE)
 616                         sensor_inst->power_state = &sensor_inst->fields[i];
 617                 else if (sensor_inst->fields[i].attribute.attrib_id ==
 618                                         HID_USAGE_SENSOR_PROP_REPORT_STATE)
 619                         sensor_inst->report_state = &sensor_inst->fields[i];
 620         }
 621
 622         return ret;
 623 }
 624
 625 static void hid_sensor_custom_remove_attributes(struct hid_sensor_custom *
 626                                                                 sensor_inst)
 627 {
 628         int i;
 629
 630         for (i = 0; i < sensor_inst->sensor_field_count; ++i)
 631                 sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
 632                                    &sensor_inst->fields[i].
 633                                    hid_custom_attribute_group);
 634
 635         kfree(sensor_inst->fields);
 636 }
 637
 638 static ssize_t hid_sensor_custom_read(struct file *file, char __user *buf,
 639                                       size_t count, loff_t *f_ps)
 640 {
 641         struct hid_sensor_custom *sensor_inst;
 642         unsigned int copied;
 643         int ret;
 644
 645         sensor_inst = container_of(file->private_data,
 646                                    struct hid_sensor_custom, custom_dev);
 647
 648         if (count < sizeof(struct hid_sensor_sample))
 649                 return -EINVAL;
 650
 651         do {
 652                 if (kfifo_is_empty(&sensor_inst->data_fifo)) {
 653                         if (file->f_flags & O_NONBLOCK)
 654                                 return -EAGAIN;
 655
 656                         ret = wait_event_interruptible(sensor_inst->wait,
 657                                 !kfifo_is_empty(&sensor_inst->data_fifo));
 658                         if (ret)
 659                                 return ret;
 660                 }
 661                 ret = kfifo_to_user(&sensor_inst->data_fifo, buf, count,
 662                                     &copied);
 663                 if (ret)
 664                         return ret;
 665
 666         } while (copied == 0);
 667
 668         return copied;
 669 }
 670
 671 static int hid_sensor_custom_release(struct inode *inode, struct file *file)
 672 {
 673         struct hid_sensor_custom *sensor_inst;
 674
 675         sensor_inst = container_of(file->private_data,
 676                                    struct hid_sensor_custom, custom_dev);
 677
 678         clear_bit(0, &sensor_inst->misc_opened);
 679
 680         return 0;
 681 }
 682
 683 static int hid_sensor_custom_open(struct inode *inode, struct file *file)
 684 {
 685         struct hid_sensor_custom *sensor_inst;
 686
 687         sensor_inst = container_of(file->private_data,
 688                                    struct hid_sensor_custom, custom_dev);
 689         /* We essentially have single reader and writer */
 690         if (test_and_set_bit(0, &sensor_inst->misc_opened))
 691                 return -EBUSY;
 692
 693         return stream_open(inode, file);
 694 }
 695
 696 static __poll_t hid_sensor_custom_poll(struct file *file,
 697                                            struct poll_table_struct *wait)
 698 {
 699         struct hid_sensor_custom *sensor_inst;
 700         __poll_t mask = 0;
 701
 702         sensor_inst = container_of(file->private_data,
 703                                    struct hid_sensor_custom, custom_dev);
 704
 705         poll_wait(file, &sensor_inst->wait, wait);
 706
 707         if (!kfifo_is_empty(&sensor_inst->data_fifo))
 708                 mask = EPOLLIN | EPOLLRDNORM;
 709
 710         return mask;
 711 }
 712
 713 static const struct file_operations hid_sensor_custom_fops = {
 714         .open =  hid_sensor_custom_open,
 715         .read =  hid_sensor_custom_read,
 716         .release = hid_sensor_custom_release,
 717         .poll = hid_sensor_custom_poll,
 718         .llseek = noop_llseek,
 719 };
 720
 721 static int hid_sensor_custom_dev_if_add(struct hid_sensor_custom *sensor_inst)
 722 {
 723         int ret;
 724
 725         ret = kfifo_alloc(&sensor_inst->data_fifo, HID_CUSTOM_FIFO_SIZE,
 726                           GFP_KERNEL);
 727         if (ret)
 728                 return ret;
 729
 730         init_waitqueue_head(&sensor_inst->wait);
 731
 732         sensor_inst->custom_dev.minor = MISC_DYNAMIC_MINOR;
 733         sensor_inst->custom_dev.name = dev_name(&sensor_inst->pdev->dev);
 734         sensor_inst->custom_dev.fops = &hid_sensor_custom_fops,
 735         ret = misc_register(&sensor_inst->custom_dev);
 736         if (ret) {
 737                 kfifo_free(&sensor_inst->data_fifo);
 738                 return ret;
 739         }
 740         return 0;
 741 }
 742
 743 static void hid_sensor_custom_dev_if_remove(struct hid_sensor_custom
 744                                                                 *sensor_inst)
 745 {
 746         wake_up(&sensor_inst->wait);
 747         misc_deregister(&sensor_inst->custom_dev);
 748         kfifo_free(&sensor_inst->data_fifo);
 749
 750 }
 751
 752 /* luid defined in FW (e.g. ISH).  Maybe used to identify sensor. */
 753 static const char *const known_sensor_luid[] = { "020B000000000000" };
 754
 755 static int get_luid_table_index(unsigned char *usage_str)
 756 {
 757         int i;
 758
 759         for (i = 0; i < ARRAY_SIZE(known_sensor_luid); i++) {
 760                 if (!strncmp(usage_str, known_sensor_luid[i],
 761                              strlen(known_sensor_luid[i])))
 762                         return i;
 763         }
 764
 765         return -ENODEV;
 766 }
 767
 768 static int get_known_custom_sensor_index(struct hid_sensor_hub_device *hsdev)
 769 {
 770         struct hid_sensor_hub_attribute_info sensor_manufacturer = { 0 };
 771         struct hid_sensor_hub_attribute_info sensor_luid_info = { 0 };
 772         int report_size;
 773         int ret;
 774         static u16 w_buf[HID_CUSTOM_MAX_FEATURE_BYTES];
 775         static char buf[HID_CUSTOM_MAX_FEATURE_BYTES];
 776         int i;
 777
 778         memset(w_buf, 0, sizeof(w_buf));
 779         memset(buf, 0, sizeof(buf));
 780
 781         /* get manufacturer info */
 782         ret = sensor_hub_input_get_attribute_info(hsdev,
 783                         HID_FEATURE_REPORT, hsdev->usage,
 784                         HID_USAGE_SENSOR_PROP_MANUFACTURER, &sensor_manufacturer);
 785         if (ret < 0)
 786                 return ret;
 787
 788         report_size =
 789                 sensor_hub_get_feature(hsdev, sensor_manufacturer.report_id,
 790                                        sensor_manufacturer.index, sizeof(w_buf),
 791                                        w_buf);
 792         if (report_size <= 0) {
 793                 hid_err(hsdev->hdev,
 794                         "Failed to get sensor manufacturer info %d\n",
 795                         report_size);
 796                 return -ENODEV;
 797         }
 798
 799         /* convert from wide char to char */
 800         for (i = 0; i < ARRAY_SIZE(buf) - 1 && w_buf[i]; i++)
 801                 buf[i] = (char)w_buf[i];
 802
 803         /* ensure it's ISH sensor */
 804         if (strncmp(buf, "INTEL", strlen("INTEL")))
 805                 return -ENODEV;
 806
 807         memset(w_buf, 0, sizeof(w_buf));
 808         memset(buf, 0, sizeof(buf));
 809
 810         /* get real usage id */
 811         ret = sensor_hub_input_get_attribute_info(hsdev,
 812                         HID_FEATURE_REPORT, hsdev->usage,
 813                         HID_USAGE_SENSOR_PROP_SERIAL_NUM, &sensor_luid_info);
 814         if (ret < 0)
 815                 return ret;
 816
 817         report_size = sensor_hub_get_feature(hsdev, sensor_luid_info.report_id,
 818                                              sensor_luid_info.index, sizeof(w_buf),
 819                                              w_buf);
 820         if (report_size <= 0) {
 821                 hid_err(hsdev->hdev, "Failed to get real usage info %d\n",
 822                         report_size);
 823                 return -ENODEV;
 824         }
 825
 826         /* convert from wide char to char */
 827         for (i = 0; i < ARRAY_SIZE(buf) - 1 && w_buf[i]; i++)
 828                 buf[i] = (char)w_buf[i];
 829
 830         if (strlen(buf) != strlen(known_sensor_luid[0]) + 5) {
 831                 hid_err(hsdev->hdev,
 832                         "%s luid length not match %zu != (%zu + 5)\n", __func__,
 833                         strlen(buf), strlen(known_sensor_luid[0]));
 834                 return -ENODEV;
 835         }
 836
 837         /* get table index with luid (not matching 'LUID: ' in luid) */
 838         return get_luid_table_index(&buf[5]);
 839 }
 840
 841 static struct platform_device *
 842 hid_sensor_register_platform_device(struct platform_device *pdev,
 843                                     struct hid_sensor_hub_device *hsdev,
 844                                     int index)
 845 {
 846         char real_usage[HID_SENSOR_USAGE_LENGTH] = { 0 };
 847         struct platform_device *custom_pdev;
 848         const char *dev_name;
 849         char *c;
 850
 851         /* copy real usage id */
 852         memcpy(real_usage, known_sensor_luid[index], 4);
 853
 854         /* usage id are all lowcase */
 855         for (c = real_usage; *c != '\0'; c++)
 856                 *c = tolower(*c);
 857
 858         /* HID-SENSOR-INT-REAL_USAGE_ID */
 859         dev_name = kasprintf(GFP_KERNEL, "HID-SENSOR-INT-%s", real_usage);
 860         if (!dev_name)
 861                 return ERR_PTR(-ENOMEM);
 862
 863         custom_pdev = platform_device_register_data(pdev->dev.parent, dev_name,
 864                                                     PLATFORM_DEVID_NONE, hsdev,
 865                                                     sizeof(*hsdev));
 866         kfree(dev_name);
 867         return custom_pdev;
 868 }
 869
 870 static int hid_sensor_custom_probe(struct platform_device *pdev)
 871 {
 872         struct hid_sensor_custom *sensor_inst;
 873         struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
 874         int ret;
 875         int index;
 876
 877         sensor_inst = devm_kzalloc(&pdev->dev, sizeof(*sensor_inst),
 878                                    GFP_KERNEL);
 879         if (!sensor_inst)
 880                 return -ENOMEM;
 881
 882         sensor_inst->callbacks.capture_sample = hid_sensor_capture_sample;
 883         sensor_inst->callbacks.send_event = hid_sensor_send_event;
 884         sensor_inst->callbacks.pdev = pdev;
 885         sensor_inst->hsdev = hsdev;
 886         sensor_inst->pdev = pdev;
 887         mutex_init(&sensor_inst->mutex);
 888         platform_set_drvdata(pdev, sensor_inst);
 889
 890         index = get_known_custom_sensor_index(hsdev);
 891         if (index >= 0 && index < ARRAY_SIZE(known_sensor_luid)) {
 892                 sensor_inst->custom_pdev =
 893                         hid_sensor_register_platform_device(pdev, hsdev, index);
 894
 895                 ret = PTR_ERR_OR_ZERO(sensor_inst->custom_pdev);
 896                 if (ret) {
 897                         dev_err(&pdev->dev,
 898                                 "register_platform_device failed\n");
 899                         return ret;
 900                 }
 901
 902                 return 0;
 903         }
 904
 905         ret = sensor_hub_register_callback(hsdev, hsdev->usage,
 906                                            &sensor_inst->callbacks);
 907         if (ret < 0) {
 908                 dev_err(&pdev->dev, "callback reg failed\n");
 909                 return ret;
 910         }
 911
 912         ret = sysfs_create_group(&sensor_inst->pdev->dev.kobj,
 913                                  &enable_sensor_attr_group);
 914         if (ret)
 915                 goto err_remove_callback;
 916
 917         ret = hid_sensor_custom_add_attributes(sensor_inst);
 918         if (ret)
 919                 goto err_remove_group;
 920
 921         ret = hid_sensor_custom_dev_if_add(sensor_inst);
 922         if (ret)
 923                 goto err_remove_attributes;
 924
 925         return 0;
 926
 927 err_remove_attributes:
 928         hid_sensor_custom_remove_attributes(sensor_inst);
 929 err_remove_group:
 930         sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
 931                            &enable_sensor_attr_group);
 932 err_remove_callback:
 933         sensor_hub_remove_callback(hsdev, hsdev->usage);
 934
 935         return ret;
 936 }
 937
 938 static int hid_sensor_custom_remove(struct platform_device *pdev)
 939 {
 940         struct hid_sensor_custom *sensor_inst = platform_get_drvdata(pdev);
 941         struct hid_sensor_hub_device *hsdev = pdev->dev.platform_data;
 942
 943         if (sensor_inst->custom_pdev) {
 944                 platform_device_unregister(sensor_inst->custom_pdev);
 945                 return 0;
 946         }
 947
 948         hid_sensor_custom_dev_if_remove(sensor_inst);
 949         hid_sensor_custom_remove_attributes(sensor_inst);
 950         sysfs_remove_group(&sensor_inst->pdev->dev.kobj,
 951                            &enable_sensor_attr_group);
 952         sensor_hub_remove_callback(hsdev, hsdev->usage);
 953
 954         return 0;
 955 }
 956
 957 static const struct platform_device_id hid_sensor_custom_ids[] = {
 958         {
 959                 .name = "HID-SENSOR-2000e1",
 960         },
 961         {
 962                 .name = "HID-SENSOR-2000e2",
 963         },
 964         { /* sentinel */ }
 965 };
 966 MODULE_DEVICE_TABLE(platform, hid_sensor_custom_ids);
 967
 968 static struct platform_driver hid_sensor_custom_platform_driver = {
 969         .id_table = hid_sensor_custom_ids,
 970         .driver = {
 971                 .name   = KBUILD_MODNAME,
 972         },
 973         .probe          = hid_sensor_custom_probe,
 974         .remove         = hid_sensor_custom_remove,
 975 };
 976 module_platform_driver(hid_sensor_custom_platform_driver);
 977
 978 MODULE_DESCRIPTION("HID Sensor Custom and Generic sensor Driver");
 979 MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
 980 MODULE_LICENSE("GPL");