msv4l.c

基于osip、eXosip、speex、ffmpeg的VoIP源代码

	{
		ms_warning("MSV4l: cannot get device capabilities: %s.",strerror(errno));
		return -1;
	}

	ms_message("Found %s device. (maxsize=%ix%i)",cap.name, cap.maxwidth, cap.maxheight);
	for (i=0;i<cap.channels;i++)
	{
		chan.channel=i;
		err=ioctl(s->fd,VIDIOCGCHAN,&chan);
		if (err==0)
		{
			ms_message("Getting video channel %s",chan.name);
			switch(chan.type){
				case VIDEO_TYPE_TV:
					ms_message("Channel is a TV.");
				break;
				case VIDEO_TYPE_CAMERA:
					ms_message("Channel is a camera");
				break;
				default:
					ms_warning("unknown video channel type.");
			}
			found=1;
			break;  /* find the first channel */
		}
	}
	if (found) ms_message("A valid video channel was found.");
	/* select this channel */
	ioctl(s->fd,VIDIOCSCHAN,&chan);
	
	/* get picture properties */
	err=ioctl(s->fd,VIDIOCGPICT,&pict);
	if (err<0){
		ms_warning("Could not get picture properties: %s",strerror(errno));
		return -1;
	}
	ms_message("Default picture properties: brightness=%i,hue=%i,colour=%i,contrast=%i,depth=%i, palette=%i.",
						pict.brightness,pict.hue,pict.colour, pict.contrast,pict.depth, pict.palette);
	
	/* trying color format */
	if (try_format(s->fd,&pict,VIDEO_PALETTE_YUV420P,16)){
		ms_message("Driver supports YUV420P, using that format.");
		s->pix_fmt=MS_YUV420P;
	}else if (try_format(s->fd, &pict,VIDEO_PALETTE_RGB24,24)){
		ms_message("Driver supports RGB24, using that format.");
		s->pix_fmt=MS_RGB24;
	}else if (try_format(s->fd, &pict,VIDEO_PALETTE_YUV422, 16)){
		ms_message("Driver supports YUV422, using that format.");
		s->pix_fmt=MS_YUYV;
	}else if (try_format(s->fd, &pict,VIDEO_PALETTE_UYVY, 16)){
		ms_message("Driver supports UYVY, using that format.");
		s->pix_fmt=MS_UYVY;
	}else{	
	  s->vsize.width=MS_VIDEO_SIZE_CIF_W;
	  s->vsize.height=MS_VIDEO_SIZE_CIF_H;
		s->pix_fmt=MS_YUV420P;
		ms_fatal("Unsupported video pixel format.");
		return -1;
	}

	if (!try_size(s,s->vsize)) {
		if (!try_size(s,MS_VIDEO_SIZE_NS1)){
			if (!try_size(s,MS_VIDEO_SIZE_VGA)){
				if (!try_size(s,MS_VIDEO_SIZE_CIF)) {
					if (!try_size(s,MS_VIDEO_SIZE_QCIF)) {
						if (!try_size(s,MS_VIDEO_SIZE_QVGA)) {
						  if (!try_size(s,MS_VIDEO_SIZE_4CIF)) {
						    if (!try_size(s,MS_VIDEO_SIZE_1024)) {
						      return -1;
						    }
						  }
						}
					}
				}
			}
		}
	}

	/* Try HW frame rate control */
	fps = s->fps;
	if (ioctl(s->fd, VIDIOSFPS, &fps) < 0 )
		ms_message("v4l_configure: cannot set HW frame rate control");
	else
		ms_message("v4l_configure: set HW fps to be : %d", fps);
	
	return 0;
}	


int ms_to_v4l_pix_fmt(MSPixFmt p){
	switch(p){
		case MS_YUV420P:
				return VIDEO_PALETTE_YUV420P;
		case MS_RGB24:
				return VIDEO_PALETTE_RGB24;
		case MS_YUYV:
				return VIDEO_PALETTE_YUV422;
		case MS_UYVY:
				return VIDEO_PALETTE_UYVY;
		default:
			ms_fatal("unsupported pix fmt");
			return -1;
	}
}

static void plane_copy(uint8_t *dest, int dw, int dh, uint8_t *src, int sw, int sh, int bpp){
	int diffw=dw-sw;
	int diffh=dh-sh;
	int dlsize=dw*bpp;
	int slsize=sw*bpp;
	int i;
	int dstartx=(diffw>0) ? diffw/2 : 0;
	int dstarty=(diffh>0) ? diffh/2 : 0;
	int sstartx=(diffw<0) ? diffw/2 : 0;
	int sstarty=(diffh<0) ? diffh/2 : 0;
	uint8_t *tmp1=dest;
	uint8_t *tmp2=src;	

	/* copy orig into dest */
	tmp2+=sstarty*slsize;
	tmp1+=dstarty*dlsize;
	for(i=dstarty;i<dh-dstarty;++i){
		memcpy(tmp1+(dstartx*bpp),tmp2+(sstartx*bpp),(sw-(2*sstartx))*bpp);
		tmp1+=dlsize;
		tmp2+=slsize;
	}
}

static void pic_copy(uint8_t *dest, int dw, int dh, uint8_t *src, int sw, int sh, MSPixFmt fmt){
	if (fmt==MS_YUV420P){
		plane_copy(dest,dw,dh,src,sw,sh,1);
		dest+=dw*dh;
		src+=sw*sh;
		plane_copy(dest,dw/2,dh/2,src,sw/2,sh/2,1);
		dest+=dw*dh/4;
		src+=sw*sh/4;
		plane_copy(dest,dw/2,dh/2,src,sw/2,sh/2,1);
	}else if (fmt==MS_RGB24){
		plane_copy(dest,dw,dh,src,sw,sh,3);
	}else if (fmt==MS_YUYV||fmt==MS_UYVY){
		plane_copy(dest,dw,dh,src,sw,sh,4);
	}
}

static mblk_t *crop_or_pad(V4lState *s, mblk_t *pic){
	int size=s->vsize.width*s->vsize.height;
	mblk_t *newpic;
	if (s->pix_fmt==MS_YUV420P) size=size*3/2;
	else if (s->pix_fmt==MS_YUYV) size=size*2;
	else if (s->pix_fmt==MS_UYVY) size=size*2;
	else if (s->pix_fmt==MS_RGB24) size=size*3;
	else ms_fatal("crop_or_pad: unsupported pixel format.");
	newpic=allocb(size,0);
	memset(newpic->b_wptr,0,size);
	pic_copy(newpic->b_wptr, s->vsize.width, s->vsize.height,
		pic->b_rptr,s->got_vsize.width,s->got_vsize.height,s->pix_fmt);
	newpic->b_wptr+=size;
	return newpic;
}

static mblk_t * v4l_grab_image_mmap(V4lState *s){
	struct video_mmap vmap;
	int err;
	int syncframe;
	int jitter=s->frame_max-1;
	int query_frame;
	mblk_t *ret;
	vmap.width=s->got_vsize.width;
	vmap.height=s->got_vsize.height;
	vmap.format=ms_to_v4l_pix_fmt(s->pix_fmt);
	
	query_frame=(s->frame_ind) % s->frame_max;
	/*ms_message("v4l_mmap_process: query_frame=%i",
			obj->query_frame);*/
	vmap.frame=query_frame;
	err=ioctl(s->fd,VIDIOCMCAPTURE,&vmap);
	if (err<0) {
		ms_warning("v4l_grab_image_mmap: error in VIDIOCMCAPTURE: %s.",strerror(errno));
		usleep(10000);
		return NULL;
	}
	/*g_message("v4l_mmap_process: query_frame=%i done",
			obj->query_frame);*/
	syncframe=(s->frame_ind-jitter);
	s->frame_ind++;
	if (syncframe>=0){
		syncframe=syncframe%s->frame_max;
		/*ms_message("Syncing on frame %i",syncframe);*/
		err=ioctl(s->fd,VIDIOCSYNC,&syncframe);
		if (err<0) {
			ms_warning("v4l_grab_image_mmap: error in VIDIOCSYNC: %s.",strerror(errno));	
			return NULL;
		}
		/*g_message("got frame %i",syncframe);*/
	}else {
		return NULL;
	}
	ret=s->frames[syncframe];
	/* crop or pad picture if obtained size is not what we want */
	if (s->vsize.width!=s->got_vsize.width){
		ret=crop_or_pad(s,ret);
	}
	return ret;
}

static mblk_t * v4l_make_mire(V4lState *s){
	unsigned char *data;
	int i,j,line,pos;
	int patternw=s->vsize.width/6; 
	int patternh=s->vsize.height/6;
	int red,green=0,blue=0;
	if (s->mire==NULL){
		s->mire=allocb(s->vsize.width*s->vsize.height*3,0);
		s->mire->b_wptr=s->mire->b_datap->db_lim;
	}
	data=s->mire->b_rptr;
	for (i=0;i<s->vsize.height;++i){
		line=i*s->vsize.width*3;
		if ( ((i+s->frame_ind)/patternh) & 0x1) red=255;
		else red= 0;
		for (j=0;j<s->vsize.width;++j){
			pos=line+(j*3);
			
			if ( ((j+s->frame_ind)/patternw) & 0x1) blue=255;
			else blue= 0;
			
			data[pos]=red;
			data[pos+1]=green;
			data[pos+2]=blue;
		}
	}
	s->frame_ind++;
	return s->mire;
}

static mblk_t * v4l_make_nowebcam(V4lState *s){
	if (s->mire==NULL && s->frame_ind==0){
		s->mire=ms_load_nowebcam(&s->vsize, -1);
	}
	s->frame_ind++;
	return s->mire;
}

static void v4l_purge(V4lState *s){
	int i;
	int err;
	int jitter=s->frame_max-1;
	for (i=s->frame_ind-jitter;i<s->frame_ind;++i){
		int syncframe=i%s->frame_max;
		ms_message("syncing last frame");
		err=ioctl(s->fd,VIDIOCSYNC,&syncframe);
		if (err<0) {
			ms_warning("v4l_mmap_process: error in VIDIOCSYNC: %s.",strerror(errno));	
		}
	}
}

#ifdef HAVE_LINUX_VIDEODEV2_H
static void v4lv2_purge(V4lState *s){
	struct v4l2_buffer buf;
	memset(&buf,0,sizeof(buf));

	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
	
	for(;s->queued>0;s->queued--){
		if (ioctl(s->fd, VIDIOC_DQBUF, &buf)==-1){
			ms_warning("v4lv2_purge: Could not DQ buffer: %s",strerror(errno));
		}
	}
}
#endif

static void v4l_do_munmap(V4lState *s){
	int i;
#ifdef HAVE_LINUX_VIDEODEV2_H
	enum v4l2_buf_type type;
	if (s->v4lv2){
		v4lv2_purge(s);
		/*stop capture immediately*/
		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		if (-1 ==ioctl (s->fd, VIDIOC_STREAMOFF, &type)){
			ms_error("VIDIOC_STREAMOFF failed: %s",strerror(errno));
		}
	}
#endif
	if (!s->v4lv2){
		v4l_purge(s);
	}
	if (s->mmapdbuf!=NULL){
		if (munmap(s->mmapdbuf,s->msize)<0){
			ms_warning("MSV4l: Fail to unmap: %s",strerror(errno));
		}
		ms_message("munmap() done (%p,%i)",s->mmapdbuf,s->msize);
		s->mmapdbuf=NULL;
	}
	s->msize=0;
	for(i=0;i<s->frame_max;++i){
		if (s->v4lv2){
			mblk_t *msg=s->frames[i];
			int len=msg->b_datap->db_lim-msg->b_datap->db_base;
			if (munmap(msg->b_datap->db_base,len)<0){
				ms_warning("MSV4l: Fail to unmap: %s",strerror(errno));
			}
		}
		freemsg(s->frames[i]);
		s->frames[i]=NULL;
	}
}


static void *v4l_thread(void *ptr){
	V4lState *s=(V4lState*)ptr;
	int err=-1;
	ms_message("v4l_thread starting");
	if (s->v4lv2){
#ifdef HAVE_LINUX_VIDEODEV2_H
		err=v4lv2_do_mmap(s);
#endif
	}else{
		err=v4l_do_mmap(s);
	}
	if (err<0){
		ms_thread_exit(NULL);
	}
	while(s->run){
		mblk_t *m;
#ifdef HAVE_LINUX_VIDEODEV2_H
		if (s->v4lv2)
			m=v4lv2_grab_image(s);
		else
#endif
			m=v4l_grab_image_mmap(s);

		if (s->vsize.width!=s->got_vsize.width){
			if (m){
				/* mblock was allocated by crop or pad! */ 
				ms_mutex_lock(&s->mutex);
				putq(&s->rq,m);
				ms_mutex_unlock(&s->mutex);
			}else{
				ms_error("grabbing failed !");
			}
		} else if (m!=NULL) {
			mblk_t *dm=dupmsg(m);
			ms_mutex_lock(&s->mutex);
			putq(&s->rq,dm);
			ms_mutex_unlock(&s->mutex);
		}
	}
	v4l_do_munmap(s);
	ms_message("v4l_thread exited.");
	ms_thread_exit(NULL);
}


static void v4l_process(MSFilter * obj){
	V4lState *s=(V4lState*)obj->data;
	uint32_t timestamp;
	int cur_frame;
	if (s->frame_count==-1){
		s->start_time=obj->ticker->time;
		s->frame_count=0;
	}
	cur_frame=((obj->ticker->time-s->start_time)*s->fps/1000.0);
	if (cur_frame>=s->frame_count){
		mblk_t *om=NULL;
		ms_mutex_lock(&s->mutex);
		/*keep the most recent frame if several frames have been captured */
		if (s->fd!=-1){
			om=getq(&s->rq);
		}else{
			if (s->usemire){
				om=dupmsg(v4l_make_mire(s));
			}else {
				mblk_t *tmpm=v4l_make_nowebcam(s);
				if (tmpm) {
					om=dupmsg(tmpm);
					mblk_set_precious_flag(om,1);
				}
			}
		}
		ms_mutex_unlock(&s->mutex);
		if (om!=NULL){
			timestamp=obj->ticker->time*90;/* rtp uses a 90000 Hz clockrate for video*/
			mblk_set_timestamp_info(om,timestamp);
			mblk_set_marker_info(om,TRUE);
			ms_queue_put(obj->outputs[0],om);
			/*ms_message("picture sent");*/
			s->frame_count++;
		}
	}else flushq(&s->rq,0);
}

static void v4l_preprocess(MSFilter *f){
	V4lState *s=(V4lState*)f->data;
	if (s->fd==-1){
		s->auto_started=TRUE;
		v4l_start(f,NULL);
		v4l_start_capture(s);
	}
	else
	{
		v4l_start_capture(s);
	}
}

static void v4l_postprocess(MSFilter *f){
	V4lState *s=(V4lState*)f->data;
	if (s->auto_started){
		v4l_stop_capture(s);
		v4l_stop(f,NULL);
	}
	else
	{
		v4l_stop_capture(s);
	}
}

static int v4l_set_fps(MSFilter *f, void *arg){
	V4lState *s=(V4lState*)f->data;
	s->fps=*((float*)arg);
	s->frame_count=-1;
	return 0;
}

static int v4l_get_pix_fmt(MSFilter *f,void *arg){
	V4lState *s=(V4lState*)f->data;
	MSPixFmt res;
	if (s->fd==-1){
		v4l_start(f,NULL);
		res=s->pix_fmt;
		v4l_stop(f,NULL);
	}else res=s->pix_fmt;
	*((MSPixFmt*)arg)=res;
	return 0;
}

static int v4l_set_vsize(MSFilter *f, void *arg){
	V4lState *s=(V4lState*)f->data;
	s->vsize=*((MSVideoSize*)arg);
	return 0;
}

static int v4l_get_vsize(MSFilter *f, void *arg){
	V4lState *s=(V4lState*)f->data;
	*(MSVideoSize*)arg=s->vsize;
	return 0;
}

static MSFilterMethod methods[]={
	{	MS_FILTER_SET_FPS	,	v4l_set_fps	},
	{	MS_FILTER_GET_PIX_FMT	,	v4l_get_pix_fmt	},
	{	MS_FILTER_SET_VIDEO_SIZE, 	v4l_set_vsize	},
	{	MS_V4L_START			,	v4l_start	},
	{	MS_V4L_STOP			,	v4l_stop	},
	{	MS_FILTER_GET_VIDEO_SIZE,	v4l_get_vsize },
	{	0	,	NULL			}
};

static int v4l_set_devfile(MSFilter *f, void *arg){
	V4lState *s=(V4lState*)f->data;
	if (s->dev) ms_free(s->dev);
	s->dev=ms_strdup((char*)arg);
	return 0;
}

MSFilterDesc ms_v4l_desc={
	.id=MS_V4L_ID,
	.name="MSV4l",
	.text="A video4linux compatible source filter to stream pictures.",
	.ninputs=0,
	.noutputs=1,
	.category=MS_FILTER_OTHER,
	.init=v4l_init,
	.preprocess=v4l_preprocess,
	.process=v4l_process,
	.postprocess=v4l_postprocess,
	.uninit=v4l_uninit,
	.methods=methods
};

MS_FILTER_DESC_EXPORT(ms_v4l_desc)

static MSFilter *v4l_create_reader(MSWebCam *obj){
	MSFilter *f=ms_filter_new_from_desc(&ms_v4l_desc);
	V4lState *s=(V4lState*)f->data;
	v4l_set_devfile(f,obj->name);
	s->force_v1=TRUE;
	return f;
}

static void v4l_detect(MSWebCamManager *obj);

static void v4l_cam_init(MSWebCam *cam){
	
}

MSWebCamDesc v4l_desc={
	"V4L",
	&v4l_detect,
	&v4l_cam_init,
	&v4l_create_reader,
	NULL
};

static void v4l_detect(MSWebCamManager *obj){
	struct video_capability cap;
	const char *devname="/dev/video0";
	int fd=open(devname,O_RDWR);
	if (fd!=-1){
		if (ioctl (fd, VIDIOCGCAP, &cap)==0) {
			/* is a V4Lv1 */
			MSWebCam *cam=ms_web_cam_new(&v4l_desc);
			cam->name=ms_strdup(devname);
			ms_web_cam_manager_add_cam(obj,cam);
		}
		close(fd);
	}
	devname="/dev/video1";
	fd=open(devname,O_RDWR);
	if (fd!=-1){
		if (ioctl (fd, VIDIOCGCAP, &cap)==0) {
			/* is a V4Lv1 */
			MSWebCam *cam=ms_web_cam_new(&v4l_desc);
			cam->name=ms_strdup(devname);
			ms_web_cam_manager_add_cam(obj,cam);
		}
		close(fd);
	}
}


#endif