1 /*
2 * Copyright (C) 2018 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 #define LOG_TAG "goog_gyro_direct"
18
19 #include <android/frameworks/sensorservice/1.0/ISensorManager.h>
20 #include <android/frameworks/sensorservice/1.0/types.h>
21 #include <android/hardware/sensors/1.0/types.h>
22 #include <hardware/sensors.h>
23 #include <utils/Log.h>
24 #include <utils/SystemClock.h>
25
26 #include "goog_gralloc_wrapper.h"
27
28 #include "goog_gyro_direct.h"
29
30 namespace android {
31 namespace camera_sensor_listener {
32
33 using ::android::frameworks::sensorservice::V1_0::ISensorManager;
34 using ::android::frameworks::sensorservice::V1_0::Result;
35 using ::android::hardware::graphics::mapper::V3_0::IMapper;
36 using ::android::hardware::sensors::V1_0::RateLevel;
37 using ::android::hardware::sensors::V1_0::SensorFlagBits;
38 using ::android::hardware::sensors::V1_0::SensorsEventFormatOffset;
39 using ::android::hardware::sensors::V1_0::SensorType;
40
GoogGyroDirect(RateLevel rate_level,size_t gyro_direct_buf_length)41 GoogGyroDirect::GoogGyroDirect(RateLevel rate_level,
42 size_t gyro_direct_buf_length)
43 : gyro_direct_initialized_(false),
44 gyro_direct_enabled_(false),
45 gyro_direct_rate_level_(rate_level),
46 gyro_direct_buf_length_(gyro_direct_buf_length),
47 goog_gralloc_wrapper_ptr_(nullptr),
48 gyro_direct_channel_native_buf_handle_(nullptr),
49 gyro_direct_channel_addr_(nullptr) {
50 goog_gralloc_wrapper_ptr_ = std::make_unique<GoogGrallocWrapper>();
51 }
52
~GoogGyroDirect()53 GoogGyroDirect::~GoogGyroDirect() {
54 if (gyro_direct_enabled_) {
55 DisableDirectChannel();
56 }
57 if (gyro_direct_channel_addr_) {
58 goog_gralloc_wrapper_ptr_->Unlock(gyro_direct_channel_native_buf_handle_);
59 }
60 if (gyro_direct_channel_native_buf_handle_) {
61 goog_gralloc_wrapper_ptr_->FreeBuffer(
62 gyro_direct_channel_native_buf_handle_);
63 }
64 }
65
Create(int64_t gyro_sampling_period_us,size_t event_queue_size)66 std::unique_ptr<GoogGyroDirect> GoogGyroDirect::Create(
67 int64_t gyro_sampling_period_us, size_t event_queue_size) {
68 // convert sampling period us to RateLevel.
69 float sampling_rate =
70 static_cast<float>(1e6) / static_cast<float>(gyro_sampling_period_us);
71 RateLevel rate_level;
72 if (sampling_rate < 110.f) {
73 rate_level = RateLevel::NORMAL;
74 } else if (sampling_rate >= 110.f && sampling_rate < 440.f) {
75 rate_level = RateLevel::FAST;
76 } else {
77 rate_level = RateLevel::VERY_FAST;
78 }
79
80 std::unique_ptr<GoogGyroDirect> gyro_direct_ptr =
81 std::unique_ptr<GoogGyroDirect>(
82 new GoogGyroDirect(rate_level, event_queue_size));
83 if (gyro_direct_ptr == nullptr) {
84 ALOGE("%s %d failed to create GoogGyroDirect", __func__, __LINE__);
85 } else {
86 status_t result = gyro_direct_ptr->EnableDirectChannel();
87 if (result != 0) {
88 ALOGE("%s %d failed to enable GoogGyroDirect", __func__, __LINE__);
89 } else {
90 ALOGI("%s %d successfully enabled GoogGyroDirect", __func__, __LINE__);
91 }
92 }
93 return gyro_direct_ptr;
94 }
95
DisableDirectChannel()96 status_t GoogGyroDirect::DisableDirectChannel() {
97 if (gyro_direct_initialized_ && gyro_direct_enabled_) {
98 gyro_direct_channel_->configure(sensor_info_.sensorHandle, RateLevel::STOP,
99 [](const auto&, auto) {});
100 gyro_direct_enabled_ = false;
101 }
102 return OK;
103 }
104
EnableDirectChannel()105 status_t GoogGyroDirect::EnableDirectChannel() {
106 sp<ISensorManager> manager = ISensorManager::getService();
107 if (manager == nullptr) {
108 ALOGE("Cannot get ISensorManager");
109 return UNKNOWN_ERROR;
110 }
111 bool find = false;
112 manager->getDefaultSensor(SensorType::GYROSCOPE,
113 [&](const auto& info, auto result) {
114 if (result != Result::OK) {
115 ALOGE("Cannot find default gyroscope");
116 return;
117 }
118 sensor_info_ = info;
119 find = true;
120 });
121 ALOGV("%s %d gyro sensor handle find %d", __func__, __LINE__, find);
122 if (find == false) {
123 gyro_direct_enabled_ = false;
124 ALOGE("%s %d unable to find gyro sensor", __func__, __LINE__);
125 return UNKNOWN_ERROR;
126 }
127
128 // Initialize gyro direct channel buffer.
129 if (!gyro_direct_initialized_) {
130 bool channel_supported =
131 sensor_info_.flags & SensorFlagBits::DIRECT_CHANNEL_GRALLOC;
132 if (!channel_supported) {
133 ALOGE("%s %d Direct sesnor is not supported", __func__, __LINE__);
134 return UNKNOWN_ERROR;
135 }
136
137 size_t buffer_size =
138 static_cast<size_t>(SensorsEventFormatOffset::TOTAL_LENGTH) *
139 gyro_direct_buf_length_;
140
141 using android::hardware::graphics::common::V1_0::BufferUsage;
142 using android::hardware::graphics::common::V1_2::PixelFormat;
143 IMapper::BufferDescriptorInfo buf_desc_info = {
144 .width = static_cast<uint32_t>(buffer_size),
145 .height = 1,
146 .layerCount = 1,
147 .format = PixelFormat::BLOB,
148 .usage = static_cast<uint64_t>(BufferUsage::SENSOR_DIRECT_DATA |
149 BufferUsage::CPU_READ_OFTEN),
150 };
151
152 gyro_direct_channel_native_buf_handle_ =
153 goog_gralloc_wrapper_ptr_->AllocateOneBuffer(buf_desc_info);
154 if (gyro_direct_channel_native_buf_handle_ == nullptr) {
155 ALOGE("%s %d Failed at allocating the channel native buffer", __func__,
156 __LINE__);
157 return NO_MEMORY;
158 }
159
160 IMapper::Rect region{0, 0, static_cast<int32_t>(buf_desc_info.width),
161 static_cast<int32_t>(buf_desc_info.height)};
162 gyro_direct_channel_addr_ = goog_gralloc_wrapper_ptr_->Lock(
163 gyro_direct_channel_native_buf_handle_, buf_desc_info.usage, region,
164 /*fence=*/-1);
165 if (gyro_direct_channel_addr_ == nullptr) {
166 goog_gralloc_wrapper_ptr_->FreeBuffer(
167 gyro_direct_channel_native_buf_handle_);
168 gyro_direct_channel_native_buf_handle_ = nullptr;
169 ALOGE("%s %d Failed at lock the gralloc buffer", __func__, __LINE__);
170 return UNKNOWN_ERROR;
171 }
172
173 gyro_direct_initialized_ = true;
174 manager->createGrallocDirectChannel(
175 gyro_direct_channel_native_buf_handle_, buffer_size,
176 [&](const auto& chan, auto result) {
177 if (result != Result::OK) {
178 gyro_direct_initialized_ = false;
179 ALOGE("%s %d Failed to create gralloc direct channel", __func__,
180 __LINE__);
181 } else {
182 gyro_direct_channel_ = chan;
183 }
184 });
185 }
186
187 // configure gyro direct channel.
188 gyro_direct_channel_->configure(
189 sensor_info_.sensorHandle, gyro_direct_rate_level_,
190 [&](const auto& t, auto result) {
191 if (result != Result::OK) {
192 ALOGE("%s %d Cannot configure direct report", __func__, __LINE__);
193 }
194 });
195
196 gyro_direct_enabled_ = true;
197
198 ALOGV("%s %d Direct sensor mdoe: %d %d", __func__, __LINE__,
199 gyro_direct_initialized_, gyro_direct_enabled_);
200 return OK;
201 }
202
QueryGyroEventsBetweenTimestamps(int64_t start_time,int64_t end_time,std::vector<int64_t> * event_timestamps,std::vector<float> * motion_vector_x,std::vector<float> * motion_vector_y,std::vector<float> * motion_vector_z,std::vector<int64_t> * event_arrival_timestamps) const203 void GoogGyroDirect::QueryGyroEventsBetweenTimestamps(
204 int64_t start_time, int64_t end_time,
205 std::vector<int64_t>* event_timestamps, std::vector<float>* motion_vector_x,
206 std::vector<float>* motion_vector_y, std::vector<float>* motion_vector_z,
207 std::vector<int64_t>* event_arrival_timestamps) const {
208 event_timestamps->clear();
209 motion_vector_x->clear();
210 motion_vector_y->clear();
211 motion_vector_z->clear();
212 event_arrival_timestamps->clear();
213
214 if (gyro_direct_channel_addr_ == nullptr) {
215 return;
216 }
217 sensors_event_t* buffer_head_ptr =
218 reinterpret_cast<sensors_event_t*>(gyro_direct_channel_addr_);
219
220 // Copy shared buffer content to local buffer as lock is lacking.
221 std::vector<sensors_event_t> event_vector;
222 event_vector.insert(event_vector.begin(), buffer_head_ptr,
223 buffer_head_ptr + gyro_direct_buf_length_);
224
225 int64_t event_arrival_time = elapsedRealtimeNano();
226 int64_t earliest_timestamp = LLONG_MAX;
227 int head_pos = -1;
228 int num_events = event_vector.size();
229 for (int i = 0; i < num_events; ++i) {
230 if (event_vector[i].timestamp != 0 &&
231 event_vector[i].timestamp < earliest_timestamp) {
232 earliest_timestamp = event_vector[i].timestamp;
233 head_pos = i;
234 }
235 }
236
237 // Fill events within timestamps range to output vectors.
238 if (head_pos == -1) {
239 return;
240 }
241 for (int i = 0; i < num_events; ++i) {
242 int index = (head_pos + i) % num_events;
243 if (event_vector[index].timestamp <= start_time) {
244 continue;
245 }
246 if (event_vector[index].timestamp > end_time) {
247 break;
248 }
249
250 event_timestamps->push_back(event_vector[index].timestamp);
251 motion_vector_x->push_back(event_vector[index].data[0]);
252 motion_vector_y->push_back(event_vector[index].data[1]);
253 motion_vector_z->push_back(event_vector[index].data[2]);
254 event_arrival_timestamps->push_back(event_arrival_time);
255 }
256 }
257
258 } // namespace camera_sensor_listener
259 } // namespace android
260