cuticle/hi/webcam.c
2024-06-26 20:47:26 +03:00

138 lines
3.6 KiB
C

#include"node.h"
#include<sys/ioctl.h>
#include<sys/time.h>
#include<sys/mman.h>
#include<stdlib.h>
#include<stdio.h>
#include<fcntl.h>
#include<string.h>
#include<time.h>
#include<linux/videodev2.h>
#include<libv4l2.h>
#include<errno.h>
#include"img.h"
#include<assert.h>
#include<smmintrin.h>
static int camId = -1;
static struct Buf {
size_t length;
uint8_t *ptr;
} bufs[2];
struct v4l2_format fmt;
static void xioctl(int fh, int request, void *arg) {
int r;
do {
r = v4l2_ioctl(fh, request, arg);
} while(r == -1 && ((errno == EINTR) || (errno == EAGAIN)));
if(r == -1) {
fprintf(stderr, "error %d, %s\\n", errno, strerror(errno));
exit(EXIT_FAILURE);
}
}
static int camera_perform(CHiPubNode *pubn) {
pubn->sources[0].type = CUTIHI_VAL_SAMPLE;
fd_set fds;
FD_ZERO(&fds);
FD_SET(camId, &fds);
int r = select(camId + 1, &fds, NULL, NULL, &(struct timeval) {.tv_sec = 0, .tv_usec = 100});
if(r == -1) {
return 1;
}
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
xioctl(camId, VIDIOC_DQBUF, &buf);
CHiImage *ret;
if(pubn->sources[0].data.sample) {
ret = pubn->sources[0].data.sample;
} else {
ret = CHi_Image_New(2, 4, 8 * ((fmt.fmt.pix.width + 15) & ~15), fmt.fmt.pix.width, fmt.fmt.pix.height, NULL);
pubn->sources[0].data.sample = ret;
}
/* 24-to-32 TODO: make faster by processing 48-to-64 bytes in one iteration (like bgra32torgb24 does) */
for(size_t y = 0; y < ret->height; y++) {
for(size_t x = 0; x < ret->width; x += 2) {
__m128i asdf = _mm_loadu_si128((__m128i*) &bufs[buf.index].ptr[(y * ret->width + x) * 3]);
asdf = _mm_shuffle_epi8(asdf, _mm_set_epi8(-128, -128, 5, -128, 4, -128, 3, -128, -128, -128, 2, -128, 1, -128, 0, -128));
asdf = _mm_or_si128(asdf, _mm_set_epi32(0xFFFF0000, 0, 0xFFFF0000, 0));
_mm_stream_si128((__m128i*) ((uintptr_t) ret->data16 + y * ret->stride + x * 8), asdf);
}
}
xioctl(camId, VIDIOC_QBUF, &buf);
return 1;
}
CUTIVIS CHiPubNode *CHi_Camera() {
if(camId == -1) {
camId = v4l2_open("/dev/video0", O_RDWR | O_NONBLOCK, 0);
memset(&fmt, 0, sizeof(fmt));
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.fmt.pix.width = 640;
fmt.fmt.pix.height = 480;
fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24;
fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
xioctl(camId, VIDIOC_S_FMT, &fmt);
assert(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_BGR24);
struct v4l2_requestbuffers req;
memset(&req, 0, sizeof(req));
req.count = 2;
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
req.memory = V4L2_MEMORY_MMAP;
xioctl(camId, VIDIOC_REQBUFS, &req);
for(int i = 0; i < 2; i++) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
xioctl(camId, VIDIOC_QUERYBUF, &buf);
bufs[i].length = buf.length;
bufs[i].ptr = v4l2_mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camId, buf.m.offset);
assert(MAP_FAILED != bufs[i].ptr);
}
for(int i = 0; i < 2; i++) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
xioctl(camId, VIDIOC_QBUF, &buf);
}
xioctl(camId, VIDIOC_STREAMON, &(enum v4l2_buf_type) {V4L2_BUF_TYPE_VIDEO_CAPTURE});
}
CHiPubNode *pubn = malloc(sizeof(*pubn));
pubn->type = CUTIHI_T('CWeb','Cam ');
pubn->clean = 0;
pubn->Start = pubn->Stop = NULL;
pubn->Perform = camera_perform;
pubn->sinks = calloc(sizeof(*pubn->sinks), pubn->sinkCount = 0);
pubn->sources = calloc(sizeof(*pubn->sources), pubn->sourceCount = 1);
pubn->ng = NULL;
return pubn;
}