1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <math.h>
18 #include <floatRt.h>
19 #include <algos/time_sync.h>
20
time_sync_reset(time_sync_t * sync)21 void time_sync_reset(time_sync_t *sync) {
22 sync->n = 0;
23 sync->i = 0;
24 sync->estimate_valid = false;
25
26 sync->hold_count = 0;
27 }
28
time_sync_init(time_sync_t * sync)29 bool time_sync_init(time_sync_t *sync) {
30 time_sync_reset(sync);
31
32 return true;
33 }
34
time_sync_truncate(time_sync_t * sync,size_t window_size)35 void time_sync_truncate(time_sync_t *sync, size_t window_size) {
36 size_t k, m;
37 sync->n = (window_size < sync->n) ? window_size : sync->n;
38 sync->estimate_valid = false;
39
40 // oldest sample index (new time_base) after truncation
41 size_t bidx = (sync->i >= sync->n) ? (sync->i - sync->n)
42 : (sync->i + NUM_TIME_SYNC_DATAPOINTS - sync->n);
43
44 // left circular-shift oldest sample to index 0
45 for (k = 0; k < bidx; ++k) {
46 uint64_t tmp1 = sync->time1[0];
47 uint64_t tmp2 = sync->time2[0];
48
49 for (m = 0; m < NUM_TIME_SYNC_DATAPOINTS - 1; ++m) {
50 sync->time1[m] = sync->time1[m + 1];
51 sync->time2[m] = sync->time2[m + 1];
52 }
53 sync->time1[NUM_TIME_SYNC_DATAPOINTS - 1] = tmp1;
54 sync->time2[NUM_TIME_SYNC_DATAPOINTS - 1] = tmp2;
55 }
56
57 sync->i = (sync->n < NUM_TIME_SYNC_DATAPOINTS) ? sync->n : 0;
58 }
59
time_sync_add(time_sync_t * sync,uint64_t time1,uint64_t time2)60 bool time_sync_add(time_sync_t *sync, uint64_t time1, uint64_t time2) {
61 size_t i = sync->i;
62
63 sync->time1[i] = time1;
64 sync->time2[i] = time2;
65
66 if (++i == NUM_TIME_SYNC_DATAPOINTS) {
67 i = 0;
68 }
69
70 sync->i = i;
71
72 size_t prev_n = sync->n;
73 if (sync->n < NUM_TIME_SYNC_DATAPOINTS) {
74 ++sync->n;
75 }
76
77 sync->estimate_valid = false;
78
79 if (sync->hold_count > 0) {
80 --sync->hold_count;
81 time_sync_truncate(sync, prev_n);
82 }
83
84 return true;
85 }
86
time_sync_estimate_time1(time_sync_t * sync,uint64_t time2,uint64_t * time1)87 bool time_sync_estimate_time1(time_sync_t *sync, uint64_t time2, uint64_t *time1)
88 {
89 size_t j;
90
91 if (sync->n < 2)
92 return false;
93
94 *time1 = 0;
95
96 if (!sync->estimate_valid) {
97 size_t n = sync->n;
98
99 // Rewind to the oldest sample in the history.
100 size_t i = sync->i;
101 if (n < NUM_TIME_SYNC_DATAPOINTS) {
102 if (i != n) {
103 return false;
104 }
105 i = 0;
106 }
107
108 uint64_t time1_base = sync->time1[i];
109 uint64_t time2_base = sync->time2[i];
110
111 // Least-square linear regresison:
112 // Compute alpha and beta so that time1 = alpha + beta * time2.
113 // x = time2, y = time1
114 float mean_x = 0.0f;
115 float mean_y = 0.0f;
116 float invN = 1.0f / n;
117 size_t ii = i;
118 for (j = 0; j < n; ++j) {
119 mean_y += floatFromUint64(sync->time1[ii] - time1_base) * invN;
120 mean_x += floatFromUint64(sync->time2[ii] - time2_base) * invN;
121
122 if (++ii == NUM_TIME_SYNC_DATAPOINTS) {
123 ii = 0;
124 }
125 }
126
127 // Two-pass approach so that only values relative to mean are computed.
128 // Typically, |y| and |x| are smaller than 8e8 in nsec.
129 // So sum_x2 and sum_xy are smaller than 1e19.
130 // That leaves plenty of room for blocking tasks.
131 float sum_x2 = 0.0f, sum_xy = 0.0f;
132 ii = i;
133 for (j = 0; j < n; ++j) {
134 float y = floatFromUint64(sync->time1[ii] - time1_base) - mean_y;
135 float x = floatFromUint64(sync->time2[ii] - time2_base) - mean_x;
136
137 sum_x2 += x * x;
138 sum_xy += x * y;
139
140 if (++ii == NUM_TIME_SYNC_DATAPOINTS) {
141 ii = 0;
142 }
143 }
144
145 float beta = sum_xy / sum_x2;
146 float alpha = mean_y - beta * mean_x;
147
148 sync->alpha = alpha;
149 sync->beta = beta;
150 sync->time1_base = time1_base;
151 sync->time2_base = time2_base;
152
153 sync->estimate_valid = true;
154 }
155
156 *time1 = sync->time1_base + floatToInt64(sync->alpha + sync->beta * floatFromInt64(time2 - sync->time2_base));
157
158 return true;
159 }
160
time_sync_hold(time_sync_t * sync,uint8_t count)161 void time_sync_hold(time_sync_t *sync, uint8_t count) {
162 sync->hold_count = count;
163 }
164