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 <qemud.h>
20 #include <utils/SystemClock.h>
21 #include "multihal_sensors.h"
22 #include "sensor_list.h"
23
24 namespace goldfish {
25 using ahs21::SensorType;
26 using ahs10::SensorFlagBits;
27 using ahs10::SensorStatus;
28 using ahs10::MetaDataEventType;
29
MultihalSensors()30 MultihalSensors::MultihalSensors()
31 : m_qemuSensorsFd(qemud_channel_open("sensors"))
32 , m_batchInfo(getSensorNumber()) {
33 if (!m_qemuSensorsFd.ok()) {
34 ALOGE("%s:%d: m_qemuSensorsFd is not opened", __func__, __LINE__);
35 ::abort();
36 }
37
38 char buffer[64];
39 int len = snprintf(buffer, sizeof(buffer),
40 "time:%" PRId64, ::android::elapsedRealtimeNano());
41 if (qemud_channel_send(m_qemuSensorsFd.get(), buffer, len) < 0) {
42 ALOGE("%s:%d: qemud_channel_send failed", __func__, __LINE__);
43 ::abort();
44 }
45
46 using namespace std::literals;
47 const std::string_view kListSensorsCmd = "list-sensors"sv;
48
49 if (qemud_channel_send(m_qemuSensorsFd.get(),
50 kListSensorsCmd.data(),
51 kListSensorsCmd.size()) < 0) {
52 ALOGE("%s:%d: qemud_channel_send failed", __func__, __LINE__);
53 ::abort();
54 }
55
56 len = qemud_channel_recv(m_qemuSensorsFd.get(), buffer, sizeof(buffer) - 1);
57 if (len < 0) {
58 ALOGE("%s:%d: qemud_channel_recv failed", __func__, __LINE__);
59 ::abort();
60 }
61 buffer[len] = 0;
62 uint32_t hostSensorsMask = 0;
63 if (sscanf(buffer, "%u", &hostSensorsMask) != 1) {
64 ALOGE("%s:%d: Can't parse qemud response", __func__, __LINE__);
65 ::abort();
66 }
67
68 m_availableSensorsMask = hostSensorsMask
69 & ~(1U << kSensorHandleGyroscopeFieldUncalibrated)
70 & ((1u << getSensorNumber()) - 1);
71
72 ALOGI("%s:%d: host sensors mask=%x, available sensors mask=%x",
73 __func__, __LINE__, hostSensorsMask, m_availableSensorsMask);
74
75 if (!::android::base::Socketpair(AF_LOCAL, SOCK_STREAM, 0,
76 &m_callersFd, &m_sensorThreadFd)) {
77 ALOGE("%s:%d: Socketpair failed", __func__, __LINE__);
78 ::abort();
79 }
80
81 m_sensorThread = std::thread(&MultihalSensors::qemuSensorListenerThread, this);
82 m_batchThread = std::thread(&MultihalSensors::batchThread, this);
83 }
84
~MultihalSensors()85 MultihalSensors::~MultihalSensors() {
86 setAllQemuSensors(false);
87
88 m_batchRunning = false;
89 m_batchUpdated.notify_one();
90 m_batchThread.join();
91
92 qemuSensorThreadSendCommand(kCMD_QUIT);
93 m_sensorThread.join();
94 }
95
getName()96 const std::string MultihalSensors::getName() {
97 return "hal_sensors_2_1_impl_ranchu";
98 }
99
debug(const hidl_handle & fd,const hidl_vec<hidl_string> & args)100 Return<void> MultihalSensors::debug(const hidl_handle& fd, const hidl_vec<hidl_string>& args) {
101 (void)fd;
102 (void)args;
103 return {};
104 }
105
getSensorsList_2_1(getSensorsList_2_1_cb _hidl_cb)106 Return<void> MultihalSensors::getSensorsList_2_1(getSensorsList_2_1_cb _hidl_cb) {
107 std::vector<SensorInfo> sensors;
108
109 uint32_t mask = m_availableSensorsMask;
110 for (int i = 0; mask; ++i, mask >>= 1) {
111 if (mask & 1) {
112 sensors.push_back(*getSensorInfoByHandle(i));
113 }
114 }
115
116 _hidl_cb(sensors);
117 return {};
118 }
119
setOperationMode(const OperationMode mode)120 Return<Result> MultihalSensors::setOperationMode(const OperationMode mode) {
121 std::unique_lock<std::mutex> lock(m_mtx);
122
123 if (m_activeSensorsMask) {
124 return Result::INVALID_OPERATION;
125 } else {
126 m_opMode = mode;
127 return Result::OK;
128 }
129 }
130
activate(const int32_t sensorHandle,const bool enabled)131 Return<Result> MultihalSensors::activate(const int32_t sensorHandle,
132 const bool enabled) {
133 if (!isSensorHandleValid(sensorHandle)) {
134 return Result::BAD_VALUE;
135 }
136
137 std::unique_lock<std::mutex> lock(m_mtx);
138 BatchInfo& batchInfo = m_batchInfo[sensorHandle];
139
140 if (enabled) {
141 const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
142 LOG_ALWAYS_FATAL_IF(!sensor);
143 if (!(sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE))) {
144 if (batchInfo.samplingPeriodNs <= 0) {
145 return Result::BAD_VALUE;
146 }
147
148 BatchEventRef batchEventRef;
149 batchEventRef.timestamp =
150 ::android::elapsedRealtimeNano() + batchInfo.samplingPeriodNs;
151 batchEventRef.sensorHandle = sensorHandle;
152 batchEventRef.generation = ++batchInfo.generation;
153
154 m_batchQueue.push(batchEventRef);
155 m_batchUpdated.notify_one();
156 } else if (sensor->type == SensorType::HEART_RATE){
157 // Heart rate sensor's first data after activation should be
158 // SENSOR_STATUS_UNRELIABLE.
159 Event event;
160 event.u.heartRate.status = SensorStatus::UNRELIABLE;
161 event.u.heartRate.bpm = 0;
162 event.timestamp = ::android::elapsedRealtimeNano();
163 event.sensorHandle = sensorHandle;
164 event.sensorType = SensorType::HEART_RATE;
165 doPostSensorEventLocked(*sensor, event);
166 }
167
168 m_activeSensorsMask = m_activeSensorsMask | (1u << sensorHandle);
169 } else {
170 m_activeSensorsMask = m_activeSensorsMask & ~(1u << sensorHandle);
171 }
172 return Result::OK;
173 }
174
batch(const int32_t sensorHandle,const int64_t samplingPeriodNs,const int64_t maxReportLatencyNs)175 Return<Result> MultihalSensors::batch(const int32_t sensorHandle,
176 const int64_t samplingPeriodNs,
177 const int64_t maxReportLatencyNs) {
178 (void)maxReportLatencyNs;
179
180 if (!isSensorHandleValid(sensorHandle)) {
181 return Result::BAD_VALUE;
182 }
183
184 const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
185 LOG_ALWAYS_FATAL_IF(!sensor);
186
187 if (samplingPeriodNs < sensor->minDelay) {
188 return Result::BAD_VALUE;
189 }
190
191 std::unique_lock<std::mutex> lock(m_mtx);
192 if (m_opMode == OperationMode::NORMAL) {
193 m_batchInfo[sensorHandle].samplingPeriodNs = samplingPeriodNs;
194 }
195
196 return Result::OK;
197 }
198
flush(const int32_t sensorHandle)199 Return<Result> MultihalSensors::flush(const int32_t sensorHandle) {
200 if (!isSensorHandleValid(sensorHandle)) {
201 return Result::BAD_VALUE;
202 }
203
204 const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
205 LOG_ALWAYS_FATAL_IF(!sensor);
206
207 std::unique_lock<std::mutex> lock(m_mtx);
208 if (!isSensorActive(sensorHandle)) {
209 return Result::BAD_VALUE;
210 }
211
212 Event event;
213 event.sensorHandle = sensorHandle;
214 event.sensorType = SensorType::META_DATA;
215 event.u.meta.what = MetaDataEventType::META_DATA_FLUSH_COMPLETE;
216
217 doPostSensorEventLocked(*sensor, event);
218 return Result::OK;
219 }
220
injectSensorData_2_1(const Event & event)221 Return<Result> MultihalSensors::injectSensorData_2_1(const Event& event) {
222 if (!isSensorHandleValid(event.sensorHandle)) {
223 return Result::BAD_VALUE;
224 }
225 if (event.sensorType == SensorType::ADDITIONAL_INFO) {
226 return Result::OK;
227 }
228
229 std::unique_lock<std::mutex> lock(m_mtx);
230 if (m_opMode != OperationMode::DATA_INJECTION) {
231 return Result::INVALID_OPERATION;
232 }
233 const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle);
234 LOG_ALWAYS_FATAL_IF(!sensor);
235 if (sensor->type != event.sensorType) {
236 return Result::BAD_VALUE;
237 }
238
239 doPostSensorEventLocked(*sensor, event);
240 return Result::OK;
241 }
242
initialize(const sp<IHalProxyCallback> & halProxyCallback)243 Return<Result> MultihalSensors::initialize(const sp<IHalProxyCallback>& halProxyCallback) {
244 std::unique_lock<std::mutex> lock(m_mtx);
245 setAllQemuSensors(true); // we need to start sampling sensors for batching
246 m_opMode = OperationMode::NORMAL;
247 m_halProxyCallback = halProxyCallback;
248 return Result::OK;
249 }
250
postSensorEvent(const Event & event)251 void MultihalSensors::postSensorEvent(const Event& event) {
252 const SensorInfo* sensor = getSensorInfoByHandle(event.sensorHandle);
253 LOG_ALWAYS_FATAL_IF(!sensor);
254
255 std::unique_lock<std::mutex> lock(m_mtx);
256 if (sensor->flags & static_cast<uint32_t>(SensorFlagBits::ON_CHANGE_MODE)) {
257 if (isSensorActive(event.sensorHandle)) {
258 doPostSensorEventLocked(*sensor, event);
259 }
260 } else { // CONTINUOUS_MODE
261 m_batchInfo[event.sensorHandle].event = event;
262 }
263 }
264
doPostSensorEventLocked(const SensorInfo & sensor,const Event & event)265 void MultihalSensors::doPostSensorEventLocked(const SensorInfo& sensor,
266 const Event& event) {
267 const bool isWakeupEvent =
268 sensor.flags & static_cast<uint32_t>(SensorFlagBits::WAKE_UP);
269
270 m_halProxyCallback->postEvents(
271 {event},
272 m_halProxyCallback->createScopedWakelock(isWakeupEvent));
273 }
274
qemuSensorThreadSendCommand(const char cmd) const275 bool MultihalSensors::qemuSensorThreadSendCommand(const char cmd) const {
276 return TEMP_FAILURE_RETRY(write(m_callersFd.get(), &cmd, 1)) == 1;
277 }
278
isSensorHandleValid(int sensorHandle) const279 bool MultihalSensors::isSensorHandleValid(int sensorHandle) const {
280 if (!goldfish::isSensorHandleValid(sensorHandle)) {
281 return false;
282 }
283
284 if (!(m_availableSensorsMask & (1u << sensorHandle))) {
285 return false;
286 }
287
288 return true;
289 }
290
batchThread()291 void MultihalSensors::batchThread() {
292 while (m_batchRunning) {
293 std::unique_lock<std::mutex> lock(m_mtx);
294 if (m_batchQueue.empty()) {
295 m_batchUpdated.wait(lock);
296 } else {
297 const int64_t d =
298 m_batchQueue.top().timestamp - ::android::elapsedRealtimeNano();
299 m_batchUpdated.wait_for(lock, std::chrono::nanoseconds(d));
300 }
301
302 const int64_t nowNs = ::android::elapsedRealtimeNano();
303 while (!m_batchQueue.empty() && (nowNs >= m_batchQueue.top().timestamp)) {
304 BatchEventRef evRef = m_batchQueue.top();
305 m_batchQueue.pop();
306
307 const int sensorHandle = evRef.sensorHandle;
308 LOG_ALWAYS_FATAL_IF(!goldfish::isSensorHandleValid(sensorHandle));
309 if (!isSensorActive(sensorHandle)) {
310 continue;
311 }
312
313 BatchInfo &batchInfo = m_batchInfo[sensorHandle];
314 if (batchInfo.event.sensorType == SensorType::META_DATA) {
315 ALOGW("%s:%d the host has not provided value yet for sensorHandle=%d",
316 __func__, __LINE__, sensorHandle);
317 } else {
318 batchInfo.event.timestamp = evRef.timestamp;
319 const SensorInfo* sensor = getSensorInfoByHandle(sensorHandle);
320 LOG_ALWAYS_FATAL_IF(!sensor);
321 doPostSensorEventLocked(*sensor, batchInfo.event);
322 }
323
324 if (evRef.generation == batchInfo.generation) {
325 const int64_t samplingPeriodNs = batchInfo.samplingPeriodNs;
326 LOG_ALWAYS_FATAL_IF(samplingPeriodNs <= 0);
327
328 evRef.timestamp += samplingPeriodNs;
329 m_batchQueue.push(evRef);
330 }
331 }
332 }
333 }
334
335 /// not supported //////////////////////////////////////////////////////////////
registerDirectChannel(const SharedMemInfo & mem,registerDirectChannel_cb _hidl_cb)336 Return<void> MultihalSensors::registerDirectChannel(const SharedMemInfo& mem,
337 registerDirectChannel_cb _hidl_cb) {
338 (void)mem;
339 _hidl_cb(Result::INVALID_OPERATION, -1);
340 return {};
341 }
342
unregisterDirectChannel(int32_t channelHandle)343 Return<Result> MultihalSensors::unregisterDirectChannel(int32_t channelHandle) {
344 (void)channelHandle;
345 return Result::INVALID_OPERATION;
346 }
347
configDirectReport(int32_t sensorHandle,int32_t channelHandle,RateLevel rate,configDirectReport_cb _hidl_cb)348 Return<void> MultihalSensors::configDirectReport(int32_t sensorHandle,
349 int32_t channelHandle,
350 RateLevel rate,
351 configDirectReport_cb _hidl_cb) {
352 (void)sensorHandle;
353 (void)channelHandle;
354 (void)rate;
355 _hidl_cb(Result::INVALID_OPERATION, 0 /* reportToken */);
356 return {};
357 }
358
359 } // namespace goldfish
360