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 zcmb<-^>JflWMqH=CI$@#5Ko7Vk->z4f#F0j0|NsK1FH#x0s|+527@ev3`oA9K%ar3 zK%WteIT#oiK$r!rl!1YbfdQ=7gaKp*BZC4X0~o{PnHd-uLKs1qks*YUkwKDy0gOTN zAiF^r;vNu-k)cMP0gPF}f{Y;h83gPZ7zFH@z&L@CfuVvCVlM*&BLfoy1A~M;2s1HA z*fTM_U|;}akh?&3fN%jL1498LBN)3dFff2HNF0O%pO&P6twJL}`UMym7=oUbq#)b> zfq{Vmgh6^iF4oV$rzT-ejm4*K8Aj!wOFoM|`?_HMO8&{4 zmhep|ZQ_ldWzEmczjZ0pI5;&I&SGFtU|?X#N8$@HFfbUR@%zyD5@>umG(OCKAYqVs zH6R8QgZQdYHi!c8OF#@L2JtF{>COgrT^kD5bQ3AvZU%fFU_CCnrCdAvZO*IJJZ!K0dXmC@()gCqFr{Br`vcp&&me z2PBzXP{~kSl9F0f#E_YvT$00(Tbc*fo|9jk%8*}>n#Yg^wIeLwtOCZhjtAK|ILG3`|5oP!0yAf0$oIGBbHWDS9&41O|o> z2POtlQ2umaVi4hBVBm3JVi4nFU=RS&EDQ`HAX<=tK>|dxF)+x0Xl@1u1rW{8z@P%6 zIT#o;Kr|x*gARyhVqh=;(d-NiCLo%Vfx!Yqb1^X3fM{L@1_uz$%)sCRq6HWjJV3NC z1A`BUW@TUq0MSAW3>*%+FTEHTUUM|;eZlfT_bJPR`A_*C^grPEuM$61xM5-GIcdKw=jlu``g^2}tY+Bz6E2+XIR1fW)>y zVjCc_H9+hQ3m!8(Sn!xdVD1xMg#`~;6m*}mFz7xNI57Vy%K_b|JPh-n@)*p2dg1^7 z*Khy-f6b!F$dI7S$go6%kwL^@{!V*I6KLq7N5C$a^6^8jwRRrcgWe{+D;3&A@;X?Vw z2L}Z<5Y#8&p!-t7Vg6GFgDW6=9t$+=m3Z*~|7%8r1&3oI?xzt9(ofrsIcI%he9*NJOk;c5>UMlpnBOD7`Q<8fz$;+^~5MN zKLUyQC^SFiF_3=B!7v%@CJBS)MqrKMS0MMp+{6KOQ;Gt_ zUID1P5}o$ z0typ`;K%$74691OX&HAIMkpY{5Tq|eVE$8HklPs+JOqV_3n<+fNIwOo5e^3FrIO-WQxg~kymj6i%YBt9t2VCjkn79XIrDX`!n6T^asum1f9yJ?02 zC|v|UW@cd61#%;Z4+@vpP8`8f%79y zhTzA{ALmb3VPJUWz|F{Tg`1IK4i22b&B$QD&By>U50s}KC>(yO3QemZ{v(CM;4lQS zA0n|ID;$0*O@um-c`&m`VS7%`00YDXwdu!q#mRel!icglbIpt;rsvpcY(w}W`o>i@$dg@5T8L| z!DCQ)2xEitD3}dRBYGfp4AM_^!2W`yb(o(%c<84i!4nKw2k&0vo$gKiKkg^%%A5ffu(g>Ih&VvlEz8KC2*Bu`O=Rf`V z_y22<9H^cG#RDi$+!2J7Su0r?Udu3WKb3)&Sv(93tF}PPeUKg$v!Us4hT?p%`Z1`$R8m4K;Z}y1BG`4F0%zdWe3A68$n3g2AKtNCx^nM zryL5sPgx9lp9X;3XwduA2b7N$7CclCod5LD-~X>|q4tB)9Vnhac1bfZaJ>QLqkzfa zbb#Uy2Z&u49OgfDFz|i^3S&^(T*Aum8suhB8E0bP{fJ3m!9$SvJXVI+4hr6nKw_YJ zhRFcb23qiNiNO4)tPBetF8urdwG0Eps~V7f30Uk4fY_H10QFnJWN@0|aDdPt`!ta3 zYhq=14dR3R%;3=bl-*##Lso?a4=n`dKV@TB@UZnSxc%`MTy`2f0GInB7rY;R=-B%L zmfz=SGBU7+ojI^Wlab+pCL@E479&G~7NYzIwQVvOxS!?-a6eTs@O}hJ)1dqTZkK@C z2@1_mL1{e4K>DeQ0r%4k1L>!ryrckS!`dp03=CYLdKcst7J~(kWem8V8bH$~NDnOB z!E9*y5mB1|R0U+7(EO)CfB(M*$${br@7YS0KAUYIztKq^1ziU!e$bTZ7>Ir`mu2 zzYb+!c$Gjvj|)^!fZ+V6a)1B72HD2~ZAbk41CFbQpfU*LPEeT#l85m@WeTjU1=TAc zHK6uY3WGPe&Bh=&|EcKT|F1#iX^MgOqYMFQa9bnv?|+o`8pv-uKCpuEUvp!}%V;Qcd@TiKB09vfVKiYx~T1CU*s3=FT<;PNB1 z4w+y8sY7}M=0DB&1F5$`_JJ^{4*{|t)`!RunEzA^EgV7l)dQCu(EO?c4Mzik`A_}- z{C~{^ZEt|;zc~trA#Hkw1rK3i2@1muZAOM1ZAJ$EFqZ=@Si+J=0Mbs!6`nIL_&%Dz z!2NWBf%H@Kuw!LlkOGP03b$kihF1Xw-j7_6`U>iQ{=e=q@P1^2B&PJ||7(yrp!5jp zFM!HOP}vC*gS9`Hp=Bqitpd^yDmy{);P5q={4_wp`;i8;EZonGnMMVmdTN{){7d$p%V0d)}8ZTh=(7qj*4b{6tVgAzq0cmhO1*-EH z4Hi6HBQXEzv)>SZfyxX}eT>{51m!zW|1#!4^P`>z-jDbh_PyX|Sn~p%W@ni9f}Nq~ z1xPI@{R9X=!V?r&%!vk$b z1_KY>mpmJKAA$N55}>vYs80dyw}a{&5d&^;I@@6A{ScHl_8E9T6fy9AB%>hxlo48v zPX>jb!GedNa294@ShXC+H<ERD#)8L13=FR>fZ{R$(jGw33vH`xah(6O3#13+27}2@|0zU2WB|2`6c#*OAP8xP z)c^kf`kz7c!(xN~&lnAuAA!sP<#$lp1NF&aa!gP;ZUzP}SbGfYPG~v+v!QObfx6j2 zaQ@SDB=cF2%n$*&odJv6p=SJmmLs6LEDXsE4yYOY3=CWspy?58Hx_qnftqnZVE$9r z-~YkwG*CVP#r+~i2DrZkpk{*n25yhxaaRJ=%mRV=PYsaVrGR9H1p#--K+OP^U-FRn z0k_9^E_gpOfVRg#X^}&RkwHO+k-!Ms68|V zmpM>>RY2peMPUBZ4Zr@s76P^PaOr{C?E)=RSp*>CRN!!gj6FSc5SagT9@MX(dJHr+ z!-HjPhKYlL0aTAo(P3nmp~J|q0h9*9G8%5^Ffs_}GBVicBGLrOt+78oZVBiAP9VG_Mk3ixe zHaPAKAaMs8F9elYpfnGX1GTw)47?vbfYyb9zy7~=1J}dSPr>@E5c)U`7Cc7Rw+5=u z`Pcu~77Prp7C_T3*lcK717?H6iQ!cXH0^@=09wEPzxIdPh3;NhISO(w6Bc`2p!TZ% z`u|!AY6i%?Oi1qK!lF+As!#ma|JOVW46i;w(`TA6|2 z6)3zweFjh+4vIewP}!ug;325K1eFH zfb@Xs8;0P=+zbr6V0t(NW`p}6*`RhWRGyWAVHc?G1i2S9o(D?*AR5$G2GKCRAh*Q) z{{I@x2ep?JnxBH&%^-1DdP>k`WXREFgy)e4T}Fltx{M5abQu{gV96Vxwh5-y^x1k%3_q$ZmeXS1i!<2C5(XenQ(<29uxmD0n{twcS8rW6y}${yPCp7htoH z&4rd#YoKKnsJ~b76W0EF@K}+7;Z=`;^iw9t7&R9wL-1p11_mimdC0&p8Qd-gg#)M_ z0GkbUXAHEwOc9v>GzDay!2?jc=amC4cR=;2K=py@$S|-z&=@_#D^Pj?`3)BDuriTB zkC7oVtf4_ckC7ookC9=99-=%1^@YHFH38|TQyAty)d!6aDlB-|19F2x^V1d(4YoJ< zF(U(m6exZr7#Lnb`hB3jF=!5G@>5X14^;ny$1x8qco+fI2g*kQP&Rn{MWOd8kHLb6 zj0y`Lg4%+ILH0q@JS_cSO9N;07#S)Ym>3cqm>5zVm>4n~m>6;#nBe2@pt>I94^Ur4 zK!N)yhk*3c8I1Fv{sxWhC@gq50hDGy<1hl!PdgarKm7_7Z-9ym2uMGzV4VN-Jyg5^ zDh_VXLH*ATDrZ6BP(G-?;eo`5g^w!(!z)-C)q%=`!bb(hKCs{+s6GdWmBNCD;PxJ< z-D}YM6x0TVg%5+kg2&ZJ;iJRAunX3%1C>!Q{b!)z3$g>8hM{I10E>g?alm1nfn=5> z1H&$G+%iml+5uKK`RQVP$XN7Yg9Q&kW`XBg4A8_C7CcOWst3gB&`hezkpy@}8 zfngUY?}Nexl(s;5;W(08R2Ud`J%PF%RE~klcaWM>NafJC|Nprl;WqgxNDWBd1=I#W zn%jB%|3B9fkU0#KpMu*&Na+C_rXW7F%xHj?88rg)pJxB~{~FYWx%2z~>pP%%QBZ#c zTKB{A4#R?nm;e5M9Re~RI(B~^Jl+h=XABD-o(7GNBFP^IrCGEx^A9w?I5_cx%Y6=g zMg|^zNce)n8WirZFjaugU%=*uK;w>}Ivh~TtDt%2Uhv$Yg2Mc#3Ig+=3WLmHc<`7TW?4PiAPe~U0MtOC0SH1Djy{ZtoR&%@@)n!#c2ghk91ix{XK3-SjG zw2o$knh6s}wg)B#>O+FcaZngBGi-jrz|i!RMWE^F27}8FcNiRcD8V586nUHteJl=C zCi8&ien4?z(DVS5PQdYNko<_rfcXI^k0I+9@X&q9v7z^o$_3v?3Jly&RlsBF(oZE6 z=Y#6lhawCNT%a*u4k(|4fuRe;|Dynr2c<_4{{xgS!N9-;s-r<-51?Y8vKz#|0_96G zFi0sFNIz{hSnyB*I!18@TDE}3krfoApMuIbkUWz?^8=Wg!%#J#`3jgC&=?L#jj|!a ztesGGpgs|}`~=Tcct28sjz35K|NolTV8KHX&=@Miq!$bf%}-ejnjdWtI1HW#Gf|lT z)I?zZQx=8=48_i`{eEO^Z5(DI1IVCoZ2P=8kdG>5+6q49tHhb#T^`Jci1A}U@l|lu0fs{s3YKg6Cil&uVkd|MRn^>Ztr>BR{&f?S(s7jCpAb$j<7M7+Km!zgB6r~pD z=ahohgW0N0u`2)+IVsd=d>3dFb(6i}cDC@v{VP0R)PthgkxB$bN+7SDmD zsih!cNb&(kB{+<*`?s((6`pWFF#?Gb>}u1C5|b2?6AMa8i(qLD8i(#hiAhPRDGHu` z0ihvK-`T2`>L?^tmZTO#QX4c3)JqgV3k4NY^OEyZQj5Uhq!5swlLK-yR8T=RMMpt3 z1>yFz6omkPAD@6APl$Rb-#;`2h3^^^gu?d-4M5@hg*p0w(+n)EoD=iZOBBFSl9F1M znVbssPar4*f*b@91$jC#DKjUtBr~-bk~{E)6D0d$(F(R36b>$6%M?=bQ;R_kC@w7k z=VC;hditZQEl4cMuvINqNK3I*O#wSOH3g;y8Yf^kLD^Qh1v!a%sn9$gmYI^85Ap># z>N1PL*$rxIYKlTqr9ukWMIZxJi>>lYOA1Os>sH}xgmK`s1Thby43xvci4x)obmPG4 zo%8dGOTmdX%*4k?A+-#Yk08+tmPb!-puAL^pPQ-xRt|9+M3;_2QfY}oc}8kcD#+r@ z5__nd!37IQdAw_wt6xZbuzzTfvunJwhohgntCd1=erZv1szP!`VqQ8V;6U*JvIj|z ztADVSLTX-$LVlV8BrX&*GSl<&i&9fGLDn!hJNmi?IWjPK1_$T{2e~_e@-~Pa85$af z%+~es4+rt}Q&P+H%Rqr|0MDNwJChT0Q;QOz8O2ILwSbEOE)S^^R8uq+ic(98G9kr6 zNoH}s0NRT!&cML1goP2zp2EV&0NM)-(g)g~4cf>3@Bjb& z5*9{=|NsBzgT@m+{QsZt!vbPK4JMKT?T?2kbzo&=pazDS2cjF;85k^B7#Kif`=CA4 z?#|9u3K~I)#RW;3iFpbZdd7MNx@Ou%1(}AL5FUf09s>hIVrpViCIh1i0|NuA83P*| z2O}pV7n2Z+Frx^gD5E&D1fwLQ6q5|25)Lne&J_UJ&kQ~-0J7H~!e;_e3=^Pd9x#J< z+A}bK;tV9g%m8vF0|O||Abe0vGBAMR62gb4?2qhq8^k=7#J$_VdjHU6XdJ}h&(7ILDDRQ4@yyxvl1YD(CGsV44^a#;X_ht z1Zd3y*nTmPFnDej#9(FsrBo0Lf|(hRcj$x8F_47D07RaJ;p2Y~21I;-_I-OZApFa~ zz{>!Zhq|AEK>*B;VT1SwCJ*yJ=oABxdYJnd7{nNm_t=B%1DT(Jq<<;{0|SiD1huaP zNgkvg7C)dI3*y7VA9QlUB8XSOB+PspXnexd^FZSVr2ZvXC4_*f??JNf3j+fKj1P0q z8YFp;{X3BO-xwGeV0@VRCrI+3@Q8uN7brYH`axwH#09(zoQw?MG7iLt=@(%~gs(g! z0|RIeG)RPzK^z?ZpfV1mo`FG_0TeYbJ|lxL1IT;{B>iEG3=A+n%zRLp3DGaaki>{c zkD!tbK=Ppds}Lc6uzfN}_JQOdfIJ0OzzdfDfyD2^VjgHO zBv?5E#Jm|?Fh!8`$H9%@gZu~D`w3AGk$-_E{{@K;a?cHZRQ*4Y_#pWyLa6e4koX|^ z9ASj~3>Xt!$9((`a!&)2JV-rgZ85}s{NVW7Ap+9`36C>Ke31GZVyNOt}~(lAXB`xhYbLGp9tQ04a^@j>!aK!?VFE#PI?08@%& zKWIG_Seii$>|f9tD+nKwUP0@wAbd#tZa^{*WPXAo%utB?Dv6Iqt3|S06ymhVqc2}Lj6e? z6KWqQ{6S|dKM$0n`S9)E^MNpre33 z{s%3ngXMn)1|fzop!{wRsaF{pL>T@eD?SI3>z~H0K$PmE@ zQE$M6sQ-nS_!$18na>GsM1p(+QqRC3&CI~CM3|8QlmtNjH)3XB0F`esQ1hIb8Nllh zVg3tbW&p2agYd-}qR`CCVupklOuhz9zL%MS0hE7UK+T`V%)szPgpuI~l)s6Y0lYpA zrhh+Ddq5N1f@qsRt5%8dA|VaUUOCk@Hs6Z(D<@p zWnci+M@OLU^@lWpLE6FLn+TGZV`PBkmwZ+R@Ons)H$Z13RiLTwMzS9iU(;C`7(n$U zEWNK}Wnci6k3XRHZD3_!0M%C-Q2uE&^R}}f+Rs8v+zfZnn9fF zLQmpK&npG>Dl*daE%f#Dpa`3Ea#4w)UOt0L3WG`_H4Mwt%LgCN6knd2m>r*%M-4Oc zKxP(|6ql5yrRgOzq@)(5re_wHq!z`ONO#HWZ}RFqg5pPE-vRLPK5l$e_upHiBeTgea~ zkHm}5%*!l6Q3_q$lFR^M#OGudm*~NH;6*K9Cxg~>=p{3Nj+W9(W+*esG15zBNX*Gh z&x%7Ubyb8t>}o5+BbHk7{WOL%d6*pQEp*GpY>u=qiSI zcOQQzN1u3qH@9HdkoXWsCm&ak5#ZCUl8Z~heg#KRezJoD;&>~_I)M0$)XemZ5}02g z%PQhiKue%XQyJpplZuPsCtyMSn+!fn3#vUYsWhz^d~z1D*-(|Bkclr&Es2M$5r7`h zg{%;=ydoa73Wgy*H6^hm5qwBjJj8T{G|kBKFTkT7MyID-t?Y_#*oKvtnjrxfSMXC&sOgcL8?r@^v)zzIpjJZ?9E<~A&XXDsUdID651bZ|5-N(r$`dn7;tTTgbHLdU zMGCUW26{jo7F(h6pvWsON>75FO^2*9K0d_P8JcI2f(BGT6{j*l78aq{3C&)xgb6<8 z4%tAkPmu)}^olEUOA?b9^omQ0Aan+dm6=zPT2#QGmzQ6Xs^{qBsaukm4&|limFgvx zX6B^mW~MOcfp{5-#Tg8GDV2G}mAMeQqzEEYmReK{IzkVu49bZwV$dr}%>k*0FbZ-? z81%p~s8^C&QNo}HDnJ?Zit@o(R?jiWS1+|9Go^w-FEs;FdP0;#3QmY}7(1mhFEKYW znL#f-zl1>#Vn9-HF@s)mer|4RUI_!(xWtl@qRgbylGI{w%}g8t8n*;>A3#l3&=?u0 z&X+{e3X%hHl^GboBU>PKkaIF2=QV=lKp0flgJ{rsji3=Qkh%$sj0_VP85ltAZjc-Z z!_aZ~|+)+X(1gQb>W+15pjcsvgLfj3K17VoDc~EtrZ~=|E31~7fB!B`1 zqzGy+h`9`+uAm7twg)~J7GfS`^b*u>1gT z2T}(f19f0vhyYm##UOPc{18bUsBanJz<@IH2~q>n@*Fg803J7mw80C|)Pd{-se1!8 z&j7>$g`ooj11Jwdx!^HWP&)hyF{=P3KG}hRK_A9}ngQa1)Plko#0KFb4h#$eJPZt= x{9gx+H&8wTrDd>RL5dg{7_Rs*FuVmNQYar}4u}aF1wdAJFo=Nx-bF{U7XbW4b<+R< 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)