1 /* 2 * Copyright (C) 2017 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 #include <cstdint> 17 18 #include <cutils/properties.h> 19 #include <cutils/sockets.h> 20 #include <errno.h> 21 #include <fcntl.h> 22 #include <sys/system_properties.h> 23 #include <unistd.h> 24 25 #include <algorithm> 26 27 #include "common/libs/fs/shared_select.h" 28 #include "common/libs/threads/thunkers.h" 29 #include "guest/hals/sensors/sensors_hal.h" 30 #include "guest/hals/sensors/vsoc_sensors.h" 31 #include "guest/hals/sensors/vsoc_sensors_message.h" 32 #include "guest/libs/platform_support/api_level_fixes.h" 33 #include "guest/libs/remoter/remoter_framework_pkt.h" 34 35 using cvd::LockGuard; 36 using cvd::Mutex; 37 using cvd::time::Milliseconds; 38 using cvd::time::MonotonicTimePoint; 39 using cvd::time::Nanoseconds; 40 41 namespace cvd { 42 43 namespace { 44 template <typename F> 45 struct HWDeviceThunker : ThunkerBase<hw_device_t, GceSensors, F> {}; 46 template <typename F> 47 struct SensorsThunker : ThunkerBase<sensors_poll_device_t, GceSensors, F> {}; 48 template <typename F> 49 struct SensorsThunker1 : ThunkerBase<sensors_poll_device_1, GceSensors, F> {}; 50 template <typename F> 51 struct SensorsThreadThunker : ThunkerBase<void, GceSensors, F> {}; 52 } 53 54 int GceSensors::total_sensor_count_ = -1; 55 SensorInfo* GceSensors::sensor_infos_ = NULL; 56 const int GceSensors::kInjectedEventWaitPeriods = 3; 57 const Nanoseconds GceSensors::kInjectedEventWaitTime = 58 Nanoseconds(Milliseconds(20)); 59 60 GceSensors::GceSensors() 61 : sensors_poll_device_1(), deadline_change_(&sensor_state_lock_) { 62 if (total_sensor_count_ == -1) { 63 RegisterSensors(); 64 } 65 66 // Create a pair of FDs that would be used to control the 67 // receiver thread. 68 if (control_sender_socket_->IsOpen() || control_receiver_socket_->IsOpen()) { 69 ALOGE("%s: Receiver control FDs are opened", __FUNCTION__); 70 } 71 if (!cvd::SharedFD::Pipe(&control_receiver_socket_, 72 &control_sender_socket_)) { 73 ALOGE("%s: Unable to create thread control FDs: %d -> %s", __FUNCTION__, 74 errno, strerror(errno)); 75 } 76 77 // Create the correct number of holding buffers for this client. 78 sensor_states_.resize(total_sensor_count_); 79 int i; 80 for (i = 0; i < total_sensor_count_; i++) { 81 sensor_states_[i] = new SensorState(sensor_infos_[i]); 82 } 83 } 84 85 GceSensors::~GceSensors() { 86 int i; 87 for (i = 0; i < total_sensor_count_; i++) { 88 delete sensor_states_[i]; 89 } 90 } 91 92 int GceSensors::GetSensorsList(struct sensors_module_t* /*module*/, 93 struct sensor_t const** list) { 94 *list = sensor_infos_; 95 return total_sensor_count_; 96 } 97 98 int GceSensors::SetOperationMode(unsigned int /* is_loopback_mode */) { 99 return -EINVAL; 100 } 101 102 int GceSensors::Open(const struct hw_module_t* module, const char* name, 103 struct hw_device_t** device) { 104 int status = -EINVAL; 105 106 if (!strcmp(name, SENSORS_HARDWARE_POLL)) { 107 // Create a new GceSensors object and set all the fields/functions 108 // to their default values. 109 GceSensors* rval = new GceSensors; 110 111 rval->common.tag = HARDWARE_DEVICE_TAG; 112 rval->common.version = VSOC_SENSOR_DEVICE_VERSION; 113 rval->common.module = (struct hw_module_t*)module; 114 rval->common.close = HWDeviceThunker<int()>::call<&GceSensors::Close>; 115 rval->poll = 116 SensorsThunker<int(sensors_event_t*, int)>::call<&GceSensors::Poll>; 117 rval->activate = SensorsThunker<int(int, int)>::call<&GceSensors::Activate>; 118 rval->setDelay = 119 SensorsThunker<int(int, int64_t)>::call<&GceSensors::SetDelay>; 120 #if VSOC_SENSORS_DEVICE_API_VERSION_ATLEAST(1_0) 121 rval->batch = SensorsThunker1<int(int, int, int64_t, 122 int64_t)>::call<&GceSensors::Batch>; 123 #endif 124 #if VSOC_SENSORS_DEVICE_API_VERSION_ATLEAST(1_1) 125 rval->flush = SensorsThunker1<int(int)>::call<&GceSensors::Flush>; 126 #endif 127 #if VSOC_SENSORS_DEVICE_API_VERSION_ATLEAST(1_4) 128 rval->inject_sensor_data = SensorsThunker1<int( 129 const sensors_event_t*)>::call<&GceSensors::InjectSensorData>; 130 #endif 131 132 // Spawn a thread to listen for incoming data from the remoter. 133 int err = pthread_create( 134 &rval->receiver_thread_, NULL, 135 SensorsThreadThunker<void*()>::call<&GceSensors::Receiver>, rval); 136 if (err) { 137 ALOGE("GceSensors::%s: Unable to start receiver thread (%s)", 138 __FUNCTION__, strerror(err)); 139 } 140 141 *device = &rval->common; 142 status = 0; 143 } 144 return status; 145 } 146 147 int GceSensors::Close() { 148 // Make certain the receiver thread wakes up. 149 SensorControlMessage msg; 150 msg.message_type = THREAD_STOP; 151 SendControlMessage(msg); 152 pthread_join(receiver_thread_, NULL); 153 delete this; 154 return 0; 155 } 156 157 int GceSensors::Activate(int handle, int enabled) { 158 if (handle < 0 || handle >= total_sensor_count_) { 159 ALOGE("GceSensors::%s: Bad handle %d", __FUNCTION__, handle); 160 return -1; 161 } 162 163 { 164 LockGuard<Mutex> guard(sensor_state_lock_); 165 // Update the report deadline, if changed. 166 if (enabled && !sensor_states_[handle]->enabled_) { 167 sensor_states_[handle]->deadline_ = 168 MonotonicTimePoint::Now() + sensor_states_[handle]->sampling_period_; 169 } else if (!enabled && sensor_states_[handle]->enabled_) { 170 sensor_states_[handle]->deadline_ = SensorState::kInfinity; 171 } 172 sensor_states_[handle]->enabled_ = enabled; 173 UpdateDeadline(); 174 } 175 176 D("sensor_activate(): handle %d, enabled %d", handle, enabled); 177 if (!UpdateRemoterState(handle)) { 178 ALOGE("Failed to notify remoter about new sensor enable/disable."); 179 } 180 return 0; 181 } 182 183 int GceSensors::SetDelay(int handle, int64_t sampling_period_ns) { 184 if (handle < 0 || handle >= total_sensor_count_) { 185 ALOGE("GceSensors::%s: Bad handle %d", __FUNCTION__, handle); 186 return -1; 187 } 188 int64_t min_delay_ns = sensor_infos_[handle].minDelay * 1000; 189 if (sampling_period_ns < min_delay_ns) { 190 sampling_period_ns = min_delay_ns; 191 } 192 193 { 194 LockGuard<Mutex> guard(sensor_state_lock_); 195 sensor_states_[handle]->deadline_ -= 196 sensor_states_[handle]->sampling_period_; 197 sensor_states_[handle]->sampling_period_ = Nanoseconds(sampling_period_ns); 198 sensor_states_[handle]->deadline_ += 199 sensor_states_[handle]->sampling_period_; 200 // If our sampling period has decreased, our deadline 201 // could have already passed. If so, report immediately, but not in the 202 // past. 203 MonotonicTimePoint now = MonotonicTimePoint::Now(); 204 if (sensor_states_[handle]->deadline_ < now) { 205 sensor_states_[handle]->deadline_ = now; 206 } 207 UpdateDeadline(); 208 } 209 210 D("sensor_set_delay(): handle %d, delay (ms) %" PRId64, handle, 211 Milliseconds(Nanoseconds(sampling_period_ns)).count()); 212 if (!UpdateRemoterState(handle)) { 213 ALOGE("Failed to notify remoter about new sensor delay."); 214 } 215 return 0; 216 } 217 218 int GceSensors::Poll(sensors_event_t* data, int count_unsafe) { 219 if (count_unsafe <= 0) { 220 ALOGE("Framework polled with bad count (%d)", count_unsafe); 221 return -1; 222 } 223 size_t count = size_t(count_unsafe); 224 225 // Poll will block until 1 of 2 things happens: 226 // 1. The next deadline for some active sensor 227 // occurs. 228 // 2. The next deadline changes (either because 229 // a sensor was activated/deactivated or its 230 // delay changed). 231 // In both cases, any sensors whose report deadlines 232 // have passed will report their data (or mock data), 233 // and poll will either return (if at least one deadline 234 // has passed), or repeat by blocking until the next deadline. 235 LockGuard<Mutex> guard(sensor_state_lock_); 236 current_deadline_ = UpdateDeadline(); 237 // Sleep until we have something to report 238 while (!fifo_.size()) { 239 deadline_change_.WaitUntil(current_deadline_); 240 current_deadline_ = UpdateDeadline(); 241 } 242 // Copy the events from the buffer 243 int num_copied = std::min(fifo_.size(), count); 244 FifoType::iterator first_uncopied = fifo_.begin() + num_copied; 245 std::copy(fifo_.begin(), first_uncopied, data); 246 fifo_.erase(fifo_.begin(), first_uncopied); 247 D("Reported %d sensor events. First: %d %f %f %f", num_copied, data->sensor, 248 data->data[0], data->data[1], data->data[2]); 249 return num_copied; 250 } 251 252 253 void *GceSensors::Receiver() { 254 // Initialize the server. 255 sensor_listener_socket_ = cvd::SharedFD::SocketSeqPacketServer( 256 gce_sensors_message::kSensorsHALSocketName, 0777); 257 if (!sensor_listener_socket_->IsOpen()) { 258 ALOGE("GceSensors::%s: Could not listen for sensor connections. (%s).", 259 __FUNCTION__, sensor_listener_socket_->StrError()); 260 return NULL; 261 } 262 D("GceSensors::%s: Listening for sensor connections at %s", __FUNCTION__, 263 gce_sensors_message::kSensorsHALSocketName); 264 // Announce that we are ready for the remoter to connect. 265 if (!NotifyRemoter()) { 266 ALOGI("Failed to notify remoter that HAL is ready."); 267 } else { 268 ALOGI("Notified remoter that HAL is ready."); 269 } 270 271 typedef std::vector<cvd::SharedFD> FDVec; 272 FDVec connected; 273 // Listen for incoming sensor data and control messages 274 // from the HAL. 275 while (true) { 276 cvd::SharedFDSet fds; 277 for (FDVec::iterator it = connected.begin(); it != connected.end(); ++it) { 278 fds.Set(*it); 279 } 280 fds.Set(control_receiver_socket_); 281 // fds.Set(sensor_listener_socket_); 282 int res = cvd::Select(&fds, NULL, NULL, NULL); 283 if (res == -1) { 284 ALOGE("%s: select returned %d and failed %d -> %s", __FUNCTION__, res, 285 errno, strerror(errno)); 286 break; 287 } else if (res == 0) { 288 ALOGE("%s: select timed out", __FUNCTION__); 289 break; 290 } else if (fds.IsSet(sensor_listener_socket_)) { 291 connected.push_back(cvd::SharedFD::Accept(*sensor_listener_socket_)); 292 ALOGI("GceSensors::%s: new client connected", __FUNCTION__); 293 } else if (fds.IsSet(control_receiver_socket_)) { 294 // We received a control message. 295 SensorControlMessage msg; 296 int res = 297 control_receiver_socket_->Read(&msg, sizeof(SensorControlMessage)); 298 if (res == -1) { 299 ALOGE("GceSensors::%s: Failed to receive control message.", 300 __FUNCTION__); 301 } else if (res == 0) { 302 ALOGE("GceSensors::%s: Control connection closed.", __FUNCTION__); 303 } 304 if (msg.message_type == SENSOR_STATE_UPDATE) { 305 // Forward the update to the remoter. 306 remoter_request_packet packet; 307 remoter_request_packet_init(&packet, kRemoterSensorState, 0); 308 { 309 LockGuard<Mutex> guard(sensor_state_lock_); 310 packet.params.sensor_state_params.type = 311 sensor_infos_[msg.sensor_handle].type; 312 packet.params.sensor_state_params.enabled = 313 sensor_states_[msg.sensor_handle]->enabled_; 314 packet.params.sensor_state_params.delay_ns = 315 sensor_states_[msg.sensor_handle]->sampling_period_.count(); 316 packet.params.sensor_state_params.handle = msg.sensor_handle; 317 } 318 struct msghdr msg; 319 iovec msg_iov[1]; 320 msg_iov[0].iov_base = &packet; 321 msg_iov[0].iov_len = sizeof(remoter_request_packet); 322 msg.msg_name = NULL; 323 msg.msg_namelen = 0; 324 msg.msg_iov = msg_iov; 325 msg.msg_iovlen = arraysize(msg_iov); 326 msg.msg_control = NULL; 327 msg.msg_controllen = 0; 328 msg.msg_flags = 0; 329 330 for (FDVec::iterator it = connected.begin(); it != connected.end(); 331 ++it) { 332 cvd::SharedFD &fd = *it; 333 if (fd->SendMsg(&msg, 0) == -1) { 334 ALOGE("GceSensors::%s. Could not send sensor state (%s).", 335 __FUNCTION__, fd->StrError()); 336 } 337 } 338 } 339 if (msg.message_type == THREAD_STOP) { 340 D("Received terminate control message."); 341 return NULL; 342 } 343 } else { 344 for (FDVec::iterator it = connected.begin(); it != connected.end(); 345 ++it) { 346 cvd::SharedFD &fd = *it; 347 if (fds.IsSet(fd)) { 348 // We received a sensor update from remoter. 349 sensors_event_t event; 350 struct msghdr msg; 351 iovec msg_iov[1]; 352 msg_iov[0].iov_base = &event; 353 msg_iov[0].iov_len = sizeof(event); 354 msg.msg_name = NULL; 355 msg.msg_namelen = 0; 356 msg.msg_iov = msg_iov; 357 msg.msg_iovlen = arraysize(msg_iov); 358 msg.msg_control = NULL; 359 msg.msg_controllen = 0; 360 msg.msg_flags = 0; 361 int res = fd->RecvMsg(&msg, 0); 362 if (res <= 0) { 363 if (res == 0) { 364 ALOGE("GceSensors::%s: Sensors HAL connection closed.", 365 __FUNCTION__); 366 } else { 367 ALOGE("GceSensors::%s: Failed to receive sensor message", 368 __FUNCTION__); 369 } 370 connected.erase(std::find(connected.begin(), connected.end(), fd)); 371 break; 372 } 373 374 // We received an event from the remoter. 375 if (event.sensor < 0 || event.sensor >= total_sensor_count_) { 376 ALOGE("Remoter sent us an invalid sensor event! (handle %d)", 377 event.sensor); 378 connected.erase(std::find(connected.begin(), connected.end(), fd)); 379 break; 380 } 381 382 D("Received sensor event: %d %f %f %f", event.sensor, event.data[0], 383 event.data[1], event.data[2]); 384 385 { 386 LockGuard<Mutex> guard(sensor_state_lock_); 387 // Increase the delay so that the HAL knows 388 // it shouldn't report on its own for a while. 389 SensorState *holding_buffer = sensor_states_[event.sensor]; 390 int wait_periods = 391 std::max(kInjectedEventWaitPeriods, 392 (int)(kInjectedEventWaitTime.count() / 393 holding_buffer->sampling_period_.count())); 394 holding_buffer->deadline_ = 395 MonotonicTimePoint::Now() + 396 holding_buffer->sampling_period_ * wait_periods; 397 holding_buffer->event_.data[0] = event.data[0]; 398 holding_buffer->event_.data[1] = event.data[1]; 399 holding_buffer->event_.data[2] = event.data[2]; 400 // Signal the HAL to report the newly arrived event. 401 fifo_.push_back(event); 402 deadline_change_.NotifyOne(); 403 } 404 } 405 } 406 } 407 } 408 return NULL; 409 } 410 411 bool GceSensors::NotifyRemoter() { 412 remoter_request_packet packet; 413 remoter_request_packet_init(&packet, kRemoterHALReady, 0); 414 packet.send_response = 0; 415 strncpy(packet.params.hal_ready_params.unix_socket, 416 gce_sensors_message::kSensorsHALSocketName, 417 sizeof(packet.params.hal_ready_params.unix_socket)); 418 AutoCloseFileDescriptor remoter_socket(remoter_connect()); 419 if (remoter_socket.IsError()) { 420 D("GceSensors::%s: Could not connect to remoter to notify ready (%s).", 421 __FUNCTION__, strerror(errno)); 422 return false; 423 } 424 int err = 425 remoter_do_single_request_with_socket(remoter_socket, &packet, NULL); 426 if (err == -1) { 427 D("GceSensors::%s: Notify remoter ready: Failed after connect (%s).", 428 __FUNCTION__, strerror(errno)); 429 return false; 430 } 431 D("GceSensors::%s: Notify remoter ready Succeeded.", __FUNCTION__); 432 return true; 433 } 434 435 static bool CompareTimestamps(const sensors_event_t& a, 436 const sensors_event_t& b) { 437 return a.timestamp < b.timestamp; 438 } 439 440 MonotonicTimePoint GceSensors::UpdateDeadline() { 441 // Get the minimum of all the current deadlines. 442 MonotonicTimePoint now = MonotonicTimePoint::Now(); 443 MonotonicTimePoint min = SensorState::kInfinity; 444 int i = 0; 445 bool sort_fifo = false; 446 447 for (i = 0; i < total_sensor_count_; i++) { 448 SensorState* holding_buffer = sensor_states_[i]; 449 // Ignore disabled sensors. 450 if (!holding_buffer->enabled_) { 451 continue; 452 } 453 while (holding_buffer->deadline_ < now) { 454 sensors_event_t data = holding_buffer->event_; 455 data.timestamp = holding_buffer->deadline_.SinceEpoch().count(); 456 fifo_.push_back(data); 457 holding_buffer->deadline_ += holding_buffer->sampling_period_; 458 sort_fifo = true; 459 } 460 // Now check if we should update the wake time based on the next event 461 // from this sensor. 462 if (sensor_states_[i]->deadline_ < min) { 463 min = sensor_states_[i]->deadline_; 464 } 465 } 466 // We added one or more sensor readings, so do a sort. 467 // This is likely to be cheaper than a traditional priority queue because 468 // a priority queue would try to keep its state correct for each addition. 469 if (sort_fifo) { 470 std::sort(fifo_.begin(), fifo_.end(), CompareTimestamps); 471 } 472 // If we added events or the deadline is lower notify the thread in Poll(). 473 // If the deadline went up, don't do anything. 474 if (fifo_.size() || (min < current_deadline_)) { 475 deadline_change_.NotifyOne(); 476 } 477 return min; 478 } 479 480 bool GceSensors::UpdateRemoterState(int handle) { 481 SensorControlMessage msg; 482 msg.message_type = SENSOR_STATE_UPDATE; 483 msg.sensor_handle = handle; 484 return SendControlMessage(msg); 485 } 486 487 bool GceSensors::SendControlMessage(SensorControlMessage msg) { 488 if (!control_sender_socket_->IsOpen()) { 489 ALOGE("%s: Can't send control message %d, control socket not open.", 490 __FUNCTION__, msg.message_type); 491 return false; 492 } 493 if (control_sender_socket_->Write(&msg, sizeof(SensorControlMessage)) == -1) { 494 ALOGE("GceSensors::%s. Could not send control message %d (%s).", 495 __FUNCTION__, msg.message_type, control_sender_socket_->StrError()); 496 return false; 497 } 498 return true; 499 } 500 501 int GceSensors::RegisterSensors() { 502 if (total_sensor_count_ != -1) { 503 return -1; 504 } 505 total_sensor_count_ = 9; 506 sensor_infos_ = new SensorInfo[total_sensor_count_]; 507 sensor_infos_[sensors_constants::kAccelerometerHandle] = 508 AccelerometerSensor(); 509 sensor_infos_[sensors_constants::kGyroscopeHandle] = GyroscopeSensor(); 510 sensor_infos_[sensors_constants::kLightHandle] = LightSensor(); 511 sensor_infos_[sensors_constants::kMagneticFieldHandle] = 512 MagneticFieldSensor(); 513 sensor_infos_[sensors_constants::kPressureHandle] = PressureSensor(); 514 sensor_infos_[sensors_constants::kProximityHandle] = ProximitySensor(); 515 sensor_infos_[sensors_constants::kAmbientTempHandle] = AmbientTempSensor(); 516 sensor_infos_[sensors_constants::kDeviceTempHandle] = DeviceTempSensor(); 517 sensor_infos_[sensors_constants::kRelativeHumidityHandle] = 518 RelativeHumiditySensor(); 519 int i; 520 for (i = 0; i < total_sensor_count_; i++) { 521 D("Found sensor %s with handle %d", sensor_infos_[i].name, 522 sensor_infos_[i].handle); 523 } 524 return total_sensor_count_; 525 } 526 527 } // namespace cvd 528