summaryrefslogtreecommitdiff
path: root/drivers/platform/chrome/cros_ec_sensorhub_ring.c
blob: c48e5b38a4417d27daee2efd6c42a71fd17b80f9 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
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
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
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
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
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
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
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
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
// SPDX-License-Identifier: GPL-2.0-only
/*
 * Driver for Chrome OS EC Sensor hub FIFO.
 *
 * Copyright 2020 Google LLC
 */

#include <linux/delay.h>
#include <linux/device.h>
#include <linux/iio/iio.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/platform_data/cros_ec_commands.h>
#include <linux/platform_data/cros_ec_proto.h>
#include <linux/platform_data/cros_ec_sensorhub.h>
#include <linux/platform_device.h>
#include <linux/sort.h>
#include <linux/slab.h>

/* Precision of fixed point for the m values from the filter */
#define M_PRECISION BIT(23)

/* Only activate the filter once we have at least this many elements. */
#define TS_HISTORY_THRESHOLD 8

/*
 * If we don't have any history entries for this long, empty the filter to
 * make sure there are no big discontinuities.
 */
#define TS_HISTORY_BORED_US 500000

/* To measure by how much the filter is overshooting, if it happens. */
#define FUTURE_TS_ANALYTICS_COUNT_MAX 100

static inline int
cros_sensorhub_send_sample(struct cros_ec_sensorhub *sensorhub,
			   struct cros_ec_sensors_ring_sample *sample)
{
	cros_ec_sensorhub_push_data_cb_t cb;
	int id = sample->sensor_id;
	struct iio_dev *indio_dev;

	if (id >= sensorhub->sensor_num)
		return -EINVAL;

	cb = sensorhub->push_data[id].push_data_cb;
	if (!cb)
		return 0;

	indio_dev = sensorhub->push_data[id].indio_dev;

	if (sample->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
		return 0;

	return cb(indio_dev, sample->vector, sample->timestamp);
}

/**
 * cros_ec_sensorhub_register_push_data() - register the callback to the hub.
 *
 * @sensorhub : Sensor Hub object
 * @sensor_num : The sensor the caller is interested in.
 * @indio_dev : The iio device to use when a sample arrives.
 * @cb : The callback to call when a sample arrives.
 *
 * The callback cb will be used by cros_ec_sensorhub_ring to distribute events
 * from the EC.
 *
 * Return: 0 when callback is registered.
 *         EINVAL is the sensor number is invalid or the slot already used.
 */
int cros_ec_sensorhub_register_push_data(struct cros_ec_sensorhub *sensorhub,
					 u8 sensor_num,
					 struct iio_dev *indio_dev,
					 cros_ec_sensorhub_push_data_cb_t cb)
{
	if (sensor_num >= sensorhub->sensor_num)
		return -EINVAL;
	if (sensorhub->push_data[sensor_num].indio_dev)
		return -EINVAL;

	sensorhub->push_data[sensor_num].indio_dev = indio_dev;
	sensorhub->push_data[sensor_num].push_data_cb = cb;

	return 0;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_register_push_data);

void cros_ec_sensorhub_unregister_push_data(struct cros_ec_sensorhub *sensorhub,
					    u8 sensor_num)
{
	sensorhub->push_data[sensor_num].indio_dev = NULL;
	sensorhub->push_data[sensor_num].push_data_cb = NULL;
}
EXPORT_SYMBOL_GPL(cros_ec_sensorhub_unregister_push_data);

/**
 * cros_ec_sensorhub_ring_fifo_enable() - Enable or disable interrupt generation
 *					  for FIFO events.
 * @sensorhub: Sensor Hub object
 * @on: true when events are requested.
 *
 * To be called before sleeping or when noone is listening.
 * Return: 0 on success, or an error when we can not communicate with the EC.
 *
 */
int cros_ec_sensorhub_ring_fifo_enable(struct cros_ec_sensorhub *sensorhub,
				       bool on)
{
	int ret, i;

	mutex_lock(&sensorhub->cmd_lock);
	if (sensorhub->tight_timestamps)
		for (i = 0; i < sensorhub->sensor_num; i++)
			sensorhub->batch_state[i].last_len = 0;

	sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INT_ENABLE;
	sensorhub->params->fifo_int_enable.enable = on;

	sensorhub->msg->outsize = sizeof(struct ec_params_motion_sense);
	sensorhub->msg->insize = sizeof(struct ec_response_motion_sense);

	ret = cros_ec_cmd_xfer_status(sensorhub->ec->ec_dev, sensorhub->msg);
	mutex_unlock(&sensorhub->cmd_lock);

	/* We expect to receive a payload of 4 bytes, ignore. */
	if (ret > 0)
		ret = 0;

	return ret;
}

static int cros_ec_sensor_ring_median_cmp(const void *pv1, const void *pv2)
{
	s64 v1 = *(s64 *)pv1;
	s64 v2 = *(s64 *)pv2;

	if (v1 > v2)
		return 1;
	else if (v1 < v2)
		return -1;
	else
		return 0;
}

/*
 * cros_ec_sensor_ring_median: Gets median of an array of numbers
 *
 * For now it's implemented using an inefficient > O(n) sort then return
 * the middle element. A more optimal method would be something like
 * quickselect, but given that n = 64 we can probably live with it in the
 * name of clarity.
 *
 * Warning: the input array gets modified (sorted)!
 */
static s64 cros_ec_sensor_ring_median(s64 *array, size_t length)
{
	sort(array, length, sizeof(s64), cros_ec_sensor_ring_median_cmp, NULL);
	return array[length / 2];
}

/*
 * IRQ Timestamp Filtering
 *
 * Lower down in cros_ec_sensor_ring_process_event(), for each sensor event
 * we have to calculate it's timestamp in the AP timebase. There are 3 time
 * points:
 *   a - EC timebase, sensor event
 *   b - EC timebase, IRQ
 *   c - AP timebase, IRQ
 *   a' - what we want: sensor even in AP timebase
 *
 * While a and b are recorded at accurate times (due to the EC real time
 * nature); c is pretty untrustworthy, even though it's recorded the
 * first thing in ec_irq_handler(). There is a very good change we'll get
 * added lantency due to:
 *   other irqs
 *   ddrfreq
 *   cpuidle
 *
 * Normally a' = c - b + a, but if we do that naive math any jitter in c
 * will get coupled in a', which we don't want. We want a function
 * a' = cros_ec_sensor_ring_ts_filter(a) which will filter out outliers in c.
 *
 * Think of a graph of AP time(b) on the y axis vs EC time(c) on the x axis.
 * The slope of the line won't be exactly 1, there will be some clock drift
 * between the 2 chips for various reasons (mechanical stress, temperature,
 * voltage). We need to extrapolate values for a future x, without trusting
 * recent y values too much.
 *
 * We use a median filter for the slope, then another median filter for the
 * y-intercept to calculate this function:
 *   dx[n] = x[n-1] - x[n]
 *   dy[n] = x[n-1] - x[n]
 *   m[n] = dy[n] / dx[n]
 *   median_m = median(m[n-k:n])
 *   error[i] = y[n-i] - median_m * x[n-i]
 *   median_error = median(error[:k])
 *   predicted_y = median_m * x + median_error
 *
 * Implementation differences from above:
 * - Redefined y to be actually c - b, this gives us a lot more precision
 * to do the math. (c-b)/b variations are more obvious than c/b variations.
 * - Since we don't have floating point, any operations involving slope are
 * done using fixed point math (*M_PRECISION)
 * - Since x and y grow with time, we keep zeroing the graph (relative to
 * the last sample), this way math involving *x[n-i] will not overflow
 * - EC timestamps are kept in us, it improves the slope calculation precision
 */

/**
 * cros_ec_sensor_ring_ts_filter_update() - Update filter history.
 *
 * @state: Filter information.
 * @b: IRQ timestamp, EC timebase (us)
 * @c: IRQ timestamp, AP timebase (ns)
 *
 * Given a new IRQ timestamp pair (EC and AP timebases), add it to the filter
 * history.
 */
static void
cros_ec_sensor_ring_ts_filter_update(struct cros_ec_sensors_ts_filter_state
				     *state,
				     s64 b, s64 c)
{
	s64 x, y;
	s64 dx, dy;
	s64 m; /* stored as *M_PRECISION */
	s64 *m_history_copy = state->temp_buf;
	s64 *error = state->temp_buf;
	int i;

	/* we trust b the most, that'll be our independent variable */
	x = b;
	/* y is the offset between AP and EC times, in ns */
	y = c - b * 1000;

	dx = (state->x_history[0] + state->x_offset) - x;
	if (dx == 0)
		return; /* we already have this irq in the history */
	dy = (state->y_history[0] + state->y_offset) - y;
	m = div64_s64(dy * M_PRECISION, dx);

	/* Empty filter if we haven't seen any action in a while. */
	if (-dx > TS_HISTORY_BORED_US)
		state->history_len = 0;

	/* Move everything over, also update offset to all absolute coords .*/
	for (i = state->history_len - 1; i >= 1; i--) {
		state->x_history[i] = state->x_history[i - 1] + dx;
		state->y_history[i] = state->y_history[i - 1] + dy;

		state->m_history[i] = state->m_history[i - 1];
		/*
		 * Also use the same loop to copy m_history for future
		 * median extraction.
		 */
		m_history_copy[i] = state->m_history[i - 1];
	}

	/* Store the x and y, but remember offset is actually last sample. */
	state->x_offset = x;
	state->y_offset = y;
	state->x_history[0] = 0;
	state->y_history[0] = 0;

	state->m_history[0] = m;
	m_history_copy[0] = m;

	if (state->history_len < CROS_EC_SENSORHUB_TS_HISTORY_SIZE)
		state->history_len++;

	/* Precalculate things for the filter. */
	if (state->history_len > TS_HISTORY_THRESHOLD) {
		state->median_m =
		    cros_ec_sensor_ring_median(m_history_copy,
					       state->history_len - 1);

		/*
		 * Calculate y-intercepts as if m_median is the slope and
		 * points in the history are on the line. median_error will
		 * still be in the offset coordinate system.
		 */
		for (i = 0; i < state->history_len; i++)
			error[i] = state->y_history[i] -
				div_s64(state->median_m * state->x_history[i],
					M_PRECISION);
		state->median_error =
			cros_ec_sensor_ring_median(error, state->history_len);
	} else {
		state->median_m = 0;
		state->median_error = 0;
	}
}

/**
 * cros_ec_sensor_ring_ts_filter() - Translate EC timebase timestamp to AP
 *                                   timebase
 *
 * @state: filter information.
 * @x: any ec timestamp (us):
 *
 * cros_ec_sensor_ring_ts_filter(a) => a' event timestamp, AP timebase
 * cros_ec_sensor_ring_ts_filter(b) => calculated timestamp when the EC IRQ
 *                           should have happened on the AP, with low jitter
 *
 * Note: The filter will only activate once state->history_len goes
 * over TS_HISTORY_THRESHOLD. Otherwise it'll just do the naive c - b + a
 * transform.
 *
 * How to derive the formula, starting from:
 *   f(x) = median_m * x + median_error
 * That's the calculated AP - EC offset (at the x point in time)
 * Undo the coordinate system transform:
 *   f(x) = median_m * (x - x_offset) + median_error + y_offset
 * Remember to undo the "y = c - b * 1000" modification:
 *   f(x) = median_m * (x - x_offset) + median_error + y_offset + x * 1000
 *
 * Return: timestamp in AP timebase (ns)
 */
static s64
cros_ec_sensor_ring_ts_filter(struct cros_ec_sensors_ts_filter_state *state,
			      s64 x)
{
	return div_s64(state->median_m * (x - state->x_offset), M_PRECISION)
	       + state->median_error + state->y_offset + x * 1000;
}

/*
 * Since a and b were originally 32 bit values from the EC,
 * they overflow relatively often, casting is not enough, so we need to
 * add an offset.
 */
static void
cros_ec_sensor_ring_fix_overflow(s64 *ts,
				 const s64 overflow_period,
				 struct cros_ec_sensors_ec_overflow_state
				 *state)
{
	s64 adjust;

	*ts += state->offset;
	if (abs(state->last - *ts) > (overflow_period / 2)) {
		adjust = state->last > *ts ? overflow_period : -overflow_period;
		state->offset += adjust;
		*ts += adjust;
	}
	state->last = *ts;
}

static void
cros_ec_sensor_ring_check_for_past_timestamp(struct cros_ec_sensorhub
					     *sensorhub,
					     struct cros_ec_sensors_ring_sample
					     *sample)
{
	const u8 sensor_id = sample->sensor_id;

	/* If this event is earlier than one we saw before... */
	if (sensorhub->batch_state[sensor_id].newest_sensor_event >
	    sample->timestamp)
		/* mark it for spreading. */
		sample->timestamp =
			sensorhub->batch_state[sensor_id].last_ts;
	else
		sensorhub->batch_state[sensor_id].newest_sensor_event =
			sample->timestamp;
}

/**
 * cros_ec_sensor_ring_process_event() - Process one EC FIFO event
 *
 * @sensorhub: Sensor Hub object.
 * @fifo_info: FIFO information from the EC (includes b point, EC timebase).
 * @fifo_timestamp: EC IRQ, kernel timebase (aka c).
 * @current_timestamp: calculated event timestamp, kernel timebase (aka a').
 * @in: incoming FIFO event from EC (includes a point, EC timebase).
 * @out: outgoing event to user space (includes a').
 *
 * Process one EC event, add it in the ring if necessary.
 *
 * Return: true if out event has been populated.
 */
static bool
cros_ec_sensor_ring_process_event(struct cros_ec_sensorhub *sensorhub,
				const struct ec_response_motion_sense_fifo_info
				*fifo_info,
				const ktime_t fifo_timestamp,
				ktime_t *current_timestamp,
				struct ec_response_motion_sensor_data *in,
				struct cros_ec_sensors_ring_sample *out)
{
	const s64 now = cros_ec_get_time_ns();
	int axis, async_flags;

	/* Do not populate the filter based on asynchronous events. */
	async_flags = in->flags &
		(MOTIONSENSE_SENSOR_FLAG_ODR | MOTIONSENSE_SENSOR_FLAG_FLUSH);

	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP && !async_flags) {
		s64 a = in->timestamp;
		s64 b = fifo_info->timestamp;
		s64 c = fifo_timestamp;

		cros_ec_sensor_ring_fix_overflow(&a, 1LL << 32,
					  &sensorhub->overflow_a);
		cros_ec_sensor_ring_fix_overflow(&b, 1LL << 32,
					  &sensorhub->overflow_b);

		if (sensorhub->tight_timestamps) {
			cros_ec_sensor_ring_ts_filter_update(
					&sensorhub->filter, b, c);
			*current_timestamp = cros_ec_sensor_ring_ts_filter(
					&sensorhub->filter, a);
		} else {
			s64 new_timestamp;

			/*
			 * Disable filtering since we might add more jitter
			 * if b is in a random point in time.
			 */
			new_timestamp = fifo_timestamp -
					fifo_info->timestamp  * 1000 +
					in->timestamp * 1000;
			/*
			 * The timestamp can be stale if we had to use the fifo
			 * info timestamp.
			 */
			if (new_timestamp - *current_timestamp > 0)
				*current_timestamp = new_timestamp;
		}
	}

	if (in->flags & MOTIONSENSE_SENSOR_FLAG_ODR) {
		if (sensorhub->tight_timestamps) {
			sensorhub->batch_state[in->sensor_num].last_len = 0;
			sensorhub->batch_state[in->sensor_num].penul_len = 0;
		}
		/*
		 * ODR change is only useful for the sensor_ring, it does not
		 * convey information to clients.
		 */
		return false;
	}

	if (in->flags & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
		out->sensor_id = in->sensor_num;
		out->timestamp = *current_timestamp;
		out->flag = in->flags;
		if (sensorhub->tight_timestamps)
			sensorhub->batch_state[out->sensor_id].last_len = 0;
		/*
		 * No other payload information provided with
		 * flush ack.
		 */
		return true;
	}

	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP)
		/* If we just have a timestamp, skip this entry. */
		return false;

	/* Regular sample */
	out->sensor_id = in->sensor_num;
	if (*current_timestamp - now > 0) {
		/*
		 * This fix is needed to overcome the timestamp filter putting
		 * events in the future.
		 */
		sensorhub->future_timestamp_total_ns +=
			*current_timestamp - now;
		if (++sensorhub->future_timestamp_count ==
				FUTURE_TS_ANALYTICS_COUNT_MAX) {
			s64 avg = div_s64(sensorhub->future_timestamp_total_ns,
					sensorhub->future_timestamp_count);
			dev_warn_ratelimited(sensorhub->dev,
					     "100 timestamps in the future, %lldns shaved on average\n",
					     avg);
			sensorhub->future_timestamp_count = 0;
			sensorhub->future_timestamp_total_ns = 0;
		}
		out->timestamp = now;
	} else {
		out->timestamp = *current_timestamp;
	}

	out->flag = in->flags;
	for (axis = 0; axis < 3; axis++)
		out->vector[axis] = in->data[axis];

	if (sensorhub->tight_timestamps)
		cros_ec_sensor_ring_check_for_past_timestamp(sensorhub, out);
	return true;
}

/*
 * cros_ec_sensor_ring_spread_add: Calculate proper timestamps then add to
 *                                 ringbuffer.
 *
 * This is the new spreading code, assumes every sample's timestamp
 * preceeds the sample. Run if tight_timestamps == true.
 *
 * Sometimes the EC receives only one interrupt (hence timestamp) for
 * a batch of samples. Only the first sample will have the correct
 * timestamp. So we must interpolate the other samples.
 * We use the previous batch timestamp and our current batch timestamp
 * as a way to calculate period, then spread the samples evenly.
 *
 * s0 int, 0ms
 * s1 int, 10ms
 * s2 int, 20ms
 * 30ms point goes by, no interrupt, previous one is still asserted
 * downloading s2 and s3
 * s3 sample, 20ms (incorrect timestamp)
 * s4 int, 40ms
 *
 * The batches are [(s0), (s1), (s2, s3), (s4)]. Since the 3rd batch
 * has 2 samples in them, we adjust the timestamp of s3.
 * s2 - s1 = 10ms, so s3 must be s2 + 10ms => 20ms. If s1 would have
 * been part of a bigger batch things would have gotten a little
 * more complicated.
 *
 * Note: we also assume another sensor sample doesn't break up a batch
 * in 2 or more partitions. Example, there can't ever be a sync sensor
 * in between S2 and S3. This simplifies the following code.
 */
static void
cros_ec_sensor_ring_spread_add(struct cros_ec_sensorhub *sensorhub,
			       unsigned long sensor_mask,
			       struct cros_ec_sensors_ring_sample *last_out)
{
	struct cros_ec_sensors_ring_sample *batch_start, *next_batch_start;
	int id;

	for_each_set_bit(id, &sensor_mask, sensorhub->sensor_num) {
		for (batch_start = sensorhub->ring; batch_start < last_out;
		     batch_start = next_batch_start) {
			/*
			 * For each batch (where all samples have the same
			 * timestamp).
			 */
			int batch_len, sample_idx;
			struct cros_ec_sensors_ring_sample *batch_end =
				batch_start;
			struct cros_ec_sensors_ring_sample *s;
			s64 batch_timestamp = batch_start->timestamp;
			s64 sample_period;

			/*
			 * Skip over batches that start with the sensor types
			 * we're not looking at right now.
			 */
			if (batch_start->sensor_id != id) {
				next_batch_start = batch_start + 1;
				continue;
			}

			/*
			 * Do not start a batch
			 * from a flush, as it happens asynchronously to the
			 * regular flow of events.
			 */
			if (batch_start->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
				cros_sensorhub_send_sample(sensorhub,
							   batch_start);
				next_batch_start = batch_start + 1;
				continue;
			}

			if (batch_start->timestamp <=
			    sensorhub->batch_state[id].last_ts) {
				batch_timestamp =
					sensorhub->batch_state[id].last_ts;
				batch_len = sensorhub->batch_state[id].last_len;

				sample_idx = batch_len;

				sensorhub->batch_state[id].last_ts =
				  sensorhub->batch_state[id].penul_ts;
				sensorhub->batch_state[id].last_len =
				  sensorhub->batch_state[id].penul_len;
			} else {
				/*
				 * Push first sample in the batch to the,
				 * kifo, it's guaranteed to be correct, the
				 * rest will follow later on.
				 */
				sample_idx = 1;
				batch_len = 1;
				cros_sensorhub_send_sample(sensorhub,
							   batch_start);
				batch_start++;
			}

			/* Find all samples have the same timestamp. */
			for (s = batch_start; s < last_out; s++) {
				if (s->sensor_id != id)
					/*
					 * Skip over other sensor types that
					 * are interleaved, don't count them.
					 */
					continue;
				if (s->timestamp != batch_timestamp)
					/* we discovered the next batch */
					break;
				if (s->flag & MOTIONSENSE_SENSOR_FLAG_FLUSH)
					/* break on flush packets */
					break;
				batch_end = s;
				batch_len++;
			}

			if (batch_len == 1)
				goto done_with_this_batch;

			/* Can we calculate period? */
			if (sensorhub->batch_state[id].last_len == 0) {
				dev_warn(sensorhub->dev, "Sensor %d: lost %d samples when spreading\n",
					 id, batch_len - 1);
				goto done_with_this_batch;
				/*
				 * Note: we're dropping the rest of the samples
				 * in this batch since we have no idea where
				 * they're supposed to go without a period
				 * calculation.
				 */
			}

			sample_period = div_s64(batch_timestamp -
				sensorhub->batch_state[id].last_ts,
				sensorhub->batch_state[id].last_len);
			dev_dbg(sensorhub->dev,
				"Adjusting %d samples, sensor %d last_batch @%lld (%d samples) batch_timestamp=%lld => period=%lld\n",
				batch_len, id,
				sensorhub->batch_state[id].last_ts,
				sensorhub->batch_state[id].last_len,
				batch_timestamp,
				sample_period);

			/*
			 * Adjust timestamps of the samples then push them to
			 * kfifo.
			 */
			for (s = batch_start; s <= batch_end; s++) {
				if (s->sensor_id != id)
					/*
					 * Skip over other sensor types that
					 * are interleaved, don't change them.
					 */
					continue;

				s->timestamp = batch_timestamp +
					sample_period * sample_idx;
				sample_idx++;

				cros_sensorhub_send_sample(sensorhub, s);
			}

done_with_this_batch:
			sensorhub->batch_state[id].penul_ts =
				sensorhub->batch_state[id].last_ts;
			sensorhub->batch_state[id].penul_len =
				sensorhub->batch_state[id].last_len;

			sensorhub->batch_state[id].last_ts =
				batch_timestamp;
			sensorhub->batch_state[id].last_len = batch_len;

			next_batch_start = batch_end + 1;
		}
	}
}

/*
 * cros_ec_sensor_ring_spread_add_legacy: Calculate proper timestamps then
 * add to ringbuffer (legacy).
 *
 * Note: This assumes we're running old firmware, where every sample's timestamp
 * is after the sample. Run if tight_timestamps == false.
 *
 * If there is a sample with a proper timestamp
 *
 *                        timestamp | count
 *                        -----------------
 * older_unprocess_out --> TS1      | 1
 *                         TS1      | 2
 *                out -->  TS1      | 3
 *           next_out -->  TS2      |
 *
 * We spread time for the samples [older_unprocess_out .. out]
 * between TS1 and TS2: [TS1+1/4, TS1+2/4, TS1+3/4, TS2].
 *
 * If we reach the end of the samples, we compare with the
 * current timestamp:
 *
 * older_unprocess_out --> TS1      | 1
 *                         TS1      | 2
 *                 out --> TS1      | 3
 *
 * We know have [TS1+1/3, TS1+2/3, current timestamp]
 */
static void
cros_ec_sensor_ring_spread_add_legacy(struct cros_ec_sensorhub *sensorhub,
				      unsigned long sensor_mask,
				      s64 current_timestamp,
				      struct cros_ec_sensors_ring_sample
				      *last_out)
{
	struct cros_ec_sensors_ring_sample *out;
	int i;

	for_each_set_bit(i, &sensor_mask, sensorhub->sensor_num) {
		s64 older_timestamp;
		s64 timestamp;
		struct cros_ec_sensors_ring_sample *older_unprocess_out =
			sensorhub->ring;
		struct cros_ec_sensors_ring_sample *next_out;
		int count = 1;

		for (out = sensorhub->ring; out < last_out; out = next_out) {
			s64 time_period;

			next_out = out + 1;
			if (out->sensor_id != i)
				continue;

			/* Timestamp to start with */
			older_timestamp = out->timestamp;

			/* Find next sample. */
			while (next_out < last_out && next_out->sensor_id != i)
				next_out++;

			if (next_out >= last_out) {
				timestamp = current_timestamp;
			} else {
				timestamp = next_out->timestamp;
				if (timestamp == older_timestamp) {
					count++;
					continue;
				}
			}

			/*
			 * The next sample has a new timestamp, spread the
			 * unprocessed samples.
			 */
			if (next_out < last_out)
				count++;
			time_period = div_s64(timestamp - older_timestamp,
					      count);

			for (; older_unprocess_out <= out;
					older_unprocess_out++) {
				if (older_unprocess_out->sensor_id != i)
					continue;
				older_timestamp += time_period;
				older_unprocess_out->timestamp =
					older_timestamp;
			}
			count = 1;
			/* The next_out sample has a valid timestamp, skip. */
			next_out++;
			older_unprocess_out = next_out;
		}
	}

	/* Push the event into the kfifo */
	for (out = sensorhub->ring; out < last_out; out++)
		cros_sensorhub_send_sample(sensorhub, out);
}

/**
 * cros_ec_sensorhub_ring_handler() - The trigger handler function
 *
 * @sensorhub: Sensor Hub object.
 *
 * Called by the notifier, process the EC sensor FIFO queue.
 */
static void cros_ec_sensorhub_ring_handler(struct cros_ec_sensorhub *sensorhub)
{
	struct ec_response_motion_sense_fifo_info *fifo_info =
		sensorhub->fifo_info;
	struct cros_ec_dev *ec = sensorhub->ec;
	ktime_t fifo_timestamp, current_timestamp;
	int i, j, number_data, ret;
	unsigned long sensor_mask = 0;
	struct ec_response_motion_sensor_data *in;
	struct cros_ec_sensors_ring_sample *out, *last_out;

	mutex_lock(&sensorhub->cmd_lock);

	/* Get FIFO information if there are lost vectors. */
	if (fifo_info->total_lost) {
		int fifo_info_length =
			sizeof(struct ec_response_motion_sense_fifo_info) +
			sizeof(u16) * sensorhub->sensor_num;

		/* Need to retrieve the number of lost vectors per sensor */
		sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
		sensorhub->msg->outsize = 1;
		sensorhub->msg->insize = fifo_info_length;

		if (cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg) < 0)
			goto error;

		memcpy(fifo_info, &sensorhub->resp->fifo_info,
		       fifo_info_length);

		/*
		 * Update collection time, will not be as precise as the
		 * non-error case.
		 */
		fifo_timestamp = cros_ec_get_time_ns();
	} else {
		fifo_timestamp = sensorhub->fifo_timestamp[
			CROS_EC_SENSOR_NEW_TS];
	}

	if (fifo_info->count > sensorhub->fifo_size ||
	    fifo_info->size != sensorhub->fifo_size) {
		dev_warn(sensorhub->dev,
			 "Mismatch EC data: count %d, size %d - expected %d\n",
			 fifo_info->count, fifo_info->size,
			 sensorhub->fifo_size);
		goto error;
	}

	/* Copy elements in the main fifo */
	current_timestamp = sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS];
	out = sensorhub->ring;
	for (i = 0; i < fifo_info->count; i += number_data) {
		sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_READ;
		sensorhub->params->fifo_read.max_data_vector =
			fifo_info->count - i;
		sensorhub->msg->outsize =
			sizeof(struct ec_params_motion_sense);
		sensorhub->msg->insize =
			sizeof(sensorhub->resp->fifo_read) +
			sensorhub->params->fifo_read.max_data_vector *
			  sizeof(struct ec_response_motion_sensor_data);
		ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
		if (ret < 0) {
			dev_warn(sensorhub->dev, "Fifo error: %d\n", ret);
			break;
		}
		number_data = sensorhub->resp->fifo_read.number_data;
		if (number_data == 0) {
			dev_dbg(sensorhub->dev, "Unexpected empty FIFO\n");
			break;
		}
		if (number_data > fifo_info->count - i) {
			dev_warn(sensorhub->dev,
				 "Invalid EC data: too many entry received: %d, expected %d\n",
				 number_data, fifo_info->count - i);
			break;
		}
		if (out + number_data >
		    sensorhub->ring + fifo_info->count) {
			dev_warn(sensorhub->dev,
				 "Too many samples: %d (%zd data) to %d entries for expected %d entries\n",
				 i, out - sensorhub->ring, i + number_data,
				 fifo_info->count);
			break;
		}

		for (in = sensorhub->resp->fifo_read.data, j = 0;
		     j < number_data; j++, in++) {
			if (cros_ec_sensor_ring_process_event(
						sensorhub, fifo_info,
						fifo_timestamp,
						&current_timestamp,
						in, out)) {
				sensor_mask |= BIT(in->sensor_num);
				out++;
			}
		}
	}
	mutex_unlock(&sensorhub->cmd_lock);
	last_out = out;

	if (out == sensorhub->ring)
		/* Unexpected empty FIFO. */
		goto ring_handler_end;

	/*
	 * Check if current_timestamp is ahead of the last sample. Normally,
	 * the EC appends a timestamp after the last sample, but if the AP
	 * is slow to respond to the IRQ, the EC may have added new samples.
	 * Use the FIFO info timestamp as last timestamp then.
	 */
	if (!sensorhub->tight_timestamps &&
	    (last_out - 1)->timestamp == current_timestamp)
		current_timestamp = fifo_timestamp;

	/* Warn on lost samples. */
	if (fifo_info->total_lost)
		for (i = 0; i < sensorhub->sensor_num; i++) {
			if (fifo_info->lost[i]) {
				dev_warn_ratelimited(sensorhub->dev,
						     "Sensor %d: lost: %d out of %d\n",
						     i, fifo_info->lost[i],
						     fifo_info->total_lost);
				if (sensorhub->tight_timestamps)
					sensorhub->batch_state[i].last_len = 0;
			}
		}

	/*
	 * Spread samples in case of batching, then add them to the
	 * ringbuffer.
	 */
	if (sensorhub->tight_timestamps)
		cros_ec_sensor_ring_spread_add(sensorhub, sensor_mask,
					       last_out);
	else
		cros_ec_sensor_ring_spread_add_legacy(sensorhub, sensor_mask,
						      current_timestamp,
						      last_out);

ring_handler_end:
	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] = current_timestamp;
	return;

error:
	mutex_unlock(&sensorhub->cmd_lock);
}

static int cros_ec_sensorhub_event(struct notifier_block *nb,
				   unsigned long queued_during_suspend,
				   void *_notify)
{
	struct cros_ec_sensorhub *sensorhub;
	struct cros_ec_device *ec_dev;

	sensorhub = container_of(nb, struct cros_ec_sensorhub, notifier);
	ec_dev = sensorhub->ec->ec_dev;

	if (ec_dev->event_data.event_type != EC_MKBP_EVENT_SENSOR_FIFO)
		return NOTIFY_DONE;

	if (ec_dev->event_size != sizeof(ec_dev->event_data.data.sensor_fifo)) {
		dev_warn(ec_dev->dev, "Invalid fifo info size\n");
		return NOTIFY_DONE;
	}

	if (queued_during_suspend)
		return NOTIFY_OK;

	memcpy(sensorhub->fifo_info, &ec_dev->event_data.data.sensor_fifo.info,
	       sizeof(*sensorhub->fifo_info));
	sensorhub->fifo_timestamp[CROS_EC_SENSOR_NEW_TS] =
		ec_dev->last_event_time;
	cros_ec_sensorhub_ring_handler(sensorhub);

	return NOTIFY_OK;
}

/**
 * cros_ec_sensorhub_ring_add() - Add the FIFO functionality if the EC
 *				  supports it.
 *
 * @sensorhub : Sensor Hub object.
 *
 * Return: 0 on success.
 */
int cros_ec_sensorhub_ring_add(struct cros_ec_sensorhub *sensorhub)
{
	struct cros_ec_dev *ec = sensorhub->ec;
	int ret;
	int fifo_info_length =
		sizeof(struct ec_response_motion_sense_fifo_info) +
		sizeof(u16) * sensorhub->sensor_num;

	/* Allocate the array for lost events. */
	sensorhub->fifo_info = devm_kzalloc(sensorhub->dev, fifo_info_length,
					    GFP_KERNEL);
	if (!sensorhub->fifo_info)
		return -ENOMEM;

	/* Retrieve FIFO information */
	sensorhub->msg->version = 2;
	sensorhub->params->cmd = MOTIONSENSE_CMD_FIFO_INFO;
	sensorhub->msg->outsize = 1;
	sensorhub->msg->insize = fifo_info_length;

	ret = cros_ec_cmd_xfer_status(ec->ec_dev, sensorhub->msg);
	if (ret < 0)
		return ret;

	/*
	 * Allocate the full fifo. We need to copy the whole FIFO to set
	 * timestamps properly.
	 */
	sensorhub->fifo_size = sensorhub->resp->fifo_info.size;
	sensorhub->ring = devm_kcalloc(sensorhub->dev, sensorhub->fifo_size,
				       sizeof(*sensorhub->ring), GFP_KERNEL);
	if (!sensorhub->ring)
		return -ENOMEM;

	/*
	 * Allocate the callback area based on the number of sensors.
	 */
	sensorhub->push_data = devm_kcalloc(
			sensorhub->dev, sensorhub->sensor_num,
			sizeof(*sensorhub->push_data),
			GFP_KERNEL);
	if (!sensorhub->push_data)
		return -ENOMEM;

	sensorhub->fifo_timestamp[CROS_EC_SENSOR_LAST_TS] =
		cros_ec_get_time_ns();

	sensorhub->tight_timestamps = cros_ec_check_features(
			ec, EC_FEATURE_MOTION_SENSE_TIGHT_TIMESTAMPS);

	if (sensorhub->tight_timestamps) {
		sensorhub->batch_state = devm_kcalloc(sensorhub->dev,
				sensorhub->sensor_num,
				sizeof(*sensorhub->batch_state),
				GFP_KERNEL);
		if (!sensorhub->batch_state)
			return -ENOMEM;
	}

	/* Register the notifier that will act as a top half interrupt. */
	sensorhub->notifier.notifier_call = cros_ec_sensorhub_event;
	ret = blocking_notifier_chain_register(&ec->ec_dev->event_notifier,
					       &sensorhub->notifier);
	if (ret < 0)
		return ret;

	/* Start collection samples. */
	return cros_ec_sensorhub_ring_fifo_enable(sensorhub, true);
}

void cros_ec_sensorhub_ring_remove(void *arg)
{
	struct cros_ec_sensorhub *sensorhub = arg;
	struct cros_ec_device *ec_dev = sensorhub->ec->ec_dev;

	/* Disable the ring, prevent EC interrupt to the AP for nothing. */
	cros_ec_sensorhub_ring_fifo_enable(sensorhub, false);
	blocking_notifier_chain_unregister(&ec_dev->event_notifier,
					   &sensorhub->notifier);
}