/* * Copyright (c) 2022 Alibaba Group. All rights reserved. * License-Identifier: Apache-2.0 * * Licensed under the Apache License, Version 2.0 (the "License"); you may * not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. * */ #include #include #include #include #include #include #include #include #include "process_linker_types.h" #include "video_mem.h" #ifndef NULL #define NULL ((void *)0) #endif #define MAX_NUM_OF_INPUTS 4 #define NUM_OF_BUFFERS 5 #define errExit(msg) do { perror(msg); exit(EXIT_FAILURE); \ } while (0) #define STITCHER_MIN(a, b) ((a) < (b) ? (a) : (b)) typedef enum _StitchLayout { STITCH_LAYOUT_Vertical = 0, STITCH_LAYOUT_Horizontal, STITCH_LAYOUT_Matrix, STITCH_LAYOUT_Max } StitchLayout; typedef struct _StitherRegion { int width; int height; int offset_uv; int offset_y; } StitcherRegion; typedef struct _StitcherParams { char *in_name[MAX_NUM_OF_INPUTS]; char *out_name; StitchLayout layout; PlinkColorFormat format; int width; int height; int stride; } StitcherParams; typedef struct _StitcherPort { char *name; int index; PlinkChannelID id; PlinkHandle plink; int sendid; int backid; int *exit; int available_bufs; void *vmem; pthread_mutex_t pic_mutex; pthread_mutex_t *count_mutex; sem_t *sem_ready; sem_t *sem_done; StitchLayout layout; PlinkColorFormat format; void *buffer; int width; int height; int stride; int offset; int offset_uv; int *in_count; } StitcherPort; typedef struct _StitcherContext { StitcherPort in[MAX_NUM_OF_INPUTS]; StitcherPort out; pthread_mutex_t count_mutex; sem_t sem_ready; sem_t sem_done; int in_count; int exitcode; } StitcherContext; typedef struct _PictureBuffer { unsigned int bus_address; void *virtual_address; unsigned int size; int fd; } PictureBuffer; static void printUsage(char *name) { printf("usage: %s [options]\n" "\n" " Stitch multiple pictures to one. Maximum # of pictures to be stitched is %d\n" " Available options:\n" " -i plink file name of input port #n (default: /tmp/plink.stitch.in). n is 0 based.\n" " -o plink file name of output port (default: /tmp/plink.stitch.out)\n" " -l layout (default: 0)\n" " 0 - vertical\n" " 1 - horizontal\n" " 2 - matrix\n" " -f output color format (default: 3)\n" " 3 - NV12\n" " -w output video width (default: 800)\n" " -h output video height (default: 1280)\n" " -s output video buffer stride (default: video width)\n" " --help print this message\n" "\n", name, MAX_NUM_OF_INPUTS); } static void parseParams(int argc, char **argv, StitcherParams *params) { int i = 1; memset(params, 0, sizeof(*params)); params->in_name[0] = "/tmp/plink.stitch.in0"; params->in_name[1] = "/tmp/plink.stitch.in1"; params->in_name[2] = "/tmp/plink.stitch.in2"; params->in_name[3] = "/tmp/plink.stitch.in3"; params->out_name = "/tmp/plink.stitch.out"; params->layout = STITCH_LAYOUT_Vertical; params->format = PLINK_COLOR_FormatYUV420SemiPlanar; params->width = 800; params->height = 1280; while (i < argc) { if (argv[i][0] != '-' || strlen(argv[i]) < 2) { i++; continue; } if (argv[i][1] == 'i') { if (++i < argc) { if (strlen(argv[i-1]) > 2) // input name { int id = atoi(argv[i-1]+2); if (id < MAX_NUM_OF_INPUTS) params->in_name[id] = argv[i++]; } else params->in_name[0] = argv[i++]; } } else if (argv[i][1] == 'o') { if (++i < argc) params->out_name = argv[i++]; } else if (argv[i][1] == 'l') { if (++i < argc) params->layout = atoi(argv[i++]); } else if (argv[i][1] == 'f') { if (++i < argc) params->format = atoi(argv[i++]); } else if (argv[i][1] == 'w') { if (++i < argc) params->width = atoi(argv[i++]); } else if (argv[i][1] == 'h') { if (++i < argc) params->height = atoi(argv[i++]); } else if (argv[i][1] == 's') { if (++i < argc) params->stride = atoi(argv[i++]); } else if (strcmp(argv[i], "--help") == 0) { params->layout = STITCH_LAYOUT_Max; i++; } } if (params->stride == 0) params->stride = params->width; printf("[STITCHER] Input 0 Name : %s\n", params->in_name[0]); printf("[STITCHER] Input 1 Name : %s\n", params->in_name[1]); printf("[STITCHER] Input 2 Name : %s\n", params->in_name[2]); printf("[STITCHER] Input 3 Name : %s\n", params->in_name[3]); printf("[STITCHER] Output Name : %s\n", params->out_name); printf("[STITCHER] Output Layout : %d\n", params->layout); printf("[STITCHER] Output Format : %d\n", params->format); printf("[STITCHER] Output Resolution : %dx%d\n", params->width, params->height); printf("[STITCHER] Output Stride : %d\n", params->stride); } static int checkParams(StitcherParams *params) { if (params->format != PLINK_COLOR_FormatYUV420SemiPlanar || params->layout >= STITCH_LAYOUT_Max) return -1; return 0; } static int getBufferSize(StitcherParams *params) { int size = 0; switch (params->format) { case PLINK_COLOR_FormatYUV420Planar: case PLINK_COLOR_FormatYUV420SemiPlanar: size = params->stride * params->height * 3 / 2; break; default: size = 0; } return size; } static void AllocateBuffers(PictureBuffer picbuffers[NUM_OF_BUFFERS], unsigned int size, void *vmem) { unsigned int buffer_size = (size + 0xFFF) & ~0xFFF; VmemParams params; params.size = buffer_size; params.flags = VMEM_FLAG_CONTIGUOUS | VMEM_FLAG_4GB_ADDR; for (int i = 0; i < NUM_OF_BUFFERS; i++) { params.fd = 0; VMEM_allocate(vmem, ¶ms); VMEM_mmap(vmem, ¶ms); VMEM_export(vmem, ¶ms); printf("[STITCHER] mmap %p from %x with size %d, dma-buf fd %d\n", params.vir_address, params.phy_address, params.size, params.fd); picbuffers[i].virtual_address = params.vir_address; picbuffers[i].bus_address = params.phy_address; picbuffers[i].size = buffer_size; picbuffers[i].fd = params.fd; } } static void FreeBuffers(PictureBuffer picbuffers[NUM_OF_BUFFERS], void *vmem) { VmemParams params; memset(¶ms, 0, sizeof(params)); for (int i = 0; i < NUM_OF_BUFFERS; i++) { close(picbuffers[i].fd); params.size = picbuffers[i].size; params.vir_address = picbuffers[i].virtual_address; params.phy_address = picbuffers[i].bus_address; VMEM_free(vmem, ¶ms); } } static void getRegion(StitcherPort *out, int index, int in_count, StitcherRegion *region) { region->offset_uv = 0; //out->stride * out->height; if (out->layout == STITCH_LAYOUT_Horizontal) { int width = (out->width / in_count + 1) & ~1; region->offset_y = width * index; region->offset_uv += width * index; region->width = width; region->height = out->height; } else if (out->layout == STITCH_LAYOUT_Matrix && in_count >= 3) { int y = index >> 1; int x = index & 1; int width = (out->width / 2 + 1) & ~1; int height = (out->height / 2 + 1) & ~1; region->offset_y = out->stride * (height * y); region->offset_y += width * x; region->offset_uv += out->stride * (height * y / 2); region->offset_uv += width * x; region->width = width; region->height = height; } else // (out->layout == STITCH_LAYOUT_Vertical) { int height = (out->height / in_count + 1) & ~1; region->offset_y = out->stride * (height * index); region->offset_uv += out->stride * (height * index / 2); region->width = out->width; region->height = height; } } static int stitchOneFrame(StitcherContext *ctx) { StitcherPort *in = NULL; StitcherPort *out = &ctx->out; int in_count = 0; do { pthread_mutex_lock(out->count_mutex); in_count = ctx->in_count; pthread_mutex_unlock(out->count_mutex); if (in_count > 0) break; else sleep(1); } while (1); for (int i = 0; i < in_count; i++) { in = &ctx->in[i]; if (in->index == 0) { sem_wait(&ctx->sem_ready); // wait for picture from input port0. if (ctx->exitcode == 1) return 1; } StitcherRegion region; getRegion(out, in->index, in_count, ®ion); pthread_mutex_lock(&in->pic_mutex); int width = STITCHER_MIN(region.width, in->width); int height = STITCHER_MIN(region.height, in->height); void *dst = out->buffer + out->offset + region.offset_y; void *src = in->buffer + in->offset; //fprintf(stderr, "%d: %p, %d, %d, %p, %d\n", in->index, out->buffer, out->offset, region.offset_y, in->buffer, in->offset); //fprintf(stderr, "%d: %dx%d, %d, %d\n", in->index, width, height, in->stride, out->stride); if (in->available_bufs > 0) { if (in->format == PLINK_COLOR_FormatYUV420SemiPlanarP010 || in->format == PLINK_COLOR_FormatRawBayer10bit || in->format == PLINK_COLOR_FormatRawBayer12bit) { int shift = in->format == PLINK_COLOR_FormatYUV420SemiPlanarP010 ? 2 : 4; unsigned int temp[4]; for (int h = 0; h < height; h++) { unsigned int *dst32 = (unsigned int *)dst; unsigned short *src16 = (unsigned short *)src; for (int w = 0; w < width; w+=4) { temp[0] = ((unsigned int)(src16[w+0] >> shift) & 0xFF); temp[1] = ((unsigned int)(src16[w+1] >> shift) & 0xFF) << 8; temp[2] = ((unsigned int)(src16[w+2] >> shift) & 0xFF) << 16; temp[3] = ((unsigned int)(src16[w+3] >> shift) & 0xFF) << 24; dst32[w>>2] = temp[0] | temp[1] | temp[2] | temp[3]; } dst += out->stride; src += in->stride; } } else { for (int h = 0; h < height; h++) { memcpy(dst, src, width); dst += out->stride; src += in->stride; } } } else { for (int h = 0; h < height; h++) { memset(dst, 0x00, width); dst += out->stride; } } dst = out->buffer + out->offset_uv + region.offset_uv; src = in->buffer + in->offset_uv; //fprintf(stderr, "%d: %p, %d, %d, %p, %d\n", in->index, out->buffer, out->offset_uv, region.offset_uv, in->buffer, in->offset_uv); if (in->format == PLINK_COLOR_FormatYUV420SemiPlanar && in->available_bufs > 0) { for (int h = 0; h < height/2; h++) { memcpy(dst, src, width); dst += out->stride; src += in->stride; } } else // treat all the other formats as monochrome { for (int h = 0; h < height/2; h++) { memset(dst, 0x80, width); dst += out->stride; } } pthread_mutex_unlock(&in->pic_mutex); if (in->index == 0) sem_post(&ctx->sem_done); // Resume input port0. // !Assume output thread is fast enough, so input port0 won't be blocked for long time. } return 0; } static void constructYuvInfo(PlinkYuvInfo *info, StitcherParams *params, unsigned int bus_address, int id) { int size_y = params->width * params->stride; int size_uv = size_y / 2; int size_yuv = size_y + size_uv; info->header.type = PLINK_TYPE_2D_YUV; info->header.size = DATA_SIZE(*info); info->header.id = id + 1; info->format = params->format; info->bus_address_y = bus_address; info->bus_address_u = info->bus_address_y + size_y; info->bus_address_v = info->bus_address_u + (size_uv / 2); info->pic_width = params->width; info->pic_height = params->height; info->stride_y = params->stride; info->stride_u = params->format == PLINK_COLOR_FormatYUV420Planar ? params->stride/2 : params->stride; info->stride_v = info->stride_u; } int getBufferCount(PlinkPacket *pkt) { int ret = 0; for (int i = 0; i < pkt->num; i++) { PlinkDescHdr *hdr = (PlinkDescHdr *)(pkt->list[i]); if (hdr->type == PLINK_TYPE_MESSAGE) { int *data = (int *)(pkt->list[i] + DATA_HEADER_SIZE); if (*data == PLINK_EXIT_CODE) { ret |= 0x80000000; // set bit 31 to 1 to indicate 'exit' } else if (*data >= 0) ret++; } } return ret; } static void retreiveSentBuffers(PlinkHandle plink, StitcherPort *port) { PlinkStatus sts = PLINK_STATUS_OK; PlinkPacket pkt = {0}; while (port->available_bufs < NUM_OF_BUFFERS && sts == PLINK_STATUS_OK) { do { sts = PLINK_recv(plink, port->id, &pkt); int count = getBufferCount(&pkt); if (count > 0) { port->available_bufs += count; } } while (sts == PLINK_STATUS_MORE_DATA); } } static void *input_thread(void *args) { StitcherPort *port = (StitcherPort *)args; PlinkHandle plink = NULL; PlinkStatus sts = PLINK_STATUS_OK; void *vmem = port->vmem; pthread_mutex_init(&port->pic_mutex, NULL); if (PLINK_create(&plink, port->name, PLINK_MODE_CLIENT) != PLINK_STATUS_OK) return NULL; do { sts = PLINK_connect_ex(plink, NULL, 1000); } while (sts == PLINK_STATUS_TIMEOUT && *port->exit == 0); if (sts != PLINK_STATUS_OK) { PLINK_close(plink, 0); return NULL; } pthread_mutex_lock(port->count_mutex); port->index = *(port->in_count); *(port->in_count) = *(port->in_count) + 1; pthread_mutex_unlock(port->count_mutex); PlinkPacket sendpkt = {0}; PlinkPacket recvpkt = {0}; PlinkMsg msg = {0}; VmemParams params; int exitcode = 0; do { sts = PLINK_recv(plink, 0, &recvpkt); if (sts == PLINK_STATUS_ERROR) break; for (int i = 0; i < recvpkt.num; i++) { PlinkDescHdr *hdr = (PlinkDescHdr *)(recvpkt.list[i]); if (hdr->type == PLINK_TYPE_2D_YUV || hdr->type == PLINK_TYPE_2D_RAW) { // return previous buffer to source if (sendpkt.num > 0) { if (port->index == 0) sem_wait(port->sem_done); pthread_mutex_lock(&port->pic_mutex); if (VMEM_release(vmem, ¶ms) != VMEM_STATUS_OK) fprintf(stderr, "[STITCHER] ERROR: Failed to release buffer.\n"); sts = PLINK_send(plink, 0, &sendpkt); if (sts == PLINK_STATUS_ERROR) break; if (sendpkt.fd != PLINK_INVALID_FD) close(sendpkt.fd); port->available_bufs--; sendpkt.num = 0; } else pthread_mutex_lock(&port->pic_mutex); memset(¶ms, 0, sizeof(params)); if (recvpkt.fd != PLINK_INVALID_FD) { params.fd = recvpkt.fd; if (VMEM_import(vmem, ¶ms) != VMEM_STATUS_OK) break; if (VMEM_mmap(vmem, ¶ms) != VMEM_STATUS_OK) break; } if (port->index == 0) sem_post(port->sem_ready); // signal output thread that one picture is ready } if (hdr->type == PLINK_TYPE_2D_YUV) { PlinkYuvInfo *pic = (PlinkYuvInfo *)(recvpkt.list[i]); printf("[STITCHER] Input%d: Received YUV frame %d 0x%010llx from %s: fd %d, %dx%d, stride = luma %d, chroma %d\n", port->index, pic->header.id, pic->bus_address_y, port->name, recvpkt.fd, pic->pic_width, pic->pic_height, pic->stride_y, pic->stride_u); port->buffer = params.vir_address; port->format = pic->format; port->width = pic->pic_width; port->height = pic->pic_height; port->stride = pic->stride_y; port->offset = pic->offset_y; port->offset_uv = pic->offset_u > 0 ? pic->offset_u : (pic->offset_y + pic->pic_height*pic->stride_y); } else if (hdr->type == PLINK_TYPE_2D_RAW) { PlinkRawInfo *img = (PlinkRawInfo *)(recvpkt.list[i]); printf("[STITCHER] Input%d: Received RAW frame %d 0x%010llx from %s: fd %d, %dx%d, stride %d\n", port->index, img->header.id, img->bus_address, port->name, recvpkt.fd, img->img_width, img->img_height, img->stride); port->buffer = params.vir_address; port->format = img->format; port->width = img->img_width; port->height = img->img_height; port->stride = img->stride; port->offset = img->offset; } else if (hdr->type == PLINK_TYPE_MESSAGE) { PlinkMsg *msg = (PlinkMsg *)(recvpkt.list[i]); if (msg->msg == PLINK_EXIT_CODE) { exitcode = 1; printf("[STITCHER] Input %d: Exit\n", port->index); break; } } if (hdr->type == PLINK_TYPE_2D_YUV || hdr->type == PLINK_TYPE_2D_RAW) { port->available_bufs++; pthread_mutex_unlock(&port->pic_mutex); msg.header.type = PLINK_TYPE_MESSAGE; msg.header.size = DATA_SIZE(PlinkMsg); msg.msg = hdr->id; sendpkt.list[0] = &msg; sendpkt.num = 1; sendpkt.fd = recvpkt.fd; } } } while (exitcode == 0 && *port->exit == 0 && sts != PLINK_STATUS_ERROR); if (sendpkt.num > 0) { pthread_mutex_lock(&port->pic_mutex); if (VMEM_release(vmem, ¶ms) != VMEM_STATUS_OK) fprintf(stderr, "[STITCHER] ERROR: Failed to release buffer.\n"); sts = PLINK_send(plink, 0, &sendpkt); port->available_bufs--; pthread_mutex_unlock(&port->pic_mutex); } if (port->index == 0) { *port->exit = 1; sem_post(port->sem_ready); } msg.header.type = PLINK_TYPE_MESSAGE; msg.header.size = DATA_SIZE(PlinkMsg); msg.msg = PLINK_EXIT_CODE; sendpkt.list[0] = &msg; sendpkt.num = 1; sendpkt.fd = PLINK_INVALID_FD; sts = PLINK_send(plink, 0, &sendpkt); PLINK_close(plink, 0); pthread_mutex_destroy(&port->pic_mutex); return NULL; } int main(int argc, char **argv) { PlinkStatus sts = PLINK_STATUS_OK; StitcherParams params; StitcherContext ctx; PlinkHandle plink = NULL; PlinkPacket pkt = {0}; PlinkYuvInfo pic; PlinkMsg msg; parseParams(argc, argv, ¶ms); if (checkParams(¶ms) != 0) { printUsage(argv[0]); return 0; } memset(&ctx, 0, sizeof(ctx)); void *vmem = NULL; if (VMEM_create(&vmem) != VMEM_STATUS_OK) errExit("Failed to create VMEM."); int width = params.width; int height = params.height; int stride = params.stride; StitchLayout layout = params.layout; int size = getBufferSize(¶ms); if (size == 0) errExit("Wrong format or wrong resolution."); PictureBuffer picbuffers[NUM_OF_BUFFERS]; AllocateBuffers(picbuffers, size, vmem); sem_init(&ctx.sem_ready, 0, 0); sem_init(&ctx.sem_done, 0, 0); pthread_mutex_init(&ctx.count_mutex, NULL); pthread_t thread_in[MAX_NUM_OF_INPUTS]; pthread_attr_t attr; pthread_attr_init(&attr); for (int i = 0; i < MAX_NUM_OF_INPUTS; i++) { ctx.in[i].name = params.in_name[i]; ctx.in[i].count_mutex = &ctx.count_mutex; ctx.in[i].sem_ready = &ctx.sem_ready; ctx.in[i].sem_done = &ctx.sem_done; ctx.in[i].in_count = &ctx.in_count; ctx.in[i].exit = &ctx.exitcode; ctx.in[i].vmem = vmem; if (pthread_create(&thread_in[i], &attr, input_thread, &ctx.in[i]) != 0) fprintf(stderr, "[STITCHER] ERROR: Failed to create thread for input %d\n", i); } sts = PLINK_create(&plink, params.out_name, PLINK_MODE_SERVER); StitcherPort *out = &ctx.out; out->available_bufs = NUM_OF_BUFFERS; out->count_mutex = &ctx.count_mutex; out->sem_ready = &ctx.sem_ready; out->sem_done = &ctx.sem_done; out->format = params.format; out->layout = params.layout; out->width = params.width; out->height = params.height; out->stride = params.stride; out->offset = 0; out->offset_uv = params.height * params.stride; out->exit = &ctx.exitcode; sts = PLINK_connect(plink, &out->id); int exitcode = 0; do { int sendid = out->sendid; out->buffer = picbuffers[sendid].virtual_address; if (stitchOneFrame(&ctx) != 0) break; constructYuvInfo(&pic, ¶ms, picbuffers[sendid].bus_address, sendid); printf("[STITCHER] Processed frame %d 0x%010llx: %dx%d, stride = luma %d, chroma %d\n", sendid, pic.bus_address_y, pic.pic_width, pic.pic_height, pic.stride_y, pic.stride_u); pkt.list[0] = &pic; pkt.num = 1; pkt.fd = picbuffers[sendid].fd; sts = PLINK_send(plink, out->id, &pkt); out->sendid = (out->sendid + 1) % NUM_OF_BUFFERS; out->available_bufs -= 1; int timeout = out->available_bufs == 0 ? 100 : 0; if (PLINK_wait(plink, out->id, timeout) == PLINK_STATUS_OK) { do { sts = PLINK_recv(plink, out->id, &pkt); int count = getBufferCount(&pkt); if (count < 0) exitcode = 1; out->available_bufs += count; } while (sts == PLINK_STATUS_MORE_DATA); } } while (exitcode == 0); cleanup: ctx.exitcode = 1; sem_post(&ctx.sem_done); msg.header.type = PLINK_TYPE_MESSAGE; msg.header.size = DATA_SIZE(PlinkMsg); msg.msg = PLINK_EXIT_CODE; pkt.list[0] = &msg; pkt.num = 1; pkt.fd = PLINK_INVALID_FD; sts = PLINK_send(plink, out->id, &pkt); retreiveSentBuffers(plink, out); PLINK_recv_ex(plink, out->id, &pkt, 1000); for (int i = 0; i < MAX_NUM_OF_INPUTS; i++) pthread_join(thread_in[i], NULL); //sleep(1); // Sleep one second to make sure client is ready for exit if (vmem) FreeBuffers(picbuffers, vmem); PLINK_close(plink, PLINK_CLOSE_ALL); VMEM_destroy(vmem); pthread_mutex_destroy(&ctx.count_mutex); sem_destroy(&ctx.sem_ready); sem_destroy(&ctx.sem_done); exit(EXIT_SUCCESS); }