plink_stitcher.c 24 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766
  1. /*
  2. * Copyright (c) 2022 Alibaba Group. All rights reserved.
  3. * License-Identifier: Apache-2.0
  4. *
  5. * Licensed under the Apache License, Version 2.0 (the "License"); you may
  6. * not use this file except in compliance with the License.
  7. * You may obtain a copy of the License at
  8. *
  9. * http://www.apache.org/licenses/LICENSE-2.0
  10. *
  11. * Unless required by applicable law or agreed to in writing, software
  12. * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
  13. * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14. * See the License for the specific language governing permissions and
  15. * limitations under the License.
  16. *
  17. */
  18. #include <stdio.h>
  19. #include <stdlib.h>
  20. #include <unistd.h>
  21. #include <fcntl.h>
  22. #include <sys/mman.h>
  23. #include <pthread.h>
  24. #include <semaphore.h>
  25. #include <memory.h>
  26. #include "process_linker_types.h"
  27. #include "video_mem.h"
  28. #ifndef NULL
  29. #define NULL ((void *)0)
  30. #endif
  31. #define MAX_NUM_OF_INPUTS 4
  32. #define NUM_OF_BUFFERS 5
  33. #define errExit(msg) do { perror(msg); exit(EXIT_FAILURE); \
  34. } while (0)
  35. #define STITCHER_MIN(a, b) ((a) < (b) ? (a) : (b))
  36. typedef enum _StitchLayout
  37. {
  38. STITCH_LAYOUT_Vertical = 0,
  39. STITCH_LAYOUT_Horizontal,
  40. STITCH_LAYOUT_Matrix,
  41. STITCH_LAYOUT_Max
  42. } StitchLayout;
  43. typedef struct _StitherRegion
  44. {
  45. int width;
  46. int height;
  47. int offset_uv;
  48. int offset_y;
  49. } StitcherRegion;
  50. typedef struct _StitcherParams
  51. {
  52. char *in_name[MAX_NUM_OF_INPUTS];
  53. char *out_name;
  54. StitchLayout layout;
  55. PlinkColorFormat format;
  56. int width;
  57. int height;
  58. int stride;
  59. } StitcherParams;
  60. typedef struct _StitcherPort
  61. {
  62. char *name;
  63. int index;
  64. PlinkChannelID id;
  65. PlinkHandle plink;
  66. int sendid;
  67. int backid;
  68. int *exit;
  69. int available_bufs;
  70. void *vmem;
  71. pthread_mutex_t pic_mutex;
  72. pthread_mutex_t *count_mutex;
  73. sem_t *sem_ready;
  74. sem_t *sem_done;
  75. StitchLayout layout;
  76. PlinkColorFormat format;
  77. void *buffer;
  78. int width;
  79. int height;
  80. int stride;
  81. int offset;
  82. int offset_uv;
  83. int *in_count;
  84. } StitcherPort;
  85. typedef struct _StitcherContext
  86. {
  87. StitcherPort in[MAX_NUM_OF_INPUTS];
  88. StitcherPort out;
  89. pthread_mutex_t count_mutex;
  90. sem_t sem_ready;
  91. sem_t sem_done;
  92. int in_count;
  93. int exitcode;
  94. } StitcherContext;
  95. typedef struct _PictureBuffer
  96. {
  97. unsigned int bus_address;
  98. void *virtual_address;
  99. unsigned int size;
  100. int fd;
  101. } PictureBuffer;
  102. static void printUsage(char *name)
  103. {
  104. printf("usage: %s [options]\n"
  105. "\n"
  106. " Stitch multiple pictures to one. Maximum # of pictures to be stitched is %d\n"
  107. " Available options:\n"
  108. " -i<n> plink file name of input port #n (default: /tmp/plink.stitch.in<n>). n is 0 based.\n"
  109. " -o plink file name of output port (default: /tmp/plink.stitch.out)\n"
  110. " -l layout (default: 0)\n"
  111. " 0 - vertical\n"
  112. " 1 - horizontal\n"
  113. " 2 - matrix\n"
  114. " -f output color format (default: 3)\n"
  115. " 3 - NV12\n"
  116. " -w output video width (default: 800)\n"
  117. " -h output video height (default: 1280)\n"
  118. " -s output video buffer stride (default: video width)\n"
  119. " --help print this message\n"
  120. "\n", name, MAX_NUM_OF_INPUTS);
  121. }
  122. static void parseParams(int argc, char **argv, StitcherParams *params)
  123. {
  124. int i = 1;
  125. memset(params, 0, sizeof(*params));
  126. params->in_name[0] = "/tmp/plink.stitch.in0";
  127. params->in_name[1] = "/tmp/plink.stitch.in1";
  128. params->in_name[2] = "/tmp/plink.stitch.in2";
  129. params->in_name[3] = "/tmp/plink.stitch.in3";
  130. params->out_name = "/tmp/plink.stitch.out";
  131. params->layout = STITCH_LAYOUT_Vertical;
  132. params->format = PLINK_COLOR_FormatYUV420SemiPlanar;
  133. params->width = 800;
  134. params->height = 1280;
  135. while (i < argc)
  136. {
  137. if (argv[i][0] != '-' || strlen(argv[i]) < 2)
  138. {
  139. i++;
  140. continue;
  141. }
  142. if (argv[i][1] == 'i')
  143. {
  144. if (++i < argc)
  145. {
  146. if (strlen(argv[i-1]) > 2) // input name
  147. {
  148. int id = atoi(argv[i-1]+2);
  149. if (id < MAX_NUM_OF_INPUTS)
  150. params->in_name[id] = argv[i++];
  151. }
  152. else
  153. params->in_name[0] = argv[i++];
  154. }
  155. }
  156. else if (argv[i][1] == 'o')
  157. {
  158. if (++i < argc)
  159. params->out_name = argv[i++];
  160. }
  161. else if (argv[i][1] == 'l')
  162. {
  163. if (++i < argc)
  164. params->layout = atoi(argv[i++]);
  165. }
  166. else if (argv[i][1] == 'f')
  167. {
  168. if (++i < argc)
  169. params->format = atoi(argv[i++]);
  170. }
  171. else if (argv[i][1] == 'w')
  172. {
  173. if (++i < argc)
  174. params->width = atoi(argv[i++]);
  175. }
  176. else if (argv[i][1] == 'h')
  177. {
  178. if (++i < argc)
  179. params->height = atoi(argv[i++]);
  180. }
  181. else if (argv[i][1] == 's')
  182. {
  183. if (++i < argc)
  184. params->stride = atoi(argv[i++]);
  185. }
  186. else if (strcmp(argv[i], "--help") == 0)
  187. {
  188. params->layout = STITCH_LAYOUT_Max;
  189. i++;
  190. }
  191. }
  192. if (params->stride == 0)
  193. params->stride = params->width;
  194. printf("[STITCHER] Input 0 Name : %s\n", params->in_name[0]);
  195. printf("[STITCHER] Input 1 Name : %s\n", params->in_name[1]);
  196. printf("[STITCHER] Input 2 Name : %s\n", params->in_name[2]);
  197. printf("[STITCHER] Input 3 Name : %s\n", params->in_name[3]);
  198. printf("[STITCHER] Output Name : %s\n", params->out_name);
  199. printf("[STITCHER] Output Layout : %d\n", params->layout);
  200. printf("[STITCHER] Output Format : %d\n", params->format);
  201. printf("[STITCHER] Output Resolution : %dx%d\n", params->width, params->height);
  202. printf("[STITCHER] Output Stride : %d\n", params->stride);
  203. }
  204. static int checkParams(StitcherParams *params)
  205. {
  206. if (params->format != PLINK_COLOR_FormatYUV420SemiPlanar ||
  207. params->layout >= STITCH_LAYOUT_Max)
  208. return -1;
  209. return 0;
  210. }
  211. static int getBufferSize(StitcherParams *params)
  212. {
  213. int size = 0;
  214. switch (params->format)
  215. {
  216. case PLINK_COLOR_FormatYUV420Planar:
  217. case PLINK_COLOR_FormatYUV420SemiPlanar:
  218. size = params->stride * params->height * 3 / 2;
  219. break;
  220. default:
  221. size = 0;
  222. }
  223. return size;
  224. }
  225. static void AllocateBuffers(PictureBuffer picbuffers[NUM_OF_BUFFERS], unsigned int size, void *vmem)
  226. {
  227. unsigned int buffer_size = (size + 0xFFF) & ~0xFFF;
  228. VmemParams params;
  229. params.size = buffer_size;
  230. params.flags = VMEM_FLAG_CONTIGUOUS | VMEM_FLAG_4GB_ADDR;
  231. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  232. {
  233. params.fd = 0;
  234. VMEM_allocate(vmem, &params);
  235. VMEM_mmap(vmem, &params);
  236. VMEM_export(vmem, &params);
  237. printf("[STITCHER] mmap %p from %x with size %d, dma-buf fd %d\n",
  238. params.vir_address, params.phy_address, params.size, params.fd);
  239. picbuffers[i].virtual_address = params.vir_address;
  240. picbuffers[i].bus_address = params.phy_address;
  241. picbuffers[i].size = buffer_size;
  242. picbuffers[i].fd = params.fd;
  243. }
  244. }
  245. static void FreeBuffers(PictureBuffer picbuffers[NUM_OF_BUFFERS], void *vmem)
  246. {
  247. VmemParams params;
  248. memset(&params, 0, sizeof(params));
  249. for (int i = 0; i < NUM_OF_BUFFERS; i++)
  250. {
  251. close(picbuffers[i].fd);
  252. params.size = picbuffers[i].size;
  253. params.vir_address = picbuffers[i].virtual_address;
  254. params.phy_address = picbuffers[i].bus_address;
  255. VMEM_free(vmem, &params);
  256. }
  257. }
  258. static void getRegion(StitcherPort *out, int index, int in_count, StitcherRegion *region)
  259. {
  260. region->offset_uv = 0; //out->stride * out->height;
  261. if (out->layout == STITCH_LAYOUT_Horizontal)
  262. {
  263. int width = (out->width / in_count + 1) & ~1;
  264. region->offset_y = width * index;
  265. region->offset_uv += width * index;
  266. region->width = width;
  267. region->height = out->height;
  268. }
  269. else if (out->layout == STITCH_LAYOUT_Matrix && in_count >= 3)
  270. {
  271. int y = index >> 1;
  272. int x = index & 1;
  273. int width = (out->width / 2 + 1) & ~1;
  274. int height = (out->height / 2 + 1) & ~1;
  275. region->offset_y = out->stride * (height * y);
  276. region->offset_y += width * x;
  277. region->offset_uv += out->stride * (height * y / 2);
  278. region->offset_uv += width * x;
  279. region->width = width;
  280. region->height = height;
  281. }
  282. else // (out->layout == STITCH_LAYOUT_Vertical)
  283. {
  284. int height = (out->height / in_count + 1) & ~1;
  285. region->offset_y = out->stride * (height * index);
  286. region->offset_uv += out->stride * (height * index / 2);
  287. region->width = out->width;
  288. region->height = height;
  289. }
  290. }
  291. static int stitchOneFrame(StitcherContext *ctx)
  292. {
  293. StitcherPort *in = NULL;
  294. StitcherPort *out = &ctx->out;
  295. int in_count = 0;
  296. do
  297. {
  298. pthread_mutex_lock(out->count_mutex);
  299. in_count = ctx->in_count;
  300. pthread_mutex_unlock(out->count_mutex);
  301. if (in_count > 0)
  302. break;
  303. else
  304. sleep(1);
  305. } while (1);
  306. for (int i = 0; i < in_count; i++)
  307. {
  308. in = &ctx->in[i];
  309. if (in->index == 0)
  310. {
  311. sem_wait(&ctx->sem_ready); // wait for picture from input port0.
  312. if (ctx->exitcode == 1)
  313. return 1;
  314. }
  315. StitcherRegion region;
  316. getRegion(out, in->index, in_count, &region);
  317. pthread_mutex_lock(&in->pic_mutex);
  318. int width = STITCHER_MIN(region.width, in->width);
  319. int height = STITCHER_MIN(region.height, in->height);
  320. void *dst = out->buffer + out->offset + region.offset_y;
  321. void *src = in->buffer + in->offset;
  322. //fprintf(stderr, "%d: %p, %d, %d, %p, %d\n", in->index, out->buffer, out->offset, region.offset_y, in->buffer, in->offset);
  323. //fprintf(stderr, "%d: %dx%d, %d, %d\n", in->index, width, height, in->stride, out->stride);
  324. if (in->available_bufs > 0)
  325. {
  326. if (in->format == PLINK_COLOR_FormatYUV420SemiPlanarP010 ||
  327. in->format == PLINK_COLOR_FormatRawBayer10bit ||
  328. in->format == PLINK_COLOR_FormatRawBayer12bit)
  329. {
  330. int shift = in->format == PLINK_COLOR_FormatYUV420SemiPlanarP010 ? 2 : 4;
  331. unsigned int temp[4];
  332. for (int h = 0; h < height; h++)
  333. {
  334. unsigned int *dst32 = (unsigned int *)dst;
  335. unsigned short *src16 = (unsigned short *)src;
  336. for (int w = 0; w < width; w+=4)
  337. {
  338. temp[0] = ((unsigned int)(src16[w+0] >> shift) & 0xFF);
  339. temp[1] = ((unsigned int)(src16[w+1] >> shift) & 0xFF) << 8;
  340. temp[2] = ((unsigned int)(src16[w+2] >> shift) & 0xFF) << 16;
  341. temp[3] = ((unsigned int)(src16[w+3] >> shift) & 0xFF) << 24;
  342. dst32[w>>2] = temp[0] | temp[1] | temp[2] | temp[3];
  343. }
  344. dst += out->stride;
  345. src += in->stride;
  346. }
  347. }
  348. else
  349. {
  350. for (int h = 0; h < height; h++)
  351. {
  352. memcpy(dst, src, width);
  353. dst += out->stride;
  354. src += in->stride;
  355. }
  356. }
  357. }
  358. else
  359. {
  360. for (int h = 0; h < height; h++)
  361. {
  362. memset(dst, 0x00, width);
  363. dst += out->stride;
  364. }
  365. }
  366. dst = out->buffer + out->offset_uv + region.offset_uv;
  367. src = in->buffer + in->offset_uv;
  368. //fprintf(stderr, "%d: %p, %d, %d, %p, %d\n", in->index, out->buffer, out->offset_uv, region.offset_uv, in->buffer, in->offset_uv);
  369. if (in->format == PLINK_COLOR_FormatYUV420SemiPlanar &&
  370. in->available_bufs > 0)
  371. {
  372. for (int h = 0; h < height/2; h++)
  373. {
  374. memcpy(dst, src, width);
  375. dst += out->stride;
  376. src += in->stride;
  377. }
  378. }
  379. else // treat all the other formats as monochrome
  380. {
  381. for (int h = 0; h < height/2; h++)
  382. {
  383. memset(dst, 0x80, width);
  384. dst += out->stride;
  385. }
  386. }
  387. pthread_mutex_unlock(&in->pic_mutex);
  388. if (in->index == 0)
  389. sem_post(&ctx->sem_done); // Resume input port0.
  390. // !Assume output thread is fast enough, so input port0 won't be blocked for long time.
  391. }
  392. return 0;
  393. }
  394. static void constructYuvInfo(PlinkYuvInfo *info, StitcherParams *params, unsigned int bus_address, int id)
  395. {
  396. int size_y = params->width * params->stride;
  397. int size_uv = size_y / 2;
  398. int size_yuv = size_y + size_uv;
  399. info->header.type = PLINK_TYPE_2D_YUV;
  400. info->header.size = DATA_SIZE(*info);
  401. info->header.id = id + 1;
  402. info->format = params->format;
  403. info->bus_address_y = bus_address;
  404. info->bus_address_u = info->bus_address_y + size_y;
  405. info->bus_address_v = info->bus_address_u + (size_uv / 2);
  406. info->pic_width = params->width;
  407. info->pic_height = params->height;
  408. info->stride_y = params->stride;
  409. info->stride_u =
  410. params->format == PLINK_COLOR_FormatYUV420Planar ?
  411. params->stride/2 : params->stride;
  412. info->stride_v = info->stride_u;
  413. }
  414. int getBufferCount(PlinkPacket *pkt)
  415. {
  416. int ret = 0;
  417. for (int i = 0; i < pkt->num; i++)
  418. {
  419. PlinkDescHdr *hdr = (PlinkDescHdr *)(pkt->list[i]);
  420. if (hdr->type == PLINK_TYPE_MESSAGE)
  421. {
  422. int *data = (int *)(pkt->list[i] + DATA_HEADER_SIZE);
  423. if (*data == PLINK_EXIT_CODE)
  424. {
  425. ret |= 0x80000000; // set bit 31 to 1 to indicate 'exit'
  426. }
  427. else if (*data >= 0)
  428. ret++;
  429. }
  430. }
  431. return ret;
  432. }
  433. static void retreiveSentBuffers(PlinkHandle plink, StitcherPort *port)
  434. {
  435. PlinkStatus sts = PLINK_STATUS_OK;
  436. PlinkPacket pkt = {0};
  437. while (port->available_bufs < NUM_OF_BUFFERS && sts == PLINK_STATUS_OK)
  438. {
  439. do
  440. {
  441. sts = PLINK_recv(plink, port->id, &pkt);
  442. int count = getBufferCount(&pkt);
  443. if (count > 0)
  444. {
  445. port->available_bufs += count;
  446. }
  447. } while (sts == PLINK_STATUS_MORE_DATA);
  448. }
  449. }
  450. static void *input_thread(void *args)
  451. {
  452. StitcherPort *port = (StitcherPort *)args;
  453. PlinkHandle plink = NULL;
  454. PlinkStatus sts = PLINK_STATUS_OK;
  455. void *vmem = port->vmem;
  456. pthread_mutex_init(&port->pic_mutex, NULL);
  457. if (PLINK_create(&plink, port->name, PLINK_MODE_CLIENT) != PLINK_STATUS_OK)
  458. return NULL;
  459. do
  460. {
  461. sts = PLINK_connect_ex(plink, NULL, 1000);
  462. } while (sts == PLINK_STATUS_TIMEOUT && *port->exit == 0);
  463. if (sts != PLINK_STATUS_OK)
  464. {
  465. PLINK_close(plink, 0);
  466. return NULL;
  467. }
  468. pthread_mutex_lock(port->count_mutex);
  469. port->index = *(port->in_count);
  470. *(port->in_count) = *(port->in_count) + 1;
  471. pthread_mutex_unlock(port->count_mutex);
  472. PlinkPacket sendpkt = {0};
  473. PlinkPacket recvpkt = {0};
  474. PlinkMsg msg = {0};
  475. VmemParams params;
  476. int exitcode = 0;
  477. do {
  478. sts = PLINK_recv(plink, 0, &recvpkt);
  479. if (sts == PLINK_STATUS_ERROR)
  480. break;
  481. for (int i = 0; i < recvpkt.num; i++)
  482. {
  483. PlinkDescHdr *hdr = (PlinkDescHdr *)(recvpkt.list[i]);
  484. if (hdr->type == PLINK_TYPE_2D_YUV ||
  485. hdr->type == PLINK_TYPE_2D_RAW)
  486. {
  487. // return previous buffer to source
  488. if (sendpkt.num > 0)
  489. {
  490. if (port->index == 0)
  491. sem_wait(port->sem_done);
  492. pthread_mutex_lock(&port->pic_mutex);
  493. if (VMEM_release(vmem, &params) != VMEM_STATUS_OK)
  494. fprintf(stderr, "[STITCHER] ERROR: Failed to release buffer.\n");
  495. sts = PLINK_send(plink, 0, &sendpkt);
  496. if (sts == PLINK_STATUS_ERROR)
  497. break;
  498. if (sendpkt.fd != PLINK_INVALID_FD)
  499. close(sendpkt.fd);
  500. port->available_bufs--;
  501. sendpkt.num = 0;
  502. }
  503. else
  504. pthread_mutex_lock(&port->pic_mutex);
  505. memset(&params, 0, sizeof(params));
  506. if (recvpkt.fd != PLINK_INVALID_FD)
  507. {
  508. params.fd = recvpkt.fd;
  509. if (VMEM_import(vmem, &params) != VMEM_STATUS_OK)
  510. break;
  511. if (VMEM_mmap(vmem, &params) != VMEM_STATUS_OK)
  512. break;
  513. }
  514. if (port->index == 0)
  515. sem_post(port->sem_ready); // signal output thread that one picture is ready
  516. }
  517. if (hdr->type == PLINK_TYPE_2D_YUV)
  518. {
  519. PlinkYuvInfo *pic = (PlinkYuvInfo *)(recvpkt.list[i]);
  520. printf("[STITCHER] Input%d: Received YUV frame %d 0x%010llx from %s: fd %d, %dx%d, stride = luma %d, chroma %d\n",
  521. port->index, pic->header.id, pic->bus_address_y, port->name, recvpkt.fd,
  522. pic->pic_width, pic->pic_height,
  523. pic->stride_y, pic->stride_u);
  524. port->buffer = params.vir_address;
  525. port->format = pic->format;
  526. port->width = pic->pic_width;
  527. port->height = pic->pic_height;
  528. port->stride = pic->stride_y;
  529. port->offset = pic->offset_y;
  530. port->offset_uv = pic->offset_u > 0 ? pic->offset_u : (pic->offset_y + pic->pic_height*pic->stride_y);
  531. }
  532. else if (hdr->type == PLINK_TYPE_2D_RAW)
  533. {
  534. PlinkRawInfo *img = (PlinkRawInfo *)(recvpkt.list[i]);
  535. printf("[STITCHER] Input%d: Received RAW frame %d 0x%010llx from %s: fd %d, %dx%d, stride %d\n",
  536. port->index, img->header.id, img->bus_address, port->name, recvpkt.fd,
  537. img->img_width, img->img_height, img->stride);
  538. port->buffer = params.vir_address;
  539. port->format = img->format;
  540. port->width = img->img_width;
  541. port->height = img->img_height;
  542. port->stride = img->stride;
  543. port->offset = img->offset;
  544. }
  545. else if (hdr->type == PLINK_TYPE_MESSAGE)
  546. {
  547. PlinkMsg *msg = (PlinkMsg *)(recvpkt.list[i]);
  548. if (msg->msg == PLINK_EXIT_CODE)
  549. {
  550. exitcode = 1;
  551. printf("[STITCHER] Input %d: Exit\n", port->index);
  552. break;
  553. }
  554. }
  555. if (hdr->type == PLINK_TYPE_2D_YUV ||
  556. hdr->type == PLINK_TYPE_2D_RAW)
  557. {
  558. port->available_bufs++;
  559. pthread_mutex_unlock(&port->pic_mutex);
  560. msg.header.type = PLINK_TYPE_MESSAGE;
  561. msg.header.size = DATA_SIZE(PlinkMsg);
  562. msg.msg = hdr->id;
  563. sendpkt.list[0] = &msg;
  564. sendpkt.num = 1;
  565. sendpkt.fd = recvpkt.fd;
  566. }
  567. }
  568. } while (exitcode == 0 && *port->exit == 0 && sts != PLINK_STATUS_ERROR);
  569. if (sendpkt.num > 0)
  570. {
  571. pthread_mutex_lock(&port->pic_mutex);
  572. if (VMEM_release(vmem, &params) != VMEM_STATUS_OK)
  573. fprintf(stderr, "[STITCHER] ERROR: Failed to release buffer.\n");
  574. sts = PLINK_send(plink, 0, &sendpkt);
  575. port->available_bufs--;
  576. pthread_mutex_unlock(&port->pic_mutex);
  577. }
  578. if (port->index == 0)
  579. {
  580. *port->exit = 1;
  581. sem_post(port->sem_ready);
  582. }
  583. msg.header.type = PLINK_TYPE_MESSAGE;
  584. msg.header.size = DATA_SIZE(PlinkMsg);
  585. msg.msg = PLINK_EXIT_CODE;
  586. sendpkt.list[0] = &msg;
  587. sendpkt.num = 1;
  588. sendpkt.fd = PLINK_INVALID_FD;
  589. sts = PLINK_send(plink, 0, &sendpkt);
  590. PLINK_close(plink, 0);
  591. pthread_mutex_destroy(&port->pic_mutex);
  592. return NULL;
  593. }
  594. int main(int argc, char **argv) {
  595. PlinkStatus sts = PLINK_STATUS_OK;
  596. StitcherParams params;
  597. StitcherContext ctx;
  598. PlinkHandle plink = NULL;
  599. PlinkPacket pkt = {0};
  600. PlinkYuvInfo pic;
  601. PlinkMsg msg;
  602. parseParams(argc, argv, &params);
  603. if (checkParams(&params) != 0)
  604. {
  605. printUsage(argv[0]);
  606. return 0;
  607. }
  608. memset(&ctx, 0, sizeof(ctx));
  609. void *vmem = NULL;
  610. if (VMEM_create(&vmem) != VMEM_STATUS_OK)
  611. errExit("Failed to create VMEM.");
  612. int width = params.width;
  613. int height = params.height;
  614. int stride = params.stride;
  615. StitchLayout layout = params.layout;
  616. int size = getBufferSize(&params);
  617. if (size == 0)
  618. errExit("Wrong format or wrong resolution.");
  619. PictureBuffer picbuffers[NUM_OF_BUFFERS];
  620. AllocateBuffers(picbuffers, size, vmem);
  621. sem_init(&ctx.sem_ready, 0, 0);
  622. sem_init(&ctx.sem_done, 0, 0);
  623. pthread_mutex_init(&ctx.count_mutex, NULL);
  624. pthread_t thread_in[MAX_NUM_OF_INPUTS];
  625. pthread_attr_t attr;
  626. pthread_attr_init(&attr);
  627. for (int i = 0; i < MAX_NUM_OF_INPUTS; i++)
  628. {
  629. ctx.in[i].name = params.in_name[i];
  630. ctx.in[i].count_mutex = &ctx.count_mutex;
  631. ctx.in[i].sem_ready = &ctx.sem_ready;
  632. ctx.in[i].sem_done = &ctx.sem_done;
  633. ctx.in[i].in_count = &ctx.in_count;
  634. ctx.in[i].exit = &ctx.exitcode;
  635. ctx.in[i].vmem = vmem;
  636. if (pthread_create(&thread_in[i], &attr, input_thread, &ctx.in[i]) != 0)
  637. fprintf(stderr, "[STITCHER] ERROR: Failed to create thread for input %d\n", i);
  638. }
  639. sts = PLINK_create(&plink, params.out_name, PLINK_MODE_SERVER);
  640. StitcherPort *out = &ctx.out;
  641. out->available_bufs = NUM_OF_BUFFERS;
  642. out->count_mutex = &ctx.count_mutex;
  643. out->sem_ready = &ctx.sem_ready;
  644. out->sem_done = &ctx.sem_done;
  645. out->format = params.format;
  646. out->layout = params.layout;
  647. out->width = params.width;
  648. out->height = params.height;
  649. out->stride = params.stride;
  650. out->offset = 0;
  651. out->offset_uv = params.height * params.stride;
  652. out->exit = &ctx.exitcode;
  653. sts = PLINK_connect(plink, &out->id);
  654. int exitcode = 0;
  655. do {
  656. int sendid = out->sendid;
  657. out->buffer = picbuffers[sendid].virtual_address;
  658. if (stitchOneFrame(&ctx) != 0)
  659. break;
  660. constructYuvInfo(&pic, &params, picbuffers[sendid].bus_address, sendid);
  661. printf("[STITCHER] Processed frame %d 0x%010llx: %dx%d, stride = luma %d, chroma %d\n",
  662. sendid, pic.bus_address_y,
  663. pic.pic_width, pic.pic_height,
  664. pic.stride_y, pic.stride_u);
  665. pkt.list[0] = &pic;
  666. pkt.num = 1;
  667. pkt.fd = picbuffers[sendid].fd;
  668. sts = PLINK_send(plink, out->id, &pkt);
  669. out->sendid = (out->sendid + 1) % NUM_OF_BUFFERS;
  670. out->available_bufs -= 1;
  671. int timeout = out->available_bufs == 0 ? 100 : 0;
  672. if (PLINK_wait(plink, out->id, timeout) == PLINK_STATUS_OK)
  673. {
  674. do
  675. {
  676. sts = PLINK_recv(plink, out->id, &pkt);
  677. int count = getBufferCount(&pkt);
  678. if (count < 0)
  679. exitcode = 1;
  680. out->available_bufs += count;
  681. } while (sts == PLINK_STATUS_MORE_DATA);
  682. }
  683. } while (exitcode == 0);
  684. cleanup:
  685. ctx.exitcode = 1;
  686. sem_post(&ctx.sem_done);
  687. msg.header.type = PLINK_TYPE_MESSAGE;
  688. msg.header.size = DATA_SIZE(PlinkMsg);
  689. msg.msg = PLINK_EXIT_CODE;
  690. pkt.list[0] = &msg;
  691. pkt.num = 1;
  692. pkt.fd = PLINK_INVALID_FD;
  693. sts = PLINK_send(plink, out->id, &pkt);
  694. retreiveSentBuffers(plink, out);
  695. PLINK_recv_ex(plink, out->id, &pkt, 1000);
  696. for (int i = 0; i < MAX_NUM_OF_INPUTS; i++)
  697. pthread_join(thread_in[i], NULL);
  698. //sleep(1); // Sleep one second to make sure client is ready for exit
  699. if (vmem)
  700. FreeBuffers(picbuffers, vmem);
  701. PLINK_close(plink, PLINK_CLOSE_ALL);
  702. VMEM_destroy(vmem);
  703. pthread_mutex_destroy(&ctx.count_mutex);
  704. sem_destroy(&ctx.sem_ready);
  705. sem_destroy(&ctx.sem_done);
  706. exit(EXIT_SUCCESS);
  707. }