1 /*
2 * eis_algo_service.h
3 *
4 * Copyright (c) 2021 Rockchip Electronics Co., Ltd.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * Author: Cody Xie <cody.xie@rock-chips.com>
19 */
20 #include "eis_algo_service.h"
21
22 #include <fstream>
23 #include <functional>
24 #include <iostream>
25 #include <memory>
26 #include <vector>
27
28 #include "Stream.h"
29 #include "dma_buffer.h"
30 #include "dvs_app.h"
31 #include "eis_loader.h"
32 #include "image_processor.h"
33 #include "imu_service.h"
34 #include "rk_aiq_mems_sensor.h"
35 #include "smart_buffer_priv.h"
36 #include "xcam_log.h"
37
38 #define __STDC_FORMAT_MACROS
39 #include <inttypes.h>
40
41 #define SUCCESS 0
42 #define ERROR 1
43
44 //#define DEBUG_FEC
45
46 using namespace XCam;
47
48 namespace RkCam {
49
50 namespace {
51
52 template <typename T>
53 struct Callback;
54
55 template <typename Ret, typename... Params>
56 struct Callback<Ret(Params...)> {
57 template <typename... Args>
callbackRkCam::__anon4d6f15bc0111::Callback58 static Ret callback(Args... args) {
59 return func(args...);
60 }
61 static std::function<Ret(Params...)> func;
62 };
63
64 template <typename Ret, typename... Params>
65 std::function<Ret(Params...)> Callback<Ret(Params...)>::func;
66
67 } // namespace
68
~EisAlgoAdaptor()69 EisAlgoAdaptor::~EisAlgoAdaptor() {
70 Stop();
71
72 if (lib_ != nullptr && engine_ != nullptr) {
73 lib_->GetOps()->DeInit(engine_.get());
74 }
75 }
76
LoadLibrary()77 XCamReturn EisAlgoAdaptor::LoadLibrary() {
78 lib_ = std::make_shared<DvsLibrary>();
79
80 if (!lib_->Init()) {
81 return XCAM_RETURN_ERROR_FAILED;
82 }
83
84 if (!lib_->LoadSymbols()) {
85 return XCAM_RETURN_ERROR_FAILED;
86 }
87
88 return XCAM_RETURN_NO_ERROR;
89 }
90
CreateImuService(const rk_aiq_mems_sensor_intf_t * mems_sensor_intf)91 XCamReturn EisAlgoAdaptor::CreateImuService(const rk_aiq_mems_sensor_intf_t* mems_sensor_intf) {
92 if (mems_sensor_intf == nullptr) {
93 return XCAM_RETURN_ERROR_PARAM;
94 }
95
96 auto adaptor = std::make_shared<EisImuAdaptor>(*mems_sensor_intf);
97 if (XCAM_RETURN_NO_ERROR == adaptor->Init(1000.0)) {
98 imu_ = std::unique_ptr<ImuService>(
99 new ImuService(std::unique_ptr<ImuTask>(new ImuTask(std::move(adaptor))), false, 1,
100 std::chrono::milliseconds(10)));
101 } else {
102 imu_ = nullptr;
103 return XCAM_RETURN_ERROR_PARAM;
104 }
105
106 return XCAM_RETURN_NO_ERROR;
107 }
108
CreateScalerService()109 XCamReturn EisAlgoAdaptor::CreateScalerService() {
110 std::unique_ptr<ImageProcessor> proc(new ImageProcessor());
111 proc->set_operator("rga");
112 scl_ = std::unique_ptr<ScalerService>(
113 new ScalerService(std::unique_ptr<ScalerTask>(new ScalerTask(std::move(proc))), true, 7));
114
115 return XCAM_RETURN_NO_ERROR;
116 }
117
CreateFecRemapBackend(const FecMeshConfig & config,const isp_drv_share_mem_ops_t * mem_ops)118 XCamReturn EisAlgoAdaptor::CreateFecRemapBackend(const FecMeshConfig& config,
119 const isp_drv_share_mem_ops_t* mem_ops) {
120 XCAM_ASSERT(mem_ops != nullptr);
121
122 remap_ = std::unique_ptr<FecRemapBackend>(new FecRemapBackend(config, mem_ops));
123
124 return XCAM_RETURN_NO_ERROR;
125 }
126
OnMeshCallback(struct dvsEngine * engine,struct meshxyFEC * mesh)127 int EisAlgoAdaptor::OnMeshCallback(struct dvsEngine* engine, struct meshxyFEC* mesh) {
128 XCAM_ASSERT(mesh != nullptr);
129
130 LOGD_AEIS("OnMeshCallback got img id %d , mesh idx %d, img idx %d", mesh->image_index,
131 mesh->mesh_buffer_index, mesh->image_buffer_index);
132 remap_->Remap(mesh);
133 if (lib_ != nullptr) {
134 auto* new_mesh = remap_->GetAvailUserBuffer();
135 if (new_mesh != nullptr) {
136 auto dvs_new_mesh = dvs_meshes_.find(new_mesh->Index);
137 if (dvs_new_mesh != dvs_meshes_.end()) {
138 LOGD_AEIS("OnMeshCallBack push back available mesh id %d",
139 dvs_new_mesh->second->mesh_buffer_index);
140 lib_->GetOps()->PutMesh(engine_.get(), dvs_new_mesh->second.get());
141 }
142 }
143 }
144
145 auto img = dvs_images_.find(mesh->image_index);
146 if (img != dvs_images_.end()) {
147 dvs_images_.erase(img);
148 } else {
149 return 1;
150 }
151 return 0;
152 }
153
Config(const AlgoCtxInstanceCfg * config,const CalibDbV2_Eis_t * calib)154 XCamReturn EisAlgoAdaptor::Config(const AlgoCtxInstanceCfg* config,
155 const CalibDbV2_Eis_t* calib) {
156 calib_ = calib;
157 enable_ = calib_->enable;
158
159 if (config->isp_hw_version == 1) {
160 valid_ = false;
161 LOGE_AEIS("EIS does not compatible with ISP21");
162 return XCAM_RETURN_BYPASS;
163 }
164
165 if (XCAM_RETURN_NO_ERROR != LoadLibrary()) {
166 LOGE_AEIS("EIS library does not exists");
167 valid_ = false;
168 return XCAM_RETURN_BYPASS;
169 }
170
171 valid_ = true;
172
173 return XCAM_RETURN_NO_ERROR;
174 }
175
Prepare(const rk_aiq_mems_sensor_intf_t * mems_sensor_intf,const isp_drv_share_mem_ops_t * mem_ops)176 XCamReturn EisAlgoAdaptor::Prepare(const rk_aiq_mems_sensor_intf_t* mems_sensor_intf,
177 const isp_drv_share_mem_ops_t* mem_ops) {
178 if (!calib_->enable && !enable_) {
179 return XCAM_RETURN_NO_ERROR;
180 }
181
182 if (!valid_) {
183 LOGE_AEIS("EIS Invalid, bypassing!");
184 return XCAM_RETURN_BYPASS;
185 }
186
187 XCamReturn ret;
188
189 if (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG) {
190 ret = CreateImuService(mems_sensor_intf);
191 if (ret != XCAM_RETURN_NO_ERROR) {
192 valid_ = false;
193 LOGE_AEIS("EIS IMU interface invalid, bypassing!");
194 return XCAM_RETURN_BYPASS;
195 }
196 }
197
198 if (calib_->mode == EIS_MODE_IMG_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG) {
199 ret = CreateScalerService();
200 if (ret != XCAM_RETURN_NO_ERROR) {
201 valid_ = false;
202 if (calib_->mode == EIS_MODE_IMU_AND_IMG) {
203 imu_ = nullptr;
204 }
205 LOGE_AEIS("EIS scaler interface invalid, bypassing!");
206 return XCAM_RETURN_BYPASS;
207 }
208 }
209
210 int mesh_size;
211 lib_->GetOps()->GetMeshSize(calib_->src_image_height, calib_->src_image_width, &mesh_size);
212 FecMeshConfig FecCfg;
213 FecCfg.Width = calib_->src_image_width;
214 FecCfg.Height = calib_->src_image_height;
215 if (FecCfg.Width <= 1920)
216 FecCfg.MeshDensity = 0;
217 else
218 FecCfg.MeshDensity = 1;
219 FecCfg.MeshSize = mesh_size;
220
221 // TODO(Cody): FEC may support different src and dst image width/height
222 ret = CreateFecRemapBackend(FecCfg, mem_ops);
223 if (ret != XCAM_RETURN_NO_ERROR) {
224 valid_ = false;
225 LOGE_AEIS("EIS remap backend invalid, bypassing!");
226 return XCAM_RETURN_BYPASS;
227 }
228
229 // TODO(Cody): width/height may change during runtime
230 engine_ = std::unique_ptr<dvsEngine>(new dvsEngine());
231
232 lib_->GetOps()->Prepare(engine_.get());
233
234 initialParams init_params;
235 init_params.image_buffer_number = 7;
236 init_params.input_image_size.width = calib_->src_image_width;
237 init_params.input_image_size.height = calib_->src_image_height;
238 init_params.output_image_size.width = calib_->src_image_width;
239 init_params.output_image_size.height = calib_->src_image_width;
240 init_params.clip_ratio_x = calib_->clip_ratio_x;
241 init_params.clip_ratio_y = calib_->clip_ratio_y;
242 lib_->GetOps()->InitParams(engine_.get(), &init_params);
243
244 if (!lib_->GetOps()->InitFromXmlFile(engine_.get(), calib_->debug_xml_path)) {
245 valid_ = false;
246 LOGE_AEIS("EIS init algo from xml failed, bypassing!");
247 return XCAM_RETURN_BYPASS;
248 }
249
250 for (int i = 0; i < 7; i++) {
251 auto* mesh = remap_->AllocUserBuffer();
252 struct meshxyFEC* dvs_mesh = new meshxyFEC;
253 dvs_mesh->is_skip = false;
254 dvs_mesh->image_buffer_index = mesh->ImageBufferIndex;
255 dvs_mesh->image_index = mesh->FrameId;
256 dvs_mesh->mesh_buffer_index = mesh->Index;
257 dvs_mesh->mesh_size = remap_->GetConfig().MeshSize;
258 dvs_mesh->pMeshXF = mesh->MeshXf;
259 dvs_mesh->pMeshXI = mesh->MeshXi;
260 dvs_mesh->pMeshYF = mesh->MeshYf;
261 dvs_mesh->pMeshYI = mesh->MeshYi;
262 dvs_meshes_.emplace(dvs_mesh->mesh_buffer_index, dvs_mesh);
263 remap_meshes_.emplace(mesh->Index, mesh);
264 if (i == 6) {
265 dvs_mesh->image_index = mesh->FrameId = -1;
266 lib_->GetOps()->GetOriginalMeshXY(calib_->src_image_width, calib_->src_image_height,
267 calib_->clip_ratio_x, calib_->clip_ratio_y, dvs_mesh);
268 remap_->Remap(dvs_mesh);
269 default_mesh_ = mesh;
270 } else {
271 lib_->GetOps()->PutMesh(engine_.get(), dvs_mesh);
272 }
273 }
274
275 Callback<int(struct dvsEngine*, struct meshxyFEC*)>::func = std::bind(
276 &EisAlgoAdaptor::OnMeshCallback, this, std::placeholders::_1, std::placeholders::_2);
277 dvsFrameCallBackFEC mesh_cb = static_cast<dvsFrameCallBackFEC>(
278 Callback<int(struct dvsEngine*, struct meshxyFEC*)>::callback);
279 lib_->GetOps()->RegisterRemap(engine_.get(), mesh_cb);
280
281 return XCAM_RETURN_NO_ERROR;
282 }
283
Start()284 void EisAlgoAdaptor::Start() {
285 if (started_ || !valid_) {
286 return;
287 }
288
289 if (imu_ != nullptr &&
290 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
291 imu_->start();
292 }
293
294 if (scl_ != nullptr &&
295 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
296 scl_->start();
297 }
298
299 if (lib_->GetOps()->Start(engine_.get())) {
300 lib_->GetOps()->DeInit(engine_.get());
301 goto error_out;
302 }
303
304 started_ = true;
305 return;
306
307 error_out:
308 if (imu_ != nullptr &&
309 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
310 imu_->stop();
311 imu_ = nullptr;
312 }
313
314 if (scl_ != nullptr &&
315 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
316 scl_->stop();
317 scl_ = nullptr;
318 }
319
320 started_ = false;
321 engine_ = nullptr;
322 valid_ = false;
323 }
324
OnFrameEvent(const RkAiqAlgoProcAeis * input)325 void EisAlgoAdaptor::OnFrameEvent(const RkAiqAlgoProcAeis* input) {
326 if (input->orb_stats_buf == nullptr || input->nr_img_buf == nullptr) {
327 // valid_ = false;
328 LOGE_AEIS("EIS process gets no orb stats/nr image, bypassing!");
329 return;
330 }
331 RkAiqOrbStats* orbStats =
332 reinterpret_cast<RkAiqOrbStats*>(input->orb_stats_buf->map(input->orb_stats_buf));
333 SmartPtr<SubVideoBuffer> nrImg = reinterpret_cast<SmartBufferPriv*>(input->nr_img_buf)
334 ->get_video_buffer()
335 .dynamic_cast_ptr<SubVideoBuffer>();
336 auto id = orbStats->orb_stats.frame_id;
337 auto nr_idx = nrImg->get_index();
338 auto nr_num = nrImg->get_buf_num();
339 auto nr_fd = nrImg->get_fd();
340
341 LOGV_AEIS("OnFrameEvent id %d idx %d fd %d sof %" PRId64 " skew %lf igt %f ag %d fw %u fh %u mode %d",
342 id, nr_idx, nr_fd, input->sof, input->rolling_shutter_skew,
343 input->integration_time, input->analog_gain, input->frame_width,
344 input->frame_height, calib_->mode);
345 if (imu_ != nullptr &&
346 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
347 auto p = imu_->dequeue();
348 if (p.state == ParamState::kAllocated) {
349 auto& param = p.payload;
350 param->frame_id = id;
351 p.unique_id = id;
352 imu_->enqueue(p);
353 } else if (p.state == ParamState::kProcessedSuccess ||
354 p.state == ParamState::kProcessedError) {
355 auto& param = p.payload;
356 if (param->data != nullptr) {
357 LOGD_AEIS("IMU-%d: get data state %d id %d count %d %" PRIu64 "", p.unique_id,
358 p.state, param->frame_id, param->data->GetCount(),
359 (param->data->GetData())[param->data->GetCount() - 1].timestamp_us);
360 lib_->GetOps()->PutImuFrame(engine_.get(),
361 (mems_sensor_event_t*)param->data->GetData(),
362 param->data->GetCount());
363 param->data.reset();
364 }
365 param->frame_id = id;
366 p.unique_id = id;
367 imu_->enqueue(p);
368 }
369 }
370
371 if (nr_idx < 0) {
372 LOGW_AEIS("Process %d frame has invalid frame idx %d", orbStats->orb_stats.frame_id,
373 nr_idx);
374 return;
375 }
376
377 image_indexes_[nr_idx] = nr_fd;
378
379 if (scl_ != nullptr &&
380 (calib_->mode == EIS_MODE_IMG_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
381 // TODO(Cody): dequeue and enqueue scaler param
382 }
383
384 struct imageData* image = new imageData();
385 if (image != nullptr) {
386 auto& meta = image->meta_data;
387 meta.iso_speed = input->analog_gain;
388 meta.exp_time = input->integration_time;
389 meta.rolling_shutter_skew = input->rolling_shutter_skew / 1000000000;
390 meta.zoom_ratio = 1;
391 meta.timestamp_sof_us = input->sof / 1000;
392
393 image->buffer_index = nr_idx;
394 image->frame_index = orbStats->orb_stats.frame_id;
395
396 dvs_images_.emplace(orbStats->orb_stats.frame_id, image);
397 }
398
399 #if DEBUG
400 if (image != nullptr) {
401 const char* dump_env = std::getenv("eis_dump_imu");
402 int dump = 0;
403 if (dump_env) {
404 dump = atoi(dump_env);
405 }
406 if (dump > 0) {
407 std::ofstream ofs("/data/img.txt", std::ios::app);
408 if (ofs.is_open()) {
409 ofs << image->frame_index << "," << image->buffer_index << ","
410 << image->meta_data.timestamp_sof_us << std::endl;
411 }
412 ofs.close();
413 }
414 LOGD_AEIS("Put img frame id %d idx %d ts %" PRId64 "", image->frame_index,
415 image->buffer_index, image->meta_data.timestamp_sof_us);
416 #endif
417 lib_->GetOps()->PutImageFrame(engine_.get(), image);
418 }
419 }
420
GetProcResult(RkAiqAlgoProcResAeis * output)421 void EisAlgoAdaptor::GetProcResult(RkAiqAlgoProcResAeis* output) {
422 auto* mesh = remap_->GetPendingHwResult();
423 auto config = remap_->GetConfig();
424 if (mesh != nullptr) {
425 LOGD_AEIS("Got DVS result : id %u, idx %d, fd %d", mesh->FrameId, mesh->ImageBufferIndex,
426 mesh->Fd);
427 output->update = 1;
428 output->frame_id = mesh->FrameId > 0 ? mesh->FrameId : 0;
429 output->img_buf_index = mesh->ImageBufferIndex;
430 if (!image_indexes_.empty()) {
431 output->img_size = image_indexes_[mesh->ImageBufferIndex];
432 } else {
433 output->img_size = 0;
434 }
435 #ifdef DEBUG
436 if (mesh->FrameId == 1) remap_->WriteMeshToFile(mesh);
437 #endif
438 output->fd = mesh->Fd;
439 output->mesh_size = config.MeshSize;
440 output->mesh_density = config.MeshDensity;
441 if (mesh->Fd >= 0)
442 delete mesh;
443 else
444 remap_->FreeUserBuffer(mesh);
445 } else {
446 output->update = 0;
447 output->fd = -1;
448 }
449 }
450
Stop()451 void EisAlgoAdaptor::Stop() {
452 if (!started_ || !valid_) {
453 return;
454 }
455
456 if (imu_ != nullptr &&
457 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
458 imu_.reset();
459 }
460
461 if (scl_ != nullptr &&
462 (calib_->mode == EIS_MODE_IMU_ONLY || calib_->mode == EIS_MODE_IMU_AND_IMG)) {
463 scl_.reset();
464 }
465
466 if (lib_ != nullptr && engine_ != nullptr) {
467 lib_->GetOps()->RequestStop(engine_.get());
468 }
469 started_ = false;
470 }
471
472 } // namespace RkCam
473