camera_frame_display.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438
  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. int sendid;
  37. int available_bufs;
  38. void *vmem;
  39. PlinkHandle plink;
  40. PlinkChannelID chnid;
  41. } CsiPlinkContext;
  42. static int get_buffer_count(PlinkPacket *pkt)
  43. {
  44. int ret = 0;
  45. for (int i = 0; i < pkt->num; i++)
  46. {
  47. PlinkDescHdr *hdr = (PlinkDescHdr *)(pkt->list[i]);
  48. if (hdr->type == PLINK_TYPE_MESSAGE)
  49. {
  50. int *data = (int *)(pkt->list[i] + DATA_HEADER_SIZE);
  51. if (*data == PLINK_EXIT_CODE)
  52. {
  53. ret |= 0x80000000; // set bit 31 to 1 to indicate 'exit'
  54. }
  55. else if (*data >= 0)
  56. ret++;
  57. }
  58. }
  59. return ret;
  60. }
  61. static void allocate_sendbuffers(CsiPictureBuffer picbuffers[NUM_OF_BUFFERS], unsigned int size, void *vmem)
  62. {
  63. unsigned int buffer_size = (size + 0xFFF) & ~0xFFF;
  64. VmemParams params;
  65. params.size = buffer_size;
  66. params.flags = VMEM_FLAG_CONTIGUOUS | VMEM_FLAG_4GB_ADDR;
  67. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  68. {
  69. VMEM_allocate(vmem, &params);
  70. VMEM_mmap(vmem, &params);
  71. VMEM_export(vmem, &params);
  72. LOG_O("[SERVER] mmap %p from %x with size %d, dma-buf fd %d\n",
  73. params.vir_address, params.phy_address, params.size, params.fd);
  74. picbuffers[i].virtual_address = params.vir_address;
  75. picbuffers[i].bus_address = params.phy_address;
  76. picbuffers[i].size = buffer_size;
  77. picbuffers[i].fd = params.fd;
  78. }
  79. }
  80. void *vi_plink_create(csi_camera_channel_cfg_s *chn_cfg)
  81. {
  82. int ret = 0;
  83. CsiPlinkContext *plink_ctx = NULL;
  84. char *env=NULL;
  85. if (chn_cfg == NULL) {
  86. LOG_E("%s failt to get chn_cfg\n", __func__);
  87. return NULL;
  88. }
  89. plink_ctx = malloc(sizeof(CsiPlinkContext));
  90. if(!plink_ctx)
  91. {
  92. LOG_E("malloc faile\n");
  93. return NULL;
  94. }
  95. char *plinknamebase0 = "/tmp/plink.test";
  96. char *plinknamebase1 = "/tmp/plink_npu_rgb.test";
  97. char *plinknamebase2 = "/tmp/plink.test2";
  98. env = getenv("ISP_PLINK_NAME0");
  99. if (env != NULL) {
  100. plinknamebase0 = env;
  101. }
  102. env = getenv("ISP_PLINK_NAME1");
  103. if (env != NULL) {
  104. plinknamebase1 = env;
  105. }
  106. env = getenv("ISP_PLINK_NAME2");
  107. if (env != NULL) {
  108. plinknamebase2 = env;
  109. }
  110. char plinkname[32];
  111. if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_0) {
  112. sprintf(plinkname, "%s", plinknamebase0);
  113. } else if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_1) {
  114. sprintf(plinkname, "%s", plinknamebase1);
  115. } else {
  116. sprintf(plinkname, "%s", plinknamebase2);
  117. }
  118. fprintf(stderr, "%s, %d: ISP_PLINK_NAME = %s\n", __func__, __LINE__, plinkname);
  119. plink_ctx->exitplink = 0;
  120. fprintf(stderr, "%s, %d: Launching plink server...\n", __func__, __LINE__);
  121. memset(&plink_ctx->sendbuffer[0], 0, sizeof(plink_ctx->sendbuffer));
  122. if (VMEM_create(&plink_ctx->vmem) != VMEM_STATUS_OK)
  123. {
  124. fprintf(stderr, "Failed to create VMEM.");
  125. return NULL;
  126. }
  127. else {
  128. int framesize = 0;
  129. int stride = (chn_cfg->img_fmt.width + 127) & (~127); // align stride to 128
  130. switch (chn_cfg->img_fmt.pix_fmt) {
  131. case CSI_PIX_FMT_BGR:
  132. {
  133. framesize = stride * 304 * 3; //stride * chn_cfg->img_fmt.height * 3;
  134. allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem);
  135. // reset to black picture
  136. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  137. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  138. memset(plink_ctx->sendbuffer[i].virtual_address, 0, framesize);
  139. break;
  140. }
  141. case CSI_PIX_FMT_NV12:
  142. default:
  143. {
  144. framesize = stride * chn_cfg->img_fmt.height * 3 / 2;
  145. allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem);
  146. // reset to black picture
  147. uint32_t lumasize = stride * chn_cfg->img_fmt.height;
  148. for (int i = 0; i < NUM_OF_BUFFERS; i++) {
  149. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[i];
  150. memset(buf->virtual_address, 0, lumasize);
  151. memset(buf->virtual_address + lumasize, 0x80, lumasize/2);
  152. }
  153. break;
  154. }
  155. }
  156. }
  157. PLINK_create(&plink_ctx->plink, plinkname, PLINK_MODE_SERVER);
  158. if (plink_ctx->plink) {
  159. PLINK_connect(plink_ctx->plink, &plink_ctx->chnid);
  160. }
  161. plink_ctx->sendid = 0;
  162. plink_ctx->available_bufs = NUM_OF_BUFFERS;
  163. return plink_ctx;
  164. }
  165. void vi_plink_release(void * plink)
  166. {
  167. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  168. if(plink_ctx)
  169. {
  170. PLINK_close(plink_ctx->plink,plink_ctx->chnid);
  171. VMEM_destroy(plink_ctx->vmem);
  172. }
  173. }
  174. void display_camera_frame(void *plink, csi_frame_s *frame)
  175. {
  176. CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
  177. if ((plink_ctx == NULL) || (frame == NULL)) {
  178. LOG_E("%s check param fail\n", __func__);
  179. return;
  180. }
  181. LOG_O("%sfmt=%d img.strides[0] = %d\n", __func__,frame->img.pix_format, frame->img.strides[0]);
  182. if (!plink_ctx->exitplink) {
  183. struct timeval tv_start, tv_end, tv_duration;
  184. gettimeofday(&tv_start, 0);
  185. // retrieve buffers
  186. PlinkPacket pkt = {0};
  187. if (plink_ctx->plink != NULL) {
  188. while (PLINK_wait(plink_ctx->plink, plink_ctx->chnid, 0) == PLINK_STATUS_OK)
  189. {
  190. if (PLINK_recv(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK) {
  191. plink_ctx->exitplink = 1;
  192. break;
  193. }
  194. int count = get_buffer_count(&pkt);
  195. if (count >= 0)
  196. plink_ctx->available_bufs += count;
  197. else {
  198. fprintf(stderr, "[SERVER] Exit!\n");
  199. plink_ctx->exitplink = 1;
  200. break;
  201. }
  202. }
  203. }
  204. /*
  205. void *phy_address = NULL;
  206. int dmabuf_fd = -1;
  207. if (vi_mem_dmabuf_isenable()) {
  208. phy_address = vi_mem_unmap(frame->img.usr_addr[0]);
  209. dmabuf_fd = vi_mem_export(phy_address);
  210. }
  211. */
  212. // send one buffer if there is available slot
  213. if (frame->img.type == CSI_IMG_TYPE_DMA_BUF && !plink_ctx->exitplink
  214. && 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))) {
  215. char str[200];
  216. static int i = 0;
  217. uint32_t dstw = frame->img.width;//800;
  218. uint32_t dsth = frame->img.height;//1280;
  219. uint32_t dsts = frame->img.strides[0];//896;
  220. i++;
  221. sprintf(str, "echo frame is %d > ~/frame", i);
  222. system(str);
  223. PlinkRawInfo info = {0};
  224. info.header.type = PLINK_TYPE_2D_RAW;
  225. info.header.size = DATA_SIZE(info);
  226. info.header.id = plink_ctx->sendid + 1;
  227. info.format = PLINK_COLOR_FormatRawBayer10bit;
  228. // info.bus_address = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  229. // vi_mem_release(info.bus_address);
  230. info.img_width = dstw;
  231. info.img_height = dsth;
  232. info.stride = frame->img.strides[0];
  233. info.offset = 0;
  234. pkt.list[0] = &info;
  235. pkt.num = 1;
  236. pkt.fd = frame->img.dmabuf[0].fds;
  237. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  238. plink_ctx->exitplink = 1;
  239. gettimeofday(&tv_end, 0);
  240. timersub(&tv_end, &tv_start, &tv_duration);
  241. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  242. plink_ctx->available_bufs -= 1;
  243. }
  244. else if (frame->img.type == CSI_IMG_TYPE_DMA_BUF && !plink_ctx->exitplink
  245. && 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))) {
  246. char str[200];
  247. static int i = 0;
  248. uint32_t dstw = frame->img.width;//800;
  249. uint32_t dsth = frame->img.height;//1280;
  250. uint32_t dsts = frame->img.strides[0];//896;
  251. i++;
  252. sprintf(str, "echo frame is %d > ~/frame", i);
  253. system(str);
  254. PlinkYuvInfo info = {0};
  255. info.header.type = PLINK_TYPE_2D_YUV;
  256. info.header.size = DATA_SIZE(info);
  257. info.header.id = plink_ctx->sendid + 1;
  258. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  259. // info.bus_address_y = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
  260. // info.bus_address_u = info.bus_address_y + frame->img.dmabuf[1].offset;
  261. // vi_mem_release(info.bus_address_y);
  262. info.pic_width = dstw;
  263. info.pic_height = dsth;
  264. info.stride_y = dsts;
  265. info.stride_u = dsts;
  266. info.stride_v = dsts;
  267. info.offset_y = 0;
  268. info.offset_u = frame->img.dmabuf[1].offset;
  269. info.offset_v = frame->img.dmabuf[2].offset;
  270. pkt.list[0] = &info;
  271. pkt.num = 1;
  272. pkt.fd = frame->img.dmabuf[0].fds;
  273. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  274. plink_ctx->exitplink = 1;
  275. gettimeofday(&tv_end, 0);
  276. timersub(&tv_end, &tv_start, &tv_duration);
  277. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  278. plink_ctx->available_bufs -= 1;
  279. } else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && (frame->img.pix_format == CSI_PIX_FMT_BGR)) {
  280. CsiPictureBuffer *buf = &plink_ctx->sendbuffer[plink_ctx->sendid];
  281. int y = 0;
  282. uint8_t *src = frame->img.usr_addr[0];
  283. uint8_t *dst = buf->virtual_address;
  284. uint32_t srcw = frame->img.width;
  285. uint32_t srch = frame->img.height;
  286. uint32_t dstw = frame->img.width;
  287. uint32_t dsth = frame->img.height;
  288. uint32_t dsts = frame->img.width;
  289. switch (frame->img.pix_format ) {
  290. case CSI_PIX_FMT_BGR:
  291. {
  292. dstw = 300;
  293. dsth = 304;
  294. uint32_t w = dstw > srcw ? srcw : dstw;
  295. uint32_t h = dsth > srch ? srch : dsth;
  296. if (w != 300 || h != 176 || dsts != 304) {
  297. fprintf(stderr, "ERROR: expect 300x300 (stride 304) image while recieved %dx%d (stride %d) image\n", w, h, dsts);
  298. break;
  299. }
  300. for (y = 0; y < h; y++)
  301. {
  302. memcpy(dst, src, w);
  303. src += srcw;
  304. dst += dsts;
  305. }
  306. src = frame->img.usr_addr[0] + srcw*srch;
  307. dst = buf->virtual_address + dsts*dsth;
  308. for (y = 0; y < h; y++)
  309. {
  310. memcpy(dst, src, w);
  311. src += srcw;
  312. dst += dsts;
  313. }
  314. src = frame->img.usr_addr[0] + srcw*srch*2;
  315. dst = buf->virtual_address + dsts*dsth*2;
  316. for (y = 0; y < h; y++)
  317. {
  318. memcpy(dst, src, w);
  319. src += srcw;
  320. dst += dsts;
  321. }
  322. PlinkRGBInfo info = {0};
  323. info.header.type = PLINK_TYPE_2D_RGB;
  324. info.header.size = DATA_SIZE(info);
  325. info.header.id = plink_ctx->sendid + 1;
  326. info.format = PLINK_COLOR_Format24BitBGR888Planar;
  327. info.bus_address_b = buf->bus_address;
  328. info.bus_address_g = info.bus_address_b + dsts*dsth;
  329. info.bus_address_r = info.bus_address_g + dsts*dsth;
  330. info.img_width = dstw;
  331. info.img_height = dsth;
  332. info.stride_b = dsts;
  333. info.stride_g = dsts;
  334. info.stride_r = dsts;
  335. pkt.list[0] = &info;
  336. pkt.num = 1;
  337. pkt.fd = buf->fd;
  338. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  339. plink_ctx->exitplink = 1;
  340. break;
  341. }
  342. case CSI_PIX_FMT_NV12:
  343. default:
  344. {
  345. uint32_t w = dstw > srcw ? srcw : dstw;
  346. uint32_t h = dsth > srch ? srch : dsth;
  347. for (y = 0; y < h; y++)
  348. {
  349. memcpy(dst, src, w);
  350. src += srcw;
  351. dst += dsts;
  352. }
  353. src = frame->img.usr_addr[0] + frame->img.strides[0] * frame->img.height;
  354. dst = buf->virtual_address + (dsts * dsth);
  355. for (y = 0; y < h/2; y++)
  356. {
  357. memcpy(dst, src, w);
  358. src += srcw;
  359. dst += dsts;
  360. }
  361. PlinkYuvInfo info = {0};
  362. info.header.type = PLINK_TYPE_2D_YUV;
  363. info.header.size = DATA_SIZE(info);
  364. info.header.id = plink_ctx->sendid + 1;
  365. info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
  366. info.bus_address_y = buf->bus_address;
  367. info.bus_address_u = buf->bus_address + dsts*dsth;
  368. info.bus_address_v = info.bus_address_u;
  369. info.pic_width = dstw;
  370. info.pic_height = dsth;
  371. info.stride_y = dsts;
  372. info.stride_u = dsts;
  373. info.stride_v = dsts;
  374. pkt.list[0] = &info;
  375. pkt.num = 1;
  376. pkt.fd = buf->fd;
  377. if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
  378. plink_ctx->exitplink = 1;
  379. break;
  380. }
  381. }
  382. gettimeofday(&tv_end, 0);
  383. timersub(&tv_end, &tv_start, &tv_duration);
  384. plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
  385. plink_ctx->available_bufs -= 1;
  386. }
  387. }
  388. LOG_O("%s exit \n", __func__);
  389. }