cam_demo_dual_ir.c 48 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584
  1. /*
  2. * Copyright (C) 2021 Alibaba Group Holding Limited
  3. * Author: ShenWuYi <shenwuyi.swy@alibaba-inc.com>
  4. *
  5. * This program is free software; you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License version 2 as
  7. * published by the Free Software Foundation.
  8. */
  9. #include <stdio.h>
  10. #include <stdlib.h>
  11. #include <string.h>
  12. #include <fcntl.h>
  13. #include <unistd.h>
  14. #define LOG_LEVEL 5
  15. #define LOG_PREFIX "cam_demo_dual_ir"
  16. #include <syslog.h>
  17. #include <csi_frame_ex.h>
  18. #include <csi_camera.h>
  19. #include <csi_camera_dev_api.h>
  20. #include "list.h"
  21. #include "video_mem.h"
  22. #include "csi_dsp_api.h"
  23. #include "csi_dsp_task_defs.h"
  24. #include "csi_dsp_post_process_defs.h"
  25. #ifdef PLATFORM_SIMULATOR
  26. #include "apputilities.h"
  27. #endif
  28. #define MAX_CAM_NUM 3
  29. #define MAX_CHANNEL_NUM 3
  30. #define TEST_DEVICE_NAME "/dev/video0"
  31. #define CSI_CAMERA_TRUE 1
  32. #define CSI_CAMERA_FALSE 0
  33. /*************************************common data struct************************************************************************/
  34. typedef struct msg_queue_item{
  35. void* payload;
  36. struct msg_queue_item *next;
  37. }msg_queue_item_t;
  38. typedef struct msg_queue{
  39. msg_queue_item_t *head;
  40. msg_queue_item_t *tail;
  41. int exit;
  42. pthread_mutex_t mutex;
  43. pthread_cond_t cond;
  44. }msg_queue_ctx_t;
  45. typedef struct cam_dsp_info{
  46. int master_fd;
  47. int slave_fd;
  48. /**add AE ctrl info********/
  49. }cam_dsp_info_t;
  50. /*************************************camera data struct*************************************************************/
  51. typedef enum{
  52. CAM_SYNC_MSG_MAIN_FRAME,
  53. CAM_SYNC_MSG_SLAVE_FRAME,
  54. CAM_SYNC_MSG_AI_INFO,
  55. CAM_SYNC_MSG_DSP_INFO,
  56. }cam_sync_message_type_e;
  57. typedef struct{
  58. }cam_ai_info_t;
  59. typedef struct camera_sync_msg{
  60. cam_sync_message_type_e type;
  61. union{
  62. csi_frame_ex_s frame;
  63. cam_ai_info_t ai_info;
  64. cam_dsp_info_t dsp_info;
  65. };
  66. }camera_sync_msg_t;
  67. typedef enum _cam_type{
  68. CAM_TYEP_MASTER =0,
  69. CAM_TYEP_SLAVE,
  70. CAM_TYEP_INVALID,
  71. }cam_type_e;
  72. typedef enum _frame_mode{
  73. FRAME_NONE =0,
  74. FRAME_SAVE_IMG =1,
  75. FRAME_SAVE_STREAM =2,
  76. FRAME_SAVE_DISPLAY,
  77. FRAME_INVALID,
  78. }frame_mode_t;
  79. typedef struct _camera_param{
  80. int video_id;
  81. int channel_num;
  82. struct {
  83. int width;
  84. int height;
  85. csi_pixel_fmt_e fmt;
  86. }out_pic[MAX_CHANNEL_NUM];
  87. cam_type_e type;
  88. int frames_to_stop;
  89. }camera_param_t;
  90. typedef struct event_queue_item{
  91. struct csi_camera_event evet;
  92. struct event_queue_item *next;
  93. }event_queue_item_t;
  94. typedef struct _camera_ctx{
  95. int cam_id;
  96. // pthread_t cam_thread;
  97. int exit;
  98. msg_queue_ctx_t *cam_sync_queue;
  99. csi_cam_handle_t cam_handle;
  100. int channel_num;
  101. csi_cam_event_handle_t event_handle;
  102. csi_camera_event_subscription_s event_subscribe;
  103. csi_camera_channel_cfg_s chn_cfg[MAX_CHANNEL_NUM];
  104. int frame_num;
  105. struct timeval init_time;
  106. float fps;
  107. // frame_mode_t frame_mode[MAX_CHANNEL_NUM];
  108. }camera_ctx_t;
  109. /********************************dsp data struct*********************************************************/
  110. #define MAX_DSP_DEVICE 2
  111. typedef struct algo_pic_out_setting{
  112. enum{
  113. ALGO_PIC_MODE_FULL_COPY,
  114. ALGO_PIC_MODE_FULL_COPY_WITH_PARAM,
  115. ALGO_PIC_MODE_FULL_COPY_NONE,
  116. }mode;
  117. int buf_id;
  118. int h_offset;
  119. int height;
  120. uint32_t frame_id;
  121. uint64_t timestap;
  122. uint32_t temp_projector; //投射器温度
  123. }algo_pic_out_setting_t;
  124. typedef struct algo_result{
  125. char lib_name[16];
  126. uint64_t timestap;
  127. }algo_result_t;
  128. typedef struct dsp_dual_ir_frame_msg{
  129. size_t size;
  130. uint32_t width;
  131. uint32_t height;
  132. uint32_t master_stride;
  133. uint32_t slave_stride;
  134. csi_pixel_fmt_e pix_format;
  135. int master_fd;
  136. int slave_fd;
  137. uint32_t frame_id;
  138. uint64_t timestap;
  139. uint32_t temp_projector; //投射器温度
  140. }dsp_dual_ir_frame_msg_t;
  141. typedef struct dsp_ctx{
  142. int dsp_id;
  143. void * instance;
  144. void* dsp_task;
  145. char* algo_name;
  146. }dsp_ctx_t;
  147. typedef enum {
  148. DUAL_DSP_ALGO_TYPE_DSP0_COPY =0,
  149. DUAL_DSP_ALGO_TYPE_DSP1_COPY,
  150. DUAL_DSP_ALGO_TYPE_DSP01_COPY,
  151. }dual_dsp_algo_type_e;
  152. typedef struct dual_dsp_handle{
  153. dsp_ctx_t dsp_ctx[MAX_DSP_DEVICE];
  154. msg_queue_ctx_t dsp_prcoess_queue;
  155. msg_queue_ctx_t *cam_sync_queue;
  156. void *mem_allocor;
  157. pthread_t thread_dsp_process;
  158. frame_mode_t frame_mode;
  159. dual_dsp_algo_type_e dsp_run_type;
  160. }dual_dsp_handle_t;
  161. int file_id = 0;
  162. extern void *vi_plink_create(csi_camera_channel_cfg_s *chn_cfg);
  163. extern void vi_plink_release(void * plink);
  164. extern void display_camera_frame(void * plink, csi_frame_ex_s *frame);
  165. static void *cam_frame_sync_process(void *ctx);
  166. static void* dsp_dual_ir_porcess(void *arg);
  167. /*******************************common func***********************************************************************/
  168. static void usage(void)
  169. {
  170. printf(" 1 : IR 1 Camera id \n");
  171. printf(" 2 : IR 2 Camera id \n");
  172. printf(" 3 : runing Camera frame num :0 not stop ,else frame num to stop \n");
  173. printf(" 4 : DSP frame mode :0 none ,1 dump enable,2 display \n");
  174. }
  175. void printUsage(char *name)
  176. {
  177. printf("usage: %s [options]\n"
  178. "\n"
  179. " Available options:\n"
  180. " -m master camera id \n"
  181. " -s slave camera device id)\n"
  182. " -w width (default: 800))\n"
  183. " -h height (default: 1280))\n"
  184. " -n runing Camera frame num :0 not stop ,else frame num to stop )\n"
  185. " -M DSP frame mode :0 none ,1 dump enable,2 display)\n"
  186. " -a DSP algo mode :0 catch master frame ,1 catch slave frame, 2 catch each half frame)\n"
  187. "\n", name);
  188. }
  189. static int parseParams(int argc, char **argv, camera_param_t *params,frame_mode_t *mode,dual_dsp_algo_type_e *dsp_type)
  190. {
  191. int index =0;
  192. int i = 1;
  193. params[index].video_id = 0;
  194. params[index].type = CAM_TYEP_MASTER;
  195. params[index].channel_num =1;
  196. params[index].out_pic[0].width = 1920;
  197. params[index].out_pic[0].height = 1088;
  198. params[index].out_pic[0].fmt = CSI_PIX_FMT_RAW_10BIT;
  199. params[index].frames_to_stop = 30;
  200. index++;
  201. params[index].video_id = 15;
  202. params[index].type = CAM_TYEP_SLAVE;
  203. params[index].channel_num = 1;
  204. params[index].out_pic[0].width = 1920;
  205. params[index].out_pic[0].height = 1088;
  206. params[index].out_pic[0].fmt =CSI_PIX_FMT_RAW_10BIT;
  207. params[index].frames_to_stop = 30;
  208. index++;
  209. *mode = FRAME_NONE;
  210. *dsp_type = DUAL_DSP_ALGO_TYPE_DSP01_COPY;
  211. while (i < argc)
  212. {
  213. if (argv[i][0] != '-' || strlen(argv[i]) < 2)
  214. {
  215. i++;
  216. continue;
  217. }
  218. if (argv[i][1] == 'm')
  219. {
  220. if (++i < argc)
  221. params[0].video_id = atoi(argv[i++]);
  222. }
  223. else if (argv[i][1] == 's')
  224. {
  225. if (++i < argc)
  226. params[1].video_id = atoi(argv[i++]);
  227. }
  228. else if (argv[i][1] == 'w')
  229. {
  230. if (++i < argc)
  231. params[0].out_pic[0].width = atoi(argv[i++]);
  232. params[1].out_pic[0].width = params[0].out_pic[0].width;
  233. }
  234. else if (argv[i][1] == 'h')
  235. {
  236. if (++i < argc)
  237. params[0].out_pic[0].height = atoi(argv[i++]);
  238. params[1].out_pic[0].height = params[0].out_pic[0].height;
  239. }
  240. else if (argv[i][1] == 'n')
  241. {
  242. if (++i < argc)
  243. params[0].frames_to_stop = atoi(argv[i++]);
  244. params[1].frames_to_stop = params[0].frames_to_stop;
  245. }
  246. else if (argv[i][1] == 'M')
  247. {
  248. if (++i < argc)
  249. *mode = atoi(argv[i++]);
  250. }
  251. else if (argv[i][1] == 'a')
  252. {
  253. if (++i < argc)
  254. *dsp_type = atoi(argv[i++]);
  255. }
  256. else if (strcmp(argv[i], "--help") == 0)
  257. {
  258. printUsage(argv[0]);
  259. return -1;
  260. }
  261. }
  262. printf("[DUAL IR] master camera : %d\n", params[0].video_id);
  263. printf("[DUAL IR] master camera : %d\n",params[1].video_id);
  264. printf("[DUAL IR] Resolution : %dx%d\n", params[0].out_pic[0].width,params[0].out_pic[0].height);
  265. printf("[DUAL IR] run frame : %d\n", params[0].frames_to_stop);
  266. printf("[DUAL IR] frame mode : %d\n", *mode);
  267. printf("[DUAL IR] dsp run type : %d\n", *dsp_type);
  268. return index;
  269. }
  270. static void get_system_time(const char *func, int line_num)
  271. {
  272. struct timeval cur_time;
  273. memset(&cur_time, 0, sizeof(cur_time));
  274. gettimeofday(&cur_time, 0);
  275. LOG_D("%s %s line_num = %d, cur_time.tv_sec = %ld, cur_time.tv_usec = %ld\n",
  276. __func__, func, line_num, cur_time.tv_sec, cur_time.tv_usec);
  277. }
  278. static msg_queue_item_t *dequeue_msg(msg_queue_ctx_t *ctx)
  279. {
  280. msg_queue_item_t *ev = ctx->head;
  281. pthread_mutex_lock(&ctx->mutex);
  282. if(!ev)
  283. {
  284. pthread_mutex_unlock(&ctx->mutex);
  285. return NULL;
  286. }
  287. if(ev == ctx->tail)
  288. ctx->tail = NULL;
  289. ctx->head = ev->next;
  290. pthread_mutex_unlock(&ctx->mutex);
  291. return ev;
  292. }
  293. static int enqueue_msg(msg_queue_ctx_t *ctx,msg_queue_item_t * item)
  294. {
  295. if(!item || !ctx)
  296. return -1;
  297. item->next = NULL;
  298. LOG_D("%s enter \n", __func__);
  299. pthread_mutex_lock(&ctx->mutex);
  300. if (ctx->tail) {
  301. ctx->tail->next = item;
  302. } else {
  303. ctx->head = item;
  304. // pthread_cond_broadcast(&ctx->cond);
  305. }
  306. ctx->tail = item;
  307. pthread_mutex_unlock(&ctx->mutex);
  308. return 0;
  309. }
  310. /***********************DSP Dual-IR process *******************************/
  311. static void dsp_dump_frame(void *frame_buf, csi_pixel_fmt_e fmt, int width,int height,int stride)
  312. {
  313. char file[128];
  314. static int file_indx=0;
  315. int size;
  316. uint32_t indexd, j;
  317. sprintf(file,"demo_save_img_%d",file_indx++%10);
  318. int fd = open(file, O_RDWR | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IROTH);
  319. if(fd == -1) {
  320. LOG_E("%s, %d, open file error!!!!!!!!!!!!!!!\n", __func__, __LINE__);
  321. return;
  322. }
  323. LOG_O("save img from to %s, fmt:%d width:%d stride:%d height:%d \n",file, fmt, width, stride,height);
  324. switch(fmt) {
  325. default:
  326. size = width*2;
  327. for (j = 0; j < height; j++) {
  328. indexd = j*stride;
  329. write(fd, frame_buf + indexd, size);
  330. }
  331. }
  332. close(fd);
  333. }
  334. static void* req_DmaBuffer(void*mem_allocor,VmemParams *params)
  335. {
  336. int pgsize = getpagesize();
  337. params->size = ((params->size+pgsize-1)/pgsize)*pgsize;
  338. if(VMEM_allocate(mem_allocor, params))
  339. {
  340. return NULL;
  341. }
  342. LOG_O("alloct dma buf @ phy:0x%x\n",params->phy_address);
  343. if(VMEM_export(mem_allocor,params))
  344. {
  345. return NULL;
  346. }
  347. LOG_D("export dma buf @fd:%x\n",params->fd);
  348. if(VMEM_mmap(mem_allocor,params))
  349. {
  350. return NULL;
  351. }
  352. memset(params->vir_address,0xff,params->size);
  353. return params->vir_address;
  354. }
  355. static int release_DmaBuffer(void*mem_allocor,VmemParams *params)
  356. {
  357. VMEM_free(mem_allocor, params);
  358. }
  359. static int dsp_construct_task(dsp_ctx_t *ctx)
  360. {
  361. if(ctx== NULL || ctx->dsp_id >= MAX_DSP_DEVICE || ctx->dsp_id <0 || ctx->algo_name == NULL)
  362. {
  363. LOG_E("prama check fail\n");
  364. goto one_err;
  365. }
  366. ctx->instance = csi_dsp_create_instance(ctx->dsp_id);
  367. if (ctx->instance==NULL) {
  368. LOG_E("dsp create fail\n");
  369. goto one_err;
  370. }
  371. ctx->dsp_task = csi_dsp_create_task(ctx->instance, CSI_DSP_TASK_SW_TO_SW);
  372. if (ctx->dsp_task==NULL) {
  373. LOG_E("dsp task create fail.\n");
  374. goto two_err;
  375. }
  376. if(csi_dsp_task_acquire_algo(ctx->dsp_task,ctx->algo_name))
  377. {
  378. LOG_E("dsp task create fail.\n");
  379. goto three_err;
  380. }
  381. return 0;
  382. three_err:
  383. csi_dsp_destroy_task(ctx->dsp_task);
  384. two_err:
  385. csi_dsp_delete_instance(ctx->instance);
  386. one_err:
  387. return -1;
  388. }
  389. static int dsp_release_task(dsp_ctx_t *ctx)
  390. {
  391. if(ctx==NULL || ctx->instance || ctx->dsp_task)
  392. {
  393. return -1;
  394. }
  395. csi_dsp_destroy_task(ctx->instance);
  396. csi_dsp_delete_instance(ctx->instance);
  397. }
  398. static int dsp_send_info_to_cam(msg_queue_ctx_t* cam_msg_queue,int master_fd,int slave_fd)
  399. {
  400. camera_sync_msg_t * msg_payload;
  401. msg_queue_item_t * msg;
  402. if(cam_msg_queue==NULL|| master_fd<=0|| slave_fd<=0)
  403. {
  404. LOG_E("param check fail\n");
  405. return -1;
  406. }
  407. msg_payload = malloc(sizeof(camera_sync_msg_t));
  408. if(msg_payload == NULL)
  409. {
  410. LOG_E("malloc fail\n");
  411. return -1;
  412. }
  413. msg = malloc(sizeof(msg_queue_item_t));
  414. if(msg==NULL)
  415. {
  416. LOG_E("malloc fail\n");
  417. free(msg_payload);
  418. return -1;
  419. }
  420. msg_payload->type = CAM_SYNC_MSG_DSP_INFO;
  421. msg_payload->dsp_info.master_fd = master_fd;
  422. msg_payload->dsp_info.slave_fd = slave_fd;
  423. msg->payload = msg_payload;
  424. if(enqueue_msg(cam_msg_queue,msg))
  425. {
  426. free(msg_payload);
  427. free(msg);
  428. return -1;
  429. }
  430. LOG_D("send dsp resp to cam sync for fd(%d,%d)\n",master_fd,slave_fd);
  431. return 0;
  432. }
  433. static dual_dsp_handle_t* dsp_process_create(frame_mode_t mode,dual_dsp_algo_type_e type)
  434. {
  435. dual_dsp_handle_t *dsp_hdl=NULL;
  436. dsp_hdl = malloc(sizeof(dual_dsp_handle_t));
  437. if(dsp_hdl==NULL)
  438. {
  439. return NULL;
  440. }
  441. dsp_hdl->dsp_ctx[0].dsp_id = 0;
  442. dsp_hdl->dsp_ctx[1].dsp_id = 1;
  443. dsp_hdl->dsp_ctx[0].algo_name = "dummy_ir_algo_flo_0";
  444. dsp_hdl->dsp_ctx[1].algo_name = "dummy_ir_algo_flo_1";
  445. dsp_hdl->dsp_prcoess_queue.head=NULL;
  446. dsp_hdl->dsp_prcoess_queue.tail=NULL;
  447. dsp_hdl->dsp_prcoess_queue.exit = 0;
  448. dsp_hdl->frame_mode = mode;
  449. dsp_hdl->dsp_run_type = type;
  450. pthread_mutex_init(&dsp_hdl->dsp_prcoess_queue.mutex,NULL);
  451. if(dsp_construct_task(&dsp_hdl->dsp_ctx[0]))
  452. {
  453. free(dsp_hdl);
  454. return NULL;
  455. }
  456. if(dsp_construct_task(&dsp_hdl->dsp_ctx[1]))
  457. {
  458. dsp_release_task(&dsp_hdl->dsp_ctx[0]);
  459. free(dsp_hdl);
  460. return NULL;
  461. }
  462. if( VMEM_create(&dsp_hdl->mem_allocor) <0)
  463. {
  464. dsp_release_task(&dsp_hdl->dsp_ctx[0]);
  465. dsp_release_task(&dsp_hdl->dsp_ctx[1]);
  466. free(dsp_hdl);
  467. return NULL;
  468. }
  469. if(pthread_create(&dsp_hdl->thread_dsp_process,NULL,dsp_dual_ir_porcess,dsp_hdl))
  470. {
  471. dsp_release_task(&dsp_hdl->dsp_ctx[0]);
  472. dsp_release_task(&dsp_hdl->dsp_ctx[1]);
  473. VMEM_destroy(dsp_hdl->mem_allocor);
  474. free(dsp_hdl);
  475. return NULL;
  476. }
  477. LOG_D("dsp process crated\n");
  478. return dsp_hdl;
  479. }
  480. void dsp_process_destroy(dual_dsp_handle_t* dsp_hdl)
  481. {
  482. if(dsp_hdl==NULL)
  483. {
  484. LOG_E("NULL Ptr\n");
  485. return;
  486. }
  487. dsp_hdl->dsp_prcoess_queue.exit=1;
  488. pthread_join(dsp_hdl->thread_dsp_process,NULL);
  489. dsp_release_task(&dsp_hdl->dsp_ctx[0]);
  490. dsp_release_task(&dsp_hdl->dsp_ctx[1]);
  491. VMEM_destroy(dsp_hdl->mem_allocor);
  492. free(dsp_hdl);
  493. return;
  494. }
  495. static void* dsp_dual_ir_porcess(void *arg)
  496. {
  497. msg_queue_item_t * msg=NULL;
  498. dsp_dual_ir_frame_msg_t *dual_ir_frame;
  499. VmemParams params_out;
  500. void* out_buf=NULL;
  501. algo_pic_out_setting_t setting;
  502. algo_result_t *dsp0_result;
  503. algo_result_t *dsp1_result;
  504. dual_dsp_handle_t *dsp_hdl =(dual_dsp_handle_t *) arg;
  505. if(dsp_hdl==NULL)
  506. {
  507. pthread_exit(0);
  508. }
  509. // alloc DSP out put buf from video memory ,which is dma buf
  510. params_out.size = 1920*1088*2;
  511. params_out.flags = VMEM_FLAG_CONTIGUOUS;
  512. out_buf = req_DmaBuffer(dsp_hdl->mem_allocor,&params_out);
  513. if(out_buf == NULL)
  514. {
  515. LOG_E("req dma Buf fail\n");
  516. pthread_exit(0);
  517. }
  518. LOG_O("Runing ...\n");
  519. while(dsp_hdl->dsp_prcoess_queue.exit!=1)
  520. {
  521. if(msg!=NULL)
  522. {
  523. free(msg);
  524. }
  525. msg = dequeue_msg(&dsp_hdl->dsp_prcoess_queue);
  526. if(msg == NULL)
  527. {
  528. usleep(1000);
  529. continue;
  530. }
  531. LOG_D("get msg\n");
  532. dual_ir_frame = (dsp_dual_ir_frame_msg_t *)msg->payload;
  533. struct csi_sw_task_req* req1 = NULL;
  534. struct csi_sw_task_req* req2 = NULL;
  535. req1 = csi_dsp_task_create_request(dsp_hdl->dsp_ctx[0].dsp_task);
  536. if (req1 == NULL) {
  537. LOG_W("req create fail.\n");
  538. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  539. continue;
  540. }
  541. req2 = csi_dsp_task_create_request(dsp_hdl->dsp_ctx[1].dsp_task);
  542. if (req2 == NULL) {
  543. LOG_W("req create fail.\n");
  544. csi_dsp_task_release_request(req1);
  545. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  546. continue;
  547. }
  548. struct csi_dsp_buffer buf1;
  549. struct csi_dsp_buffer buf2;
  550. struct csi_dsp_buffer buf3;
  551. struct csi_dsp_buffer buf4;
  552. // import main camera frame
  553. buf1.buf_id = 0;
  554. buf1.dir = CSI_DSP_BUFFER_IN;
  555. buf1.type = CSI_DSP_BUF_TYPE_DMA_BUF_IMPORT;
  556. buf1.plane_count = 1;
  557. buf1.width = dual_ir_frame->width;
  558. buf1.height = dual_ir_frame->height;
  559. buf1.format = 2;
  560. buf1.planes[0].stride = dual_ir_frame->master_stride;
  561. buf1.planes[0].size = dual_ir_frame->size;
  562. buf1.planes[0].fd = dual_ir_frame->master_fd;
  563. // import slave cmaera frame
  564. buf2.buf_id = 1;
  565. buf2.dir = CSI_DSP_BUFFER_IN;
  566. buf2.type = CSI_DSP_BUF_TYPE_DMA_BUF_IMPORT;
  567. buf2.plane_count = 1;
  568. buf2.width = dual_ir_frame->width;
  569. buf2.height = dual_ir_frame->height;
  570. buf2.format=2;
  571. buf2.planes[0].stride = dual_ir_frame->slave_stride;
  572. buf2.planes[0].size = dual_ir_frame->size;
  573. buf2.planes[0].fd = dual_ir_frame->slave_fd;
  574. // import output frame buf
  575. buf3.buf_id = 2;
  576. buf3.dir = CSI_DSP_BUFFER_OUT;
  577. buf3.type = CSI_DSP_BUF_TYPE_DMA_BUF_IMPORT;
  578. buf3.plane_count = 1;
  579. buf3.width = dual_ir_frame->width;
  580. buf3.height = dual_ir_frame->height;
  581. buf3.format=2;
  582. buf3.planes[0].stride = dual_ir_frame->width*2;
  583. buf3.planes[0].size = params_out.size;
  584. buf3.planes[0].fd = params_out.fd;
  585. // malloc algo result buf
  586. buf4.buf_id = 3;
  587. buf4.dir = CSI_DSP_BUFFER_OUT;
  588. buf4.type = CSI_DSP_BUF_ALLOC_DRV;
  589. buf4.plane_count = 1;
  590. buf4.planes[0].size = sizeof(algo_result_t);
  591. /**********************add buf for dsp0 req*****************************************/
  592. /* buf0:master frame; buf1:output buf; buf2: slave frame,buf3:algo result buf********/
  593. if(csi_dsp_request_add_buffer(req1,&buf1))
  594. {
  595. csi_dsp_task_release_request(req1);
  596. csi_dsp_task_release_request(req2);
  597. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  598. LOG_W("req create fail.\n");
  599. continue;
  600. }
  601. if(csi_dsp_request_add_buffer(req1,&buf3))
  602. {
  603. csi_dsp_task_release_request(req1);
  604. csi_dsp_task_release_request(req2);
  605. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  606. LOG_W("req create fail.\n");
  607. continue;
  608. }
  609. if(csi_dsp_request_add_buffer(req1,&buf2))
  610. {
  611. csi_dsp_task_release_request(req1);
  612. csi_dsp_task_release_request(req2);
  613. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  614. LOG_W("req create fail.\n");
  615. continue;
  616. }
  617. if(csi_dsp_request_add_buffer(req1,&buf4))
  618. {
  619. csi_dsp_task_release_request(req1);
  620. csi_dsp_task_release_request(req2);
  621. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  622. LOG_W("req create fail.\n");
  623. continue;
  624. }
  625. dsp0_result = (algo_result_t *)buf4.planes[0].buf_vir;
  626. /**********************add buf for dsp1 req*****************************************/
  627. /* buf0:salve frame; buf1:output buf; buf2: master frame,buf3:algo result buf********/
  628. if(csi_dsp_request_add_buffer(req2,&buf2))
  629. {
  630. csi_dsp_task_release_request(req1);
  631. csi_dsp_task_release_request(req2);
  632. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  633. LOG_W("req create fail.\n");
  634. continue;
  635. }
  636. if(csi_dsp_request_add_buffer(req2,&buf3))
  637. {
  638. csi_dsp_task_release_request(req1);
  639. csi_dsp_task_release_request(req2);
  640. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  641. LOG_W("req create fail.\n");
  642. continue;
  643. }
  644. if(csi_dsp_request_add_buffer(req2,&buf1))
  645. {
  646. csi_dsp_task_release_request(req1);
  647. csi_dsp_task_release_request(req2);
  648. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  649. LOG_W("req create fail.\n");
  650. continue;
  651. }
  652. if(csi_dsp_request_add_buffer(req2,&buf4))
  653. {
  654. csi_dsp_task_release_request(req1);
  655. csi_dsp_task_release_request(req2);
  656. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  657. LOG_W("req create fail.\n");
  658. continue;
  659. }
  660. dsp1_result = (algo_result_t *)buf4.planes[0].buf_vir;
  661. if(dsp_hdl->dsp_run_type == DUAL_DSP_ALGO_TYPE_DSP01_COPY )
  662. {
  663. setting.mode = ALGO_PIC_MODE_FULL_COPY_WITH_PARAM;
  664. setting.buf_id=0;
  665. setting.h_offset = dual_ir_frame->height/2;
  666. setting.height =dual_ir_frame->height/2;
  667. }else if(dsp_hdl->dsp_run_type == DUAL_DSP_ALGO_TYPE_DSP0_COPY)
  668. {
  669. setting.mode = ALGO_PIC_MODE_FULL_COPY;
  670. }
  671. else
  672. {
  673. setting.mode = ALGO_PIC_MODE_FULL_COPY_NONE;
  674. }
  675. setting.timestap = dual_ir_frame->timestap;
  676. if(csi_dsp_request_set_property(req1,&setting,sizeof(setting)))
  677. {
  678. csi_dsp_task_release_request(req1);
  679. csi_dsp_task_release_request(req2);
  680. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  681. LOG_W("req create fail.\n");
  682. continue;
  683. }
  684. if(dsp_hdl->dsp_run_type == DUAL_DSP_ALGO_TYPE_DSP01_COPY )
  685. {
  686. setting.mode = ALGO_PIC_MODE_FULL_COPY_WITH_PARAM;
  687. setting.buf_id=0;
  688. setting.h_offset = 0;
  689. setting.height =dual_ir_frame->height/2;
  690. }else if(dsp_hdl->dsp_run_type == DUAL_DSP_ALGO_TYPE_DSP1_COPY)
  691. {
  692. setting.mode = ALGO_PIC_MODE_FULL_COPY;
  693. }
  694. else
  695. {
  696. setting.mode = ALGO_PIC_MODE_FULL_COPY_NONE;
  697. }
  698. setting.timestap = dual_ir_frame->timestap;
  699. if(csi_dsp_request_set_property(req2,&setting,sizeof(setting)))
  700. {
  701. csi_dsp_task_release_request(req1);
  702. csi_dsp_task_release_request(req2);
  703. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  704. LOG_W("req create fail.\n");
  705. continue;
  706. }
  707. if(csi_dsp_request_enqueue(req1))
  708. {
  709. csi_dsp_task_release_request(req1);
  710. csi_dsp_task_release_request(req2);
  711. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  712. LOG_W("req create fail.\n");
  713. continue;
  714. }
  715. if(csi_dsp_request_enqueue(req2))
  716. {
  717. req1 = csi_dsp_request_dequeue(dsp_hdl->dsp_ctx[0].dsp_task);
  718. csi_dsp_task_release_request(req1);
  719. csi_dsp_task_release_request(req2);
  720. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  721. LOG_W("req create fail.\n");
  722. continue;
  723. }
  724. req1 = csi_dsp_request_dequeue(dsp_hdl->dsp_ctx[0].dsp_task);
  725. if(req1 == NULL)
  726. {
  727. LOG_W("dequeue fail.\n");
  728. }
  729. req2 =csi_dsp_request_dequeue(dsp_hdl->dsp_ctx[1].dsp_task);
  730. if(req2 == NULL)
  731. {
  732. LOG_W("dequeue fail.\n");
  733. }
  734. LOG_D("dsp0 lib:%s timestap:%lx,dsp1 lib:%s,timestap:%lx\n",dsp0_result->lib_name,dsp0_result->timestap,
  735. dsp1_result->lib_name,dsp1_result->timestap);
  736. dsp_send_info_to_cam(dsp_hdl->cam_sync_queue, dual_ir_frame->master_fd,dual_ir_frame->slave_fd);
  737. switch(dsp_hdl->frame_mode)
  738. {
  739. case FRAME_NONE:
  740. break;
  741. case FRAME_SAVE_IMG:
  742. dsp_dump_frame((void *)buf3.planes[0].buf_vir,0,buf3.width,buf3.height,buf3.planes[0].stride);
  743. break;
  744. default:
  745. LOG_W("unsupport frame mode %d\n",dsp_hdl->frame_mode);
  746. }
  747. csi_dsp_task_release_request(req1);
  748. csi_dsp_task_release_request(req2);
  749. }
  750. release_DmaBuffer(dsp_hdl->mem_allocor,&params_out);
  751. LOG_O("exit\n");
  752. pthread_exit(0);
  753. }
  754. /************************************Camera sync process***************************/
  755. #define MAX_FIFO_FACE_INFO_NUM 2
  756. #define MAX_FIFO_FRAME_NUM 2
  757. typedef struct {
  758. struct list_head head;
  759. void* item;
  760. }frame_item_t;
  761. typedef struct cam_sync_process_ctx{
  762. msg_queue_ctx_t cam_sync_queue;
  763. msg_queue_ctx_t *dsp_prcoess_queue; //DSP 消息queue
  764. csi_cam_handle_t master_cam_handle;
  765. csi_cam_handle_t slave_cam_handle;
  766. struct list_head cam_master_frame_list; // 缓存master camera 的frame
  767. struct list_head cam_slave_frame_list; // slave camera 的frame
  768. struct list_head cam_busy_frame_list; //缓存发送给DSP的frame
  769. struct list_head ai_info_list;
  770. pthread_t thread_cam_sync;
  771. uint32_t paried_frame_num;
  772. }cam_sync_process_ctx_t;
  773. static cam_sync_process_ctx_t *cam_sync_create()
  774. {
  775. cam_sync_process_ctx_t * ctx;
  776. ctx = malloc(sizeof(cam_sync_process_ctx_t));
  777. if(ctx == NULL)
  778. {
  779. return NULL;
  780. }
  781. ctx->cam_sync_queue.head=NULL;
  782. ctx->cam_sync_queue.tail = NULL;
  783. ctx->cam_sync_queue.exit =0;
  784. ctx->paried_frame_num = 0;
  785. pthread_mutex_init(&ctx->cam_sync_queue.mutex,NULL);
  786. INIT_LIST_HEAD(&ctx->cam_master_frame_list);
  787. INIT_LIST_HEAD(&ctx->ai_info_list);
  788. INIT_LIST_HEAD(&ctx->cam_busy_frame_list);
  789. INIT_LIST_HEAD(&ctx->cam_slave_frame_list);
  790. if(pthread_create(&ctx->thread_cam_sync,NULL,cam_frame_sync_process,ctx))
  791. {
  792. LOG_E("cam sync thread create fail\n");
  793. free(ctx);
  794. return NULL;
  795. }
  796. return ctx;
  797. }
  798. static void cam_sync_destroy(void *arg)
  799. {
  800. cam_sync_process_ctx_t *ctx= (cam_sync_process_ctx_t *)arg;
  801. if(ctx==NULL)
  802. {
  803. return ;
  804. }
  805. ctx->cam_sync_queue.exit =1;
  806. pthread_join(ctx->thread_cam_sync,NULL);
  807. free(ctx);
  808. LOG_O("cam sync destroy\n");
  809. }
  810. static int push_new_frame(struct list_head *frame_list, csi_frame_ex_s * new_frame)
  811. {
  812. int entry_num =0;
  813. frame_item_t *frame_item = NULL;
  814. frame_item_t * new_item = NULL;
  815. if(frame_list == NULL || new_frame==NULL)
  816. {
  817. return -1;
  818. }
  819. list_for_each_entry(frame_item,frame_list,head){
  820. entry_num++;
  821. }
  822. if((entry_num+1)>MAX_FIFO_FRAME_NUM)
  823. {
  824. frame_item = list_first_entry(frame_list,frame_item_t,head);
  825. LOG_D("release frame fd:%d\n",((csi_frame_ex_s *)frame_item->item)->frame_data.fd[0]);
  826. csi_camera_put_frame((csi_frame_ex_s *)frame_item->item);
  827. free(frame_item->item);
  828. list_del(&frame_item->head);
  829. free(frame_item);
  830. }
  831. new_item = malloc(sizeof(frame_item_t));
  832. new_item->item = new_frame;
  833. list_add_tail(&new_item->head,frame_list);
  834. return 0;
  835. }
  836. static csi_frame_ex_s * pop_matched_frame_with_fd(struct list_head *frame_list,int fd)
  837. {
  838. csi_frame_ex_s * frame = NULL;
  839. frame_item_t *frame_item = NULL;
  840. frame_item_t * temp;
  841. if(frame_list==NULL)
  842. {
  843. return NULL;
  844. }
  845. list_for_each_entry_safe(frame_item,temp,frame_list,head){
  846. frame = (csi_frame_ex_s *)frame_item->item;
  847. if(frame->frame_data.fd[0]==fd)
  848. {
  849. list_del(&frame_item->head);
  850. free(frame_item);
  851. LOG_D("get frame fd:%d\n",frame->frame_data.fd[0]);
  852. return frame;
  853. }
  854. }
  855. return NULL;
  856. }
  857. static csi_frame_ex_s * pop_matched_frame_with_ts(struct list_head *frame_list, csi_frame_ex_s * target_frame,uint32_t rang_us,bool clear_early)
  858. {
  859. csi_camera_meta_s *meta_data = (csi_camera_meta_s *)target_frame->frame_meta.data;
  860. csi_camera_meta_unit_s target_ts,loop_ts;
  861. csi_frame_ex_s * frame;
  862. frame_item_t *frame_item = NULL;
  863. frame_item_t * tmp;
  864. struct timeval time_value;
  865. long long target_us,loop_us,deta_us;
  866. if(target_frame == NULL || frame_list==NULL)
  867. {
  868. return NULL;
  869. }
  870. if(list_empty(frame_list))
  871. {
  872. return NULL;
  873. }
  874. csi_camera_frame_get_meta_unit(
  875. &target_ts, meta_data, CSI_CAMERA_META_ID_TIMESTAMP);
  876. target_us = target_ts.time_value.tv_sec*1000000 + target_ts.time_value.tv_usec;
  877. list_for_each_entry_safe(frame_item,tmp,frame_list,head){
  878. frame = ( csi_frame_ex_s *)frame_item->item;
  879. meta_data = (csi_camera_meta_s *)frame->frame_meta.data;
  880. csi_camera_frame_get_meta_unit(
  881. &loop_ts, meta_data, CSI_CAMERA_META_ID_TIMESTAMP);
  882. loop_us = loop_ts.time_value.tv_sec*1000000 + loop_ts.time_value.tv_usec;
  883. deta_us = (target_us-loop_us);
  884. if(deta_us <=rang_us || deta_us>=(-rang_us))
  885. {
  886. list_del(&frame_item->head);
  887. free(frame_item);
  888. return frame;
  889. }
  890. if(clear_early && deta_us < 0)
  891. {
  892. csi_camera_put_frame(frame);
  893. list_del(&frame_item->head);
  894. free(frame_item);
  895. }
  896. frame_item=NULL;
  897. }
  898. return NULL;
  899. }
  900. static int send_paired_frame_to_dsp(msg_queue_ctx_t *queue,csi_frame_ex_s *master_frame,csi_frame_ex_s *slave_frame,struct list_head *send_frames)
  901. {
  902. dsp_dual_ir_frame_msg_t *dsp_msg_payload;
  903. msg_queue_item_t *dsp_msg;
  904. frame_item_t *main_item,*slave_item;
  905. csi_camera_meta_s *meta_data;
  906. csi_camera_meta_unit_s timestap;
  907. if(queue==NULL || master_frame==NULL || slave_frame==NULL)
  908. {
  909. return -1;
  910. }
  911. if(master_frame->frame_info.width!=slave_frame->frame_info.width||
  912. master_frame->frame_info.height!=slave_frame->frame_info.height)
  913. {
  914. LOG_E("paird frame not the same picture param\n");
  915. return -1;
  916. }
  917. dsp_msg=malloc(sizeof(msg_queue_item_t));
  918. if(dsp_msg==NULL){
  919. return -1;
  920. }
  921. dsp_msg_payload = malloc(sizeof(dsp_dual_ir_frame_msg_t));
  922. if(dsp_msg_payload==NULL)
  923. {
  924. free(dsp_msg);
  925. return -1;
  926. }
  927. meta_data = (csi_camera_meta_s *)master_frame->frame_meta.data;
  928. csi_camera_frame_get_meta_unit(
  929. &timestap, meta_data, CSI_CAMERA_META_ID_TIMESTAMP);
  930. dsp_msg->payload = dsp_msg_payload;
  931. dsp_msg_payload->size = master_frame->frame_info.height*master_frame->frame_data.stride[0];
  932. dsp_msg_payload->width = master_frame->frame_info.width;
  933. dsp_msg_payload->height = master_frame->frame_info.height;
  934. dsp_msg_payload->pix_format = master_frame->frame_info.pixel_format;
  935. dsp_msg_payload->master_stride = master_frame->frame_data.stride[0];
  936. dsp_msg_payload->master_fd = master_frame->frame_data.fd[0];
  937. dsp_msg_payload->slave_fd = slave_frame->frame_data.fd[0];
  938. dsp_msg_payload->slave_stride = slave_frame->frame_data.stride[0];
  939. dsp_msg_payload->timestap = timestap.time_value.tv_sec*1000000+timestap.time_value.tv_usec;
  940. enqueue_msg(queue,dsp_msg);
  941. main_item = malloc(sizeof(frame_item_t));
  942. main_item->item = master_frame;
  943. slave_item = malloc(sizeof(frame_item_t));
  944. slave_item->item = slave_frame;
  945. list_add_tail(&main_item->head,send_frames);
  946. list_add_tail(&slave_item->head,send_frames);
  947. LOG_D("send paired fd (%d,%d)\n",dsp_msg_payload->master_fd,dsp_msg_payload->slave_fd);
  948. return 0;
  949. }
  950. static int cam_dual_frame_process(struct list_head * pair_frame_list,struct list_head * free_frame_list, struct list_head *busy_frame_list,csi_frame_ex_s *frame, msg_queue_ctx_t *dsp_prcoess_queue,cam_sync_message_type_e type)
  951. {
  952. csi_frame_ex_s *new_frame = NULL;
  953. csi_frame_ex_s *pair_frame = NULL;
  954. csi_frame_ex_s *master_frame = NULL;
  955. csi_frame_ex_s *slave_frame = NULL;
  956. if(pair_frame_list == NULL ||free_frame_list==NULL ||busy_frame_list ==NULL ||
  957. frame == NULL || dsp_prcoess_queue == NULL || type >CAM_SYNC_MSG_SLAVE_FRAME)
  958. {
  959. LOG_E("param check fail \n");
  960. return -1;
  961. }
  962. new_frame = malloc(sizeof(csi_frame_ex_s));
  963. if(new_frame==NULL)
  964. {
  965. LOG_E("malloc fail\n");
  966. csi_camera_put_frame(frame);
  967. return -1;
  968. }
  969. memcpy(new_frame,frame,sizeof(csi_frame_ex_s));
  970. pair_frame = pop_matched_frame_with_ts(pair_frame_list,frame,0,true);
  971. if(pair_frame == NULL)
  972. {
  973. if(push_new_frame(free_frame_list,new_frame))
  974. {
  975. csi_camera_put_frame(new_frame);
  976. free(new_frame);
  977. LOG_E("save new frame fail\n");
  978. return -1;
  979. }
  980. }else
  981. {
  982. if(type == CAM_SYNC_MSG_MAIN_FRAME)
  983. {
  984. master_frame = new_frame;
  985. slave_frame = pair_frame;
  986. }else
  987. {
  988. master_frame = pair_frame;
  989. slave_frame = new_frame;
  990. }
  991. if(send_paired_frame_to_dsp(dsp_prcoess_queue,master_frame,slave_frame,busy_frame_list))
  992. {
  993. LOG_E("fail to send paired frame fd (%d,%d) to dsp\n",new_frame->frame_data.fd[0],pair_frame->frame_data.fd[0]);
  994. csi_camera_put_frame(master_frame);
  995. free(master_frame);
  996. csi_camera_put_frame(slave_frame);
  997. free(slave_frame);
  998. return -1;
  999. }
  1000. }
  1001. return 0;
  1002. }
  1003. static void *cam_frame_sync_process(void *arg)
  1004. {
  1005. camera_sync_msg_t *msg;
  1006. csi_frame_ex_s * master_frame=NULL;
  1007. csi_frame_ex_s * slave_frame=NULL;
  1008. struct list_head *matched_list=NULL;
  1009. msg_queue_item_t * item;
  1010. cam_sync_process_ctx_t *ctx =(cam_sync_process_ctx_t *)arg;
  1011. LOG_O("frame sync process running....\n");
  1012. while(ctx->cam_sync_queue.exit!=1)
  1013. {
  1014. item = dequeue_msg(&ctx->cam_sync_queue);
  1015. if(item == NULL)
  1016. {
  1017. usleep(1000);
  1018. continue;
  1019. }
  1020. msg = (camera_sync_msg_t *)item->payload;
  1021. free(item);
  1022. LOG_D("get msg:%d\n",msg->type);
  1023. switch(msg->type)
  1024. {
  1025. case CAM_SYNC_MSG_MAIN_FRAME:
  1026. cam_dual_frame_process(&ctx->cam_slave_frame_list,&ctx->cam_master_frame_list,&ctx->cam_busy_frame_list,&msg->frame,ctx->dsp_prcoess_queue,CAM_SYNC_MSG_MAIN_FRAME);
  1027. break;
  1028. case CAM_SYNC_MSG_SLAVE_FRAME:
  1029. cam_dual_frame_process(&ctx->cam_master_frame_list,&ctx->cam_slave_frame_list,&ctx->cam_busy_frame_list,&msg->frame,ctx->dsp_prcoess_queue,CAM_SYNC_MSG_SLAVE_FRAME);
  1030. break;
  1031. case CAM_SYNC_MSG_AI_INFO:
  1032. break;
  1033. case CAM_SYNC_MSG_DSP_INFO:
  1034. master_frame = pop_matched_frame_with_fd(&ctx->cam_busy_frame_list,msg->dsp_info.master_fd);
  1035. if(master_frame == NULL)
  1036. {
  1037. LOG_E("fail to find sending fd\n");
  1038. }else
  1039. {
  1040. csi_camera_put_frame(master_frame);
  1041. free(master_frame);
  1042. }
  1043. slave_frame =pop_matched_frame_with_fd(&ctx->cam_busy_frame_list,msg->dsp_info.slave_fd);
  1044. if(slave_frame == NULL)
  1045. {
  1046. LOG_E("fail to find sending fd\n");
  1047. }else
  1048. {
  1049. csi_camera_put_frame(slave_frame);
  1050. free(slave_frame);
  1051. }
  1052. ctx->paried_frame_num++;
  1053. /***********done some thing for*********/
  1054. break;
  1055. default:
  1056. LOG_W("invalid msg type:%d\n",msg->type);
  1057. break;
  1058. }
  1059. free(msg);
  1060. }
  1061. LOG_O("frame sync process exit....\n");
  1062. pthread_exit(0);
  1063. }
  1064. /********************************Main prcoess *******************************************************************/
  1065. static int camera_get_chl_id(int env_type, int *chl_id)
  1066. {
  1067. if (chl_id == NULL) {
  1068. LOG_E("fail to check param chl_id = %p\n", chl_id);
  1069. return -1;
  1070. }
  1071. switch (env_type) {
  1072. case CSI_CAMERA_EVENT_TYPE_CHANNEL0:
  1073. *chl_id = CSI_CAMERA_CHANNEL_0;
  1074. break;
  1075. case CSI_CAMERA_EVENT_TYPE_CHANNEL1:
  1076. *chl_id = CSI_CAMERA_CHANNEL_1;
  1077. break;
  1078. case CSI_CAMERA_EVENT_TYPE_CHANNEL2:
  1079. *chl_id = CSI_CAMERA_CHANNEL_2;
  1080. break;
  1081. default:
  1082. LOG_D("fail to check env_type = %d unsupport\n", env_type);
  1083. break;
  1084. }
  1085. return 0;
  1086. }
  1087. static int cam_send_frame_to_sync_process(msg_queue_ctx_t *queue,csi_frame_ex_s *frame, cam_sync_message_type_e type)
  1088. {
  1089. camera_sync_msg_t *msg_palyload = malloc(sizeof(camera_sync_msg_t));
  1090. msg_queue_item_t * item=NULL;
  1091. if(msg_palyload==NULL)
  1092. {
  1093. return -1;
  1094. }
  1095. item = malloc(sizeof(msg_queue_item_t));
  1096. if(item==NULL)
  1097. {
  1098. free(msg_palyload);
  1099. return -1;
  1100. }
  1101. msg_palyload->type= type;
  1102. memcpy(&msg_palyload->frame,frame,sizeof(csi_frame_ex_s));
  1103. item->payload = msg_palyload;
  1104. return enqueue_msg(queue,item);
  1105. }
  1106. static int camera_event_process(void *arg)
  1107. {
  1108. int ret = 0;
  1109. if (arg == NULL) {
  1110. LOG_E("NULL Ptr\n");
  1111. return -1;
  1112. }
  1113. camera_ctx_t* ctx = (camera_ctx_t*)arg;
  1114. csi_cam_event_handle_t ev_handle = ctx->event_handle;
  1115. csi_camera_channel_cfg_s *ch_cfg=NULL;
  1116. cam_sync_message_type_e type;
  1117. type = ctx->cam_id==0?CAM_SYNC_MSG_MAIN_FRAME:CAM_SYNC_MSG_SLAVE_FRAME;
  1118. struct timeval cur_time;
  1119. struct csi_camera_event event;
  1120. csi_frame_ex_s frame;
  1121. if (ev_handle == NULL) {
  1122. LOG_E("fail to get ev_handle ev_handle\n");
  1123. return -1;
  1124. }
  1125. if(ctx->exit!=0)
  1126. {
  1127. return 1;
  1128. }
  1129. int timeout =0; // unit: ms, -1 means wait forever, or until error occurs
  1130. ret = csi_camera_get_event(ev_handle, &event, timeout);
  1131. if(ret)
  1132. {
  1133. return -1;
  1134. }
  1135. LOG_D("Camera_%d event.type = %d, event.id = %d\n",ctx->cam_id ,event.type, event.id);
  1136. switch (event.type) {
  1137. case CSI_CAMERA_EVENT_TYPE_CAMERA:
  1138. switch (event.id) {
  1139. case CSI_CAMERA_EVENT_ERROR:
  1140. // do sth.
  1141. LOG_E("get CAMERA EVENT CSI_CAMERA_EVENT_ERROR!\n");
  1142. break;
  1143. case CSI_CAMERA_EVENT_WARNING:
  1144. LOG_W("get CAMERA EVENT CSI_CAMERA_EVENT_WRN,RC: %s\n",event.bin);
  1145. break;
  1146. default:
  1147. break;
  1148. }
  1149. break;
  1150. case CSI_CAMERA_EVENT_TYPE_CHANNEL0:
  1151. case CSI_CAMERA_EVENT_TYPE_CHANNEL1:
  1152. case CSI_CAMERA_EVENT_TYPE_CHANNEL2:
  1153. case CSI_CAMERA_EVENT_TYPE_CHANNEL3:
  1154. switch (event.id) {
  1155. case CSI_CAMERA_CHANNEL_EVENT_FRAME_READY: {
  1156. int chn_id = 0;
  1157. ret = camera_get_chl_id(event.type, &chn_id);
  1158. if (ret) {
  1159. LOG_E("fail to get chl_id = %d\n", chn_id);
  1160. return -1;
  1161. }
  1162. ch_cfg = &ctx->chn_cfg[chn_id];
  1163. get_system_time(__func__, __LINE__);
  1164. int read_frame_count = csi_camera_get_frame_count(ctx->cam_handle,
  1165. chn_id);
  1166. for (int i = 0; i < read_frame_count; i++) {
  1167. csi_camera_get_frame(ctx->cam_handle, chn_id, &frame, timeout);
  1168. ctx->frame_num++;
  1169. if(cam_send_frame_to_sync_process(ctx->cam_sync_queue,&frame,type))
  1170. {
  1171. csi_camera_put_frame(&frame);
  1172. }
  1173. }
  1174. unsigned long diff;
  1175. if (ctx->init_time.tv_usec == 0)
  1176. gettimeofday(&ctx->init_time,0);//osGetTick();
  1177. gettimeofday(&cur_time, 0);
  1178. diff = 1000000 * (cur_time.tv_sec-ctx->init_time.tv_sec)+ cur_time.tv_usec-ctx->init_time.tv_usec;
  1179. if (diff != 0)
  1180. ctx->fps = (float) ctx->frame_num / diff * 1000000.0f;
  1181. LOG_O("cam:%d,read_frame_count = %d, frame_count = %d, fps = %.2f\n", ctx->cam_id,read_frame_count, ctx->frame_num,ctx->fps);
  1182. break;
  1183. }
  1184. default:
  1185. break;
  1186. }
  1187. break;
  1188. default:
  1189. break;
  1190. }
  1191. return 0;
  1192. }
  1193. static void camera_start_all(camera_ctx_t * ctx)
  1194. {
  1195. camera_ctx_t * cam_ctx = ctx;
  1196. int loop_ch;
  1197. for(loop_ch=0;loop_ch<cam_ctx->channel_num;loop_ch++)
  1198. {
  1199. csi_camera_channel_start(cam_ctx->cam_handle, cam_ctx->chn_cfg[loop_ch].chn_id);
  1200. }
  1201. }
  1202. static void camera_stop_all(camera_ctx_t * ctx)
  1203. {
  1204. camera_ctx_t * cam_ctx = ctx;
  1205. int loop_ch;
  1206. for(loop_ch=0;loop_ch<cam_ctx->channel_num;loop_ch++)
  1207. {
  1208. csi_camera_channel_stop(cam_ctx->cam_handle, cam_ctx->chn_cfg[loop_ch].chn_id);
  1209. LOG_I("Stop CAM:%d,channel:%d\n",cam_ctx->cam_id,loop_ch);
  1210. }
  1211. }
  1212. static camera_ctx_t * camera_open(camera_param_t *params)
  1213. {
  1214. int ret = 0;
  1215. char dev_name[128];
  1216. camera_ctx_t * cam_ctx = NULL;
  1217. int loop_ch;
  1218. LOG_O("Open Camera %d\n",params->video_id);
  1219. if(params==NULL || params->video_id <0)
  1220. {
  1221. LOG_E("param err\n");
  1222. return NULL;
  1223. }
  1224. if(params->channel_num> MAX_CHANNEL_NUM)
  1225. {
  1226. LOG_E("unsupoort channle num:%d \n",params->channel_num);
  1227. return NULL;
  1228. }
  1229. cam_ctx = malloc(sizeof(camera_ctx_t));
  1230. if(!cam_ctx)
  1231. {
  1232. return NULL;
  1233. }
  1234. memset(cam_ctx,0x0,sizeof(camera_ctx_t));
  1235. // 打开Camera设备获取句柄,作为后续操对象
  1236. sprintf(dev_name, "/dev/video%d", params->video_id);
  1237. if(csi_camera_open(&cam_ctx->cam_handle, dev_name))
  1238. {
  1239. LOG_E("Fail to open cam :%s\n",dev_name);
  1240. goto ONE_ERR;
  1241. }
  1242. for(loop_ch= CSI_CAMERA_CHANNEL_0;loop_ch<params->channel_num;loop_ch++)
  1243. {
  1244. cam_ctx->chn_cfg[loop_ch].chn_id = loop_ch;
  1245. cam_ctx->chn_cfg[loop_ch].img_fmt.pix_fmt = params->out_pic[loop_ch].fmt;
  1246. cam_ctx->chn_cfg[loop_ch].img_fmt.width= params->out_pic[loop_ch].width;
  1247. cam_ctx->chn_cfg[loop_ch].img_fmt.height = params->out_pic[loop_ch].height;
  1248. cam_ctx->chn_cfg[loop_ch].img_type = CSI_IMG_TYPE_DMA_BUF;
  1249. cam_ctx->chn_cfg[loop_ch].meta_fields = CSI_CAMERA_META_DEFAULT_FIELDS;
  1250. if(csi_camera_channel_open(cam_ctx->cam_handle,&cam_ctx->chn_cfg[loop_ch]))
  1251. {
  1252. LOG_E("Fail to open cam %s,channel :%d\n",dev_name,loop_ch);
  1253. goto TWO_ERR;
  1254. }
  1255. cam_ctx->channel_num++;
  1256. }
  1257. if(csi_camera_create_event(&cam_ctx->event_handle,cam_ctx->cam_handle))
  1258. {
  1259. LOG_E("Fail to create event handler for cam %s\n",dev_name);
  1260. goto TWO_ERR;
  1261. }
  1262. cam_ctx->event_subscribe.type = CSI_CAMERA_EVENT_TYPE_CAMERA;
  1263. cam_ctx->event_subscribe.id = CSI_CAMERA_EVENT_WARNING | CSI_CAMERA_EVENT_ERROR;
  1264. if(csi_camera_subscribe_event(cam_ctx->event_handle,&cam_ctx->event_subscribe))
  1265. {
  1266. LOG_E("Fail to subscribe eventfor cam %s\n",dev_name);
  1267. goto TWO_ERR;
  1268. }
  1269. for(loop_ch=0;loop_ch<cam_ctx->channel_num;loop_ch++)
  1270. {
  1271. cam_ctx->event_subscribe.type = CSI_CAMERA_EVENT_TYPE_CHANNEL0+loop_ch;
  1272. cam_ctx->event_subscribe.id = CSI_CAMERA_CHANNEL_EVENT_FRAME_READY;
  1273. if(csi_camera_subscribe_event(cam_ctx->event_handle,&cam_ctx->event_subscribe))
  1274. {
  1275. LOG_E("Fail to subscribe eventfor cam %s\n",dev_name);
  1276. goto TWO_ERR;
  1277. }
  1278. }
  1279. LOG_O("%s open successfully\n",dev_name);
  1280. if(params->type == CAM_TYEP_SLAVE)
  1281. {
  1282. // csi_camera_floodlight_led_set_flash_bright(cam_ctx->cam_handle, 500); //500ma
  1283. csi_camera_projection_led_set_flash_bright(cam_ctx->cam_handle, 500); //500ma
  1284. csi_camera_projection_led_set_mode(cam_ctx->cam_handle, LED_IR_ENABLE);
  1285. // csi_camera_floodlight_led_set_mode(cam_ctx->cam_handle, LED_IR_ENABLE);
  1286. csi_camera_led_enable(cam_ctx->cam_handle, LED_PROJECTION);
  1287. // csi_camera_led_set_switch_mode(cam_ctx->cam_handle, SWITCH_MODE_PROJECTION_ALWAYS_ON);
  1288. }
  1289. get_system_time(__func__, __LINE__);
  1290. return cam_ctx;
  1291. TWO_ERR:
  1292. for(loop_ch=0;loop_ch<cam_ctx->channel_num;loop_ch++)
  1293. {
  1294. csi_camera_channel_close(cam_ctx->cam_handle, cam_ctx->chn_cfg[loop_ch].chn_id);
  1295. }
  1296. csi_camera_close(cam_ctx->cam_handle);
  1297. ONE_ERR:
  1298. free(cam_ctx);
  1299. return NULL;
  1300. }
  1301. static void camera_close(camera_ctx_t *ctx)
  1302. {
  1303. int loop_ch;
  1304. if(ctx->cam_handle==NULL )
  1305. {
  1306. return;
  1307. }
  1308. csi_camera_unsubscribe_event(ctx->event_handle, &ctx->event_subscribe);
  1309. csi_camera_destory_event(ctx->event_handle);
  1310. for(loop_ch=0;loop_ch<ctx->channel_num;loop_ch++)
  1311. {
  1312. csi_camera_channel_close(ctx->cam_handle, ctx->chn_cfg[loop_ch].chn_id);
  1313. }
  1314. csi_camera_close(ctx->cam_handle);
  1315. free(ctx);
  1316. }
  1317. cam_sync_process_ctx_t *cam_sync_hdl;
  1318. dual_dsp_handle_t *dsp_hdl;
  1319. int main(int argc, char *argv[])
  1320. {
  1321. char dev_name[128];
  1322. int camera_id = 0;
  1323. // 打印HAL接口版本号
  1324. csi_api_version_u version;
  1325. csi_camera_get_version(&version);
  1326. camera_param_t params[MAX_CAM_NUM];
  1327. camera_ctx_t * ctx[MAX_CAM_NUM]={NULL};
  1328. frame_mode_t frame_mode ;
  1329. dual_dsp_algo_type_e dsp_type;
  1330. int cam_num=0;
  1331. bool running = false;
  1332. cam_num =parseParams(argc,argv,params,&frame_mode,&dsp_type);
  1333. if(cam_num <=0 || cam_num>MAX_CAM_NUM)
  1334. {
  1335. LOG_E("not camera is active\n");
  1336. exit(0);
  1337. }
  1338. dsp_hdl =dsp_process_create(frame_mode,dsp_type);
  1339. if(dsp_hdl==NULL)
  1340. {
  1341. LOG_E("dsp create faile\n");
  1342. exit(0);
  1343. }
  1344. cam_sync_hdl =cam_sync_create();
  1345. if(cam_sync_hdl==NULL)
  1346. {
  1347. LOG_E("cam sync create fail\n");
  1348. dsp_process_destroy(dsp_hdl);
  1349. exit(0);
  1350. }
  1351. dsp_hdl->cam_sync_queue = &cam_sync_hdl->cam_sync_queue;
  1352. cam_sync_hdl->dsp_prcoess_queue = &dsp_hdl->dsp_prcoess_queue;
  1353. for(int i =cam_num-1;i>=0;i--)
  1354. {
  1355. ctx[i]=camera_open(&params[i]);
  1356. if(ctx[i]==NULL)
  1357. {
  1358. LOG_E("camera %d open %dfail\n",i,params[i].type);
  1359. goto ONR_ERR;
  1360. }
  1361. ctx[i]->cam_id = i;
  1362. ctx[i]->frame_num =0;
  1363. ctx[i]->cam_sync_queue = &cam_sync_hdl->cam_sync_queue;
  1364. if(params[i].type ==CAM_TYEP_MASTER )
  1365. {
  1366. cam_sync_hdl->master_cam_handle=ctx[i]->cam_handle;
  1367. }
  1368. else if(params[i].type ==CAM_TYEP_SLAVE)
  1369. {
  1370. cam_sync_hdl->slave_cam_handle = ctx[i]->cam_handle;
  1371. }
  1372. camera_start_all(ctx[i]);
  1373. running = true;
  1374. }
  1375. int all_exit=1;
  1376. do{
  1377. usleep(1000);
  1378. all_exit=1;
  1379. for(int i =cam_num-1;i>=0;i--)
  1380. {
  1381. camera_event_process(ctx[i]);
  1382. if(ctx[i]&&
  1383. ctx[i]->exit==0 &&
  1384. params[i].frames_to_stop>0 &&
  1385. cam_sync_hdl->paried_frame_num >=params[i].frames_to_stop)
  1386. {
  1387. camera_stop_all(ctx[i]);
  1388. ctx[i]->exit=1;
  1389. }
  1390. if(ctx[i])
  1391. all_exit &= ctx[i]->exit;
  1392. }
  1393. if(all_exit ==1)
  1394. {
  1395. LOG_O("All camera are stop\n");
  1396. running = false;
  1397. }
  1398. }while(running);
  1399. ONR_ERR:
  1400. for(int i =0;i<cam_num;i++)
  1401. {
  1402. if(ctx[i])
  1403. camera_close(ctx[i]);
  1404. }
  1405. dsp_process_destroy(dsp_hdl);
  1406. cam_sync_destroy(cam_sync_hdl);
  1407. exit(0);
  1408. }