fb827c295842f0e4ebe9e81974c513bef9c6638e
[muen/linux.git] / 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 += snprintf(&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 += snprintf(&buf[len], PAGE_SIZE - len,
340                                                 "%lld ", value);
341                         }
342                         len += snprintf(&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");