138 lines
3.6 KiB
C
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;
|
|
}
|