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