camera_frame_display.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503
  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_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].img.dmabuf[0].fds!= 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].img.dmabuf[0].fds);
  99. plink_ctx->frame_buf[resp_id].img.dmabuf[0].fds = 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. }
  141. void *vi_plink_create(csi_camera_channel_cfg_s *chn_cfg)
  142. {
  143. int ret = 0;
  144. CsiPlinkContext *plink_ctx = NULL;
  145. char *env=NULL;
  146. if (chn_cfg == NULL) {
  147. LOG_E("%s failt to get chn_cfg\n", __func__);
  148. return NULL;
  149. }
  150. plink_ctx = malloc(sizeof(CsiPlinkContext));
  151. if(!plink_ctx)
  152. {
  153. LOG_E("malloc faile\n");
  154. return NULL;
  155. }
  156. char *plinknamebase0 = "/tmp/plink.test";
  157. char *plinknamebase1 = "/tmp/plink_npu_rgb.test";
  158. char *plinknamebase2 = "/tmp/plink.test2";
  159. env = getenv("ISP_PLINK_NAME0");
  160. if (env != NULL) {
  161. plinknamebase0 = env;
  162. }
  163. env = getenv("ISP_PLINK_NAME1");
  164. if (env != NULL) {
  165. plinknamebase1 = env;
  166. }
  167. env = getenv("ISP_PLINK_NAME2");
  168. if (env != NULL) {
  169. plinknamebase2 = env;
  170. }
  171. char plinkname[32];
  172. if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_0) {
  173. sprintf(plinkname, "%s", plinknamebase0);
  174. } else if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_1) {
  175. sprintf(plinkname, "%s", plinknamebase1);
  176. } else {
  177. sprintf(plinkname, "%s", plinknamebase2);
  178. }
  179. fprintf(stderr, "%s, %d: ISP_PLINK_NAME = %s\n", __func__, __LINE__, plinkname);
  180. plink_ctx->exitplink = 0;
  181. fprintf(stderr, "%s, %d: Launching plink server...\n", __func__, __LINE__);
  182. memset(&plink_ctx->sendbuffer[0], 0, sizeof(plink_ctx->sendbuffer));
  183. if (VMEM_create(&plink_ctx->vmem) != VMEM_STATUS_OK)
  184. {
  185. fprintf(stderr, "Failed to create VMEM.");
  186. return NULL;
  187. }
  188. else {
  189. int framesize = 0;
  190. int stride = (chn_cfg->img_fmt.width + 127) & (~127); // align stride to 128
  191. switch (chn_cfg->img_fmt.pix_fmt) {
  192. case CSI_PIX_FMT_BGR:
  193. {
  194. framesize = stride * 304 * 3; //stride * chn_cfg->img_fmt.height * 3;
  195. if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
  196. return NULL;
  197. }
  198. // reset to black picture
  199. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  200. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  201. memset(plink_ctx->sendbuffer[i].virtual_address, 0, framesize);
  202. break;
  203. }
  204. case CSI_PIX_FMT_NV12:
  205. default:
  206. {
  207. framesize = stride * chn_cfg->img_fmt.height * 3 / 2;
  208. if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
  209. return NULL;
  210. }
  211. // reset to black picture
  212. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  213. for (int i = 0; i < NUM_OF_BUFFERS; i++) {
  214. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[i];
  215. memset(buf->virtual_address, 0, lumasize);
  216. memset(buf->virtual_address + lumasize, 0x80, lumasize/2);
  217. }
  218. break;
  219. }
  220. }
  221. }
  222. PLINK_create(&plink_ctx->plink, plinkname, PLINK_MODE_SERVER);
  223. if (plink_ctx->plink) {
  224. PLINK_connect(plink_ctx->plink, &plink_ctx->chnid);
  225. }
  226. plink_ctx->sendid = 0;
  227. plink_ctx->available_bufs = NUM_OF_BUFFERS;
  228. memset(plink_ctx->frame_buf,0x0,sizeof(plink_ctx->frame_buf));
  229. pthread_mutex_init(&plink_ctx->mutex,NULL);
  230. pthread_create(&plink_ctx->buf_release_thread,NULL,(void *)camera_buf_release_process,plink_ctx);
  231. return plink_ctx;
  232. }
  233. void vi_plink_release(void * plink)
  234. {
  235. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  236. if(plink_ctx)
  237. {
  238. plink_ctx->exitplink=1;
  239. pthread_join(plink_ctx->buf_release_thread,NULL);
  240. PLINK_close(plink_ctx->plink,plink_ctx->chnid);
  241. VMEM_destroy(plink_ctx->vmem);
  242. }
  243. }
  244. void display_camera_frame(void *plink, csi_frame_s *frame)
  245. {
  246. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  247. if ((plink_ctx == NULL) || (frame == NULL)) {
  248. LOG_E("%s check param fail\n", __func__);
  249. return;
  250. }
  251. LOG_O("fmt=%d img.strides[0] = %d\n",frame->img.pix_format, frame->img.strides[0]);
  252. if (!plink_ctx->exitplink) {
  253. struct timeval tv_start, tv_end, tv_duration;
  254. gettimeofday(&tv_start, 0);
  255. PlinkPacket pkt = {0};
  256. // send one buffer if there is available slot
  257. if (frame->img.type == CSI_IMG_TYPE_DMA_BUF && !plink_ctx->exitplink
  258. && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && ((frame->img.pix_format == CSI_PIX_FMT_RAW_8BIT)|| (frame->img.pix_format == CSI_PIX_FMT_RAW_12BIT))) {
  259. char str[200];
  260. static int i = 0;
  261. uint32_t dstw = frame->img.width;//800;
  262. uint32_t dsth = frame->img.height;//1280;
  263. uint32_t dsts = frame->img.strides[0];//896;
  264. PlinkRawInfo info = {0};
  265. info.header.type = PLINK_TYPE_2D_RAW;
  266. info.header.size = DATA_SIZE(info);
  267. info.header.id = plink_ctx->sendid;
  268. info.format = PLINK_COLOR_FormatRawBayer10bit;
  269. // info.bus_address = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  270. // vi_mem_release(info.bus_address);
  271. info.img_width = dstw;
  272. info.img_height = dsth;
  273. info.stride = frame->img.strides[0];
  274. info.offset = 0;
  275. pkt.list[0] = &info;
  276. pkt.num = 1;
  277. pkt.fd = frame->img.dmabuf[0].fds;
  278. if(plink_ctx->frame_buf[plink_ctx->sendid].img.dmabuf[0].fds!=0)
  279. {
  280. LOG_W("previous sendid :%d,fd %d not release,release it now\n",plink_ctx->sendid,plink_ctx->frame_buf[plink_ctx->sendid].img.dmabuf[0].fds);
  281. csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
  282. }
  283. memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_s));
  284. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  285. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  286. plink_ctx->exitplink = 1;
  287. gettimeofday(&tv_end, 0);
  288. timersub(&tv_end, &tv_start, &tv_duration);
  289. pthread_mutex_lock(&plink_ctx->mutex);
  290. plink_ctx->available_bufs -= 1;
  291. pthread_mutex_unlock(&plink_ctx->mutex);
  292. }
  293. else if (frame->img.type == CSI_IMG_TYPE_DMA_BUF && !plink_ctx->exitplink
  294. && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && ((frame->img.pix_format == CSI_PIX_FMT_YUV_SEMIPLANAR_420)|| (frame->img.pix_format == CSI_PIX_FMT_NV12))) {
  295. char str[200];
  296. static int i = 0;
  297. uint32_t dstw = frame->img.width;//800;
  298. uint32_t dsth = frame->img.height;//1280;
  299. uint32_t dsts = frame->img.strides[0];//896;
  300. PlinkYuvInfo info = {0};
  301. info.header.type = PLINK_TYPE_2D_YUV;
  302. info.header.size = DATA_SIZE(info);
  303. info.header.id = plink_ctx->sendid;
  304. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  305. // info.bus_address_y = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  306. // info.bus_address_u = info.bus_address_y + frame->img.dmabuf[1].offset;
  307. // vi_mem_release(info.bus_address_y);
  308. info.pic_width = dstw;
  309. info.pic_height = dsth;
  310. info.stride_y = dsts;
  311. info.stride_u = dsts;
  312. info.stride_v = dsts;
  313. info.offset_y = 0;
  314. info.offset_u = frame->img.dmabuf[1].offset;
  315. info.offset_v = frame->img.dmabuf[2].offset;
  316. pkt.list[0] = &info;
  317. pkt.num = 1;
  318. pkt.fd = frame->img.dmabuf[0].fds;
  319. if(plink_ctx->frame_buf[plink_ctx->sendid].img.dmabuf[0].fds!=0)
  320. {
  321. LOG_W("previous sendid :%d,fd %d not release,release it now\n",plink_ctx->sendid,plink_ctx->frame_buf[plink_ctx->sendid].img.dmabuf[0].fds);
  322. csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
  323. }
  324. memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_s));
  325. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  326. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  327. plink_ctx->exitplink = 1;
  328. gettimeofday(&tv_end, 0);
  329. timersub(&tv_end, &tv_start, &tv_duration);
  330. pthread_mutex_lock(&plink_ctx->mutex);
  331. plink_ctx->available_bufs -= 1;
  332. pthread_mutex_unlock(&plink_ctx->mutex);
  333. } else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && (frame->img.pix_format == CSI_PIX_FMT_BGR)) {
  334. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[plink_ctx->sendid];
  335. int y = 0;
  336. // if (1) {
  337. // void *pbuf[3];
  338. // void *phyaddr = vi_mem_import(frame->img.dmabuf[0].fds);
  339. // pbuf[0] = vi_mem_map(phyaddr) + frame->img.dmabuf[0].offset;
  340. // pbuf[1] = pbuf[0] + frame->img.dmabuf[1].offset;
  341. // vi_mem_release(phyaddr);
  342. // frame->img.usr_addr[0] = pbuf[0];
  343. // }
  344. uint8_t *src = frame->img.usr_addr[0];
  345. uint8_t *dst = buf->virtual_address;
  346. uint32_t srcw = frame->img.width;
  347. uint32_t srch = frame->img.height;
  348. uint32_t dstw = frame->img.width;
  349. uint32_t dsth = frame->img.height;
  350. uint32_t dsts = frame->img.width;
  351. switch (frame->img.pix_format ) {
  352. case CSI_PIX_FMT_BGR:
  353. {
  354. dstw = 300;
  355. dsth = 304;
  356. uint32_t w = dstw > srcw ? srcw : dstw;
  357. uint32_t h = dsth > srch ? srch : dsth;
  358. if (w != 300 || h != 176 || dsts != 304) {
  359. fprintf(stderr, "ERROR: expect 300x300 (stride 304) image while recieved %dx%d (stride %d) image\n", w, h, dsts);
  360. break;
  361. }
  362. for (y = 0; y < h; y++)
  363. {
  364. memcpy(dst, src, w);
  365. src += srcw;
  366. dst += dsts;
  367. }
  368. src = frame->img.usr_addr[0] + srcw*srch;
  369. dst = buf->virtual_address + dsts*dsth;
  370. for (y = 0; y < h; y++)
  371. {
  372. memcpy(dst, src, w);
  373. src += srcw;
  374. dst += dsts;
  375. }
  376. src = frame->img.usr_addr[0] + srcw*srch*2;
  377. dst = buf->virtual_address + dsts*dsth*2;
  378. for (y = 0; y < h; y++)
  379. {
  380. memcpy(dst, src, w);
  381. src += srcw;
  382. dst += dsts;
  383. }
  384. PlinkRGBInfo info = {0};
  385. info.header.type = PLINK_TYPE_2D_RGB;
  386. info.header.size = DATA_SIZE(info);
  387. info.header.id = plink_ctx->sendid + 1;
  388. info.format = PLINK_COLOR_Format24BitBGR888Planar;
  389. info.bus_address_b = buf->bus_address;
  390. info.bus_address_g = info.bus_address_b + dsts*dsth;
  391. info.bus_address_r = info.bus_address_g + dsts*dsth;
  392. info.img_width = dstw;
  393. info.img_height = dsth;
  394. info.stride_b = dsts;
  395. info.stride_g = dsts;
  396. info.stride_r = dsts;
  397. pkt.list[0] = &info;
  398. pkt.num = 1;
  399. pkt.fd = buf->fd;
  400. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  401. plink_ctx->exitplink = 1;
  402. break;
  403. }
  404. case CSI_PIX_FMT_NV12:
  405. default:
  406. {
  407. uint32_t w = dstw > srcw ? srcw : dstw;
  408. uint32_t h = dsth > srch ? srch : dsth;
  409. for (y = 0; y < h; y++)
  410. {
  411. memcpy(dst, src, w);
  412. src += srcw;
  413. dst += dsts;
  414. }
  415. src = frame->img.usr_addr[0] + frame->img.strides[0] * frame->img.height;
  416. dst = buf->virtual_address + (dsts * dsth);
  417. for (y = 0; y < h/2; y++)
  418. {
  419. memcpy(dst, src, w);
  420. src += srcw;
  421. dst += dsts;
  422. }
  423. PlinkYuvInfo info = {0};
  424. info.header.type = PLINK_TYPE_2D_YUV;
  425. info.header.size = DATA_SIZE(info);
  426. info.header.id = plink_ctx->sendid + 1;
  427. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  428. info.bus_address_y = buf->bus_address;
  429. info.bus_address_u = buf->bus_address + dsts*dsth;
  430. info.bus_address_v = info.bus_address_u;
  431. info.pic_width = dstw;
  432. info.pic_height = dsth;
  433. info.stride_y = dsts;
  434. info.stride_u = dsts;
  435. info.stride_v = dsts;
  436. pkt.list[0] = &info;
  437. pkt.num = 1;
  438. pkt.fd = buf->fd;
  439. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  440. plink_ctx->exitplink = 1;
  441. break;
  442. }
  443. }
  444. gettimeofday(&tv_end, 0);
  445. timersub(&tv_end, &tv_start, &tv_duration);
  446. pthread_mutex_lock(&plink_ctx->mutex);
  447. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  448. plink_ctx->available_bufs -= 1;
  449. pthread_mutex_unlock(&plink_ctx->mutex);
  450. }
  451. }
  452. LOG_O("%s exit \n", __func__);
  453. }