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 <log/log.h>
18 #include <utils/SystemClock.h>
19 #include <math.h>
20 #include <qemud.h>
21 #include "multihal_sensors.h"
22 #include "sensor_list.h"
23 
24 namespace goldfish {
25 using ahs10::EventPayload;
26 using ahs10::SensorType;
27 using ahs10::SensorStatus;
28 
29 namespace {
testPrefix(const char * i,const char * end,const char * v,const char sep)30 const char* testPrefix(const char* i, const char* end, const char* v, const char sep) {
31     while (i < end) {
32         if (*v == 0) {
33             return (*i == sep) ? (i + 1) : nullptr;
34         } else if (*v == *i) {
35             ++v;
36             ++i;
37         } else {
38             return nullptr;
39         }
40     }
41 
42     return nullptr;
43 }
44 
approximatelyEqual(double a,double b,double eps)45 bool approximatelyEqual(double a, double b, double eps) {
46     return fabs(a - b) <= std::max(fabs(a), fabs(b)) * eps;
47 }
48 
weigthedAverage(const int64_t a,int64_t aw,int64_t b,int64_t bw)49 int64_t weigthedAverage(const int64_t a, int64_t aw, int64_t b, int64_t bw) {
50     return (a * aw + b * bw) / (aw + bw);
51 }
52 
53 }  // namespace
54 
activateQemuSensorImpl(const int pipe,const int sensorHandle,const bool enabled)55 bool MultihalSensors::activateQemuSensorImpl(const int pipe,
56                                              const int sensorHandle,
57                                              const bool enabled) {
58     char buffer[64];
59     int len = snprintf(buffer, sizeof(buffer),
60                        "set:%s:%d",
61                        getQemuSensorNameByHandle(sensorHandle),
62                        (enabled ? 1 : 0));
63 
64     if (qemud_channel_send(pipe, buffer, len) < 0) {
65         ALOGE("%s:%d: qemud_channel_send failed", __func__, __LINE__);
66         return false;
67     } else {
68         return true;
69     }
70 }
71 
setAllQemuSensors(const bool enabled)72 bool MultihalSensors::setAllQemuSensors(const bool enabled) {
73     uint32_t mask = m_availableSensorsMask;
74     for (int i = 0; mask; ++i, mask >>= 1) {
75         if (mask & 1) {
76             if (!activateQemuSensorImpl(m_qemuSensorsFd.get(), i, enabled)) {
77                 return false;
78             }
79         }
80     }
81 
82     return true;
83 }
84 
parseQemuSensorEvent(const int pipe,QemuSensorsProtocolState * state)85 void MultihalSensors::parseQemuSensorEvent(const int pipe,
86                                            QemuSensorsProtocolState* state) {
87     char buf[256];
88     const int len = qemud_channel_recv(pipe, buf, sizeof(buf) - 1);
89     if (len < 0) {
90         ALOGE("%s:%d: qemud_channel_recv failed", __func__, __LINE__);
91     }
92     const int64_t nowNs = ::android::elapsedRealtimeNano();
93     buf[len] = 0;
94     const char* end = buf + len;
95 
96     bool parsed = false;
97     Event event;
98     EventPayload* payload = &event.u;
99     ahs10::Vec3* vec3 = &payload->vec3;
100     ahs10::Uncal* uncal = &payload->uncal;
101 
102     if (const char* values = testPrefix(buf, end, "acceleration", ':')) {
103         if (sscanf(values, "%f:%f:%f",
104                    &vec3->x, &vec3->y, &vec3->z) == 3) {
105             vec3->status = SensorStatus::ACCURACY_MEDIUM;
106             event.timestamp = nowNs + state->timeBiasNs;
107             event.sensorHandle = kSensorHandleAccelerometer;
108             event.sensorType = SensorType::ACCELEROMETER;
109             postSensorEvent(event);
110             parsed = true;
111         }
112     } else if (const char* values = testPrefix(buf, end, "gyroscope", ':')) {
113         if (sscanf(values, "%f:%f:%f",
114                    &vec3->x, &vec3->y, &vec3->z) == 3) {
115             vec3->status = SensorStatus::ACCURACY_MEDIUM;
116             event.timestamp = nowNs + state->timeBiasNs;
117             event.sensorHandle = kSensorHandleGyroscope;
118             event.sensorType = SensorType::GYROSCOPE;
119             postSensorEvent(event);
120             parsed = true;
121         }
122     } else if (const char* values = testPrefix(buf, end, "gyroscope-uncalibrated", ':')) {
123         if (sscanf(values, "%f:%f:%f",
124                    &uncal->x, &uncal->y, &uncal->z) == 3) {
125             uncal->x_bias = 0.0;
126             uncal->y_bias = 0.0;
127             uncal->z_bias = 0.0;
128             event.timestamp = nowNs + state->timeBiasNs;
129             event.sensorHandle = kSensorHandleGyroscopeFieldUncalibrated;
130             event.sensorType = SensorType::GYROSCOPE_UNCALIBRATED;
131             postSensorEvent(event);
132             parsed = true;
133         }
134     } else if (const char* values = testPrefix(buf, end, "orientation", ':')) {
135         if (sscanf(values, "%f:%f:%f",
136                    &vec3->x, &vec3->y, &vec3->z) == 3) {
137             vec3->status = SensorStatus::ACCURACY_HIGH;
138             event.timestamp = nowNs + state->timeBiasNs;
139             event.sensorHandle = kSensorHandleOrientation;
140             event.sensorType = SensorType::ORIENTATION;
141             postSensorEvent(event);
142             parsed = true;
143         }
144     } else if (const char* values = testPrefix(buf, end, "magnetic", ':')) {
145         if (sscanf(values, "%f:%f:%f",
146                    &vec3->x, &vec3->y, &vec3->z) == 3) {
147             vec3->status = SensorStatus::ACCURACY_HIGH;
148             event.timestamp = nowNs + state->timeBiasNs;
149             event.sensorHandle = kSensorHandleMagneticField;
150             event.sensorType = SensorType::MAGNETIC_FIELD;
151             postSensorEvent(event);
152             parsed = true;
153         }
154     } else if (const char* values = testPrefix(buf, end, "magnetic-uncalibrated", ':')) {
155         if (sscanf(values, "%f:%f:%f",
156                    &uncal->x, &uncal->y, &uncal->z) == 3) {
157             uncal->x_bias = 0.0;
158             uncal->y_bias = 0.0;
159             uncal->z_bias = 0.0;
160             event.timestamp = nowNs + state->timeBiasNs;
161             event.sensorHandle = kSensorHandleMagneticFieldUncalibrated;
162             event.sensorType = SensorType::MAGNETIC_FIELD_UNCALIBRATED;
163             postSensorEvent(event);
164             parsed = true;
165         }
166     } else if (const char* values = testPrefix(buf, end, "temperature", ':')) {
167         if (sscanf(values, "%f", &payload->scalar) == 1) {
168             if (!approximatelyEqual(state->lastAmbientTemperatureValue,
169                                     payload->scalar, 0.001)) {
170                 event.timestamp = nowNs + state->timeBiasNs;
171                 event.sensorHandle = kSensorHandleAmbientTemperature;
172                 event.sensorType = SensorType::AMBIENT_TEMPERATURE;
173                 postSensorEvent(event);
174                 state->lastAmbientTemperatureValue = payload->scalar;
175             }
176             parsed = true;
177         }
178     } else if (const char* values = testPrefix(buf, end, "proximity", ':')) {
179         if (sscanf(values, "%f", &payload->scalar) == 1) {
180             if (!approximatelyEqual(state->lastProximityValue,
181                                     payload->scalar, 0.001)) {
182                 event.timestamp = nowNs + state->timeBiasNs;
183                 event.sensorHandle = kSensorHandleProximity;
184                 event.sensorType = SensorType::PROXIMITY;
185                 postSensorEvent(event);
186                 state->lastProximityValue = payload->scalar;
187             }
188             parsed = true;
189         }
190     } else if (const char* values = testPrefix(buf, end, "light", ':')) {
191         if (sscanf(values, "%f", &payload->scalar) == 1) {
192             if (!approximatelyEqual(state->lastLightValue,
193                                     payload->scalar, 0.001)) {
194                 event.timestamp = nowNs + state->timeBiasNs;
195                 event.sensorHandle = kSensorHandleLight;
196                 event.sensorType = SensorType::LIGHT;
197                 postSensorEvent(event);
198                 state->lastLightValue = payload->scalar;
199             }
200             parsed = true;
201         }
202     } else if (const char* values = testPrefix(buf, end, "pressure", ':')) {
203         if (sscanf(values, "%f", &payload->scalar) == 1) {
204             event.timestamp = nowNs + state->timeBiasNs;
205             event.sensorHandle = kSensorHandlePressure;
206             event.sensorType = SensorType::PRESSURE;
207             postSensorEvent(event);
208             parsed = true;
209         }
210     } else if (const char* values = testPrefix(buf, end, "humidity", ':')) {
211         if (sscanf(values, "%f", &payload->scalar) == 1) {
212             if (!approximatelyEqual(state->lastRelativeHumidityValue,
213                                     payload->scalar, 0.001)) {
214                 event.timestamp = nowNs + state->timeBiasNs;
215                 event.sensorHandle = kSensorHandleRelativeHumidity;
216                 event.sensorType = SensorType::RELATIVE_HUMIDITY;
217                 postSensorEvent(event);
218                 state->lastRelativeHumidityValue = payload->scalar;
219             }
220             parsed = true;
221         }
222     } else if (const char* values = testPrefix(buf, end, "guest-sync", ':')) {
223         long long value;
224         if ((sscanf(values, "%lld", &value) == 1) && (value >= 0)) {
225             const int64_t guestTimeNs = static_cast<int64_t>(value * 1000ll);
226             const int64_t timeBiasNs = guestTimeNs - nowNs;
227             state->timeBiasNs =
228                 std::min(int64_t(0),
229                          weigthedAverage(state->timeBiasNs, 3, timeBiasNs, 1));
230             parsed = true;
231         }
232     } else if (const char* values = testPrefix(buf, end, "sync", ':')) {
233         parsed = true;
234     }
235 
236     if (!parsed) {
237         ALOGW("%s:%d: don't know how to parse '%s'", __func__, __LINE__, buf);
238     }
239 }
240 
241 }  // namespace
242