cam_demo_dual_ir.c 47 KB

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