123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516 |
- /*
- * Copyright (C) 2021 Alibaba Group Holding Limited
- * Author: ShenWuYi <shenwuyi.swy@alibaba-inc.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
- #include <syslog.h>
- #include <string.h>
- #include <unistd.h>
- #include <pthread.h>
- #include <semaphore.h>
- #include <time.h>
- #include <sys/time.h>
- #include <sys/ioctl.h>
- #include <fcntl.h>
- #include <csi_camera.h>
- #include <stdlib.h>
- #include "process_linker_types.h"
- #include "process_linker.h"
- #include "video_mem.h"
- #define NUM_OF_BUFFERS 5
- typedef struct _CsiPictureBuffer
- {
- unsigned int bus_address;
- void *virtual_address;
- unsigned int size;
- int fd;
- } CsiPictureBuffer;
- typedef struct _CsiPlinkContext
- {
- int useplink;
- int exitplink;
- CsiPictureBuffer sendbuffer[NUM_OF_BUFFERS];
- csi_frame_ex_s frame_buf[NUM_OF_BUFFERS];
- int sendid;
- int available_bufs;
- void *vmem;
- PlinkHandle plink;
- PlinkChannelID chnid;
- pthread_t buf_release_thread;
- pthread_mutex_t mutex;
- } CsiPlinkContext;
- static int get_buffer_count(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* camera_buf_release_process(CsiPlinkContext * plink_ctx)
- {
- PlinkPacket pkt = {0};
- if(plink_ctx == NULL)
- {
- return NULL;
- }
- LOG_O("Process is runing.....\n");
- while(!plink_ctx->exitplink)
- {
- if (plink_ctx->plink != NULL) {
- if (PLINK_wait(plink_ctx->plink, plink_ctx->chnid, 1000) == PLINK_STATUS_OK)
- {
- if (PLINK_recv(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK) {
- plink_ctx->exitplink = 1;
- break;
- }
- int count = get_buffer_count(&pkt);
- if (count >= 0)
- {
- PlinkMsg *hdr;
- int resp_id;
- for(int i=0;i<count;i++)
- {
- hdr = (PlinkMsg *)(pkt.list[i]);
- resp_id = hdr->msg;
- if((resp_id>=NUM_OF_BUFFERS))
- {
- LOG_W("invalid resp_id:%d\n",resp_id);
- continue;
- }
- if(plink_ctx->frame_buf[resp_id].frame_data.fd[0]!= 0)
- {
- csi_camera_put_frame(&plink_ctx->frame_buf[resp_id]);
- LOG_D("release resp_id:%d, fd:%d\n",resp_id,plink_ctx->frame_buf[resp_id].frame_data.fd[0]);
- plink_ctx->frame_buf[resp_id].frame_data.fd[0] = 0;
- }
- }
- pthread_mutex_lock(&plink_ctx->mutex);
- plink_ctx->available_bufs += count;
- pthread_mutex_unlock(&plink_ctx->mutex);
- }
- else {
- fprintf(stderr, "[SERVER] Exit!\n");
- plink_ctx->exitplink = 1;
- break;
- }
- }else
- {
- LOG_W("Plink Resp timeout\n");
- }
- }
- }
- LOG_O("Process is exit .....\n");
- return NULL;
- }
- static int allocate_sendbuffers(CsiPictureBuffer 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++)
- {
- if (VMEM_allocate(vmem, ¶ms) != 0) {
- return -1;
- }
- VMEM_mmap(vmem, ¶ms);
- VMEM_export(vmem, ¶ms);
- LOG_O("[SERVER] 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;
- }
- return 0;
- }
- void *vi_plink_create(csi_camera_channel_cfg_s *chn_cfg)
- {
- int ret = 0;
- CsiPlinkContext *plink_ctx = NULL;
- char *env = NULL;
- if (chn_cfg == NULL) {
- LOG_E("%s failt to get chn_cfg\n", __func__);
- return NULL;
- }
- plink_ctx = malloc(sizeof(CsiPlinkContext));
- if(!plink_ctx)
- {
- LOG_E("malloc faile\n");
- return NULL;
- }
- char *plinknamebase0 = "/tmp/plink.test";
- char *plinknamebase1 = "/tmp/plink_npu_rgb.test";
- char *plinknamebase2 = "/tmp/plink.test2";
- env = getenv("ISP_PLINK_NAME0");
- if (env != NULL) {
- plinknamebase0 = env;
- }
- env = getenv("ISP_PLINK_NAME1");
- if (env != NULL) {
- plinknamebase1 = env;
- }
- env = getenv("ISP_PLINK_NAME2");
- if (env != NULL) {
- plinknamebase2 = env;
- }
- char plinkname[32];
- if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_0) {
- sprintf(plinkname, "%s", plinknamebase0);
- } else if (chn_cfg->chn_id == CSI_CAMERA_CHANNEL_1) {
- sprintf(plinkname, "%s", plinknamebase1);
- } else {
- sprintf(plinkname, "%s", plinknamebase2);
- }
- fprintf(stderr, "%s, %d: ISP_PLINK_NAME = %s\n", __func__, __LINE__, plinkname);
- plink_ctx->exitplink = 0;
- fprintf(stderr, "%s, %d: Launching plink server...\n", __func__, __LINE__);
- memset(&plink_ctx->sendbuffer[0], 0, sizeof(plink_ctx->sendbuffer));
- if (VMEM_create(&plink_ctx->vmem) != VMEM_STATUS_OK)
- {
- fprintf(stderr, "Failed to create VMEM.");
- return NULL;
- }
- else {
- int framesize = 0;
- int stride = (chn_cfg->img_fmt.width + 127) & (~127); // align stride to 128
- switch (chn_cfg->img_fmt.pix_fmt) {
- case CSI_PIX_FMT_BGR:
- {
- framesize = stride * 304 * 3; //stride * chn_cfg->img_fmt.height * 3;
- if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
- return NULL;
- }
- // reset to black picture
- uint32_t lumasize = stride * chn_cfg->img_fmt.height;
- for (int i = 0; i < NUM_OF_BUFFERS; i++)
- memset(plink_ctx->sendbuffer[i].virtual_address, 0, framesize);
- break;
- }
- case CSI_PIX_FMT_NV12:
- default:
- {
- framesize = stride * chn_cfg->img_fmt.height * 3 / 2;
- if (allocate_sendbuffers(plink_ctx->sendbuffer, framesize, plink_ctx->vmem) != 0) {
- return NULL;
- }
- // reset to black picture
- uint32_t lumasize = stride * chn_cfg->img_fmt.height;
- for (int i = 0; i < NUM_OF_BUFFERS; i++) {
- CsiPictureBuffer *buf = &plink_ctx->sendbuffer[i];
- memset(buf->virtual_address, 0, lumasize);
- memset(buf->virtual_address + lumasize, 0x80, lumasize/2);
- }
- break;
- }
- }
- }
- PLINK_create(&plink_ctx->plink, plinkname, PLINK_MODE_SERVER);
- if (plink_ctx->plink) {
- PLINK_connect(plink_ctx->plink, &plink_ctx->chnid);
- }
- plink_ctx->sendid = 0;
- plink_ctx->available_bufs = NUM_OF_BUFFERS;
- memset(plink_ctx->frame_buf,0x0,sizeof(plink_ctx->frame_buf));
- pthread_mutex_init(&plink_ctx->mutex,NULL);
- pthread_create(&plink_ctx->buf_release_thread,NULL,(void *)camera_buf_release_process,plink_ctx);
- return plink_ctx;
- }
- void vi_plink_release(void * plink)
- {
- CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
- if(plink_ctx)
- {
- plink_ctx->exitplink=1;
- pthread_join(plink_ctx->buf_release_thread,NULL);
-
- PLINK_close(plink_ctx->plink,plink_ctx->chnid);
- VMEM_destroy(plink_ctx->vmem);
- }
- }
- void display_camera_frame(void *plink, csi_frame_ex_s *frame)
- {
-
- CsiPlinkContext * plink_ctx = (CsiPlinkContext *)plink;
- if ((plink_ctx == NULL) || (frame == NULL)) {
- LOG_E("%s check param fail\n", __func__);
- return;
- }
- LOG_I("fmt=%d,fd=%d,stride:%d\n",frame->frame_info.pixel_format,frame->frame_data.fd[0],frame->frame_data.stride[0]);
- if (!plink_ctx->exitplink) {
- struct timeval tv_start, tv_end, tv_duration;
- gettimeofday(&tv_start, 0);
- PlinkPacket pkt = {0};
- // send one buffer if there is available slot
- if (!plink_ctx->exitplink&& plink_ctx->available_bufs > 0 &&
- plink_ctx->plink != NULL && ((frame->frame_info.pixel_format == CSI_PIX_FMT_RAW_8BIT)||
- (frame->frame_info.pixel_format == CSI_PIX_FMT_RAW_10BIT)||
- (frame->frame_info.pixel_format== CSI_PIX_FMT_RAW_12BIT)))
- {
- char str[200];
- static int i = 0;
- uint32_t dstw = frame->frame_info.width;//800;
- uint32_t dsth = frame->frame_info.height;//1280;
- uint32_t dsts = frame->frame_data.stride[0];//896;
- PlinkRawInfo info = {0};
- info.header.type = PLINK_TYPE_2D_RAW;
- info.header.size = DATA_SIZE(info);
- info.header.id = plink_ctx->sendid;
- info.format = PLINK_COLOR_FormatRawBayer10bit;
- // info.bus_address = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
- // vi_mem_release(info.bus_address);
- info.img_width = dstw;
- info.img_height = dsth;
- info.stride = frame->frame_data.stride[0];
- info.offset = 0;
- pkt.list[0] = &info;
- pkt.num = 1;
- pkt.fd = frame->frame_data.fd[0];
- if(plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]!=0)
- {
- 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]);
- csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
- }
- memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_ex_s));
- plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
- if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
- plink_ctx->exitplink = 1;
- gettimeofday(&tv_end, 0);
- timersub(&tv_end, &tv_start, &tv_duration);
- pthread_mutex_lock(&plink_ctx->mutex);
- plink_ctx->available_bufs -= 1;
- pthread_mutex_unlock(&plink_ctx->mutex);
- }
- else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 &&
- plink_ctx->plink != NULL && ((frame->frame_info.pixel_format == CSI_PIX_FMT_YUV_SEMIPLANAR_420)|| (frame->frame_info.pixel_format == CSI_PIX_FMT_NV12))) {
- char str[200];
- static int i = 0;
- uint32_t dstw = frame->frame_info.width;//800;
- uint32_t dsth = frame->frame_info.height;//1280;
- uint32_t dsts = frame->frame_data.stride[0];//896;
- PlinkYuvInfo info = {0};
- info.header.type = PLINK_TYPE_2D_YUV;
- info.header.size = DATA_SIZE(info);
- info.header.id = plink_ctx->sendid;
- info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
- // info.bus_address_y = vi_mem_import(frame->img.dmabuf[0].fds) + frame->img.dmabuf[0].offset;
- // info.bus_address_u = info.bus_address_y + frame->img.dmabuf[1].offset;
- // vi_mem_release(info.bus_address_y);
- info.pic_width = dstw;
- info.pic_height = dsth;
- info.stride_y = dsts;
- info.stride_u = dsts;
- info.stride_v = dsts;
- info.offset_y = 0;
- info.offset_u = frame->frame_data.offset[1];
- info.offset_v = frame->frame_data.offset[2];
- pkt.list[0] = &info;
- pkt.num = 1;
- pkt.fd = frame->frame_data.fd[0];
-
- if(plink_ctx->frame_buf[plink_ctx->sendid].frame_data.fd[0]!=0)
- {
- 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]);
- csi_camera_put_frame(&plink_ctx->frame_buf[plink_ctx->sendid]);
- }
- memcpy(&plink_ctx->frame_buf[plink_ctx->sendid],frame,sizeof(csi_frame_ex_s));
- plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
- if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
- plink_ctx->exitplink = 1;
- gettimeofday(&tv_end, 0);
- timersub(&tv_end, &tv_start, &tv_duration);
-
- pthread_mutex_lock(&plink_ctx->mutex);
- plink_ctx->available_bufs -= 1;
- pthread_mutex_unlock(&plink_ctx->mutex);
-
- } else if (!plink_ctx->exitplink && plink_ctx->available_bufs > 0 && plink_ctx->plink != NULL && (frame->frame_info.pixel_format == CSI_PIX_FMT_BGR)) {
- CsiPictureBuffer *buf = &plink_ctx->sendbuffer[plink_ctx->sendid];
- int y = 0;
- int buf_size = frame->frame_info.width*frame->frame_info.height*3;
- uint8_t *src = frame->frame_data.vir_addr[0] ;
- uint8_t *dst = buf->virtual_address;
- uint32_t srcw = frame->frame_info.width;
- uint32_t srch = frame->frame_info.height;
- uint32_t dstw = frame->frame_info.width;
- uint32_t dsth = frame->frame_info.height;
- uint32_t dsts = frame->frame_info.width;
- switch (frame->frame_info.pixel_format) {
- case CSI_PIX_FMT_BGR:
- {
- dstw = 300;
- dsth = 304;
- uint32_t w = dstw > srcw ? srcw : dstw;
- uint32_t h = dsth > srch ? srch : dsth;
- if (w != 300 || h != 176 || dsts != 304) {
- fprintf(stderr, "ERROR: expect 300x300 (stride 304) image while recieved %dx%d (stride %d) image\n", w, h, dsts);
- break;
- }
- for (y = 0; y < h; y++)
- {
- memcpy(dst, src, w);
- src += srcw;
- dst += dsts;
- }
- src = frame->frame_data.vir_addr[0] + srcw*srch;
- dst = buf->virtual_address + dsts*dsth;
- for (y = 0; y < h; y++)
- {
- memcpy(dst, src, w);
- src += srcw;
- dst += dsts;
- }
- src = frame->frame_data.vir_addr[0] + srcw*srch*2;
- dst = buf->virtual_address + dsts*dsth*2;
- for (y = 0; y < h; y++)
- {
- memcpy(dst, src, w);
- src += srcw;
- dst += dsts;
- }
- PlinkRGBInfo info = {0};
- info.header.type = PLINK_TYPE_2D_RGB;
- info.header.size = DATA_SIZE(info);
- info.header.id = plink_ctx->sendid + 1;
- info.format = PLINK_COLOR_Format24BitBGR888Planar;
- info.bus_address_b = buf->bus_address;
- info.bus_address_g = info.bus_address_b + dsts*dsth;
- info.bus_address_r = info.bus_address_g + dsts*dsth;
- info.img_width = dstw;
- info.img_height = dsth;
- info.stride_b = dsts;
- info.stride_g = dsts;
- info.stride_r = dsts;
- pkt.list[0] = &info;
- pkt.num = 1;
- pkt.fd = buf->fd;
- if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
- plink_ctx->exitplink = 1;
- break;
- }
- case CSI_PIX_FMT_NV12:
- default:
- {
- uint32_t w = dstw > srcw ? srcw : dstw;
- uint32_t h = dsth > srch ? srch : dsth;
- for (y = 0; y < h; y++)
- {
- memcpy(dst, src, w);
- src += srcw;
- dst += dsts;
- }
- src = frame->frame_data.vir_addr[0] + frame->frame_data.stride[0] * frame->frame_info.height;
- dst = buf->virtual_address + (dsts * dsth);
- for (y = 0; y < h/2; y++)
- {
- memcpy(dst, src, w);
- src += srcw;
- dst += dsts;
- }
- PlinkYuvInfo info = {0};
- info.header.type = PLINK_TYPE_2D_YUV;
- info.header.size = DATA_SIZE(info);
- info.header.id = plink_ctx->sendid + 1;
- info.format = PLINK_COLOR_FormatYUV420SemiPlanar;
- info.bus_address_y = buf->bus_address;
- info.bus_address_u = buf->bus_address + dsts*dsth;
- info.bus_address_v = info.bus_address_u;
- info.pic_width = dstw;
- info.pic_height = dsth;
- info.stride_y = dsts;
- info.stride_u = dsts;
- info.stride_v = dsts;
- pkt.list[0] = &info;
- pkt.num = 1;
- pkt.fd = buf->fd;
- if (PLINK_send(plink_ctx->plink, plink_ctx->chnid, &pkt) != PLINK_STATUS_OK)
- plink_ctx->exitplink = 1;
- break;
- }
- }
- csi_camera_put_frame(frame);
- gettimeofday(&tv_end, 0);
- timersub(&tv_end, &tv_start, &tv_duration);
- pthread_mutex_lock(&plink_ctx->mutex);
- plink_ctx->sendid = (plink_ctx->sendid + 1) % NUM_OF_BUFFERS;
- plink_ctx->available_bufs -= 1;
- pthread_mutex_unlock(&plink_ctx->mutex);
- }
- else
- {
- LOG_W("frame not send by plink,available_bufs:%d,fmt:%d\n",plink_ctx->available_bufs,
- frame->frame_info.pixel_format);
- csi_camera_put_frame(frame);
- }
- }
- else
- {
- LOG_E("plink is exit\n");
- csi_camera_put_frame(frame);
- }
- LOG_I("%s exit \n");
- }
|