drv0-v4l2-old.c

It s a tool designed to extract as much information as possible from Bluetooth devices without the r

	fprintf(stderr,"v4l2: WARNING: framebuffer base address mismatch\n");
	fprintf(stderr,"v4l2: me=%p v4l=%p\n",base,h->ov_fb.base);
	h->ov_error = 1;
	return -1;
    }
    if (h->ov_fb.fmt.width  != fmt->width ||
	h->ov_fb.fmt.height != fmt->height) {
	fprintf(stderr,"v4l2: WARNING: framebuffer size mismatch\n");
	fprintf(stderr,"v4l2: me=%dx%d v4l=%dx%d\n",
		fmt->width,fmt->height,h->ov_fb.fmt.width,h->ov_fb.fmt.height);
	h->ov_error = 1;
	return -1;
    }
    if ((h->ov_fb.fmt.flags & V4L2_FMT_FLAG_BYTESPERLINE) &&
	fmt->bytesperline >  0 &&
	fmt->bytesperline != h->ov_fb.fmt.bytesperline) {
	fprintf(stderr,"v4l2: WARNING: framebuffer bpl mismatch\n");
	fprintf(stderr,"v4l2: me=%d v4l=%d\n",
		fmt->bytesperline,h->ov_fb.fmt.bytesperline);
	h->ov_error = 1;
	return -1;
    }
#if 0
    if (h->ov_fb.fmt.pixelformat != xawtv_pixelformat[fmt->fmtid]) {
	fprintf(stderr,"v4l2: WARNING: framebuffer format mismatch\n");
	fprintf(stderr,"v4l2: me=%c%c%c%c [%s]   v4l=%c%c%c%c\n",
		xawtv_pixelformat[fmt->fmtid] & 0xff,
		(xawtv_pixelformat[fmt->fmtid] >>  8) & 0xff,
		(xawtv_pixelformat[fmt->fmtid] >> 16) & 0xff,
		(xawtv_pixelformat[fmt->fmtid] >> 24) & 0xff,
		ng_vfmt_to_desc[fmt->fmtid],
		h->ov_fb.fmt.pixelformat & 0xff,
		(h->ov_fb.fmt.pixelformat >>  8) & 0xff,
		(h->ov_fb.fmt.pixelformat >> 16) & 0xff,
		(h->ov_fb.fmt.pixelformat >> 24) & 0xff);
	h->ov_error = 1;
	return -1;
    }
#endif
    return 0;
}

static int
v4l2_overlay(void *handle, struct ng_video_fmt *fmt, int x, int y,
	     struct OVERLAY_CLIP *oc, int count, int aspect)
{
    struct v4l2_handle *h = handle;
    int rc,i;

    if (h->ov_error)
	return -1;
    
    if (NULL == fmt) {
	if (ng_debug)
	    fprintf(stderr,"v4l2: overlay off\n");
	if (h->ov_enabled) {
	    h->ov_enabled = 0;
	    h->ov_on = 0;
	    xioctl(h->fd, VIDIOC_PREVIEW, &h->ov_on, 0);
	}
	return 0;
    }

    if (ng_debug)
	fprintf(stderr,"v4l2: overlay win=%dx%d+%d+%d, %d clips\n",
		fmt->width,fmt->height,x,y,count);
    h->ov_win.x          = x;
    h->ov_win.y          = y;
    h->ov_win.width      = fmt->width;
    h->ov_win.height     = fmt->height;

    /* check against max. size */
    ioctl(h->fd,VIDIOC_QUERYCAP,&h->cap);
    if (h->ov_win.width > h->cap.maxwidth) {
	h->ov_win.width = h->cap.maxwidth;
	h->ov_win.x += (fmt->width - h->ov_win.width)/2;
    }
    if (h->ov_win.height > h->cap.maxheight) {
	h->ov_win.height = h->cap.maxheight;
	h->ov_win.y +=  (fmt->height - h->ov_win.height)/2;
    }
    if (aspect)
	ng_ratio_fixup(&h->ov_win.width,&h->ov_win.height,
		       &h->ov_win.x,&h->ov_win.y);

    /* fixups */
    ng_check_clipping(h->ov_win.width, h->ov_win.height,
		      x - h->ov_win.x, y - h->ov_win.y,
		      oc, &count);

    if (h->ov_fb.capability & V4L2_FBUF_CAP_CLIPPING) {
	h->ov_win.clips      = h->ov_clips;
	h->ov_win.clipcount  = count;
	
	for (i = 0; i < count; i++) {
	    h->ov_clips[i].next   = (i+1 == count) ? NULL : &h->ov_clips[i+1];
	    h->ov_clips[i].x      = oc[i].x1;
	    h->ov_clips[i].y      = oc[i].y1;
	    h->ov_clips[i].width  = oc[i].x2-oc[i].x1;
	    h->ov_clips[i].height = oc[i].y2-oc[i].y1;
	}
    }
#if 0
    if (h->ov_fb.flags & V4L2_FBUF_FLAG_CHROMAKEY) {
	h->ov_win.chromakey  = 0;    /* FIXME */
    }
#endif
    rc = xioctl(h->fd, VIDIOC_S_WIN, &h->ov_win, 0);

    h->ov_enabled = (0 == rc) ? 1 : 0;
    h->ov_on      = (0 == rc) ? 1 : 0;
    xioctl(h->fd, VIDIOC_PREVIEW, &h->ov_on, 0);

    return 0;
}

/* ---------------------------------------------------------------------- */
/* capture helpers                                                        */

static int
v4l2_queue_buffer(struct v4l2_handle *h)
{
    int frame = h->queue % h->reqbufs.count;
    int rc;

    if (0 != h->buf_me[frame].refcount) {
	if (0 != h->queue - h->waiton)
	    return -1;
	fprintf(stderr,"v4l2: waiting for a free buffer\n");
	ng_waiton_video_buf(h->buf_me+frame);
    }

    rc = xioctl(h->fd,VIDIOC_QBUF,&h->buf_v4l2[frame], 0);
    if (0 == rc)
	h->queue++;
    return rc;
}

static void
v4l2_queue_all(struct v4l2_handle *h)
{
    for (;;) {
	if (h->queue - h->waiton >= h->reqbufs.count)
	    return;
	if (0 != v4l2_queue_buffer(h))
	    return;
    }
}

static int
v4l2_waiton(struct v4l2_handle *h)
{
    struct v4l2_buffer buf;
    struct timeval tv;
    fd_set rdset;
    
    /* wait for the next frame */
 again:
    tv.tv_sec  = 5;
    tv.tv_usec = 0;
    FD_ZERO(&rdset);
    FD_SET(h->fd, &rdset);
    switch (select(h->fd + 1, &rdset, NULL, NULL, &tv)) {
    case -1:
	if (EINTR == errno)
	    goto again;
	perror("v4l2: select");
	return -1;
    case  0:
	fprintf(stderr,"v4l2: oops: select timeout\n");
	return -1;
    }

    /* get it */
    memset(&buf,0,sizeof(buf));
    buf.type = V4L2_BUF_TYPE_CAPTURE;
    if (-1 == xioctl(h->fd,VIDIOC_DQBUF,&buf, 0))
	return -1;
    h->waiton++;
    h->buf_v4l2[buf.index] = buf;
    return buf.index;
}

static int
v4l2_start_streaming(struct v4l2_handle *h, int buffers)
{
    int disable_overlay = 0;
    int i;
    
    /* setup buffers */
    h->reqbufs.count = buffers;
    h->reqbufs.type  = V4L2_BUF_TYPE_CAPTURE;
    if (-1 == xioctl(h->fd, VIDIOC_REQBUFS, &h->reqbufs, 0))
	return -1;
    for (i = 0; i < h->reqbufs.count; i++) {
	h->buf_v4l2[i].index = i;
	h->buf_v4l2[i].type  = V4L2_BUF_TYPE_CAPTURE;
	if (-1 == ioctl(h->fd, VIDIOC_QUERYBUF, &h->buf_v4l2[i]))
	    return -1;
	h->buf_me[i].fmt  = h->fmt_me;
	h->buf_me[i].size = h->buf_me[i].fmt.bytesperline *
	    h->buf_me[i].fmt.height;
	h->buf_me[i].data = mmap(NULL, h->buf_v4l2[i].length,
				 PROT_READ | PROT_WRITE, MAP_SHARED,
				 h->fd, h->buf_v4l2[i].offset);
	if ((void*)-1 == h->buf_me[i].data) {
	    perror("mmap");
	    return -1;
	}
	if (ng_debug)
	    print_bufinfo(&h->buf_v4l2[i]);
    }

    /* queue up all buffers */
    v4l2_queue_all(h);

 try_again:
    /* turn off preview (if needed) */
    if (disable_overlay) {
	h->ov_on = 0;
	xioctl(h->fd, VIDIOC_PREVIEW, &h->ov_on, 0);
	if (ng_debug)
	    fprintf(stderr,"v4l2: overlay off (start_streaming)\n");
    }

    /* start capture */
    if (-1 == xioctl(h->fd,VIDIOC_STREAMON,&h->fmt_v4l2.type,
		     h->ov_on ? EBUSY : 0)) {
	if (h->ov_on && errno == EBUSY) {
	    disable_overlay = 1;
	    goto try_again;
	}
	return -1;
    }
    return 0;
}

static void
v4l2_stop_streaming(struct v4l2_handle *h)
{
    int i;
    
    /* stop capture */
    if (-1 == ioctl(h->fd,VIDIOC_STREAMOFF,&h->fmt_v4l2.type))
	perror("ioctl VIDIOC_STREAMOFF");
    
    /* free buffers */
    for (i = 0; i < h->reqbufs.count; i++) {
	if (0 != h->buf_me[i].refcount)
	    ng_waiton_video_buf(&h->buf_me[i]);
	if (-1 == munmap(h->buf_me[i].data,h->buf_me[i].size))
	    perror("munmap");
    }
    h->queue = 0;
    h->waiton = 0;

    /* turn on preview (if needed) */
    if (h->ov_on != h->ov_enabled) {
	h->ov_on = h->ov_enabled;
	xioctl(h->fd, VIDIOC_PREVIEW, &h->ov_on, 0);
	if (ng_debug)
	    fprintf(stderr,"v4l2: overlay on (stop_streaming)\n");
    }
}

/* ---------------------------------------------------------------------- */
/* capture interface                                                      */

/* set capture parameters */
static int
v4l2_setformat(void *handle, struct ng_video_fmt *fmt)
{
    struct v4l2_handle *h = handle;
    
    h->fmt_v4l2.type = V4L2_BUF_TYPE_CAPTURE;
    h->fmt_v4l2.fmt.pix.pixelformat  = xawtv_pixelformat[fmt->fmtid];
    h->fmt_v4l2.fmt.pix.flags        = V4L2_FMT_FLAG_INTERLACED;
    h->fmt_v4l2.fmt.pix.depth        = ng_vfmt_to_depth[fmt->fmtid];
    h->fmt_v4l2.fmt.pix.width        = fmt->width;
    h->fmt_v4l2.fmt.pix.height       = fmt->height;
    h->fmt_v4l2.fmt.pix.bytesperline = fmt->bytesperline;

    if (-1 == xioctl(h->fd, VIDIOC_S_FMT, &h->fmt_v4l2, EINVAL))
	return -1;
    if (h->fmt_v4l2.fmt.pix.pixelformat != xawtv_pixelformat[fmt->fmtid])
	return -1;
    fmt->width        = h->fmt_v4l2.fmt.pix.width;
    fmt->height       = h->fmt_v4l2.fmt.pix.height;
    fmt->bytesperline = h->fmt_v4l2.fmt.pix.bytesperline;
    if (0 == fmt->bytesperline)
	fmt->bytesperline = fmt->width * ng_vfmt_to_depth[fmt->fmtid] / 8;
    h->fmt_me = *fmt;
    if (ng_debug)
	fprintf(stderr,"v4l2: new capture params (%dx%d, %c%c%c%c, %d byte)\n",
		fmt->width,fmt->height,
		h->fmt_v4l2.fmt.pix.pixelformat & 0xff,
		(h->fmt_v4l2.fmt.pix.pixelformat >>  8) & 0xff,
		(h->fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
		(h->fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff,
		h->fmt_v4l2.fmt.pix.sizeimage);
    return 0;
}

/* start/stop video */
static int
v4l2_startvideo(void *handle, int fps, unsigned int buffers)
{
    struct v4l2_handle *h = handle;

    if (0 != h->fps)
	fprintf(stderr,"v4l2_startvideo: oops: fps!=0\n");
    h->fps = fps;
    h->first = 1;
    h->start = 0;

    if (h->cap.flags & V4L2_FLAG_STREAMING)
	return v4l2_start_streaming(h,buffers);
    return 0;
}

static void
v4l2_stopvideo(void *handle)
{
    struct v4l2_handle *h = handle;

    if (0 == h->fps)
	fprintf(stderr,"v4l2_stopvideo: oops: fps==0\n");
    h->fps = 0;

    if (h->cap.flags & V4L2_FLAG_STREAMING)
	v4l2_stop_streaming(h);
}

/* read images */
static struct ng_video_buf*
v4l2_nextframe(void *handle)
{
    struct v4l2_handle *h = handle;
    struct ng_video_buf *buf = NULL;
    int rc,size,frame = 0;

    if (h->cap.flags & V4L2_FLAG_STREAMING) {
	v4l2_queue_all(h);
	frame = v4l2_waiton(h);
	if (-1 == frame)
	    return NULL;
	h->buf_me[frame].refcount++;
	buf = &h->buf_me[frame];
	memset(&buf->info,0,sizeof(buf->info));
	buf->info.ts = h->buf_v4l2[frame].timestamp;
    } else {
	size = h->fmt_me.bytesperline * h->fmt_me.height;
	buf = ng_malloc_video_buf(&h->fmt_me,size);
	rc = read(h->fd,buf->data,size);
	if (rc != size) {
	    if (-1 == rc) {
		perror("v4l2: read");
	    } else {
		fprintf(stderr, "v4l2: read: rc=%d/size=%d\n",rc,size);
	    }
	    ng_release_video_buf(buf);
	    return NULL;
	}
	memset(&buf->info,0,sizeof(buf->info));
	buf->info.ts = ng_get_timestamp();
    }

    if (h->first) {
	h->first = 0;
	h->start = buf->info.ts;
	if (ng_debug)
	    fprintf(stderr,"v4l2: start ts=%lld\n",h->start);
    }
    buf->info.ts -= h->start;
    return buf;
}

static struct ng_video_buf*
v4l2_getimage(void *handle)
{
    struct v4l2_handle *h = handle;
    struct ng_video_buf *buf; 
    int size,frame,rc;

    size = h->fmt_me.bytesperline * h->fmt_me.height;
    buf = ng_malloc_video_buf(&h->fmt_me,size);
    if (h->cap.flags & V4L2_FLAG_READ) {
	rc = read(h->fd,buf->data,size);
	if (rc != size) {
	    if (-1 == rc) {
		perror("v4l2: read");
	    } else {
		fprintf(stderr, "v4l2: read: rc=%d/size=%d\n",rc,size);
	    }
	    ng_release_video_buf(buf);
	    return NULL;
	}
    } else {
	if (-1 == v4l2_start_streaming(h,1)) {
	    v4l2_stop_streaming(h);
	    return NULL;
	}
	frame = v4l2_waiton(h);
	if (-1 == frame) {
	    v4l2_stop_streaming(h);
	    return NULL;
	}
	memcpy(buf->data,h->buf_me[0].data,size);
	v4l2_stop_streaming(h);
    }
    return buf;
}

/* ---------------------------------------------------------------------- */

extern void ng_plugin_init(void);
void ng_plugin_init(void)
{
    ng_vid_driver_register(NG_PLUGIN_MAGIC,__FILE__,&v4l2_driver);
}