camera_frame_display.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516
  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 <syslog.h>
  10. #include <string.h>
  11. #include <unistd.h>
  12. #include <pthread.h>
  13. #include <semaphore.h>
  14. #include <time.h>
  15. #include <sys/time.h>
  16. #include <sys/ioctl.h>
  17. #include <fcntl.h>
  18. #include <csi_camera.h>
  19. #include <stdlib.h>
  20. #include "process_linker_types.h"
  21. #include "process_linker.h"
  22. #include "video_mem.h"
  23. #define NUM_OF_BUFFERS 5
  24. typedef struct _CsiPictureBuffer
  25. {
  26. unsigned int bus_address;
  27. void *virtual_address;
  28. unsigned int size;
  29. int fd;
  30. } CsiPictureBuffer;
  31. typedef struct _CsiPlinkContext
  32. {
  33. int useplink;
  34. int exitplink;
  35. CsiPictureBuffer sendbuffer[NUM_OF_BUFFERS];
  36. csi_frame_ex_s frame_buf[NUM_OF_BUFFERS];
  37. int sendid;
  38. int available_bufs;
  39. void *vmem;
  40. PlinkHandle plink;
  41. PlinkChannelID chnid;
  42. pthread_t buf_release_thread;
  43. pthread_mutex_t mutex;
  44. } CsiPlinkContext;
  45. static int get_buffer_count(PlinkPacket *pkt)
  46. {
  47. int ret = 0;
  48. for (int i = 0; i < pkt->num; i++)
  49. {
  50. PlinkDescHdr *hdr = (PlinkDescHdr *)(pkt->list[i]);
  51. if (hdr->type == PLINK_TYPE_MESSAGE)
  52. {
  53. int *data = (int *)(pkt->list[i] + DATA_HEADER_SIZE);
  54. if (*data == PLINK_EXIT_CODE)
  55. {
  56. ret |= 0x80000000; // set bit 31 to 1 to indicate 'exit'
  57. }
  58. else if (*data >= 0)
  59. ret++;
  60. }
  61. }
  62. return ret;
  63. }
  64. static void* camera_buf_release_process(CsiPlinkContext * plink_ctx)
  65. {
  66. PlinkPacket pkt = {0};
  67. if(plink_ctx == NULL)
  68. {
  69. return NULL;
  70. }
  71. LOG_O("Process is runing.....\n");
  72. while(!plink_ctx->exitplink)
  73. {
  74. if (plink_ctx->plink != NULL) {
  75. if (PLINK_wait(plink_ctx->plink, plink_ctx->chnid, 1000) == PLINK_STATUS_OK)
  76. {
  77. if (PLINK_recv(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK) {
  78. plink_ctx->exitplink = 1;
  79. break;
  80. }
  81. int count = get_buffer_count(&pkt);
  82. if (count >= 0)
  83. {
  84. PlinkMsg *hdr;
  85. int resp_id;
  86. for(int i=0;i<count;i++)
  87. {
  88. hdr = (PlinkMsg *)(pkt.list[i]);
  89. resp_id = hdr->msg;
  90. if((resp_id>=NUM_OF_BUFFERS))
  91. {
  92. LOG_W("invalid resp_id:%d\n",resp_id);
  93. continue;
  94. }
  95. if(plink_ctx->frame_buf[resp_id].frame_data.fd[0]!= 0)
  96. {
  97. csi_camera_put_frame(&plink_ctx->frame_buf[resp_id]);
  98. LOG_D("release resp_id:%d, fd:%d\n",resp_id,plink_ctx->frame_buf[resp_id].frame_data.fd[0]);
  99. plink_ctx->frame_buf[resp_id].frame_data.fd[0] = 0;
  100. }
  101. }
  102. pthread_mutex_lock(&plink_ctx->mutex);
  103. plink_ctx->available_bufs += count;
  104. pthread_mutex_unlock(&plink_ctx->mutex);
  105. }
  106. else {
  107. fprintf(stderr, "[SERVER] Exit!\n");
  108. plink_ctx->exitplink = 1;
  109. break;
  110. }
  111. }else
  112. {
  113. LOG_W("Plink Resp timeout\n");
  114. }
  115. }
  116. }
  117. LOG_O("Process is exit .....\n");
  118. return NULL;
  119. }
  120. static int allocate_sendbuffers(CsiPictureBuffer picbuffers[NUM_OF_BUFFERS], unsigned int size, void *vmem)
  121. {
  122. unsigned int buffer_size = (size + 0xFFF) & ~0xFFF;
  123. VmemParams params;
  124. params.size = buffer_size;
  125. params.flags = VMEM_FLAG_CONTIGUOUS | VMEM_FLAG_4GB_ADDR;
  126. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  127. {
  128. if (VMEM_allocate(vmem, &params) != 0) {
  129. return -1;
  130. }
  131. VMEM_mmap(vmem, &params);
  132. VMEM_export(vmem, &params);
  133. LOG_O("[SERVER] mmap %p from %x with size %d, dma-buf fd %d\n",
  134. params.vir_address, params.phy_address, params.size, params.fd);
  135. picbuffers[i].virtual_address = params.vir_address;
  136. picbuffers[i].bus_address = params.phy_address;
  137. picbuffers[i].size = buffer_size;
  138. picbuffers[i].fd = params.fd;
  139. }
  140. return 0;
  141. }
  142. void *vi_plink_create(csi_camera_channel_cfg_s *chn_cfg)
  143. {
  144. int ret = 0;
  145. CsiPlinkContext *plink_ctx = NULL;
  146. char *env = NULL;
  147. if (chn_cfg == NULL) {
  148. LOG_E("%s failt to get chn_cfg\n", __func__);
  149. return NULL;
  150. }
  151. plink_ctx = malloc(sizeof(CsiPlinkContext));
  152. if(!plink_ctx)
  153. {
  154. LOG_E("malloc faile\n");
  155. return NULL;
  156. }
  157. char *plinknamebase0 = "/tmp/plink.test";
  158. char *plinknamebase1 = "/tmp/plink_npu_rgb.test";
  159. char *plinknamebase2 = "/tmp/plink.test2";
  160. env = getenv("ISP_PLINK_NAME0");
  161. if (env != NULL) {
  162. plinknamebase0 = env;
  163. }
  164. env = getenv("ISP_PLINK_NAME1");
  165. if (env != NULL) {
  166. plinknamebase1 = env;
  167. }
  168. env = getenv("ISP_PLINK_NAME2");
  169. if (env != NULL) {
  170. plinknamebase2 = env;
  171. }
  172. char plinkname[32];
  173. if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_0) {
  174. sprintf(plinkname, "%s", plinknamebase0);
  175. } else if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_1) {
  176. sprintf(plinkname, "%s", plinknamebase1);
  177. } else {
  178. sprintf(plinkname, "%s", plinknamebase2);
  179. }
  180. fprintf(stderr, "%s, %d: ISP_PLINK_NAME = %s\n", __func__, __LINE__, plinkname);
  181. plink_ctx->exitplink = 0;
  182. fprintf(stderr, "%s, %d: Launching plink server...\n", __func__, __LINE__);
  183. memset(&plink_ctx->sendbuffer[0], 0, sizeof(plink_ctx->sendbuffer));
  184. if (VMEM_create(&plink_ctx->vmem) != VMEM_STATUS_OK)
  185. {
  186. fprintf(stderr, "Failed to create VMEM.");
  187. return NULL;
  188. }
  189. else {
  190. int framesize = 0;
  191. int stride = (chn_cfg->img_fmt.width + 127) & (~127); // align stride to 128
  192. switch (chn_cfg->img_fmt.pix_fmt) {
  193. case CSI_PIX_FMT_BGR:
  194. {
  195. framesize = stride * 304 * 3; //stride * chn_cfg->img_fmt.height * 3;
  196. if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
  197. return NULL;
  198. }
  199. // reset to black picture
  200. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  201. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  202. memset(plink_ctx->sendbuffer[i].virtual_address, 0, framesize);
  203. break;
  204. }
  205. case CSI_PIX_FMT_NV12:
  206. default:
  207. {
  208. framesize = stride * chn_cfg->img_fmt.height * 3 / 2;
  209. if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
  210. return NULL;
  211. }
  212. // reset to black picture
  213. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  214. for (int i = 0; i < NUM_OF_BUFFERS; i++) {
  215. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[i];
  216. memset(buf->virtual_address, 0, lumasize);
  217. memset(buf->virtual_address + lumasize, 0x80, lumasize/2);
  218. }
  219. break;
  220. }
  221. }
  222. }
  223. PLINK_create(&plink_ctx->plink, plinkname, PLINK_MODE_SERVER);
  224. if (plink_ctx->plink) {
  225. PLINK_connect(plink_ctx->plink, &plink_ctx->chnid);
  226. }
  227. plink_ctx->sendid = 0;
  228. plink_ctx->available_bufs = NUM_OF_BUFFERS;
  229. memset(plink_ctx->frame_buf,0x0,sizeof(plink_ctx->frame_buf));
  230. pthread_mutex_init(&plink_ctx->mutex,NULL);
  231. pthread_create(&plink_ctx->buf_release_thread,NULL,(void *)camera_buf_release_process,plink_ctx);
  232. return plink_ctx;
  233. }
  234. void vi_plink_release(void * plink)
  235. {
  236. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  237. if(plink_ctx)
  238. {
  239. plink_ctx->exitplink=1;
  240. pthread_join(plink_ctx->buf_release_thread,NULL);
  241. PLINK_close(plink_ctx->plink,plink_ctx->chnid);
  242. VMEM_destroy(plink_ctx->vmem);
  243. }
  244. }
  245. void display_camera_frame(void *plink, csi_frame_ex_s *frame)
  246. {
  247. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  248. if ((plink_ctx == NULL) || (frame == NULL)) {
  249. LOG_E("%s check param fail\n", __func__);
  250. return;
  251. }
  252. LOG_I("fmt=%d,fd=%d,stride:%d\n",frame->frame_info.pixel_format,frame->frame_data.fd[0],frame->frame_data.stride[0]);
  253. if (!plink_ctx->exitplink) {
  254. struct timeval tv_start, tv_end, tv_duration;
  255. gettimeofday(&tv_start, 0);
  256. PlinkPacket pkt = {0};
  257. // send one buffer if there is available slot
  258. if (!plink_ctx->exitplink&& plink_ctx->available_bufs > 0 &&
  259. plink_ctx->plink != NULL && ((frame->frame_info.pixel_format == CSI_PIX_FMT_RAW_8BIT)||
  260. (frame->frame_info.pixel_format == CSI_PIX_FMT_RAW_10BIT)||
  261. (frame->frame_info.pixel_format== CSI_PIX_FMT_RAW_12BIT)))
  262. {
  263. char str[200];
  264. static int i = 0;
  265. uint32_t dstw = frame->frame_info.width;//800;
  266. uint32_t dsth = frame->frame_info.height;//1280;
  267. uint32_t dsts = frame->frame_data.stride[0];//896;
  268. PlinkRawInfo info = {0};
  269. info.header.type = PLINK_TYPE_2D_RAW;
  270. info.header.size = DATA_SIZE(info);
  271. info.header.id = plink_ctx->sendid;
  272. info.format = PLINK_COLOR_FormatRawBayer10bit;
  273. // info.bus_address = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  274. // vi_mem_release(info.bus_address);
  275. info.img_width = dstw;
  276. info.img_height = dsth;
  277. info.stride = frame->frame_data.stride[0];
  278. info.offset = 0;
  279. pkt.list[0] = &info;
  280. pkt.num = 1;
  281. pkt.fd = frame->frame_data.fd[0];
  282. if(plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]!=0)
  283. {
  284. LOG_W("previous sendid :%d,fd %d not release,release it now\n",plink_ctx->sendid,plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]);
  285. csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
  286. }
  287. memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_ex_s));
  288. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  289. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  290. plink_ctx->exitplink = 1;
  291. gettimeofday(&tv_end, 0);
  292. timersub(&tv_end, &tv_start, &tv_duration);
  293. pthread_mutex_lock(&plink_ctx->mutex);
  294. plink_ctx->available_bufs -= 1;
  295. pthread_mutex_unlock(&plink_ctx->mutex);
  296. }
  297. else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 &&
  298. plink_ctx->plink != NULL && ((frame->frame_info.pixel_format == CSI_PIX_FMT_YUV_SEMIPLANAR_420)|| (frame->frame_info.pixel_format == CSI_PIX_FMT_NV12))) {
  299. char str[200];
  300. static int i = 0;
  301. uint32_t dstw = frame->frame_info.width;//800;
  302. uint32_t dsth = frame->frame_info.height;//1280;
  303. uint32_t dsts = frame->frame_data.stride[0];//896;
  304. PlinkYuvInfo info = {0};
  305. info.header.type = PLINK_TYPE_2D_YUV;
  306. info.header.size = DATA_SIZE(info);
  307. info.header.id = plink_ctx->sendid;
  308. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  309. // info.bus_address_y = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  310. // info.bus_address_u = info.bus_address_y + frame->img.dmabuf[1].offset;
  311. // vi_mem_release(info.bus_address_y);
  312. info.pic_width = dstw;
  313. info.pic_height = dsth;
  314. info.stride_y = dsts;
  315. info.stride_u = dsts;
  316. info.stride_v = dsts;
  317. info.offset_y = 0;
  318. info.offset_u = frame->frame_data.offset[1];
  319. info.offset_v = frame->frame_data.offset[2];
  320. pkt.list[0] = &info;
  321. pkt.num = 1;
  322. pkt.fd = frame->frame_data.fd[0];
  323. if(plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]!=0)
  324. {
  325. LOG_W("previous sendid :%d,fd %d not release,release it now\n",plink_ctx->sendid,plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]);
  326. csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
  327. }
  328. memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_ex_s));
  329. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  330. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  331. plink_ctx->exitplink = 1;
  332. gettimeofday(&tv_end, 0);
  333. timersub(&tv_end, &tv_start, &tv_duration);
  334. pthread_mutex_lock(&plink_ctx->mutex);
  335. plink_ctx->available_bufs -= 1;
  336. pthread_mutex_unlock(&plink_ctx->mutex);
  337. } else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && (frame->frame_info.pixel_format == CSI_PIX_FMT_BGR)) {
  338. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[plink_ctx->sendid];
  339. int y = 0;
  340. int buf_size = frame->frame_info.width*frame->frame_info.height*3;
  341. uint8_t *src = frame->frame_data.vir_addr[0] ;
  342. uint8_t *dst = buf->virtual_address;
  343. uint32_t srcw = frame->frame_info.width;
  344. uint32_t srch = frame->frame_info.height;
  345. uint32_t dstw = frame->frame_info.width;
  346. uint32_t dsth = frame->frame_info.height;
  347. uint32_t dsts = frame->frame_info.width;
  348. switch (frame->frame_info.pixel_format) {
  349. case CSI_PIX_FMT_BGR:
  350. {
  351. dstw = 300;
  352. dsth = 304;
  353. uint32_t w = dstw > srcw ? srcw : dstw;
  354. uint32_t h = dsth > srch ? srch : dsth;
  355. if (w != 300 || h != 176 || dsts != 304) {
  356. fprintf(stderr, "ERROR: expect 300x300 (stride 304) image while recieved %dx%d (stride %d) image\n", w, h, dsts);
  357. break;
  358. }
  359. for (y = 0; y < h; y++)
  360. {
  361. memcpy(dst, src, w);
  362. src += srcw;
  363. dst += dsts;
  364. }
  365. src = frame->frame_data.vir_addr[0] + srcw*srch;
  366. dst = buf->virtual_address + dsts*dsth;
  367. for (y = 0; y < h; y++)
  368. {
  369. memcpy(dst, src, w);
  370. src += srcw;
  371. dst += dsts;
  372. }
  373. src = frame->frame_data.vir_addr[0] + srcw*srch*2;
  374. dst = buf->virtual_address + dsts*dsth*2;
  375. for (y = 0; y < h; y++)
  376. {
  377. memcpy(dst, src, w);
  378. src += srcw;
  379. dst += dsts;
  380. }
  381. PlinkRGBInfo info = {0};
  382. info.header.type = PLINK_TYPE_2D_RGB;
  383. info.header.size = DATA_SIZE(info);
  384. info.header.id = plink_ctx->sendid + 1;
  385. info.format = PLINK_COLOR_Format24BitBGR888Planar;
  386. info.bus_address_b = buf->bus_address;
  387. info.bus_address_g = info.bus_address_b + dsts*dsth;
  388. info.bus_address_r = info.bus_address_g + dsts*dsth;
  389. info.img_width = dstw;
  390. info.img_height = dsth;
  391. info.stride_b = dsts;
  392. info.stride_g = dsts;
  393. info.stride_r = dsts;
  394. pkt.list[0] = &info;
  395. pkt.num = 1;
  396. pkt.fd = buf->fd;
  397. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  398. plink_ctx->exitplink = 1;
  399. break;
  400. }
  401. case CSI_PIX_FMT_NV12:
  402. default:
  403. {
  404. uint32_t w = dstw > srcw ? srcw : dstw;
  405. uint32_t h = dsth > srch ? srch : dsth;
  406. for (y = 0; y < h; y++)
  407. {
  408. memcpy(dst, src, w);
  409. src += srcw;
  410. dst += dsts;
  411. }
  412. src = frame->frame_data.vir_addr[0] + frame->frame_data.stride[0] * frame->frame_info.height;
  413. dst = buf->virtual_address + (dsts * dsth);
  414. for (y = 0; y < h/2; y++)
  415. {
  416. memcpy(dst, src, w);
  417. src += srcw;
  418. dst += dsts;
  419. }
  420. PlinkYuvInfo info = {0};
  421. info.header.type = PLINK_TYPE_2D_YUV;
  422. info.header.size = DATA_SIZE(info);
  423. info.header.id = plink_ctx->sendid + 1;
  424. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  425. info.bus_address_y = buf->bus_address;
  426. info.bus_address_u = buf->bus_address + dsts*dsth;
  427. info.bus_address_v = info.bus_address_u;
  428. info.pic_width = dstw;
  429. info.pic_height = dsth;
  430. info.stride_y = dsts;
  431. info.stride_u = dsts;
  432. info.stride_v = dsts;
  433. pkt.list[0] = &info;
  434. pkt.num = 1;
  435. pkt.fd = buf->fd;
  436. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  437. plink_ctx->exitplink = 1;
  438. break;
  439. }
  440. }
  441. csi_camera_put_frame(frame);
  442. gettimeofday(&tv_end, 0);
  443. timersub(&tv_end, &tv_start, &tv_duration);
  444. pthread_mutex_lock(&plink_ctx->mutex);
  445. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  446. plink_ctx->available_bufs -= 1;
  447. pthread_mutex_unlock(&plink_ctx->mutex);
  448. }
  449. else
  450. {
  451. LOG_W("frame not send by plink,available_bufs:%d,fmt:%d\n",plink_ctx->available_bufs,
  452. frame->frame_info.pixel_format);
  453. csi_camera_put_frame(frame);
  454. }
  455. }
  456. else
  457. {
  458. LOG_E("plink is exit\n");
  459. csi_camera_put_frame(frame);
  460. }
  461. LOG_I("%s exit \n");
  462. }