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 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 29 bool time_sync_init(time_sync_t *sync) { 30 time_sync_reset(sync); 31 32 return true; 33 } 34 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 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 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 161 void time_sync_hold(time_sync_t *sync, uint8_t count) { 162 sync->hold_count = count; 163 } 164