From d5fe55d2e0bd0dc61f4de4ca8a008bdfabdcb163 Mon Sep 17 00:00:00 2001 From: Kamil Trzcinski Date: Mon, 4 Apr 2022 13:51:29 +0200 Subject: [PATCH] WIP --- Makefile | 2 + buffer.c | 72 ++++++++++++++++++++++ buffer.h | 21 +++++++ buffer_list.c | 158 +++++++++++++++++++++++++++++++++++++++++++++++++ buffer_list.h | 30 ++++++++++ buffer_queue.c | 120 +++++++++++++++++++++++++++++++++++++ camera_stream | Bin 0 -> 22608 bytes device.c | 141 +++++++++++++++++++++++++++++++++++++++++++ device.h | 21 +++++++ main.c | 82 +++++++++++++++++++++++++ v4l2.c | 69 +++++++++++++++++++++ v4l2.h | 46 ++++++++++++++ 12 files changed, 762 insertions(+) create mode 100644 Makefile create mode 100644 buffer.c create mode 100644 buffer.h create mode 100644 buffer_list.c create mode 100644 buffer_list.h create mode 100644 buffer_queue.c create mode 100755 camera_stream create mode 100644 device.c create mode 100644 device.h create mode 100644 main.c create mode 100644 v4l2.c create mode 100644 v4l2.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..261bebb --- /dev/null +++ b/Makefile @@ -0,0 +1,2 @@ +camera_stream: *.c *.h + gcc -o camera_stream *.c \ No newline at end of file diff --git a/buffer.c b/buffer.c new file mode 100644 index 0000000..0174013 --- /dev/null +++ b/buffer.c @@ -0,0 +1,72 @@ +#include "buffer.h" +#include "buffer_list.h" +#include "device.h" + +buffer_t *buffer_open(const char *name, buffer_list_t *buf_list, int index) { + buffer_t *buf = calloc(1, sizeof(buffer_t)); + device_t *dev = buf_list->device; + + buf->name = strdup(name); + buf->index = index; + buf->buf_list = buf_list; + buf->dma_fd = -1; + + if (buf_list->do_mmap) { + buf->v4l2_buffer.type = buf_list->type; + buf->v4l2_buffer.memory = V4L2_MEMORY_MMAP; + buf->v4l2_buffer.index = index; + + if (buf_list->do_mplanes) { + buf->v4l2_buffer.length = 1; + buf->v4l2_buffer.m.planes = &buf->v4l2_plane; + } + + E_XIOCTL(buf_list, dev->fd, VIDIOC_QUERYBUF, &buf->v4l2_buffer, "Cannot query buffer %d", index); + + if (buf_list->do_mplanes) { + buf->offset = buf->v4l2_plane.m.mem_offset; + buf->length = buf->v4l2_plane.length; + } else { + buf->offset = buf->v4l2_buffer.m.offset; + buf->length = buf->v4l2_buffer.length; + } + + buf->start = mmap(NULL, buf->length, PROT_READ | PROT_WRITE, MAP_SHARED, dev->fd, buf->offset); + if (buf->start == MAP_FAILED) { + goto error; + } + } + + if (buf_list->do_dma) { + struct v4l2_exportbuffer v4l2_exp = {0}; + v4l2_exp.type = buf_list->type; + v4l2_exp.index = index; + v4l2_exp.plane = 0; + E_XIOCTL(buf_list, dev->fd, VIDIOC_EXPBUF, &v4l2_exp, "Can't export queue buffer=%u to DMA", index); + buf->dma_fd = v4l2_exp.fd; + } + + buffer_capture_enqueue(buf); + + return buf; + +error: + buffer_close(buf); + return NULL; +} + +void buffer_close(buffer_t *buf) +{ + if (buf == NULL) { + return; + } + + if (buf->start && buf->start != MAP_FAILED) { + munmap(buf->start, buf->length); + } + if (buf->dma_fd >= 0) { + close(buf->dma_fd); + } + free(buf->name); + free(buf); +} diff --git a/buffer.h b/buffer.h new file mode 100644 index 0000000..649d1e0 --- /dev/null +++ b/buffer.h @@ -0,0 +1,21 @@ +#pragma once + +#include "v4l2.h" + +typedef struct buffer_s { + char *name; + struct buffer_list_s *buf_list; + int index; + void *start; + size_t offset; + size_t length; + struct v4l2_buffer v4l2_buffer; + struct v4l2_plane v4l2_plane; + int dma_fd; + bool enqueued; +} buffer_t; + +buffer_t *buffer_open(const char *name, struct buffer_list_s *buf_list, int buffer); +void buffer_close(buffer_t *buf); +bool buffer_output_dequeue(buffer_t *buf); +bool buffer_capture_enqueue(buffer_t *buf); diff --git a/buffer_list.c b/buffer_list.c new file mode 100644 index 0000000..546c20d --- /dev/null +++ b/buffer_list.c @@ -0,0 +1,158 @@ +#include "buffer.h" +#include "buffer_list.h" +#include "device.h" + +buffer_list_t *buffer_list_open(const char *name, struct device_s *dev, unsigned type) +{ + buffer_list_t *buf_list = calloc(1, sizeof(buffer_list_t)); + + buf_list->device = dev; + buf_list->name = strdup(name); + buf_list->type = type; + + switch(type) { + case V4L2_BUF_TYPE_VIDEO_OUTPUT: + break; + + case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE: + buf_list->do_mplanes = true; + break; + + case V4L2_BUF_TYPE_VIDEO_CAPTURE: + buf_list->do_dma = true; + buf_list->do_mmap = true; + buf_list->do_capture = true; + break; + + case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE: + buf_list->do_dma = true; + buf_list->do_mmap = true; + buf_list->do_mplanes = true; + buf_list->do_capture = true; + break; + + default: + E_LOG_PERROR(buf_list, "Unknown type=%d", type); + goto error; + } + + return buf_list; + +error: + buffer_list_close(buf_list); + return NULL; +} + +void buffer_list_close(buffer_list_t *buf_list) +{ + if (!buf_list) { + return; + } + + if (buf_list->bufs) { + for (int i = 0; i < buf_list->nbufs; i++) { + buffer_close(buf_list->bufs[i]); + } + free(buf_list->bufs); + buf_list->bufs = NULL; + buf_list->nbufs = 0; + } + + free(buf_list->name); + free(buf_list); +} + +int buffer_list_set_format(buffer_list_t *buf_list, unsigned width, unsigned height, unsigned format) +{ + struct v4l2_format *fmt = &buf_list->v4l2_format; + + fmt->type = buf_list->type; + + if (buf_list->do_mplanes) { + fmt->fmt.pix.colorspace = V4L2_COLORSPACE_RAW; + fmt->fmt.pix.width = width; + fmt->fmt.pix.height = height; + fmt->fmt.pix.pixelformat = format; + fmt->fmt.pix.field = V4L2_FIELD_ANY; + fmt->fmt.pix.bytesperline = fourcc_to_stride(width, format); + } else { + fmt->fmt.pix_mp.colorspace = V4L2_COLORSPACE_JPEG; + fmt->fmt.pix_mp.width = width; + fmt->fmt.pix_mp.height = height; + fmt->fmt.pix_mp.pixelformat = format; + fmt->fmt.pix_mp.field = V4L2_FIELD_ANY; + fmt->fmt.pix_mp.num_planes = 1; + fmt->fmt.pix_mp.plane_fmt[0].bytesperline = fourcc_to_stride(width, format); + } + + E_LOG_DEBUG(buf_list, "Configuring format ..."); + E_XIOCTL(buf_list, buf_list->device->fd, VIDIOC_S_FMT, fmt, "Can't set format"); + + if (fmt->fmt.pix.width != width || fmt->fmt.pix.height != height) { + E_LOG_ERROR(buf_list, "Requested resolution=%ux%u is unavailable. Got %ux%u.", + width, height, fmt->fmt.pix.width, fmt->fmt.pix.height); + } + + if (fmt->fmt.pix.pixelformat != format) { + E_LOG_ERROR(buf_list, "Could not obtain the requested format=%s; driver gave us %s", + fourcc_to_string(format).buf, + fourcc_to_string(fmt->fmt.pix.pixelformat).buf); + } + + E_LOG_INFO(buf_list, "Using: %ux%u/%s", + fmt->fmt.pix.width, fmt->fmt.pix.height, fourcc_to_string(fmt->fmt.pix.pixelformat).buf); + + return 0; + +error: + return -1; +} + +int buffer_list_request(buffer_list_t *buf_list, int nbufs) +{ + struct v4l2_requestbuffers v4l2_req = {0}; + v4l2_req.count = nbufs; + v4l2_req.type = buf_list->type; + v4l2_req.memory = buf_list->do_mmap ? V4L2_MEMORY_MMAP : V4L2_MEMORY_DMABUF; + + E_LOG_DEBUG(buf_list, "Requesting %u buffers", v4l2_req.count); + + E_XIOCTL(buf_list, buf_list->device->fd, VIDIOC_REQBUFS, &v4l2_req, "Can't request buffers"); + if (v4l2_req.count < 1) { + E_LOG_ERROR(buf_list, "Insufficient buffer memory: %u", v4l2_req.count); + } + + E_LOG_DEBUG(buf_list, "Got %u buffers", v4l2_req.count); + + buf_list->bufs = calloc(v4l2_req.count, sizeof(buffer_t*)); + buf_list->nbufs = v4l2_req.count; + + for (unsigned i = 0; i < buf_list->nbufs; i++) { + char name[64]; + sprintf(name, "%s:buf%d", buf_list->name, i); + buffer_t *buf = buffer_open(name, buf_list, i); + if (!buf) { + E_LOG_ERROR(buf_list, "Cannot open buffer: %u", i); + goto error; + } + buf_list->bufs[i] = buf; + } + + E_LOG_DEBUG(buf_list, "Opened %u buffers", buf_list->nbufs); + return 0; + +error: + return -1; +} + +int buffer_list_stream(buffer_list_t *buf_list, bool do_on) +{ + enum v4l2_buf_type type = buf_list->type; + + E_XIOCTL(buf_list, buf_list->device->fd, do_on ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type, "Cannot set streaming state"); + + return 0; + +error: + return -1; +} diff --git a/buffer_list.h b/buffer_list.h new file mode 100644 index 0000000..1ee2933 --- /dev/null +++ b/buffer_list.h @@ -0,0 +1,30 @@ +#pragma once + +#include "v4l2.h" + +typedef struct buffer_list_s { + char *name; + struct device_s *device; + buffer_t **bufs; + int nbufs; + int type; + + struct v4l2_format v4l2_format; + bool do_mplanes; + bool do_mmap; + bool do_dma; + bool do_capture; +} buffer_list_t; + +buffer_list_t *buffer_list_open(const char *name, struct device_s *dev, unsigned type); +void buffer_list_close(buffer_list_t *buf_list); + +int buffer_list_set_format(buffer_list_t *buffer_list, unsigned width, unsigned height, unsigned format); +int buffer_list_request(buffer_list_t *buf_list, int nbufs); + +bool buffer_list_wait_pool(buffer_list_t *buf_list, int timeout); + +buffer_t *buffer_list_output_enqueue(buffer_list_t *buf_list, buffer_t *dma_buf); +buffer_t *buffer_list_capture_dequeue(buffer_list_t *buf_list); + +int buffer_list_stream(buffer_list_t *buf_list, bool do_on); diff --git a/buffer_queue.c b/buffer_queue.c new file mode 100644 index 0000000..94664d8 --- /dev/null +++ b/buffer_queue.c @@ -0,0 +1,120 @@ +#include "buffer.h" +#include "buffer_list.h" +#include "device.h" + +bool buffer_output_dequeue(buffer_t *buf) +{ + // if (buf->enqueued || buf->buf_list->do_capture) { + // return false; + // } + + return false; +} + +bool buffer_capture_enqueue(buffer_t *buf) +{ + if (buf->enqueued || !buf->buf_list->do_mmap || !buf->buf_list->do_capture) { + return false; + } + + E_LOG_DEBUG(buf, "Queuing buffer..."); + E_XIOCTL(buf, buf->buf_list->device->fd, VIDIOC_QBUF, &buf->v4l2_buffer, "Can't queue buffer."); + + buf->enqueued = true; + +error: + return true; +} + +buffer_t *buffer_list_output_enqueue(buffer_list_t *buf_list, buffer_t *dma_buf) +{ + // if (dma_buf->enqueued || dma_buf->dma_fd < 0) { + // return NULL; + // } + + // struct v4l2_buffer v4l2_buf = {0}; + // struct v4l2_plane v4l2_plane = {0}; + + // v4l2_buf.type = buf_list->type; + // v4l2_buf.memory = V4L2_MEMORY_MMAP; + + // if (buf_list->do_mplanes) { + // v4l2_buf.length = 1; + // v4l2_buf.m.planes = &v4l2_plane; + // } + + // E_XIOCTL(buf_list, buf_list->device->fd, VIDIOC_DQBUF, &v4l2_buf, "Can't grab capture buffer"); + // E_LOG_DEBUG(buf_list, "Grabbed INPUT buffer=%u", v4l2_buf.index); + + return NULL; +} + +buffer_t *buffer_list_capture_dequeue(buffer_list_t *buf_list) +{ + if (!buf_list->do_mmap || !buf_list->do_capture) { + return NULL; + } + + struct v4l2_buffer v4l2_buf = {0}; + struct v4l2_plane v4l2_plane = {0}; + + v4l2_buf.type = buf_list->type; + v4l2_buf.memory = V4L2_MEMORY_MMAP; + + if (buf_list->do_mplanes) { + v4l2_buf.length = 1; + v4l2_buf.m.planes = &v4l2_plane; + } + + E_XIOCTL(buf_list, buf_list->device->fd, VIDIOC_DQBUF, &v4l2_buf, "Can't grab capture buffer"); + E_LOG_DEBUG(buf_list, "Grabbed INPUT buffer=%u, bytes=%d", + v4l2_buf.index, v4l2_buf.length); + + buffer_t *buf = buf_list->bufs[v4l2_buf.index]; + buf->v4l2_plane = v4l2_plane; + buf->v4l2_buffer = v4l2_buf; + if (buf_list->do_mplanes) { + buf->v4l2_buffer.m.planes = &buf->v4l2_plane; + } + + buf->enqueued = false; + + return buf; + +error: + return NULL; +} + +bool buffer_list_wait_pool(buffer_list_t *buf_list, int timeout) { + struct pollfd fds = {buf_list->device->fd, POLLIN, 0}; + + if (poll(&fds, 1, timeout) < 0 && errno != EINTR) { + E_LOG_ERROR(buf_list, "Can't poll encoder"); + } + + E_LOG_DEBUG(buf_list, "Polling encoder %d, %d...", errno, fds.revents); + if (fds.revents & POLLIN) { + return true; + } + if (fds.revents & POLLPRI) { + E_LOG_DEBUG(buf_list, "fd POLLPRI"); + } + if (fds.revents & POLLOUT) { + E_LOG_DEBUG(buf_list, "fd POLLOUT"); + } + if (fds.revents & POLLERR) { + E_LOG_DEBUG(buf_list, "fd POLLERR"); + device_consume_event(buf_list->device); + } + if (fds.revents & POLLHUP) { + E_LOG_DEBUG(buf_list, "fd POLLHUP"); + } + if (fds.revents & POLLNVAL) { + E_LOG_DEBUG(buf_list, "fd POLLNVAL"); + } + + return false; + +error: + return false; +} \ No newline at end of file diff --git a/camera_stream b/camera_stream new file mode 100755 index 0000000000000000000000000000000000000000..1dd7effb7e56c9cebad28d689ded373f3459250d GIT binary patch literal 22608 zcmeHPe{|H5FtWpze-q9$uJ$2yhtvzKey0p5j+q!2tt99FR_#=}z7QwD`S6ghY+0Wb#Ul;zSR&Wo5Ki~?cKb3(-z`8%8uB3BhRxeq zC8!U|%!@o~kXT3j%%@wspZwYrYoFcy;=F(S(VsthXZ&X?W&=lh(q4%);+YZnyTB*@ z6ViDqIzCO+PvWQaqHI(}B(AHFX33KJL!P|ms!Z(uuZQ<8Ke2D-Cog<)@8AAv_N;F_ ze5mgBk9~2kZ}V-xNqy_fdvAYp?Bw2#qkC8V&u8mgi{3o;zw=&xBR}}fS%1D^x}GiOi&Z7~n@ZqaC2(g6ytxF9mB923b5VW)U+SXw+$y5|v8>3X(y>Ig zTZB84X+YU@B$pC>ec_bo3diHgF3}h5%S5vx6pE(PiDU>>;cP6K5UFH5PGnbV51K`Q z6|rPj7InFVY#vW$q9U1!CPcUHBNSrip-eWM&W8HJF<{cssMyf5aqYU$3g1d}($kkr zXo8Rwy9}Q9ZvR~He)_)|YfUURlOB6WazI>wzOM!W^>iIWRKxyOTvz z{s*g^x5`JX@ecx!EcQtnwa&Y+OcJ>XUE3QJ@7%S1F(H9+Sh_kh;0Spys9ke=8i2Z&>xt1)q}R) z3tDgEaO3QJ;j8ce`0x+BL+{$o3)|HO(9Zt?`x*5PJXt#oSzUgN&Hqdh@YyW-co}ku z=7v|?Ll2Jhp1yNrho~y<@>Uf)oT_5pn;&(F{OEqP6aIX`;|I>au;}(5!uYF-JH*1` zCpE4ddh!4#ocoK+JDi0@k1p>**{#dM86G`?GWitz&r8|NLH}$&WUp zEct<^R<|A2Z80q>*&peGy3H=<{t?!-0Io*v5r?Eqwf_4@$p5+7gT))J6z9Dds~33c z2h;Ac6U%9nLsB-@!2(h);T%G}ICu2A!D0vL{SV4^2f^nf82?V-D8G>jQ&P$lK|9Lf z)pB)cxCSt$=Q$wy2cow8xQp8bv_SWJ}70NEis-^0ULdgcF;Dc*8NqZ zjZ0)k{dycDT4zfDxA`}X1lSffE*JU23bZ#iXoGDz&LDEweY6F+k)h4mG;$RF$Tlxt zA+XJVXkQ?9jz_&e+mAokH`?QUyBfZ4JwR9P%@@ib!@0M|q)bP2Tgdi=l$kWZpVxSy;qMWK+}}k`KjJuy+(y1y z$VY#y@m9gdtBNndE}VN`XB$2D*hdZY!@1-IzehFB%o)zTTHr}XpQ}ag@~<12f$^39 zdhkp2=sQs55-%VfohJ37-j75cF8If=^K@}B+8KBRuqhX;@+is+-vb@_nf7dhd3UZ! z`wZ*Nypv_pW#>-2lq)G4v*d|a}{%_bE8lCzyR#N-?F_|>(jV(V4UzPGBmXMyb^cl|0a?x6NZEWlb!<%xgf}Dgg zH%A!{O__64mbH)gNC%rPmE)T1BNq^#YY?A<7)LXFqy;__gpY8ZHiLH!WU19<6GN#l z`UCMME?tdrvc3wkFut(jR{glV0o6#Qkf#*8jrA?D=6GWS9(T45aSg$ySH{Vk073U&lW;`L@z|^ye zwTsc0(Njchl6y7*o>zk><7otT>h&sL-HtkYzXshbI6?mthor5h()&@Y8y@sM1V4Hm za^W6Y_UD0JSZ`vx7k!z1t4qgq%EkFbxk+1wz54TGW2?g zT~>Q9td1%w$omD3 zgRzr!X8%#HV<-12Y|q$9yorTD_~#PLfBMD0luy#*vHgHIAEWS-Zu(mg__)26Tyj5l3As8n&lGt2Ex8D7^p)Q!zmxY?Vqx*WHEh;l zuG2NBKU-4IeH@?15O;w%=%4Ew_a8Eswo!YkxVE2J|G3u4vbJ9kc=p!kJfn1vcSh^{9r8HJpH3k*ao#@*o&ONLc^^;O z4#?}eMfpAVLx-p1qsP?`JmuIAK11MxN4)3g6SVhqeK+Zt>h>xcdC#olV*q>}(PQO0 z#%COmW85c9j4HzeG42Hp!}dpzhUf$By#qf-?rm_1HAu!MN}tm>+BR7^2D#QtTM?$; zRbbtbvLilbwXOS0XbF?{ckow3+h5`rSexWC9FC8)_i|hqPcw44$3dMLkCAl;;%gA` zb%(>}qB5oywqjq#Il;AItKNs*j4^QS$ifd;&$6_go_Bm+$e2Z+Cyvh>TM&`WWjK&a2zaM?3h>xeMASr<0d!IpsYA|9kt*vGa@2 zhWg??1J`isYzbo%Xw=%N*HG;j@cT+x55IT&A1?Cw^bWK;rs7$F>j>70CnOEpc)Slw zo_E97%$isMytFg==DJ4zm+PC(OYkJPX4V^j>VW^pk<1(!KzRu1B+_Xl z<8Nkez*h=0;Aiw@GcWMmYJ6VIaPBIad->NanZfBX_Ko#N#t$=3Ok2Ws+u3Bj?XIt z@6xVbx8oYwF5AKGXlK?+V|Qb3;d6f=pe;X@FYGY&;1$$m`TPZYtZMj|3-vB< ze)MVfr}u%zf2JDCbC9_oF}?#Sg4Bx?M>6B!TF-ItUd4-e>A`r8xCV>A$9+A=bC5oR z@l>P!0o1>t>j!kb7xn$9e@)k?biLfq>G9+JK_%C2qYvJ1G+Qv_m-<-`{TLrzs`1oM zy(vRKT+hje_LuuTluPv^pr607^s^K(!R+hg{f2J;oY6JgQ56eE~Mz3mXn2A3**p z^8Lt5ZM<{)&URyG^NfP;F=}A17QWkoyo>`kVN9I+)GNo!dH!Qd7VK-DcuC97xW@QS znx9y4=r?0yl}c}iWb7lp33xkJ;GJycG4g@#XfjK83Q_I>_uO9UsP-U z8N}-|d@l;#($|rX&mEMXhYz-CIobbz0B*M6p91DSS^5v~Kc>(6C&$dU^n3}doh9SG z2WO0`k*HtlntC@nbuf4AcSC&c$h`vRI` z13$aZDH}BS_(d!8g?FV{^&V)}ygLoYjdM(oNqP9*8TxJDa}mg5*R7qRZv7N> zh=sB(-Y3X4+NF7#dYh-IR_>!P1X`}(^^u;&}5o$U5rJYVqO`9eMH;~=KjqdlMT)?PQrHMRhKR1)@R z82iP~5Pl1OKs^1C@C6NHy{fzp<7M5juH#%K{DOuN4;MIq??)_gbnJLe$Cky6=a9b) zIO5Cv_YWJIr!@`VR~Q;T!y(Ng9{(jR)y;u3^PT*&PcMYGRiqAvG zcz1yP{E+u)`5tT~Hplwn;m(H-l_;m}u))16HBg|m*&=ety& zIQ^w5$%H&wqYxmIjYgbwG?R?ycpeO5^h1(Z#>pkZyTh?~xHBI0IU6uOi9r{#?Yd+x z9&tGAWM>vfz?^Jv6l@GbCDLt+Gaqsy>DX>4q$j*P>f|!c;>`FMwr3#yYQ=Y%Rj=jZ z&>)IZC|gaVd+1YlTtd4sk%2K{U9o5)YwUuPZpri>cB*ux1~$gL%<#A4i7MbJ=L+xShAc6BImwZVqm5(>=rfOP$U=*=R<2o7Mqz%!7hXG|`pB89}AHAZjSH zu7uAoMVgSkJK_Ylw6p|UH!6A!Z`s~930~jYItgyx9-IVkZf|UnV>I&*^9}MylSHDs zV_i_A)D?7?-a~;@Ee9R$jKyQwSTv(%$EEZHonrb<(R5tDO|mm5l8k03Kqi-xvv|U$ z8@H6wro!1?%mSx70(DA}qvLZ}+eFHw%d7iR@o*xl?b#lSM3dvD!<3X>qo>QBwr$zox^8`FU325+4eM8fTQ1!d zb-H@Pi5{gm`Xl?X+N|HQZ8dxl>h0F^%UKfZNhH(J$c@q->l*J|-`Xg|#%;ldZCFsC zB3a(KeS5oIZfMzpqjfl^4bhkFrtpVL?Xj_B7WHGuDdz>`Er9>ni@%RI zR{-PuQzXNW`2b!%ufj_kKHEnoHmqB>+F8;X&ZIhH;e@lwcZ<*8aO=(KRBZW;3KESz z(1oLTq$gbUC|2AmuDHVEn&GM}o8zAAs&QTInqOY)y2iDjtj=})G(+Zh1$dRqUmBzT zaafJ_;5aNTAZRm1Pbr0<&3KhdODbC`7^kXn8C{plU*HQtTktBE))9g>Q!w8C;i4^h zl}k@hKBZuK1Y7{R=T$B}PJMqt!Sp=kdkV%IN?cy}BCm4&D}H{9LBZ-*)&1izy-E43 z!qbzK&nlQ6rM{J*V0s`f_$04#p*(fRI6P0*^Zks%(^Ds|a{P+dzREFUlw_*bEq*=L zjH0mzfA_r!FuymUu9-sOwfyvY!n;cFX8in$0r5sYA?AtAniDhjNB+H5`!85<8D=)y z@3-)z=bTqpm6VTjUtL#;ujvvq!+%W)eYHjZ$`temto~jH-oQql;})LsoUq{ESg@gg z$-+y0Y5PzgY|ogcu9;#6iUv0AYY_Jg{{1NOH_)tcArg@vW1PC?3Yx~iE_^Y9{24dZ zHB+>sXkf#i-|A4;9MK89ff?DzpZ%$8zWBIdg$m;@{QauBX375Qtp3=3n1hnnOo_i} z!JjcG;0-_i7E)b`-;qiKD}V4*O~4!}6QE~UQ%7CqbVU1;!%f8K(LKYHzC{7DNY{;)HFA2LPMoBlh-{bio?AD*#Nm-1Jh zyHb~uZ^+_D{vFpDyppfqf{9g6dC&)1>Vemer4r#z-AwTFFlAq056FLV3)3#~l{?501yr zYKd>N;6Foq%FmT~4&wE##?%tVaUj4vTSxqC#JhIDoFCjgkp2f);?4Tt03Qe5tY2Ql zw|fCIUgS0XrvUSep5b>Gu(`N;9s_)`8t1dYm;Aq9f`1$E1A4sV|L=e~KWdPZ{ujt6 z`kyC;0n6Gr?|>c&`vIHvXHMBw;tzmr{xj4TMv6%J76Lwo@zVv^eg$C0 zw_O@u2bkv&&G@zg9vFw`i@ySFF7o>*V9KlL=Zn21_y+-V{+!kPzXVr4cwvv{0Go^by<))*;ENVaya$YSfgi8gW%I<`ui{mEqn@aOC8Q7Yy1O%8Q-7L@KL~=&mDUF z{{)!xvjaTH?@7SsqP)K-fnP&A+rH<4XZ+;-1Ka;A^kMc(gf9ZW3yXG}?r$b;KJ4~a z1D^1=`QkdjjQ4ph?*{-gzM1xG0CPQhM)$V`u(>F2n0UnNJj(1p37F^dI1GLxslSB& zvw%(iwAYsabGb1A-w?~ ztEa1L8NR=zzr@v($nmbCw|m*DWj>$&KNY$won7uriu#DC5C18c#lWl!pJxi)6Aj-T z>Q4MAcqYg*oy}x%^u^aDa9AVS6T=~&bST>w>WU{5(G0%N6iJ49;>pf%JQT?$)0t2> z*Dtz~IDHVsaUkDv{|8pxGBs7GJC=xr!s&E)PYA~W(tGgjC%hI9MRI+8d(gy!LZHk} zBGrdmx`Zm=Gg6r>WYmy6)FMUZnGO{AX(?p8SH@Q$3&-(NKa`2x8x_%bcLSOGlH1Vb z9j$o3A6mb;DHH-UsnduEHSOHoc<07-lQHtsRU)*ZWy{*eme7_v?%1}zE!5Vywq-q8 z$gf*CGDH)v=0Dgh!xqQE=(ZWX8XMrAG+$<6By~ zc>$dJZS z1}zYdNkB{L^I$eNBcM7a1k0*X*wV>iAOqf|OIR+{(*na#W8`(x~CGCGn;hH$1N5{ED0>uEMp zkMN#YB-<-O8@HgpNGuV`Wug)JJvEzFt4C@^e;mz5TP6xwnw&IZeR8eDUWpzvK)NAa z8t~)~x7i)3Z@bwwx@+Zd29U>jq9J)Ahy0{3SdKbLuzSLoys2aoAA;i--Yog)Q8>h= zKM*%XuNqI|Wzs#J=G$~OWhm5k=Q=%SZPrR)b+BkspL%*3$CRJBvl+^Uc7^a|_Vi`L zoyfCkmG_zwzLXbDr-Tm&prXFUwHq6<;T{cPE%0^bV)#m9EFye_dc&Ds;fw4^fS<~< zX@$YD#tc89C&@Gr0xlhmlU@~4@vQJk3;MFrev}zNflA6L>uYSi(+3xf^b23KS4B@n zt^%hbH)Sxz;lVC+m;@um0AWV>5W4#ijfG?v&SJ*nycGidd%yVH659lUr623PTJ!l# z(_F;yT7-m0cao0JYWTfO2e7%g*7I79Wa#)z?I7stB}V-*bWH&98-*nM<+HZOLC5`! zS-MHb#Ai2ENTlO4xDFHopyg9HWRiwYxOkw8+d|UuIcpvO`I(Efygq`&XQZU#GrJaa z!hMD5m-S3{Bk`Fk%Y2rX1)YPk-7i3KFA|^Vk&e&!4ug*J7=9ds2kCw!(yc|}v%h0N z@cAz3_(aXn9YC3~vLEhgR)U}!Wpj~^*QbySoq9%#J)Fd-Kcr*&Jd&Z~9&srcpSfPq z@UP)_#G>Q9h37^kx1lt2k7zoMmwRR}=sGB@z6{+r0Hfd3KIH1Vu!^60dO6YWw-lY! zAMY{ymrdZv((wB}%6!sE8>{cUPT)`_=}7xyB>Z+w((#_O1_$*F4^)rSJ&!WQCO`FE z*okk#qDs=yZfB6FKhnv2)&TBRSk@PLGZl~w9q%o71i%+{<}xz;5*6f2e%u460+Z>8 z<3(GNpPv9O(k+rD^@lj7H;}+9rE2kuk^s&z8UQ(IcrmBV%=JtFdW2(f{(lUK^3iYU gqjIdsdA-mg#IKAq12+8l6xpWxK`VUOAT7H81N=pG(*OVf literal 0 HcmV?d00001 diff --git a/device.c b/device.c new file mode 100644 index 0000000..b4fc1a4 --- /dev/null +++ b/device.c @@ -0,0 +1,141 @@ +#include "device.h" +#include "buffer.h" +#include "buffer_list.h" + +device_t *device_open(const char *name, const char *path) { + device_t *dev = calloc(1, sizeof(device_t)); + dev->name = strdup(name); + dev->path = strdup(path); + dev->fd = open(path, O_RDWR|O_NONBLOCK); + if(dev->fd < 0) { + E_LOG_ERROR(dev, "Can't open device"); + } + + E_LOG_DEBUG(dev, "Querying device capabilities ..."); + E_XIOCTL(dev, dev->fd, VIDIOC_QUERYCAP, &dev->v4l2_cap, "Can't query device capabilities");\ + + if (!(dev->v4l2_cap.capabilities & V4L2_CAP_STREAMING)) { + E_LOG_ERROR(dev, "Device doesn't support streaming IO"); + } + + E_LOG_INFO(dev, "Device path=%s fd=%d opened", dev->path, dev->fd); + return dev; + +error: + device_close(dev); + return NULL; +} + +void device_close(device_t *dev) { + if(dev == NULL) { + return; + } + + if (dev->capture_list) { + buffer_list_close(dev->capture_list); + dev->capture_list = NULL; + } + + if (dev->output_list) { + buffer_list_close(dev->output_list); + dev->output_list = NULL; + } + + if(dev->fd >= 0) { + close(dev->fd); + } + + free(dev); +} + +int device_open_buffer_list(device_t *dev, bool do_capture, unsigned width, unsigned height, unsigned format, int nbufs) +{ + unsigned type; + char name[64]; + struct buffer_list_s **buf_list = NULL; + + if (do_capture) { + buf_list = &dev->capture_list; + + if (dev->v4l2_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) { + type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + sprintf(name, "%s:capture", dev->name); + } else if (dev->v4l2_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) { + type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; + sprintf(name, "%s:capture:mplane", dev->name); + } else { + E_LOG_ERROR(dev, "Video capture is not supported by device"); + } + } else { + buf_list = &dev->capture_list; + + if (dev->v4l2_cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) { + type = V4L2_BUF_TYPE_VIDEO_OUTPUT; + sprintf(name, "%s:output", dev->name); + } else if (dev->v4l2_cap.capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE) { + type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; + sprintf(name, "%s:output:mplane", dev->name); + } else { + E_LOG_ERROR(dev, "Video output is not supported by device"); + } + } + + *buf_list = buffer_list_open(name, dev, type); + if (!*buf_list) { + goto error; + } + + if (buffer_list_set_format(*buf_list, width, height, format) < 0) { + goto error; + } + + if (buffer_list_request(*buf_list, nbufs) < 0) { + goto error; + } + + return 0; + +error: + buffer_list_close(*buf_list); + *buf_list = NULL; + return -1; +} + +int device_stream(device_t *dev, bool do_on) +{ + if (dev->capture_list) { + if (buffer_list_stream(dev->capture_list, do_on) < 0) { + return -1; + } + } + + if (dev->output_list) { + if (buffer_list_stream(dev->output_list, do_on) < 0) { + return -1; + } + } + + return 0; +} + +int device_consume_event(device_t *dev) +{ + struct v4l2_event event; + + E_LOG_DEBUG(dev, "Consuming V4L2 event ..."); + E_XIOCTL(dev, dev->fd, VIDIOC_DQEVENT, &event, "Got some V4L2 device event, but where is it?"); + + switch (event.type) { + case V4L2_EVENT_SOURCE_CHANGE: + E_LOG_INFO(dev, "Got V4L2_EVENT_SOURCE_CHANGE: source changed"); + return -1; + case V4L2_EVENT_EOS: + E_LOG_INFO(dev, "Got V4L2_EVENT_EOS: end of stream (ignored)"); + return 0; + } + + return 0; + +error: + return -1; +} diff --git a/device.h b/device.h new file mode 100644 index 0000000..4eecb4e --- /dev/null +++ b/device.h @@ -0,0 +1,21 @@ +#pragma once + +#include "v4l2.h" + +typedef struct device_s { + char *name; + char *path; + int fd; + struct v4l2_capability v4l2_cap; + + struct buffer_list_s *capture_list; + struct buffer_list_s *output_list; +} device_t; + +device_t *device_open(const char *name, const char *path); +void device_close(device_t *device); + +int device_open_buffer_list(device_t *dev, bool do_capture, unsigned width, unsigned height, unsigned format, int nbufs); +int device_consume_event(device_t *device); + +int device_stream(device_t *dev, bool do_on); diff --git a/main.c b/main.c new file mode 100644 index 0000000..b5700a1 --- /dev/null +++ b/main.c @@ -0,0 +1,82 @@ +#include "buffer.h" +#include "buffer_list.h" +#include "device.h" +#include "v4l2.h" + +int camera_width = 1920; +int camera_height = 1080; +int camera_format = V4L2_PIX_FMT_SRGGB10P; +int camera_nbufs = 4; + +device_t *camera = NULL; +device_t *isp_srgb = NULL; +device_t *isp_yuuv = NULL; +device_t *isp_yuuv_low = NULL; + +int open_camera(const char *path) +{ + camera = device_open("CAMERA", path); + if (!camera) { + return -1; + } + + if (device_open_buffer_list(camera, true, camera_width, camera_height, camera_format, camera_nbufs) < 0) { + return -1; + } + + return 0; +} + +int open_isp(const char *srgb_path, const char *yuuv_path, const char *yuuv_low_path) +{ + isp_srgb = device_open("ISP-SRGB", srgb_path); + isp_yuuv = device_open("ISP-YUUV", yuuv_path); + isp_yuuv_low = device_open("ISP-YUUV-LOW", yuuv_low_path); + + if (!isp_srgb || !isp_yuuv || !isp_yuuv_low) { + return -1; + } + + if (device_open_buffer_list(isp_srgb, false, camera_width, camera_height, camera_format, camera_nbufs) < 0 || + device_open_buffer_list(isp_yuuv, true, camera_width, camera_height, V4L2_PIX_FMT_YUYV, camera_nbufs) < 0 || + device_open_buffer_list(isp_yuuv, true, camera_width / 2, camera_height / 2, V4L2_PIX_FMT_YUYV, camera_nbufs) < 0) { + return -1; + } + + return 0; +} + +int main(int argc, char *argv[]) +{ + if (open_camera("/dev/video0") < 0) { + goto error; + } + + // if (open_isp("/dev/video13", "/dev/video14", "/dev/video15") < 0) { + // goto error; + // } + +// return; + + if (device_stream(camera, true) < 0) { + goto error; + } + //return; + + while(true) { + if (buffer_list_wait_pool(camera->capture_list, 1000000)) { + buffer_t *buf; + if (buf = buffer_list_capture_dequeue(camera->capture_list)) { + E_LOG_INFO(camera, "Got camera buffer: %p", buf); + buffer_capture_enqueue(buf); + } + } + } + +error: + device_close(isp_yuuv_low); + device_close(isp_yuuv); + device_close(isp_srgb); + device_close(camera); + return 0; +} diff --git a/v4l2.c b/v4l2.c new file mode 100644 index 0000000..7f5cd9c --- /dev/null +++ b/v4l2.c @@ -0,0 +1,69 @@ +#include "v4l2.h" + +int xioctl(int fd, int request, void *arg) +{ + int retries = XIOCTL_RETRIES; + int retval = -1; + + do { + retval = ioctl(fd, request, arg); + } while ( + retval + && retries-- + && ( + errno == EINTR + || errno == EAGAIN + || errno == ETIMEDOUT + ) + ); + + // cppcheck-suppress knownConditionTrueFalse + if (retval && retries <= 0) { + E_LOG_PERROR(NULL, "ioctl(%d) retried %u times; giving up", request, XIOCTL_RETRIES); + } + return retval; +} + +fourcc_string fourcc_to_string(unsigned format) +{ + fourcc_string fourcc; + char *ptr = fourcc.buf; + *ptr++ = format & 0x7F; + *ptr++ = (format >> 8) & 0x7F; + *ptr++ = (format >> 16) & 0x7F; + *ptr++ = (format >> 24) & 0x7F; + if (format & ((unsigned)1 << 31)) { + *ptr++ = '-'; + *ptr++ = 'B'; + *ptr++ = 'E'; + *ptr++ = '\0'; + } else { + *ptr++ = '\0'; + } + *ptr++ = 0; + return fourcc; +} + +static size_t align_size(size_t size, size_t to) +{ + return ((size + (to - 1)) & ~(to - 1)); +} + +unsigned fourcc_to_stride(unsigned width, unsigned format) +{ + switch (format) { + case V4L2_PIX_FMT_YUYV: + case V4L2_PIX_FMT_UYVY: + case V4L2_PIX_FMT_RGB565: + return align_size(width * 2, 32); + + case V4L2_PIX_FMT_RGB24: + return align_size(width * 3, 32); + + case V4L2_PIX_FMT_SRGGB10P: + return align_size(width * 5 / 4, 32); + + default: + E_LOG_PERROR(NULL, "Unknown format: %s", fourcc_to_string(format)); + } +} diff --git a/v4l2.h b/v4l2.h new file mode 100644 index 0000000..6f864bd --- /dev/null +++ b/v4l2.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#ifndef CFG_XIOCTL_RETRIES +# define CFG_XIOCTL_RETRIES 4 +#endif +#define XIOCTL_RETRIES ((unsigned)(CFG_XIOCTL_RETRIES)) + +// assumes that name is first item +#define dev_name(dev) (dev ? *(const char**)dev : "?") +#define E_LOG_ERROR(dev, _msg, ...) do { fprintf(stderr, "%s: " _msg "\n", dev_name(dev), ##__VA_ARGS__); goto error; } while(0) +#define E_LOG_PERROR(dev, _msg, ...) do { fprintf(stderr, "%s: " _msg "\n", dev_name(dev), ##__VA_ARGS__); exit(-1); } while(0) +#define E_LOG_INFO(dev, _msg, ...) fprintf(stderr, "%s: " _msg "\n", dev_name(dev), ##__VA_ARGS__) +#define E_LOG_VERBOSE(dev, _msg, ...) fprintf(stderr, "%s: " _msg "\n", dev_name(dev), ##__VA_ARGS__) +#define E_LOG_DEBUG(dev, _msg, ...) fprintf(stderr, "%s: " _msg "\n", dev_name(dev), ##__VA_ARGS__) + +typedef struct { + char buf[10]; +} fourcc_string; + +fourcc_string fourcc_to_string(unsigned format); +unsigned fourcc_to_stride(unsigned width, unsigned format); +int xioctl(int fd, int request, void *arg); + +#define E_XIOCTL(dev, _fd, _request, _value, _msg, ...) do { \ + int ret; \ + if ((ret = xioctl(_fd, _request, _value)) < 0) { \ + E_LOG_ERROR(dev, "xioctl(ret=%d): " _msg, ret, ##__VA_ARGS__); \ + } \ + } while(0)