• Home
  • History
  • Annotate
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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