1 /*
2  * Copyright (C) 2020 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 <cinttypes>
18 #include <log/log.h>
19 #include <utils/SystemClock.h>
20 #include <math.h>
21 #include <random>
22 #include <multihal_sensors.h>
23 #include "sensor_list.h"
24 
25 namespace goldfish {
26 using ahs10::EventPayload;
27 using ahs21::SensorType;
28 using ahs10::SensorStatus;
29 
30 namespace {
testPrefix(const char * i,const char * end,const char * v,const char sep)31 const char* testPrefix(const char* i, const char* end, const char* v, const char sep) {
32     while (i < end) {
33         if (*v == 0) {
34             return (*i == sep) ? (i + 1) : nullptr;
35         } else if (*v == *i) {
36             ++v;
37             ++i;
38         } else {
39             return nullptr;
40         }
41     }
42 
43     return nullptr;
44 }
45 
approximatelyEqual(double a,double b,double eps)46 bool approximatelyEqual(double a, double b, double eps) {
47     return fabs(a - b) <= std::max(fabs(a), fabs(b)) * eps;
48 }
49 
weigthedAverage(const int64_t a,int64_t aw,int64_t b,int64_t bw)50 int64_t weigthedAverage(const int64_t a, int64_t aw, int64_t b, int64_t bw) {
51     return (a * aw + b * bw) / (aw + bw);
52 }
53 
54 }  // namespace
55 
setSensorsReportingImpl(SensorsTransport & st,const int sensorHandle,const bool enabled)56 bool MultihalSensors::setSensorsReportingImpl(SensorsTransport& st,
57                                               const int sensorHandle,
58                                               const bool enabled) {
59     char buffer[64];
60     int len = snprintf(buffer, sizeof(buffer),
61                        "set:%s:%d",
62                        getQemuSensorNameByHandle(sensorHandle),
63                        (enabled ? 1 : 0));
64 
65     if (st.Send(buffer, len) < 0) {
66         ALOGE("%s:%d: send for %s failed", __func__, __LINE__, st.Name());
67         return false;
68     } else {
69         return true;
70     }
71 }
72 
setAllSensorsReporting(SensorsTransport & st,uint32_t availableSensorsMask,const bool enabled)73 bool MultihalSensors::setAllSensorsReporting(SensorsTransport& st,
74                                              uint32_t availableSensorsMask,
75                                              const bool enabled) {
76     for (int i = 0; availableSensorsMask; ++i, availableSensorsMask >>= 1) {
77         if (availableSensorsMask & 1) {
78             if (!setSensorsReportingImpl(st, i, enabled)) {
79                 return false;
80             }
81         }
82     }
83 
84     return true;
85 }
86 
setSensorsGuestTime(SensorsTransport & st,const int64_t value)87 bool MultihalSensors::setSensorsGuestTime(SensorsTransport& st, const int64_t value) {
88     char buffer[64];
89     int len = snprintf(buffer, sizeof(buffer), "time:%" PRId64, value);
90     if (st.Send(buffer, len) < 0) {
91         ALOGE("%s:%d: send for %s failed", __func__, __LINE__, st.Name());
92         return false;
93     } else {
94         return true;
95     }
96 }
97 
setSensorsUpdateIntervalMs(SensorsTransport & st,const uint32_t intervalMs)98 bool MultihalSensors::setSensorsUpdateIntervalMs(SensorsTransport& st,
99                                                  const uint32_t intervalMs) {
100     char buffer[64];
101     const int len = snprintf(buffer, sizeof(buffer), "set-delay:%u", intervalMs);
102     if (st.Send(buffer, len) < 0) {
103         ALOGE("%s:%d: send for %s failed", __func__, __LINE__, st.Name());
104         return false;
105     } else {
106         return true;
107     }
108 }
109 
randomError(float lo,float hi)110 double MultihalSensors::randomError(float lo, float hi) {
111     std::uniform_real_distribution<> distribution(lo, hi);
112     return distribution(gen);
113 }
114 
parseQemuSensorEventLocked(QemuSensorsProtocolState * state)115 void MultihalSensors::parseQemuSensorEventLocked(QemuSensorsProtocolState* state) {
116     char buf[256];
117     const int len = m_sensorsTransport->Receive(buf, sizeof(buf) - 1);
118     if (len < 0) {
119         ALOGE("%s:%d: receive for %s failed", __func__, __LINE__, m_sensorsTransport->Name());
120     }
121     const int64_t nowNs = ::android::elapsedRealtimeNano();
122     buf[len] = 0;
123     const char* end = buf + len;
124     bool parsed = false;
125     Event event;
126     EventPayload* payload = &event.u;
127     ahs10::Vec3* vec3 = &payload->vec3;
128     ahs10::Uncal* uncal = &payload->uncal;
129     if (const char* values = testPrefix(buf, end, "acceleration", ':')) {
130         if (sscanf(values, "%f:%f:%f",
131                    &vec3->x, &vec3->y, &vec3->z) == 3) {
132             vec3->status = SensorStatus::ACCURACY_MEDIUM;
133             event.timestamp = nowNs + state->timeBiasNs;
134             event.sensorHandle = kSensorHandleAccelerometer;
135             event.sensorType = SensorType::ACCELEROMETER;
136             postSensorEventLocked(event);
137             parsed = true;
138         }
139     } else if (const char* values = testPrefix(buf, end, "acceleration-uncalibrated", ':')) {
140         if (sscanf(values, "%f:%f:%f",
141                    &uncal->x, &uncal->y, &uncal->z) == 3) {
142             // A little bias noise to pass CTS
143             uncal->x_bias = randomError(-0.003f, 0.003f);
144             uncal->y_bias = randomError(-0.003f, 0.003f);
145             uncal->z_bias = randomError(-0.003f, 0.003f);
146             event.timestamp = nowNs + state->timeBiasNs;
147             event.sensorHandle = kSensorHandleAccelerometerUncalibrated;
148             event.sensorType = SensorType::ACCELEROMETER_UNCALIBRATED;
149             postSensorEventLocked(event);
150             parsed = true;
151         }
152     } else if (const char* values = testPrefix(buf, end, "gyroscope", ':')) {
153         if (sscanf(values, "%f:%f:%f",
154                    &vec3->x, &vec3->y, &vec3->z) == 3) {
155             vec3->status = SensorStatus::ACCURACY_MEDIUM;
156             event.timestamp = nowNs + state->timeBiasNs;
157             event.sensorHandle = kSensorHandleGyroscope;
158             event.sensorType = SensorType::GYROSCOPE;
159             postSensorEventLocked(event);
160             parsed = true;
161         }
162     } else if (const char* values = testPrefix(buf, end, "gyroscope-uncalibrated", ':')) {
163         if (sscanf(values, "%f:%f:%f",
164                    &uncal->x, &uncal->y, &uncal->z) == 3) {
165             //Uncalibrated gyro values needs to be close to 0,0,0.
166             uncal->x += randomError(0.00005f, 0.001f);
167             uncal->y += randomError(0.00005f, 0.001f);
168             uncal->z += randomError(0.00005f, 0.001f);
169             // Bias noise
170             uncal->x_bias = randomError(-0.0003f, 0.0003f);
171             uncal->y_bias = randomError(-0.0003f, 0.0003f);
172             uncal->z_bias = randomError(-0.0003f, 0.0003f);
173             event.timestamp = nowNs + state->timeBiasNs;
174             event.sensorHandle = kSensorHandleGyroscopeFieldUncalibrated;
175             event.sensorType = SensorType::GYROSCOPE_UNCALIBRATED;
176             postSensorEventLocked(event);
177             parsed = true;
178         }
179     } else if (const char* values = testPrefix(buf, end, "orientation", ':')) {
180         if (sscanf(values, "%f:%f:%f",
181                    &vec3->x, &vec3->y, &vec3->z) == 3) {
182             vec3->status = SensorStatus::ACCURACY_HIGH;
183             event.timestamp = nowNs + state->timeBiasNs;
184             event.sensorHandle = kSensorHandleOrientation;
185             event.sensorType = SensorType::ORIENTATION;
186             postSensorEventLocked(event);
187             parsed = true;
188         }
189     } else if (const char* values = testPrefix(buf, end, "magnetic", ':')) {
190         if (sscanf(values, "%f:%f:%f",
191                    &vec3->x, &vec3->y, &vec3->z) == 3) {
192             vec3->status = SensorStatus::ACCURACY_HIGH;
193             event.timestamp = nowNs + state->timeBiasNs;
194             event.sensorHandle = kSensorHandleMagneticField;
195             event.sensorType = SensorType::MAGNETIC_FIELD;
196             postSensorEventLocked(event);
197             parsed = true;
198         }
199     } else if (const char* values = testPrefix(buf, end, "magnetic-uncalibrated", ':')) {
200         if (sscanf(values, "%f:%f:%f",
201                    &uncal->x, &uncal->y, &uncal->z) == 3) {
202             // A little bias noise to pass CTS
203             uncal->x_bias = randomError( -0.003f, 0.003f);
204             uncal->y_bias = randomError(-0.003f, 0.003f);
205             uncal->z_bias = randomError(-0.003f, 0.003f);
206             event.timestamp = nowNs + state->timeBiasNs;
207             event.sensorHandle = kSensorHandleMagneticFieldUncalibrated;
208             event.sensorType = SensorType::MAGNETIC_FIELD_UNCALIBRATED;
209             postSensorEventLocked(event);
210             parsed = true;
211         }
212     } else if (const char* values = testPrefix(buf, end, "temperature", ':')) {
213         if (sscanf(values, "%f", &payload->scalar) == 1) {
214             if (!approximatelyEqual(state->lastAmbientTemperatureValue,
215                                     payload->scalar, 0.001)) {
216                 event.timestamp = nowNs + state->timeBiasNs;
217                 event.sensorHandle = kSensorHandleAmbientTemperature;
218                 event.sensorType = SensorType::AMBIENT_TEMPERATURE;
219                 postSensorEventLocked(event);
220                 state->lastAmbientTemperatureValue = payload->scalar;
221             }
222             parsed = true;
223         }
224     } else if (const char* values = testPrefix(buf, end, "proximity", ':')) {
225         if (sscanf(values, "%f", &payload->scalar) == 1) {
226             if (!approximatelyEqual(state->lastProximityValue,
227                                     payload->scalar, 0.001)) {
228                 event.timestamp = nowNs + state->timeBiasNs;
229                 event.sensorHandle = kSensorHandleProximity;
230                 event.sensorType = SensorType::PROXIMITY;
231                 postSensorEventLocked(event);
232                 state->lastProximityValue = payload->scalar;
233             }
234             parsed = true;
235         }
236     } else if (const char* values = testPrefix(buf, end, "light", ':')) {
237         if (sscanf(values, "%f", &payload->scalar) == 1) {
238             if (!approximatelyEqual(state->lastLightValue,
239                                     payload->scalar, 0.001)) {
240                 event.timestamp = nowNs + state->timeBiasNs;
241                 event.sensorHandle = kSensorHandleLight;
242                 event.sensorType = SensorType::LIGHT;
243                 postSensorEventLocked(event);
244                 state->lastLightValue = payload->scalar;
245             }
246             parsed = true;
247         }
248     } else if (const char* values = testPrefix(buf, end, "pressure", ':')) {
249         if (sscanf(values, "%f", &payload->scalar) == 1) {
250             event.timestamp = nowNs + state->timeBiasNs;
251             event.sensorHandle = kSensorHandlePressure;
252             event.sensorType = SensorType::PRESSURE;
253             postSensorEventLocked(event);
254             parsed = true;
255         }
256     } else if (const char* values = testPrefix(buf, end, "humidity", ':')) {
257         if (sscanf(values, "%f", &payload->scalar) == 1) {
258             if (!approximatelyEqual(state->lastRelativeHumidityValue,
259                                     payload->scalar, 0.001)) {
260                 event.timestamp = nowNs + state->timeBiasNs;
261                 event.sensorHandle = kSensorHandleRelativeHumidity;
262                 event.sensorType = SensorType::RELATIVE_HUMIDITY;
263                 postSensorEventLocked(event);
264                 state->lastRelativeHumidityValue = payload->scalar;
265             }
266             parsed = true;
267         }
268     } else if (const char* values = testPrefix(buf, end, "hinge-angle0", ':')) {
269         if (sscanf(values, "%f", &payload->scalar) == 1) {
270             if (!approximatelyEqual(state->lastHingeAngle0Value,
271                                     payload->scalar, 0.001) &&
272                 // b/197586273, ignore the state tracking if system sensor
273                 // service has not enabled hinge sensor
274                 isSensorActive(kSensorHandleHingeAngle0)) {
275                 event.timestamp = nowNs + state->timeBiasNs;
276                 event.sensorHandle = kSensorHandleHingeAngle0;
277                 event.sensorType = SensorType::HINGE_ANGLE;
278                 postSensorEventLocked(event);
279                 state->lastHingeAngle0Value = payload->scalar;
280             }
281             parsed = true;
282         }
283     } else if (const char* values = testPrefix(buf, end, "hinge-angle1", ':')) {
284         if (sscanf(values, "%f", &payload->scalar) == 1) {
285             if (!approximatelyEqual(state->lastHingeAngle1Value,
286                                     payload->scalar, 0.001) &&
287                 isSensorActive(kSensorHandleHingeAngle1)) {
288                 event.timestamp = nowNs + state->timeBiasNs;
289                 event.sensorHandle = kSensorHandleHingeAngle1;
290                 event.sensorType = SensorType::HINGE_ANGLE;
291                 postSensorEventLocked(event);
292                 state->lastHingeAngle1Value = payload->scalar;
293             }
294             parsed = true;
295         }
296     } else if (const char* values = testPrefix(buf, end, "hinge-angle2", ':')) {
297         if (sscanf(values, "%f", &payload->scalar) == 1) {
298             if (!approximatelyEqual(state->lastHingeAngle2Value,
299                                     payload->scalar, 0.001) &&
300                 isSensorActive(kSensorHandleHingeAngle2)) {
301                 event.timestamp = nowNs + state->timeBiasNs;
302                 event.sensorHandle = kSensorHandleHingeAngle2;
303                 event.sensorType = SensorType::HINGE_ANGLE;
304                 postSensorEventLocked(event);
305                 state->lastHingeAngle2Value = payload->scalar;
306             }
307             parsed = true;
308         }
309     } else if (const char* values = testPrefix(buf, end, "heart-rate", ':')) {
310         if (sscanf(values, "%f", &payload->heartRate.bpm) == 1) {
311             if (!approximatelyEqual(state->lastHeartRateValue,
312                                     payload->heartRate.bpm, 0.001)) {
313                 payload->heartRate.status = SensorStatus::ACCURACY_HIGH;
314                 event.timestamp = nowNs + state->timeBiasNs;
315                 event.sensorHandle = kSensorHandleHeartRate;
316                 event.sensorType = SensorType::HEART_RATE;
317                 postSensorEventLocked(event);
318                 state->lastHeartRateValue = payload->heartRate.bpm;
319             }
320             parsed = true;
321         }
322     } else if (const char* values = testPrefix(buf, end, "wrist-tilt", ':')) {
323         long measurementId;
324         int args = sscanf(values, "%f:%ld", &payload->scalar, &measurementId);
325         if (args == 2) {
326             if (state->lastWristTiltMeasurement != measurementId) {
327                 event.timestamp = nowNs + state->timeBiasNs;
328                 event.sensorHandle = kSensorHandleWristTilt;
329                 event.sensorType = SensorType::WRIST_TILT_GESTURE;
330                 postSensorEventLocked(event);
331                 state->lastWristTiltMeasurement = measurementId;
332             }
333         }
334         if (args >= 1) {
335             // Skip if the measurement id is not included.
336             parsed = true;
337         }
338 
339      } else if (const char* values = testPrefix(buf, end, "guest-sync", ':')) {
340         long long value;
341         if ((sscanf(values, "%lld", &value) == 1) && (value >= 0)) {
342             const int64_t guestTimeNs = static_cast<int64_t>(value * 1000LL);
343             const int64_t timeBiasNs = guestTimeNs - nowNs;
344             state->timeBiasNs =
345                 std::min(int64_t(0),
346                          weigthedAverage(state->timeBiasNs, 3, timeBiasNs, 1));
347             parsed = true;
348         }
349     } else if (const char* values = testPrefix(buf, end, "sync", ':')) {
350         parsed = true;
351     }
352 
353     if (!parsed) {
354         ALOGW("%s:%d: don't know how to parse '%s'", __func__, __LINE__, buf);
355     }
356 }
357 
358 }  // namespace
359