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 #define LOG_TAG "GoogleIIOSensorSubHal"
17 
18 #include "Sensor.h"
19 #include <hardware/sensors.h>
20 #include <log/log.h>
21 #include <utils/SystemClock.h>
22 #include <cmath>
23 
24 namespace android {
25 namespace hardware {
26 namespace sensors {
27 namespace V2_1 {
28 namespace subhal {
29 namespace implementation {
30 
31 using ::android::hardware::sensors::V1_0::AdditionalInfoType;
32 using ::android::hardware::sensors::V1_0::MetaDataEventType;
33 using ::android::hardware::sensors::V1_0::SensorFlagBits;
34 using ::android::hardware::sensors::V1_0::SensorStatus;
35 using ::sensor::hal::configuration::V1_0::Location;
36 using ::sensor::hal::configuration::V1_0::Orientation;
37 
SensorBase(int32_t sensorHandle,ISensorsEventCallback * callback,SensorType type)38 SensorBase::SensorBase(int32_t sensorHandle, ISensorsEventCallback* callback, SensorType type)
39     : mIsEnabled(false),
40       mSamplingPeriodNs(0),
41       mCallback(callback),
42       mMode(OperationMode::NORMAL),
43       mSensorThread(this) {
44     mSensorInfo.type = type;
45     mSensorInfo.sensorHandle = sensorHandle;
46     mSensorInfo.vendor = "Google";
47     mSensorInfo.version = 1;
48     mSensorInfo.fifoReservedEventCount = 0;
49     mSensorInfo.fifoMaxEventCount = 0;
50     mSensorInfo.requiredPermission = "";
51     mSensorInfo.flags = 0;
52 
53     switch (type) {
54         case SensorType::ACCELEROMETER:
55             mSensorInfo.typeAsString = SENSOR_STRING_TYPE_ACCELEROMETER;
56             break;
57         case SensorType::GYROSCOPE:
58             mSensorInfo.typeAsString = SENSOR_STRING_TYPE_GYROSCOPE;
59             break;
60         default:
61             ALOGE("unsupported sensor type %d", type);
62             break;
63     }
64 
65     mSensorThread.start();
66 }
67 
~SensorBase()68 SensorBase::~SensorBase() {
69     mIsEnabled = false;
70 }
71 
isEnabled() const72 bool SensorBase::isEnabled() const {
73     return mIsEnabled;
74 }
75 
getOperationMode() const76 OperationMode SensorBase::getOperationMode() const {
77     return mMode;
78 }
79 
~HWSensorBase()80 HWSensorBase::~HWSensorBase() {
81     close(mPollFdIio.fd);
82 }
83 
getSensorInfo() const84 const SensorInfo& SensorBase::getSensorInfo() const {
85     return mSensorInfo;
86 }
87 
batch(int32_t samplingPeriodNs)88 void HWSensorBase::batch(int32_t samplingPeriodNs) {
89     samplingPeriodNs =
90             std::clamp(samplingPeriodNs, mSensorInfo.minDelay * 1000, mSensorInfo.maxDelay * 1000);
91     if (mSamplingPeriodNs != samplingPeriodNs) {
92         unsigned int sampling_frequency = ns_to_frequency(samplingPeriodNs);
93         int i = 0;
94         mSamplingPeriodNs = samplingPeriodNs;
95         std::vector<double>::iterator low =
96                 std::lower_bound(mIioData.sampling_freq_avl.begin(),
97                                  mIioData.sampling_freq_avl.end(), sampling_frequency);
98         i = low - mIioData.sampling_freq_avl.begin();
99         set_sampling_frequency(mIioData.sysfspath, mIioData.sampling_freq_avl[i]);
100         // Wake up the 'run' thread to check if a new event should be generated now
101         mSensorThread.notifyAll();
102     }
103 }
104 
sendAdditionalInfoReport()105 void HWSensorBase::sendAdditionalInfoReport() {
106     std::vector<Event> events;
107 
108     for (const auto& frame : mAdditionalInfoFrames) {
109         events.emplace_back(Event{
110                 .sensorHandle = mSensorInfo.sensorHandle,
111                 .sensorType = SensorType::ADDITIONAL_INFO,
112                 .timestamp = android::elapsedRealtimeNano(),
113                 .u.additional = frame,
114         });
115     }
116 
117     if (!events.empty()) {
118         mCallback->postEvents(events, mCallback->createScopedWakelock(isWakeUpSensor()));
119     }
120 }
121 
activate(bool enable)122 void HWSensorBase::activate(bool enable) {
123     std::unique_lock<std::mutex> lock(mSensorThread.lock());
124     if (mIsEnabled != enable) {
125         mIsEnabled = enable;
126         enable_sensor(mIioData.sysfspath, enable);
127         if (enable) sendAdditionalInfoReport();
128         mSensorThread.notifyAll();
129     }
130 }
131 
flush()132 Result SensorBase::flush() {
133     // Only generate a flush complete event if the sensor is enabled and if the sensor is not a
134     // one-shot sensor.
135     if (!mIsEnabled || (mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::ONE_SHOT_MODE))) {
136         return Result::BAD_VALUE;
137     }
138 
139     // Note: If a sensor supports batching, write all of the currently batched events for the sensor
140     // to the Event FMQ prior to writing the flush complete event.
141     Event ev;
142     ev.sensorHandle = mSensorInfo.sensorHandle;
143     ev.sensorType = SensorType::META_DATA;
144     ev.u.meta.what = MetaDataEventType::META_DATA_FLUSH_COMPLETE;
145     std::vector<Event> evs{ev};
146     mCallback->postEvents(evs, mCallback->createScopedWakelock(isWakeUpSensor()));
147     return Result::OK;
148 }
149 
flush()150 Result HWSensorBase::flush() {
151     Result result = Result::OK;
152     result = SensorBase::flush();
153     if (result == Result::OK) sendAdditionalInfoReport();
154     return result;
155 }
156 
157 template <size_t N>
getChannelData(const std::array<float,N> & channelData,int64_t map,bool negate)158 static float getChannelData(const std::array<float, N>& channelData, int64_t map, bool negate) {
159     return negate ? -channelData[map] : channelData[map];
160 }
161 
processScanData(uint8_t * data,Event * evt)162 void HWSensorBase::processScanData(uint8_t* data, Event* evt) {
163     std::array<float, NUM_OF_DATA_CHANNELS> channelData;
164     unsigned int chanIdx;
165     evt->sensorHandle = mSensorInfo.sensorHandle;
166     evt->sensorType = mSensorInfo.type;
167     for (auto i = 0u; i < mIioData.channelInfo.size(); i++) {
168         chanIdx = mIioData.channelInfo[i].index;
169 
170         const int64_t val =
171                 *reinterpret_cast<int64_t*>(data + chanIdx * mIioData.channelInfo[i].storage_bytes);
172         // If the channel index is the last, it is timestamp
173         // else it is sensor data
174         if (chanIdx == mIioData.channelInfo.size() - 1) {
175             evt->timestamp = val;
176         } else {
177             channelData[chanIdx] = static_cast<float>(val) * mIioData.scale;
178         }
179     }
180 
181     evt->u.vec3.x = getChannelData(channelData, mXMap, mXNegate);
182     evt->u.vec3.y = getChannelData(channelData, mYMap, mYNegate);
183     evt->u.vec3.z = getChannelData(channelData, mZMap, mZNegate);
184     evt->u.vec3.status = SensorStatus::ACCURACY_HIGH;
185 }
186 
pollForEvents()187 void HWSensorBase::pollForEvents() {
188     int err = poll(&mPollFdIio, 1, mSamplingPeriodNs * 1000);
189     if (err <= 0) {
190         ALOGE("Sensor %s poll returned %d", mIioData.name.c_str(), err);
191         return;
192     }
193 
194     if (mPollFdIio.revents & POLLIN) {
195         int read_size = read(mPollFdIio.fd, &mSensorRawData[0], mScanSize);
196         if (read_size <= 0) {
197             ALOGE("%s: Failed to read data from iio char device.", mIioData.name.c_str());
198             return;
199         }
200 
201         Event evt;
202         processScanData(&mSensorRawData[0], &evt);
203         mCallback->postEvents({evt}, mCallback->createScopedWakelock(isWakeUpSensor()));
204     }
205 }
206 
idleLoop()207 void HWSensorBase::idleLoop() {
208     mSensorThread.wait([this] {
209         return ((mIsEnabled && mMode == OperationMode::NORMAL) || mSensorThread.isStopped());
210     });
211 }
212 
pollSensor()213 void HWSensorBase::pollSensor() {
214     if (!mIsEnabled || mMode == OperationMode::DATA_INJECTION) {
215         idleLoop();
216     } else {
217         pollForEvents();
218     }
219 }
220 
isWakeUpSensor()221 bool SensorBase::isWakeUpSensor() {
222     return mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::WAKE_UP);
223 }
224 
setOperationMode(OperationMode mode)225 void SensorBase::setOperationMode(OperationMode mode) {
226     std::unique_lock<std::mutex> lock(mSensorThread.lock());
227     if (mMode != mode) {
228         mMode = mode;
229         mSensorThread.notifyAll();
230     }
231 }
232 
supportsDataInjection() const233 bool SensorBase::supportsDataInjection() const {
234     return mSensorInfo.flags & static_cast<uint32_t>(SensorFlagBits::DATA_INJECTION);
235 }
236 
injectEvent(const Event & event)237 Result SensorBase::injectEvent(const Event& event) {
238     Result result = Result::OK;
239     if (event.sensorType == SensorType::ADDITIONAL_INFO) {
240         // When in OperationMode::NORMAL, SensorType::ADDITIONAL_INFO is used to push operation
241         // environment data into the device.
242     } else if (!supportsDataInjection()) {
243         result = Result::INVALID_OPERATION;
244     } else if (mMode == OperationMode::DATA_INJECTION) {
245         mCallback->postEvents({event}, mCallback->createScopedWakelock(isWakeUpSensor()));
246     } else {
247         result = Result::BAD_VALUE;
248     }
249     return result;
250 }
251 
calculateScanSize()252 ssize_t HWSensorBase::calculateScanSize() {
253     ssize_t numBytes = 0;
254     for (auto i = 0u; i < mIioData.channelInfo.size(); i++) {
255         numBytes += mIioData.channelInfo[i].storage_bytes;
256     }
257     return numBytes;
258 }
259 
checkAxis(int64_t map)260 static status_t checkAxis(int64_t map) {
261     if (map < 0 || map >= NUM_OF_DATA_CHANNELS)
262         return BAD_VALUE;
263     else
264         return OK;
265 }
266 
getOrientation(std::optional<std::vector<Configuration>> config)267 static std::optional<std::vector<Orientation>> getOrientation(
268         std::optional<std::vector<Configuration>> config) {
269     if (!config) return std::nullopt;
270     if (config->empty()) return std::nullopt;
271     Configuration& sensorCfg = (*config)[0];
272     return sensorCfg.getOrientation();
273 }
274 
getLocation(std::optional<std::vector<Configuration>> config)275 static std::optional<std::vector<Location>> getLocation(
276         std::optional<std::vector<Configuration>> config) {
277     if (!config) return std::nullopt;
278     if (config->empty()) return std::nullopt;
279     Configuration& sensorCfg = (*config)[0];
280     return sensorCfg.getLocation();
281 }
282 
checkOrientation(std::optional<std::vector<Configuration>> config)283 static status_t checkOrientation(std::optional<std::vector<Configuration>> config) {
284     status_t ret = OK;
285     std::optional<std::vector<Orientation>> sensorOrientationList = getOrientation(config);
286     if (!sensorOrientationList) return OK;
287     if (sensorOrientationList->empty()) return OK;
288     Orientation& sensorOrientation = (*sensorOrientationList)[0];
289     if (!sensorOrientation.getFirstX() || !sensorOrientation.getFirstY() ||
290         !sensorOrientation.getFirstZ())
291         return BAD_VALUE;
292 
293     int64_t xMap = sensorOrientation.getFirstX()->getMap();
294     ret = checkAxis(xMap);
295     if (ret != OK) return ret;
296     int64_t yMap = sensorOrientation.getFirstY()->getMap();
297     ret = checkAxis(yMap);
298     if (ret != OK) return ret;
299     int64_t zMap = sensorOrientation.getFirstZ()->getMap();
300     ret = checkAxis(zMap);
301     if (ret != OK) return ret;
302     if (xMap == yMap || yMap == zMap || zMap == xMap) return BAD_VALUE;
303     return ret;
304 }
305 
setAxisDefaultValues()306 void HWSensorBase::setAxisDefaultValues() {
307     mXMap = 0;
308     mYMap = 1;
309     mZMap = 2;
310     mXNegate = mYNegate = mZNegate = false;
311 }
setOrientation(std::optional<std::vector<Configuration>> config)312 void HWSensorBase::setOrientation(std::optional<std::vector<Configuration>> config) {
313     std::optional<std::vector<Orientation>> sensorOrientationList = getOrientation(config);
314 
315     if (sensorOrientationList && !sensorOrientationList->empty()) {
316         Orientation& sensorOrientation = (*sensorOrientationList)[0];
317 
318         if (sensorOrientation.getRotate()) {
319             mXMap = sensorOrientation.getFirstX()->getMap();
320             mXNegate = sensorOrientation.getFirstX()->getNegate();
321             mYMap = sensorOrientation.getFirstY()->getMap();
322             mYNegate = sensorOrientation.getFirstY()->getNegate();
323             mZMap = sensorOrientation.getFirstZ()->getMap();
324             mZNegate = sensorOrientation.getFirstZ()->getNegate();
325         } else {
326             setAxisDefaultValues();
327         }
328     } else {
329         setAxisDefaultValues();
330     }
331 }
332 
checkIIOData(const struct iio_device_data & iio_data)333 static status_t checkIIOData(const struct iio_device_data& iio_data) {
334     status_t ret = OK;
335     for (auto i = 0u; i < iio_data.channelInfo.size(); i++) {
336         if (iio_data.channelInfo[i].index > NUM_OF_DATA_CHANNELS) return BAD_VALUE;
337     }
338     return ret;
339 }
340 
setSensorPlacementData(AdditionalInfo * sensorPlacement,int index,float value)341 static status_t setSensorPlacementData(AdditionalInfo* sensorPlacement, int index, float value) {
342     if (!sensorPlacement) return BAD_VALUE;
343 
344     int arraySize =
345             sizeof(sensorPlacement->u.data_float) / sizeof(sensorPlacement->u.data_float[0]);
346     if (index < 0 || index >= arraySize) return BAD_VALUE;
347 
348     sensorPlacement->u.data_float[index] = value;
349     return OK;
350 }
351 
getSensorPlacement(AdditionalInfo * sensorPlacement,const std::optional<std::vector<Configuration>> & config)352 status_t HWSensorBase::getSensorPlacement(AdditionalInfo* sensorPlacement,
353                                           const std::optional<std::vector<Configuration>>& config) {
354     if (!sensorPlacement) return BAD_VALUE;
355 
356     auto sensorLocationList = getLocation(config);
357     if (!sensorLocationList) return BAD_VALUE;
358     if (sensorLocationList->empty()) return BAD_VALUE;
359 
360     auto sensorOrientationList = getOrientation(config);
361     if (!sensorOrientationList) return BAD_VALUE;
362     if (sensorOrientationList->empty()) return BAD_VALUE;
363 
364     sensorPlacement->type = AdditionalInfoType::AINFO_SENSOR_PLACEMENT;
365     sensorPlacement->serial = 0;
366     memset(&sensorPlacement->u.data_float, 0, sizeof(sensorPlacement->u.data_float));
367 
368     Location& sensorLocation = (*sensorLocationList)[0];
369     // SensorPlacementData is given as a 3x4 matrix consisting of a 3x3 rotation matrix (R)
370     // concatenated with a 3x1 location vector (t) in row major order. Example: This raw buffer:
371     // {x1,y1,z1,l1,x2,y2,z2,l2,x3,y3,z3,l3} corresponds to the following 3x4 matrix:
372     //  x1 y1 z1 l1
373     //  x2 y2 z2 l2
374     //  x3 y3 z3 l3
375     // LOCATION_X_IDX,LOCATION_Y_IDX,LOCATION_Z_IDX corresponds to the indexes of the location
376     // vector (l1,l2,l3) in the raw buffer.
377     status_t ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_X_IDX,
378                                           sensorLocation.getX());
379     if (ret != OK) return ret;
380     ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Y_IDX,
381                                  sensorLocation.getY());
382     if (ret != OK) return ret;
383     ret = setSensorPlacementData(sensorPlacement, HWSensorBase::LOCATION_Z_IDX,
384                                  sensorLocation.getZ());
385     if (ret != OK) return ret;
386 
387     Orientation& sensorOrientation = (*sensorOrientationList)[0];
388     if (sensorOrientation.getRotate()) {
389         // If the HAL is already rotating the sensor orientation to align with the Android
390         // Coordinate system, then the sensor rotation matrix will be an identity matrix
391         // ROTATION_X_IDX, ROTATION_Y_IDX, ROTATION_Z_IDX corresponds to indexes of the
392         // (x1,y1,z1) in the raw buffer.
393         ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_X_IDX + 0, 1);
394         if (ret != OK) return ret;
395         ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Y_IDX + 4, 1);
396         if (ret != OK) return ret;
397         ret = setSensorPlacementData(sensorPlacement, HWSensorBase::ROTATION_Z_IDX + 8, 1);
398         if (ret != OK) return ret;
399     } else {
400         ret = setSensorPlacementData(
401                 sensorPlacement,
402                 HWSensorBase::ROTATION_X_IDX + 4 * sensorOrientation.getFirstX()->getMap(),
403                 sensorOrientation.getFirstX()->getNegate() ? -1 : 1);
404         if (ret != OK) return ret;
405         ret = setSensorPlacementData(
406                 sensorPlacement,
407                 HWSensorBase::ROTATION_Y_IDX + 4 * sensorOrientation.getFirstY()->getMap(),
408                 sensorOrientation.getFirstY()->getNegate() ? -1 : 1);
409         if (ret != OK) return ret;
410         ret = setSensorPlacementData(
411                 sensorPlacement,
412                 HWSensorBase::ROTATION_Z_IDX + 4 * sensorOrientation.getFirstZ()->getMap(),
413                 sensorOrientation.getFirstZ()->getNegate() ? -1 : 1);
414         if (ret != OK) return ret;
415     }
416     return OK;
417 }
418 
setAdditionalInfoFrames(const std::optional<std::vector<Configuration>> & config)419 status_t HWSensorBase::setAdditionalInfoFrames(
420         const std::optional<std::vector<Configuration>>& config) {
421     AdditionalInfo additionalInfoSensorPlacement;
422     status_t ret = getSensorPlacement(&additionalInfoSensorPlacement, config);
423     if (ret != OK) return ret;
424 
425     const AdditionalInfo additionalInfoBegin = {
426             .type = AdditionalInfoType::AINFO_BEGIN,
427             .serial = 0,
428     };
429     const AdditionalInfo additionalInfoEnd = {
430             .type = AdditionalInfoType::AINFO_END,
431             .serial = 0,
432     };
433 
434     mAdditionalInfoFrames.insert(
435             mAdditionalInfoFrames.end(),
436             {additionalInfoBegin, additionalInfoSensorPlacement, additionalInfoEnd});
437     return OK;
438 }
439 
buildSensor(int32_t sensorHandle,ISensorsEventCallback * callback,const struct iio_device_data & iio_data,const std::optional<std::vector<Configuration>> & config)440 HWSensorBase* HWSensorBase::buildSensor(int32_t sensorHandle, ISensorsEventCallback* callback,
441                                         const struct iio_device_data& iio_data,
442                                         const std::optional<std::vector<Configuration>>& config) {
443     if (checkOrientation(config) != OK) {
444         ALOGE("Orientation of the sensor %s in the configuration file is invalid",
445               iio_data.name.c_str());
446         return nullptr;
447     }
448     if (checkIIOData(iio_data) != OK) {
449         ALOGE("IIO channel index of the sensor %s  is invalid", iio_data.name.c_str());
450         return nullptr;
451     }
452     return new HWSensorBase(sensorHandle, callback, iio_data, config);
453 }
454 
HWSensorBase(int32_t sensorHandle,ISensorsEventCallback * callback,const struct iio_device_data & data,const std::optional<std::vector<Configuration>> & config)455 HWSensorBase::HWSensorBase(int32_t sensorHandle, ISensorsEventCallback* callback,
456                            const struct iio_device_data& data,
457                            const std::optional<std::vector<Configuration>>& config)
458     : SensorBase(sensorHandle, callback, data.type) {
459     std::string buffer_path;
460     mSensorInfo.flags |= SensorFlagBits::CONTINUOUS_MODE;
461     mSensorInfo.name = data.name;
462     mSensorInfo.resolution = data.resolution * data.scale;
463     mSensorInfo.maxRange = data.max_range * data.scale;
464     mSensorInfo.power = 0;
465     mIioData = data;
466     setOrientation(config);
467     status_t ret = setAdditionalInfoFrames(config);
468     if (ret == OK) mSensorInfo.flags |= SensorFlagBits::ADDITIONAL_INFO;
469     unsigned int max_sampling_frequency = 0;
470     unsigned int min_sampling_frequency = UINT_MAX;
471     for (auto i = 0u; i < data.sampling_freq_avl.size(); i++) {
472         if (max_sampling_frequency < data.sampling_freq_avl[i])
473             max_sampling_frequency = data.sampling_freq_avl[i];
474         if (min_sampling_frequency > data.sampling_freq_avl[i])
475             min_sampling_frequency = data.sampling_freq_avl[i];
476     }
477     mSensorInfo.minDelay = frequency_to_us(max_sampling_frequency);
478     mSensorInfo.maxDelay = frequency_to_us(min_sampling_frequency);
479     mScanSize = calculateScanSize();
480     buffer_path = "/dev/iio:device";
481     buffer_path.append(std::to_string(mIioData.iio_dev_num));
482     mPollFdIio.fd = open(buffer_path.c_str(), O_RDONLY | O_NONBLOCK);
483     if (mPollFdIio.fd < 0) {
484         ALOGE("%s: Failed to open iio char device (%s).", data.name.c_str(), buffer_path.c_str());
485         return;
486     }
487     mPollFdIio.events = POLLIN;
488     mSensorRawData.resize(mScanSize);
489 }
490 
491 }  // namespace implementation
492 }  // namespace subhal
493 }  // namespace V2_1
494 }  // namespace sensors
495 }  // namespace hardware
496 }  // namespace android
497