1
2
3
4
5
6
7
8#include <linux/delay.h>
9#include <linux/device.h>
10#include <linux/iio/iio.h>
11#include <linux/kernel.h>
12#include <linux/module.h>
13#include <linux/platform_data/cros_ec_commands.h>
14#include <linux/platform_data/cros_ec_proto.h>
15#include <linux/platform_data/cros_ec_sensorhub.h>
16#include <linux/platform_device.h>
17#include <linux/sort.h>
18#include <linux/slab.h>
19
20#include "cros_ec_trace.h"
21
22
23#define M_PRECISION BIT(23)
24
25
26#define TS_HISTORY_THRESHOLD 8
27
28
29
30
31
32#define TS_HISTORY_BORED_US 500000
33
34
35#define FUTURE_TS_ANALYTICS_COUNT_MAX 100
36
37static inline int
38cros_sensorhub_send_sample(struct cros_ec_sensorhub *sensorhub,
39 struct cros_ec_sensors_ring_sample *sample)
40{
41 cros_ec_sensorhub_push_data_cb_t cb;
42 int id = sample->sensor_id;
43 struct iio_dev *indio_dev;
44
45 if (id >= sensorhub->sensor_num)
46 return -EINVAL;
47
48 cb = sensorhub->push_data[id].push_data_cb;
49 if (!cb)
50 return 0;
51
52 indio_dev = sensorhub->push_data[id].indio_dev;
53
54 if (sample->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
55 return 0;
56
57 return cb(indio_dev, sample->vector, sample->timestamp);
58}
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
75 u8 sensor_num,
76 struct iio_dev *indio_dev,
77 cros_ec_sensorhub_push_data_cb_t cb)
78{
79 if (sensor_num >= sensorhub->sensor_num)
80 return -EINVAL;
81 if (sensorhub->push_data[sensor_num].indio_dev)
82 return -EINVAL;
83
84 sensorhub->push_data[sensor_num].indio_dev = indio_dev;
85 sensorhub->push_data[sensor_num].push_data_cb = cb;
86
87 return 0;
88}
89EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);
90
91void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
92 u8 sensor_num)
93{
94 sensorhub->push_data[sensor_num].indio_dev = NULL;
95 sensorhub->push_data[sensor_num].push_data_cb = NULL;
96}
97EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data);
98
99
100
101
102
103
104
105
106
107
108
109int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,
110 bool on)
111{
112 int ret, i;
113
114 mutex_lock(&sensorhub->cmd_lock);
115 if (sensorhub->tight_timestamps)
116 for (i = 0; i < sensorhub->sensor_num; i++)
117 sensorhub->batch_state[i].last_len = 0;
118
119 sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INT_ENABLE;
120 sensorhub->params->fifo_int_enable.enable = on;
121
122 sensorhub->msg->outsize = sizeof(struct ec_params_motion_sense);
123 sensorhub->msg->insize = sizeof(struct ec_response_motion_sense);
124
125 ret = cros_ec_cmd_xfer_status(sensorhub->ec->ec_dev, sensorhub->msg);
126 mutex_unlock(&sensorhub->cmd_lock);
127
128
129 if (ret > 0)
130 ret = 0;
131
132 return ret;
133}
134
135static int cros_ec_sensor_ring_median_cmp(const void *pv1, const void *pv2)
136{
137 s64 v1 = *(s64 *)pv1;
138 s64 v2 = *(s64 *)pv2;
139
140 if (v1 > v2)
141 return 1;
142 else if (v1 < v2)
143 return -1;
144 else
145 return 0;
146}
147
148
149
150
151
152
153
154
155
156
157
158static s64 cros_ec_sensor_ring_median(s64 *array, size_t length)
159{
160 sort(array, length, sizeof(s64), cros_ec_sensor_ring_median_cmp, NULL);
161 return array[length / 2];
162}
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223static void
224cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state
225 *state,
226 s64 b, s64 c)
227{
228 s64 x, y;
229 s64 dx, dy;
230 s64 m;
231 s64 *m_history_copy = state->temp_buf;
232 s64 *error = state->temp_buf;
233 int i;
234
235
236 x = b;
237
238 y = c - b * 1000;
239
240 dx = (state->x_history[0] + state->x_offset) - x;
241 if (dx == 0)
242 return;
243 dy = (state->y_history[0] + state->y_offset) - y;
244 m = div64_s64(dy * M_PRECISION, dx);
245
246
247 if (-dx > TS_HISTORY_BORED_US)
248 state->history_len = 0;
249
250
251 for (i = state->history_len - 1; i >= 1; i--) {
252 state->x_history[i] = state->x_history[i - 1] + dx;
253 state->y_history[i] = state->y_history[i - 1] + dy;
254
255 state->m_history[i] = state->m_history[i - 1];
256
257
258
259
260 m_history_copy[i] = state->m_history[i - 1];
261 }
262
263
264 state->x_offset = x;
265 state->y_offset = y;
266 state->x_history[0] = 0;
267 state->y_history[0] = 0;
268
269 state->m_history[0] = m;
270 m_history_copy[0] = m;
271
272 if (state->history_len < CROS_EC_SENSORHUB_TS_HISTORY_SIZE)
273 state->history_len++;
274
275
276 if (state->history_len > TS_HISTORY_THRESHOLD) {
277 state->median_m =
278 cros_ec_sensor_ring_median(m_history_copy,
279 state->history_len - 1);
280
281
282
283
284
285
286 for (i = 0; i < state->history_len; i++)
287 error[i] = state->y_history[i] -
288 div_s64(state->median_m * state->x_history[i],
289 M_PRECISION);
290 state->median_error =
291 cros_ec_sensor_ring_median(error, state->history_len);
292 } else {
293 state->median_m = 0;
294 state->median_error = 0;
295 }
296 trace_cros_ec_sensorhub_filter(state, dx, dy);
297}
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324static s64
325cros_ec_sensor_ring_ts_filter(struct cros_ec_sensors_ts_filter_state *state,
326 s64 x)
327{
328 return div_s64(state->median_m * (x - state->x_offset), M_PRECISION)
329 + state->median_error + state->y_offset + x * 1000;
330}
331
332
333
334
335
336
337static void
338cros_ec_sensor_ring_fix_overflow(s64 *ts,
339 const s64 overflow_period,
340 struct cros_ec_sensors_ec_overflow_state
341 *state)
342{
343 s64 adjust;
344
345 *ts += state->offset;
346 if (abs(state->last - *ts) > (overflow_period / 2)) {
347 adjust = state->last > *ts ? overflow_period : -overflow_period;
348 state->offset += adjust;
349 *ts += adjust;
350 }
351 state->last = *ts;
352}
353
354static void
355cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub
356 *sensorhub,
357 struct cros_ec_sensors_ring_sample
358 *sample)
359{
360 const u8 sensor_id = sample->sensor_id;
361
362
363 if (sensorhub->batch_state[sensor_id].newest_sensor_event >
364 sample->timestamp)
365
366 sample->timestamp =
367 sensorhub->batch_state[sensor_id].last_ts;
368 else
369 sensorhub->batch_state[sensor_id].newest_sensor_event =
370 sample->timestamp;
371}
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387static bool
388cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub,
389 const struct ec_response_motion_sense_fifo_info
390 *fifo_info,
391 const ktime_t fifo_timestamp,
392 ktime_t *current_timestamp,
393 struct ec_response_motion_sensor_data *in,
394 struct cros_ec_sensors_ring_sample *out)
395{
396 const s64 now = cros_ec_get_time_ns();
397 int axis, async_flags;
398
399
400 async_flags = in->flags &
401 (MOTIONSENSE_SENSOR_FLAG_ODR | MOTIONSENSE_SENSOR_FLAG_FLUSH);
402
403 if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP && !async_flags) {
404 s64 a = in->timestamp;
405 s64 b = fifo_info->timestamp;
406 s64 c = fifo_timestamp;
407
408 cros_ec_sensor_ring_fix_overflow(&a, 1LL << 32,
409 &sensorhub->overflow_a);
410 cros_ec_sensor_ring_fix_overflow(&b, 1LL << 32,
411 &sensorhub->overflow_b);
412
413 if (sensorhub->tight_timestamps) {
414 cros_ec_sensor_ring_ts_filter_update(
415 &sensorhub->filter, b, c);
416 *current_timestamp = cros_ec_sensor_ring_ts_filter(
417 &sensorhub->filter, a);
418 } else {
419 s64 new_timestamp;
420
421
422
423
424
425 new_timestamp = c - b * 1000 + a * 1000;
426
427
428
429
430 if (new_timestamp - *current_timestamp > 0)
431 *current_timestamp = new_timestamp;
432 }
433 trace_cros_ec_sensorhub_timestamp(in->timestamp,
434 fifo_info->timestamp,
435 fifo_timestamp,
436 *current_timestamp,
437 now);
438 }
439
440 if (in->flags & MOTIONSENSE_SENSOR_FLAG_ODR) {
441 if (sensorhub->tight_timestamps) {
442 sensorhub->batch_state[in->sensor_num].last_len = 0;
443 sensorhub->batch_state[in->sensor_num].penul_len = 0;
444 }
445
446
447
448
449 return false;
450 }
451
452 if (in->flags & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
453 out->sensor_id = in->sensor_num;
454 out->timestamp = *current_timestamp;
455 out->flag = in->flags;
456 if (sensorhub->tight_timestamps)
457 sensorhub->batch_state[out->sensor_id].last_len = 0;
458
459
460
461
462 return true;
463 }
464
465 if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP)
466
467 return false;
468
469
470 out->sensor_id = in->sensor_num;
471 trace_cros_ec_sensorhub_data(in->sensor_num,
472 fifo_info->timestamp,
473 fifo_timestamp,
474 *current_timestamp,
475 now);
476
477 if (*current_timestamp - now > 0) {
478
479
480
481
482 sensorhub->future_timestamp_total_ns +=
483 *current_timestamp - now;
484 if (++sensorhub->future_timestamp_count ==
485 FUTURE_TS_ANALYTICS_COUNT_MAX) {
486 s64 avg = div_s64(sensorhub->future_timestamp_total_ns,
487 sensorhub->future_timestamp_count);
488 dev_warn_ratelimited(sensorhub->dev,
489 "100 timestamps in the future, %lldns shaved on average\n",
490 avg);
491 sensorhub->future_timestamp_count = 0;
492 sensorhub->future_timestamp_total_ns = 0;
493 }
494 out->timestamp = now;
495 } else {
496 out->timestamp = *current_timestamp;
497 }
498
499 out->flag = in->flags;
500 for (axis = 0; axis < 3; axis++)
501 out->vector[axis] = in->data[axis];
502
503 if (sensorhub->tight_timestamps)
504 cros_ec_sensor_ring_check_for_past_timestamp(sensorhub, out);
505 return true;
506}
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539static void
540cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub *sensorhub,
541 unsigned long sensor_mask,
542 struct cros_ec_sensors_ring_sample *last_out)
543{
544 struct cros_ec_sensors_ring_sample *batch_start, *next_batch_start;
545 int id;
546
547 for_each_set_bit(id, &sensor_mask, sensorhub->sensor_num) {
548 for (batch_start = sensorhub->ring; batch_start < last_out;
549 batch_start = next_batch_start) {
550
551
552
553
554 int batch_len, sample_idx;
555 struct cros_ec_sensors_ring_sample *batch_end =
556 batch_start;
557 struct cros_ec_sensors_ring_sample *s;
558 s64 batch_timestamp = batch_start->timestamp;
559 s64 sample_period;
560
561
562
563
564
565 if (batch_start->sensor_id != id) {
566 next_batch_start = batch_start + 1;
567 continue;
568 }
569
570
571
572
573
574
575 if (batch_start->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
576 cros_sensorhub_send_sample(sensorhub,
577 batch_start);
578 next_batch_start = batch_start + 1;
579 continue;
580 }
581
582 if (batch_start->timestamp <=
583 sensorhub->batch_state[id].last_ts) {
584 batch_timestamp =
585 sensorhub->batch_state[id].last_ts;
586 batch_len = sensorhub->batch_state[id].last_len;
587
588 sample_idx = batch_len;
589
590 sensorhub->batch_state[id].last_ts =
591 sensorhub->batch_state[id].penul_ts;
592 sensorhub->batch_state[id].last_len =
593 sensorhub->batch_state[id].penul_len;
594 } else {
595
596
597
598
599
600 sample_idx = 1;
601 batch_len = 1;
602 cros_sensorhub_send_sample(sensorhub,
603 batch_start);
604 batch_start++;
605 }
606
607
608 for (s = batch_start; s < last_out; s++) {
609 if (s->sensor_id != id)
610
611
612
613
614 continue;
615 if (s->timestamp != batch_timestamp)
616
617 break;
618 if (s->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
619
620 break;
621 batch_end = s;
622 batch_len++;
623 }
624
625 if (batch_len == 1)
626 goto done_with_this_batch;
627
628
629 if (sensorhub->batch_state[id].last_len == 0) {
630 dev_warn(sensorhub->dev, "Sensor %d: lost %d samples when spreading\n",
631 id, batch_len - 1);
632 goto done_with_this_batch;
633
634
635
636
637
638
639 }
640
641 sample_period = div_s64(batch_timestamp -
642 sensorhub->batch_state[id].last_ts,
643 sensorhub->batch_state[id].last_len);
644 dev_dbg(sensorhub->dev,
645 "Adjusting %d samples, sensor %d last_batch @%lld (%d samples) batch_timestamp=%lld => period=%lld\n",
646 batch_len, id,
647 sensorhub->batch_state[id].last_ts,
648 sensorhub->batch_state[id].last_len,
649 batch_timestamp,
650 sample_period);
651
652
653
654
655
656 for (s = batch_start; s <= batch_end; s++) {
657 if (s->sensor_id != id)
658
659
660
661
662 continue;
663
664 s->timestamp = batch_timestamp +
665 sample_period * sample_idx;
666 sample_idx++;
667
668 cros_sensorhub_send_sample(sensorhub, s);
669 }
670
671done_with_this_batch:
672 sensorhub->batch_state[id].penul_ts =
673 sensorhub->batch_state[id].last_ts;
674 sensorhub->batch_state[id].penul_len =
675 sensorhub->batch_state[id].last_len;
676
677 sensorhub->batch_state[id].last_ts =
678 batch_timestamp;
679 sensorhub->batch_state[id].last_len = batch_len;
680
681 next_batch_start = batch_end + 1;
682 }
683 }
684}
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707static void
708cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub *sensorhub,
709 unsigned long sensor_mask,
710 s64 current_timestamp,
711 struct cros_ec_sensors_ring_sample
712 *last_out)
713{
714 struct cros_ec_sensors_ring_sample *out;
715 int i;
716
717 for_each_set_bit(i, &sensor_mask, sensorhub->sensor_num) {
718 s64 timestamp;
719 int count = 0;
720 s64 time_period;
721
722 for (out = sensorhub->ring; out < last_out; out++) {
723 if (out->sensor_id != i)
724 continue;
725
726
727 timestamp = out->timestamp;
728 out++;
729 count = 1;
730 break;
731 }
732 for (; out < last_out; out++) {
733
734 if (out->sensor_id != i)
735 continue;
736 count++;
737 }
738 if (count == 0)
739 continue;
740
741
742 time_period = div_s64(current_timestamp - timestamp, count);
743
744 for (out = sensorhub->ring; out < last_out; out++) {
745 if (out->sensor_id != i)
746 continue;
747 timestamp += time_period;
748 out->timestamp = timestamp;
749 }
750 }
751
752
753 for (out = sensorhub->ring; out < last_out; out++)
754 cros_sensorhub_send_sample(sensorhub, out);
755}
756
757
758
759
760
761
762
763
764static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub)
765{
766 struct ec_response_motion_sense_fifo_info *fifo_info =
767 sensorhub->fifo_info;
768 struct cros_ec_dev *ec = sensorhub->ec;
769 ktime_t fifo_timestamp, current_timestamp;
770 int i, j, number_data, ret;
771 unsigned long sensor_mask = 0;
772 struct ec_response_motion_sensor_data *in;
773 struct cros_ec_sensors_ring_sample *out, *last_out;
774
775 mutex_lock(&sensorhub->cmd_lock);
776
777
778 if (fifo_info->total_lost) {
779 int fifo_info_length =
780 sizeof(struct ec_response_motion_sense_fifo_info) +
781 sizeof(u16) * sensorhub->sensor_num;
782
783
784 sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
785 sensorhub->msg->outsize = 1;
786 sensorhub->msg->insize = fifo_info_length;
787
788 if (cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg) < 0)
789 goto error;
790
791 memcpy(fifo_info, &sensorhub->resp->fifo_info,
792 fifo_info_length);
793
794
795
796
797
798 fifo_timestamp = cros_ec_get_time_ns();
799 } else {
800 fifo_timestamp = sensorhub->fifo_timestamp[
801 CROS_EC_SENSOR_NEW_TS];
802 }
803
804 if (fifo_info->count > sensorhub->fifo_size ||
805 fifo_info->size != sensorhub->fifo_size) {
806 dev_warn(sensorhub->dev,
807 "Mismatch EC data: count %d, size %d - expected %d\n",
808 fifo_info->count, fifo_info->size,
809 sensorhub->fifo_size);
810 goto error;
811 }
812
813
814 current_timestamp = sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS];
815 out = sensorhub->ring;
816 for (i = 0; i < fifo_info->count; i += number_data) {
817 sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_READ;
818 sensorhub->params->fifo_read.max_data_vector =
819 fifo_info->count - i;
820 sensorhub->msg->outsize =
821 sizeof(struct ec_params_motion_sense);
822 sensorhub->msg->insize =
823 sizeof(sensorhub->resp->fifo_read) +
824 sensorhub->params->fifo_read.max_data_vector *
825 sizeof(struct ec_response_motion_sensor_data);
826 ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
827 if (ret < 0) {
828 dev_warn(sensorhub->dev, "Fifo error: %d\n", ret);
829 break;
830 }
831 number_data = sensorhub->resp->fifo_read.number_data;
832 if (number_data == 0) {
833 dev_dbg(sensorhub->dev, "Unexpected empty FIFO\n");
834 break;
835 }
836 if (number_data > fifo_info->count - i) {
837 dev_warn(sensorhub->dev,
838 "Invalid EC data: too many entry received: %d, expected %d\n",
839 number_data, fifo_info->count - i);
840 break;
841 }
842 if (out + number_data >
843 sensorhub->ring + fifo_info->count) {
844 dev_warn(sensorhub->dev,
845 "Too many samples: %d (%zd data) to %d entries for expected %d entries\n",
846 i, out - sensorhub->ring, i + number_data,
847 fifo_info->count);
848 break;
849 }
850
851 for (in = sensorhub->resp->fifo_read.data, j = 0;
852 j < number_data; j++, in++) {
853 if (cros_ec_sensor_ring_process_event(
854 sensorhub, fifo_info,
855 fifo_timestamp,
856 ¤t_timestamp,
857 in, out)) {
858 sensor_mask |= BIT(in->sensor_num);
859 out++;
860 }
861 }
862 }
863 mutex_unlock(&sensorhub->cmd_lock);
864 last_out = out;
865
866 if (out == sensorhub->ring)
867
868 goto ring_handler_end;
869
870
871
872
873
874
875
876 if (!sensorhub->tight_timestamps &&
877 (last_out - 1)->timestamp == current_timestamp)
878 current_timestamp = fifo_timestamp;
879
880
881 if (fifo_info->total_lost)
882 for (i = 0; i < sensorhub->sensor_num; i++) {
883 if (fifo_info->lost[i]) {
884 dev_warn_ratelimited(sensorhub->dev,
885 "Sensor %d: lost: %d out of %d\n",
886 i, fifo_info->lost[i],
887 fifo_info->total_lost);
888 if (sensorhub->tight_timestamps)
889 sensorhub->batch_state[i].last_len = 0;
890 }
891 }
892
893
894
895
896
897 if (sensorhub->tight_timestamps)
898 cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,
899 last_out);
900 else
901 cros_ec_sensor_ring_spread_add_legacy(sensorhub, sensor_mask,
902 current_timestamp,
903 last_out);
904
905ring_handler_end:
906 sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
907 return;
908
909error:
910 mutex_unlock(&sensorhub->cmd_lock);
911}
912
913static int cros_ec_sensorhub_event(struct notifier_block *nb,
914 unsigned long queued_during_suspend,
915 void *_notify)
916{
917 struct cros_ec_sensorhub *sensorhub;
918 struct cros_ec_device *ec_dev;
919
920 sensorhub = container_of(nb, struct cros_ec_sensorhub, notifier);
921 ec_dev = sensorhub->ec->ec_dev;
922
923 if (ec_dev->event_data.event_type != EC_MKBP_EVENT_SENSOR_FIFO)
924 return NOTIFY_DONE;
925
926 if (ec_dev->event_size != sizeof(ec_dev->event_data.data.sensor_fifo)) {
927 dev_warn(ec_dev->dev, "Invalid fifo info size\n");
928 return NOTIFY_DONE;
929 }
930
931 if (queued_during_suspend)
932 return NOTIFY_OK;
933
934 memcpy(sensorhub->fifo_info, &ec_dev->event_data.data.sensor_fifo.info,
935 sizeof(*sensorhub->fifo_info));
936 sensorhub->fifo_timestamp[CROS_EC_SENSOR_NEW_TS] =
937 ec_dev->last_event_time;
938 cros_ec_sensorhub_ring_handler(sensorhub);
939
940 return NOTIFY_OK;
941}
942
943
944
945
946
947
948
949
950
951int cros_ec_sensorhub_ring_allocate(struct cros_ec_sensorhub *sensorhub)
952{
953 int fifo_info_length =
954 sizeof(struct ec_response_motion_sense_fifo_info) +
955 sizeof(u16) * sensorhub->sensor_num;
956
957
958 sensorhub->fifo_info = devm_kzalloc(sensorhub->dev, fifo_info_length,
959 GFP_KERNEL);
960 if (!sensorhub->fifo_info)
961 return -ENOMEM;
962
963
964
965
966
967 sensorhub->push_data = devm_kcalloc(sensorhub->dev,
968 sensorhub->sensor_num,
969 sizeof(*sensorhub->push_data),
970 GFP_KERNEL);
971 if (!sensorhub->push_data)
972 return -ENOMEM;
973
974 sensorhub->tight_timestamps = cros_ec_check_features(
975 sensorhub->ec,
976 EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);
977
978 if (sensorhub->tight_timestamps) {
979 sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
980 sensorhub->sensor_num,
981 sizeof(*sensorhub->batch_state),
982 GFP_KERNEL);
983 if (!sensorhub->batch_state)
984 return -ENOMEM;
985 }
986
987 return 0;
988}
989
990
991
992
993
994
995
996
997
998int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
999{
1000 struct cros_ec_dev *ec = sensorhub->ec;
1001 int ret;
1002 int fifo_info_length =
1003 sizeof(struct ec_response_motion_sense_fifo_info) +
1004 sizeof(u16) * sensorhub->sensor_num;
1005
1006
1007 sensorhub->msg->version = 2;
1008 sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
1009 sensorhub->msg->outsize = 1;
1010 sensorhub->msg->insize = fifo_info_length;
1011
1012 ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
1013 if (ret < 0)
1014 return ret;
1015
1016
1017
1018
1019
1020 sensorhub->fifo_size = sensorhub->resp->fifo_info.size;
1021 sensorhub->ring = devm_kcalloc(sensorhub->dev, sensorhub->fifo_size,
1022 sizeof(*sensorhub->ring), GFP_KERNEL);
1023 if (!sensorhub->ring)
1024 return -ENOMEM;
1025
1026 sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] =
1027 cros_ec_get_time_ns();
1028
1029
1030 sensorhub->notifier.notifier_call = cros_ec_sensorhub_event;
1031 ret = blocking_notifier_chain_register(&ec->ec_dev->event_notifier,
1032 &sensorhub->notifier);
1033 if (ret < 0)
1034 return ret;
1035
1036
1037 return cros_ec_sensorhub_ring_fifo_enable(sensorhub, true);
1038}
1039
1040void cros_ec_sensorhub_ring_remove(void *arg)
1041{
1042 struct cros_ec_sensorhub *sensorhub = arg;
1043 struct cros_ec_device *ec_dev = sensorhub->ec->ec_dev;
1044
1045
1046 cros_ec_sensorhub_ring_fifo_enable(sensorhub, false);
1047 blocking_notifier_chain_unregister(&ec_dev->event_notifier,
1048 &sensorhub->notifier);
1049}
1050