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