00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "webcamVideoV4L.h"
00016
00017 #ifdef HAVE_VIDEO4LINUX
00018
00019 #include <fcntl.h>
00020 #include <sys/mman.h>
00021 #include <sys/ioctl.h>
00022 #include <linux/videodev2.h>
00023
00024 #ifdef HAVE_JPEG
00025 extern "C" {
00026 #include <jpeglib.h>
00027 #include <jerror.h>
00028 }
00029
00030 #include <setjmp.h>
00031 #endif
00032
00033
00034
00035 #ifndef DSTATE_READY
00036 #define DSTATE_READY 202
00037 #endif
00038
00039 TypeHandle WebcamVideoCursorV4L::_type_handle;
00040
00041 #define clamp(x) min(max(x, 0.0), 255.0)
00042
00043 INLINE static void yuv_to_rgb(unsigned char *dest, const unsigned char *src) {
00044 double y1 = (255 / 219.0) * (src[0] - 16);
00045 double pb = (255 / 224.0) * (src[1] - 128);
00046 double pr = (255 / 224.0) * (src[2] - 128);
00047 dest[2] = clamp(1.0 * y1 + 0 * pb + 1.402 * pr);
00048 dest[1] = clamp(1.0 * y1 - 0.344 * pb - 0.714 * pr);
00049 dest[0] = clamp(1.0 * y1 + 1.772 * pb + 0 * pr);
00050 }
00051
00052 INLINE static void yuyv_to_rgbrgb(unsigned char *dest, const unsigned char *src) {
00053 unsigned char yuv[] = {src[0], src[1], src[3]};
00054 yuv_to_rgb(dest, yuv);
00055 yuv[0] = src[2];
00056 yuv_to_rgb(dest + 3, yuv);
00057 }
00058
00059 INLINE static void yuyv_to_rgbargba(unsigned char *dest, const unsigned char *src) {
00060 unsigned char yuv[] = {src[0], src[1], src[3]};
00061 yuv_to_rgb(dest, yuv);
00062 yuv[0] = src[2];
00063 yuv_to_rgb(dest + 4, yuv);
00064 dest[3] = (unsigned char) -1;
00065 dest[7] = (unsigned char) -1;
00066 }
00067
00068 #if defined(HAVE_JPEG) && !defined(CPPPARSER)
00069
00070 struct my_error_mgr {
00071 struct jpeg_error_mgr pub;
00072 jmp_buf setjmp_buffer;
00073 };
00074
00075 typedef struct my_error_mgr *my_error_ptr;
00076
00077 static void my_error_exit (j_common_ptr cinfo) {
00078
00079 char buffer[JMSG_LENGTH_MAX];
00080 (*cinfo->err->format_message) (cinfo, buffer);
00081 vision_cat.error() << buffer << "\n";
00082
00083 my_error_ptr myerr = (my_error_ptr) cinfo->err;
00084
00085 longjmp(myerr->setjmp_buffer, 1);
00086 }
00087
00088 static void my_output_message (j_common_ptr cinfo){
00089 char buffer[JMSG_LENGTH_MAX];
00090 (*cinfo->err->format_message) (cinfo, buffer);
00091 vision_cat.warning() << buffer << "\n";
00092 }
00093
00094 static void my_init_source(j_decompress_ptr cinfo) {
00095 }
00096
00097 static boolean my_fill_input_buffer(j_decompress_ptr cinfo) {
00098 struct jpeg_source_mgr *src = cinfo->src;
00099 static JOCTET FakeEOI[] = {0xFF, JPEG_EOI};
00100
00101 WARNMS(cinfo, JWRN_JPEG_EOF);
00102
00103 src->next_input_byte = FakeEOI;
00104 src->bytes_in_buffer = 2;
00105
00106 return TRUE;
00107 }
00108
00109 static void my_skip_input_data(j_decompress_ptr cinfo, long num_bytes) {
00110 struct jpeg_source_mgr *src = cinfo->src;
00111
00112 if (num_bytes >= (long) src->bytes_in_buffer) {
00113 my_fill_input_buffer(cinfo);
00114 return;
00115 }
00116
00117 src->bytes_in_buffer -= num_bytes;
00118 src->next_input_byte += num_bytes;
00119 }
00120
00121 static void my_term_source(j_decompress_ptr cinfo) {
00122 }
00123
00124 #endif
00125
00126
00127
00128
00129
00130
00131 WebcamVideoCursorV4L::
00132 WebcamVideoCursorV4L(WebcamVideoV4L *src) : MovieVideoCursor(src) {
00133 _size_x = src->_size_x;
00134 _size_y = src->_size_y;
00135 _num_components = 3;
00136 _length = 1.0E10;
00137 _can_seek = false;
00138 _can_seek_fast = false;
00139 _aborted = false;
00140 _streaming = true;
00141 _ready = false;
00142 _format = (struct v4l2_format *) malloc(sizeof(struct v4l2_format));
00143 memset(_format, 0, sizeof(struct v4l2_format));
00144 #ifdef HAVE_JPEG
00145 _cinfo = NULL;
00146 #endif
00147 _buffers = NULL;
00148 _buflens = NULL;
00149 _fd = open(src->_device.c_str(), O_RDWR);
00150 if (-1 == _fd) {
00151 vision_cat.error() << "Failed to open " << src->_device.c_str() << "\n";
00152 return;
00153 }
00154
00155
00156
00157 _format->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00158 pvector<uint32_t>::iterator it;
00159 for (it = src->_pformats.begin(); it != src->_pformats.end(); ++it) {
00160 #ifdef HAVE_JPEG
00161 if (*it == V4L2_PIX_FMT_MJPEG) {
00162 _format->fmt.pix.pixelformat = *it;
00163 break;
00164 } else
00165 #endif
00166 if (*it == V4L2_PIX_FMT_YUYV) {
00167 _format->fmt.pix.pixelformat = *it;
00168 break;
00169 }
00170 }
00171 if (it == src->_pformats.end()) {
00172 vision_cat.error() << "Failed to find a suitable pixel format!\n";
00173 _ready = false;
00174 close(_fd);
00175 _fd = -1;
00176 return;
00177 }
00178
00179
00180 _format->fmt.pix.width = _size_x;
00181 _format->fmt.pix.height = _size_y;
00182 _format->fmt.pix.field = V4L2_FIELD_NONE;
00183
00184
00185 if (-1 == ioctl(_fd, VIDIOC_S_FMT, _format)) {
00186 vision_cat.error() << "Driver rejected format!\n";
00187 _ready = false;
00188 close(_fd);
00189 _fd = -1;
00190 return;
00191 }
00192
00193 _size_x = _format->fmt.pix.width;
00194 _size_y = _format->fmt.pix.height;
00195
00196 struct v4l2_streamparm streamparm;
00197 memset(&streamparm, 0, sizeof streamparm);
00198 streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00199 streamparm.parm.capture.timeperframe.numerator = 1;
00200 streamparm.parm.capture.timeperframe.denominator = src->_fps;
00201 if (ioctl(_fd, VIDIOC_S_PARM, &streamparm) < 0) {
00202 vision_cat.error() << "Driver rejected framerate!\n";
00203 }
00204
00205 struct v4l2_requestbuffers req;
00206 memset(&req, 0, sizeof req);
00207 req.count = 4;
00208 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00209 req.memory = V4L2_MEMORY_MMAP;
00210
00211 if (-1 == ioctl (_fd, VIDIOC_REQBUFS, &req)) {
00212 vision_cat.error() << "Failed to request buffers from webcam!\n";
00213 }
00214
00215 if (req.count < 2) {
00216 vision_cat.error() << "Insufficient buffer memory!\n";
00217 }
00218
00219 _bufcount = req.count;
00220 _buffers = (void* *) calloc (req.count, sizeof (void*));
00221 _buflens = (size_t*) calloc (req.count, sizeof (size_t));
00222
00223 if (!_buffers || !_buflens) {
00224 vision_cat.error() << "Not enough memory!\n";
00225 }
00226
00227
00228 struct v4l2_buffer buf;
00229 for (int i = 0; i < _bufcount; ++i) {
00230 memset(&buf, 0, sizeof buf);
00231 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00232 buf.memory = V4L2_MEMORY_MMAP;
00233 buf.index = i;
00234
00235 if (-1 == ioctl(_fd, VIDIOC_QUERYBUF, &buf)) {
00236 vision_cat.error() << "Failed to query buffer!\n";
00237 }
00238
00239 _buflens[i] = buf.length;
00240 _buffers[i] = mmap (NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, buf.m.offset);
00241
00242 if (_buffers[i] == MAP_FAILED) {
00243 vision_cat.error() << "Failed to map buffer!\n";
00244 }
00245
00246 if (-1 == ioctl(_fd, VIDIOC_QBUF, &buf)) {
00247 vision_cat.error() << "Failed to exchange buffer with driver!\n";
00248 }
00249 }
00250
00251 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00252 if (-1 == ioctl(_fd, VIDIOC_STREAMON, &type)) {
00253 vision_cat.error() << "Failed to stream from buffer!\n";
00254 }
00255
00256 #ifdef HAVE_JPEG
00257
00258 if (_format->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
00259 _cinfo = (struct jpeg_decompress_struct *) malloc(sizeof(struct jpeg_decompress_struct));
00260 jpeg_create_decompress(_cinfo);
00261
00262 _cinfo->src = (struct jpeg_source_mgr *)
00263 (*_cinfo->mem->alloc_small) ((j_common_ptr) _cinfo, JPOOL_PERMANENT,
00264 sizeof(struct jpeg_source_mgr));
00265
00266 _cinfo->src->init_source = my_init_source;
00267 _cinfo->src->fill_input_buffer = my_fill_input_buffer;
00268 _cinfo->src->skip_input_data = my_skip_input_data;
00269 _cinfo->src->resync_to_restart = jpeg_resync_to_restart;
00270 _cinfo->src->term_source = my_term_source;
00271 }
00272 #endif
00273 _ready = true;
00274 }
00275
00276
00277
00278
00279
00280
00281 WebcamVideoCursorV4L::
00282 ~WebcamVideoCursorV4L() {
00283 #ifdef HAVE_JPEG
00284 if (_cinfo != NULL) {
00285 jpeg_destroy_decompress(_cinfo);
00286 free(_cinfo);
00287 }
00288 #endif
00289 if (_format != NULL) {
00290 free(_format);
00291 }
00292 if (-1 != _fd) {
00293 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00294 ioctl(_fd, VIDIOC_STREAMOFF, &type);
00295 close(_fd);
00296 }
00297 if (_buffers) {
00298 for (int i = 0; i < _bufcount; ++i) {
00299 munmap(_buffers[i], _buflens[i]);
00300 }
00301 free(_buffers);
00302 }
00303 if (_buflens) {
00304 free(_buflens);
00305 }
00306 }
00307
00308
00309
00310
00311
00312
00313 PT(MovieVideoCursor::Buffer) WebcamVideoCursorV4L::
00314 fetch_buffer() {
00315 if (!_ready) {
00316 return NULL;
00317 }
00318
00319 PT(Buffer) buffer = get_standard_buffer();
00320 unsigned char *block = buffer->_block;
00321 struct v4l2_buffer vbuf;
00322 memset(&vbuf, 0, sizeof vbuf);
00323 vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
00324 vbuf.memory = V4L2_MEMORY_MMAP;
00325 if (-1 == ioctl(_fd, VIDIOC_DQBUF, &vbuf) && errno != EIO) {
00326 vision_cat.error() << "Failed to dequeue buffer!\n";
00327 return NULL;
00328 }
00329 nassertr(vbuf.index < _bufcount, NULL);
00330 size_t bufsize = _buflens[vbuf.index];
00331 size_t old_bpl = _format->fmt.pix.bytesperline;
00332 size_t new_bpl = _size_x * 3;
00333 unsigned char *buf = (unsigned char *) _buffers[vbuf.index];
00334
00335 if (_format->fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
00336 #ifdef HAVE_JPEG
00337 struct my_error_mgr jerr;
00338 _cinfo->err = jpeg_std_error(&jerr.pub);
00339 jerr.pub.error_exit = my_error_exit;
00340 jerr.pub.output_message = my_output_message;
00341
00342 unsigned char *newbuf = (unsigned char*) malloc(new_bpl * _size_y);
00343
00344
00345 if (setjmp(jerr.setjmp_buffer)) {
00346 if (_cinfo->global_state > DSTATE_READY) {
00347 jpeg_abort_decompress(_cinfo);
00348 }
00349 } else {
00350
00351 _cinfo->src->bytes_in_buffer = bufsize;
00352 _cinfo->src->next_input_byte = buf;
00353
00354 if (jpeg_read_header(_cinfo, TRUE) == JPEG_HEADER_OK) {
00355 _cinfo->scale_num = 1;
00356 _cinfo->scale_denom = 1;
00357 _cinfo->out_color_space = JCS_RGB;
00358
00359 if (jpeg_start_decompress(_cinfo) && _cinfo->output_components == 3
00360 && _size_x == _cinfo->output_width && _size_y == _cinfo->output_height) {
00361
00362 JSAMPLE *buffer_end = newbuf + new_bpl * _cinfo->output_height;
00363 JSAMPLE *rowptr = newbuf;
00364 while (_cinfo->output_scanline < _cinfo->output_height) {
00365 nassertd(rowptr + new_bpl <= buffer_end) break;
00366 jpeg_read_scanlines(_cinfo, &rowptr, 1);
00367 rowptr += new_bpl;
00368 }
00369
00370 if (_cinfo->output_scanline < _cinfo->output_height) {
00371 jpeg_abort_decompress(_cinfo);
00372 } else {
00373 jpeg_finish_decompress(_cinfo);
00374 }
00375 }
00376 }
00377 }
00378
00379
00380 for (size_t row = 0; row < _size_y; ++row) {
00381 memcpy(block + (_size_y - row - 1) * new_bpl, newbuf + row * new_bpl, new_bpl);
00382 }
00383 free(newbuf);
00384
00385
00386 unsigned char ex;
00387 for (size_t i = 0; i < new_bpl * _size_y; i += 3) {
00388 ex = block[i];
00389 block[i] = block[i + 2];
00390 block[i + 2] = ex;
00391 }
00392 #else
00393 nassertr(false, NULL);
00394 #endif
00395 } else {
00396 for (size_t row = 0; row < _size_y; ++row) {
00397 size_t c = 0;
00398 for (size_t i = 0; i < old_bpl; i += 4) {
00399 yuyv_to_rgbrgb(block + (_size_y - row - 1) * new_bpl + c, buf + row * old_bpl + i);
00400 c += 6;
00401 }
00402 }
00403 }
00404
00405 if (-1 == ioctl(_fd, VIDIOC_QBUF, &vbuf)) {
00406 vision_cat.error() << "Failed to exchange buffer with driver!\n";
00407 }
00408
00409 return buffer;
00410 }
00411
00412 #endif