#include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "display-X.h" #include "ycbcr-rgb-cvt.h" extern const char *__progname; #define EXPECTED_X 640 #define EXPECTED_Y 480 #define EXPECTED_SIZE (EXPECTED_X * EXPECTED_Y * 2) typedef enum { ES_IDLE, /* doing nothing */ ES_ENCODING, /* image passed to it, not done yet */ ES_RETURNING, /* image encoded, returning encoded form */ ES_RETENC, /* ES_RETURNING with an image in the pipe */ } ENCSTATE; typedef struct pkt PKT; struct pkt { unsigned char *b; int a; int l; } ; static const char *videodev = "/dev/video0"; static int echovideo = 0; static int thin = 1; static unsigned int adjustrate = 3; static char *arg1; static char *arg2; static int vfd; static int vidthin; static int nfbufs; static unsigned char **fbufmaps; static unsigned char *dbufs; static unsigned int dbuffree; static int encpipe; static int disppipe; static int net; static unsigned char encfno; static ENCSTATE encstate; static PKT pkt1; static PKT pkt2; static PKT *fpkt1; static PKT *fpkt2; static PKT *oqcur; static int oqcp; static PKT *oqpend; static int framecount; static int framecount_enc; static int framecount_busy; static int framecount_full; static int curquality; static int sockbufsize; static int packetcount; static int totalbytes; static unsigned int lastadjust; static unsigned char planar_y[EXPECTED_X*EXPECTED_Y]; static unsigned char planar_cb[(EXPECTED_X*EXPECTED_Y)/2]; static unsigned char planar_cr[(EXPECTED_X*EXPECTED_Y)/2]; #define MAXPOLL 8 static void open_video(void) { vfd = open(videodev,O_RDWR,0); if (vfd < 0) { fprintf(stderr,"%s: %s: %s\n",__progname,videodev,strerror(errno)); exit(1); } } static void queue_buffer(int inx) { struct v4l2_buffer b; b.index = inx; b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; b.memory = V4L2_MEMORY_MMAP; if (ioctl(vfd,VIDIOC_QBUF,&b) < 0) { fprintf(stderr,"%s: VIDIOC_QBUF: %s\n",__progname,strerror(errno)); exit(1); } } static void setup_video(void) { struct v4l2_requestbuffers rb; struct v4l2_buffer b; __typeof__(b.m.offset) *offsets; int i; void *mmrv; rb.count = 8; rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; rb.memory = V4L2_MEMORY_MMAP; if (ioctl(vfd,VIDIOC_REQBUFS,&rb) < 0) { fprintf(stderr,"%s: VIDIOC_REQBUFS: %s\n",__progname,strerror(errno)); exit(1); } nfbufs = rb.count; #ifdef DEBUG fprintf(stderr,"buffer count %d\n",nfbufs); #endif offsets = malloc(nfbufs*sizeof(*offsets)); for (i=nfbufs-1;i>=0;i--) { b.index = i; b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; b.memory = V4L2_MEMORY_MMAP; if (ioctl(vfd,VIDIOC_QUERYBUF,&b) < 0) { fprintf(stderr,"%s: VIDIOC_QUERYBUF: %s\n",__progname,strerror(errno)); exit(1); } if (b.length != EXPECTED_SIZE) { fprintf(stderr,"%s: VIDIOC_QUERYBUF: buffer %d length %ld, expected %d\n",__progname,i,(long int)b.length,EXPECTED_SIZE); exit(1); } offsets[i] = b.m.offset; } #ifdef DEBUG printf("queried\n"); for (i=nfbufs-1;i>=0;i--) { printf("%lu @ %lu\n",(unsigned long int)EXPECTED_SIZE,(unsigned long int)offsets[i]); } #endif fbufmaps = malloc(nfbufs*sizeof(*fbufmaps)); for (i=nfbufs-1;i>=0;i--) { mmrv = mmap(0,EXPECTED_SIZE,PROT_READ|PROT_WRITE,MAP_FILE|MAP_SHARED,vfd,offsets[i]); if (mmrv == MAP_FAILED) { fprintf(stderr,"%s: mmap buffer %d: %s\n",__progname,i,strerror(errno)); exit(1); } fbufmaps[i] = mmrv; } free(offsets); #ifdef DEBUG printf("mapped\n"); for (i=nfbufs-1;i>=0;i--) { printf("%lu @ %p\n",(unsigned long int)buflens[i],(void *)bufmaps[i]); } #endif vidthin = 0; } static void start_video(void) { int i; for (i=nfbufs-1;i>=0;i--) queue_buffer(i); i = V4L2_BUF_TYPE_VIDEO_CAPTURE; // XXX undocumented! if (ioctl(vfd,VIDIOC_STREAMON,&i) < 0) { fprintf(stderr,"%s: VIDIOC_STREAMON: %s\n",__progname,strerror(errno)); exit(1); } #ifdef DEBUG printf("started\n"); #endif } #define wrt(fd,buf,len) wrt_(#fd "," #buf "," #len,(fd),(buf),len) static void wrt_(const char *s, int fd, const void *buf, int len) { int n; n = write(fd,buf,len); if (n < 0) { fprintf(stderr,"write(%s): %s\n",s,strerror(errno)); exit(1); } if (n != len) { fprintf(stderr,"write(%s): wrote %d not %d\n",s,n,len); exit(1); } } static int get_frame(void) { struct v4l2_buffer b; int n; b.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; // XXX undocumented! b.memory = V4L2_MEMORY_MMAP; // XXX undocumented! n = ioctl(vfd,VIDIOC_DQBUF,&b); if (n < 0) { fprintf(stderr,"%s: VIDIOC_DQBUF: %s\n",__progname,strerror(errno)); exit(1); } return(b.index); } static void rcvn(int fd, void *buf, int l) { int r; r = recv(fd,buf,l,MSG_WAITALL); if (r < 0) { fprintf(stderr,"%s: recv: %s\n",__progname,strerror(errno)); exit(1); } if (r < l) { fprintf(stderr,"%s: recv wanted %d, got %d\n",__progname,l,r); exit(1); } if (r > l) { fprintf(stderr,"%s: impossible recv wanted %d, got %d\n",__progname,l,r); exit(1); } } static void encoder_main(void) { th_info thi; th_enc_ctx *thec; th_comment comm; ogg_packet pkt; th_ycbcr_buffer ibuf; int e; unsigned char msg[2]; unsigned char *ip; unsigned char *op_y; unsigned char *op_cb; unsigned char *op_cr; int y; int x; static void return_pkt(void) { char *h; int hl; char key; unsigned int pl; struct iovec iov[4]; hl = asprintf(&h,"%ld %ld %ld %lld %lld\n", (long int)pkt.bytes, (long int)pkt.b_o_s, (long int)pkt.e_o_s, (long long int)pkt.granulepos, (long long int)pkt.packetno ); key = 'p'; pl = hl + pkt.bytes; iov[0].iov_base = &key; iov[0].iov_len = 1; iov[1].iov_base = &pl; iov[1].iov_len = sizeof(pl); iov[2].iov_base = h; iov[2].iov_len = hl; iov[3].iov_base = pkt.packet; iov[3].iov_len = pkt.bytes; writev(encpipe,&iov[0],4); free(h); } static void done_pkt(void) { write(encpipe,"e",1); } thi.version_major = 1; // XXX undocumented thi.version_minor = 1; // XXX undocumented thi.version_subminor = 1; // XXX undocumented thi.frame_width = 640; thi.frame_height = 480; thi.pic_width = 640; thi.pic_height = 480; thi.pic_x = 0; thi.pic_y = 0; /* * The documentation says of these "If either is 0, the frame rate is * undefined.". What it doesn't say is that the encoder will happily * accept such settings, encode them in the resulting stream, and the * decoder will then barf with TH_EBADHEADER. */ thi.fps_numerator = 10; thi.fps_denominator = 1; thi.aspect_numerator = 1; thi.aspect_denominator = 1; thi.colorspace = TH_CS_ITU_REC_470M; thi.pixel_fmt = TH_PF_422; thi.target_bitrate = 0; thi.quality = curquality; thi.keyframe_granule_shift = 6; thec = th_encode_alloc(&thi); // th_encode_ctl(...) none needed comm.user_comments = 0; comm.comment_lengths = 0; comm.comments = 0; comm.vendor = 0; while (1) { e = th_encode_flushheader(thec,&comm,&pkt); if (e == 0) break; if (e > 0) { return_pkt(); } else { fprintf(stderr,"th_encode_flushheader error %d\n",e); exit(1); } } done_pkt(); while (1) { rcvn(encpipe,&msg[0],2); switch (msg[0]) { case 'q': // XXX data size/format for th_encode_ctl commands is undocumented! { int v; v = msg[1]; th_encode_ctl(thec,TH_ENCCTL_SET_QUALITY,&v,sizeof(v)); } break; case 'f': ip = fbufmaps[msg[1]]; op_y = &planar_y[0]; op_cb = &planar_cb[0]; op_cr = &planar_cr[0]; for (y=EXPECTED_Y;y>0;y--) { for (x=EXPECTED_X;x>0;x-=2) { *op_y++ = ip[0]; *op_y++ = ip[2]; *op_cb++ = ip[1]; *op_cr++ = ip[3]; ip += 4; } } ibuf[0].width = EXPECTED_X; ibuf[0].height = EXPECTED_Y; ibuf[0].stride = EXPECTED_X; ibuf[0].data = &planar_y[0]; ibuf[1].width = EXPECTED_X / 2; ibuf[1].height = EXPECTED_Y; ibuf[1].stride = EXPECTED_X / 2; ibuf[1].data = &planar_cb[0]; ibuf[2].width = EXPECTED_X / 2; ibuf[2].height = EXPECTED_Y; ibuf[2].stride = EXPECTED_X / 2; ibuf[2].data = &planar_cr[0]; /* XXX th_ycbcr_buffer is an array (!), hence no & */ e = th_encode_ycbcr_in(thec,ibuf); if (e != 0) { fprintf(stderr,"th_encode_ycbcr_in error %d\n",e); exit(1); } write(encpipe,"d",1); while (1) { e = th_encode_packetout(thec,0,&pkt); if (e == 0) break; if (e > 0) { return_pkt(); } else { fprintf(stderr,"th_encode_packetout error %d\n",e); exit(1); } } done_pkt(); break; default: fprintf(stderr,"%s: encoder: bad key %02x\n",__progname,msg[0]); exit(1); break; } } } static void fork_encoder(void) { int p[2]; pid_t kid; if (socketpair(AF_LOCAL,SOCK_STREAM,0,&p[0]) < 0) { fprintf(stderr,"%s: socketpair(AF_LOCAL/SOCK_STREAM): %s\n",__progname,strerror(errno)); exit(1); } fflush(0); kid = fork(); if (kid == 0) { close(p[0]); encpipe = p[1]; encoder_main(); _exit(0); } close(p[1]); encpipe = p[0]; } static void net_accept(const char *portstr) { struct addrinfo *ai0; struct addrinfo *ai; struct addrinfo hints; int e; int n; int i; int nfds; hints.ai_flags = AI_PASSIVE; hints.ai_family = PF_UNSPEC; hints.ai_socktype = SOCK_STREAM; hints.ai_protocol = 0; hints.ai_addrlen = 0; hints.ai_canonname = 0; hints.ai_addr = 0; hints.ai_next = 0; e = getaddrinfo(0,portstr,&hints,&ai0); if (e) { fprintf(stderr,"%s: %s: %s\n",__progname,portstr,gai_strerror(e)); exit(1); } if (! ai0) { fprintf(stderr,"%s: %s: success but no addresses\n",__progname,portstr); exit(1); } n = 0; for (ai=ai0;ai;ai=ai->ai_next) n ++; { int fds[n]; struct pollfd pfds[n]; int fd; nfds = 0; for (ai=ai0;ai;ai=ai->ai_next) { fd = socket(ai->ai_family,ai->ai_socktype,ai->ai_protocol); if (fd < 0) { fprintf(stderr,"%s: socket (af %d): %s\n",__progname,ai->ai_family,strerror(errno)); continue; } if (bind(fd,ai->ai_addr,ai->ai_addrlen) < 0) { fprintf(stderr,"%s: bind (af %d): %s\n",__progname,ai->ai_family,strerror(errno)); close(fd); continue; } fds[nfds++] = fd; } if (nfds < 1) { fprintf(stderr,"%s: can't establish any listening sockets\n",__progname); exit(1); } for (i=nfds-1;i>=0;i--) { if (listen(fd,1) < 0) { fprintf(stderr,"%s: listen: %s\n",__progname,strerror(errno)); exit(1); } } while (1) { for (i=nfds-1;i>=0;i--) { pfds[i].fd = fds[i]; pfds[i].events = POLLIN | POLLRDNORM; } e = poll(&pfds[0],nfds,INFTIM); if (e < 0) { if (errno == EINTR) continue; { fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno)); exit(1); } } for (i=nfds-1;i>=0;i--) { if (pfds[i].revents & (POLLIN|POLLRDNORM|POLLERR|POLLHUP|POLLNVAL)) { struct sockaddr_storage ss; socklen_t sslen; sslen = sizeof(ss); fd = accept(fds[i],(struct sockaddr *)&ss,&sslen); if (fd >= 0) { for (i=nfds-1;i>=0;i--) close(fds[i]); freeaddrinfo(ai0); net = fd; return; } fprintf(stderr,"%s: accept: %s\n",__progname,strerror(errno)); } } } } } static void net_connect(const char *hoststr, const char *portstr) { struct addrinfo *ai0; struct addrinfo *ai; struct addrinfo hints; int e; hints.ai_flags = 0; hints.ai_family = PF_UNSPEC; hints.ai_socktype = SOCK_STREAM; hints.ai_protocol = 0; hints.ai_addrlen = 0; hints.ai_canonname = 0; hints.ai_addr = 0; hints.ai_next = 0; e = getaddrinfo(hoststr,portstr,&hints,&ai0); if (e) { fprintf(stderr,"%s: %s: %s\n",__progname,portstr,gai_strerror(e)); exit(1); } if (! ai0) { fprintf(stderr,"%s: %s: success but no addresses\n",__progname,portstr); exit(1); } for (ai=ai0;ai;ai=ai->ai_next) { int fd; fd = socket(ai->ai_family,ai->ai_socktype,ai->ai_protocol); if (fd < 0) { fprintf(stderr,"%s: socket (af %d): %s\n",__progname,ai->ai_family,strerror(errno)); continue; } if (connect(fd,ai->ai_addr,ai->ai_addrlen) < 0) { fprintf(stderr,"%s: connect (af %d): %s\n",__progname,ai->ai_family,strerror(errno)); close(fd); continue; } freeaddrinfo(ai0); net = fd; return; } fprintf(stderr,"%s: can't establish network connection\n",__progname); exit(1); } static void tell_encoder(char key, unsigned char data) { unsigned char msg[2]; msg[0] = key; msg[1] = data; wrt(encpipe,&msg[0],2); } static void fetch_frame(void) { unsigned char f; unsigned char df; f = get_frame(); if (vidthin) { queue_buffer(f); vidthin --; return; } else { vidthin = thin - 1; } if (echovideo && dbuffree) { if (dbuffree & 1) { df = 0; dbuffree &= ~1; } else if (dbuffree & 2) { df = 1; dbuffree &= ~2; } else { abort(); } dbuffree &= ~(1 << df); bcopy(fbufmaps[f],dbufs+(df*EXPECTED_SIZE),EXPECTED_SIZE); wrt(disppipe,&df,1); } framecount ++; switch (encstate) { case ES_IDLE: encstate = ES_ENCODING; if (0) { case ES_RETURNING: encstate = ES_RETENC; } tell_encoder('f',f); encfno = f; framecount_enc ++; break; case ES_ENCODING: framecount_busy ++; queue_buffer(f); break; case ES_RETENC: framecount_full ++; queue_buffer(f); break; default: abort(); break; } } static void enc_read(void) { unsigned char k; unsigned int l; PKT *p; rcvn(encpipe,&k,1); switch (k) { case 'd': switch (encstate) { case ES_ENCODING: encstate = ES_RETURNING; queue_buffer(encfno); return; break; default: abort(); break; } break; case 'p': switch (encstate) { case ES_RETURNING: case ES_RETENC: if (fpkt2) { p = fpkt2; fpkt2 = 0; } else { p = fpkt1; fpkt1 = 0; } if (! p) abort(); rcvn(encpipe,&l,sizeof(l)); if (l > p->a) { free(p->b); p->a = l; p->b = malloc(l); } rcvn(encpipe,p->b,l); p->l = l; if (oqcur) { oqpend = p; } else { oqcur = p; oqcp = 0; } packetcount ++; totalbytes += l; break; default: abort(); break; } break; case 'e': switch (encstate) { case ES_RETURNING: encstate = ES_IDLE; break; case ES_RETENC: encstate = ES_ENCODING; break; default: abort(); break; } break; default: fprintf(stderr,"%s: bad encoding pipe key %02x\n",__progname,k); exit(1); break; } } static void write_net(void) { int n; if (! oqcur) abort(); if ((oqcp < 0) || (oqcp > oqcur->l)) abort(); n = write(net,oqcur->b+oqcp,oqcur->l-oqcp); if (n < 0) { switch (errno) { case EINTR: case EWOULDBLOCK: return; break; } fprintf(stderr,"%s: net write: %s\n",__progname,strerror(errno)); exit(1); } if (n == 0) { fprintf(stderr,"%s: net zero write\n",__progname); exit(1); } oqcp += n; if (oqcp >= oqcur->l) { if (fpkt1) fpkt2 = oqcur; else fpkt1 = oqcur; oqcp = 0; oqcur = oqpend; oqpend = 0; } } static void get_dispframe(void) { unsigned char fno; rcvn(disppipe,&fno,1); dbuffree |= 1 << fno; } static void stats_clear(void) { framecount = 0; framecount_enc = 0; framecount_busy = 0; framecount_full = 0; packetcount = 0; totalbytes = 0; } static void maybe_adjust(unsigned int sec) { double v; if ((sec / adjustrate) == (lastadjust / adjustrate)) return; if (framecount != framecount_enc+framecount_busy+framecount_full) abort(); printf("[%d+%d+%d",framecount_enc,framecount_busy,framecount_full); v = framecount_busy / (double)framecount; if (v < .1) { if (curquality < 63) { curquality ++; tell_encoder('q',curquality); printf("->+%d",curquality); } } else if (v > .4) { if (curquality > 0) { curquality --; tell_encoder('q',curquality); printf("->-%d",curquality); } } printf("]"); v = (totalbytes * 5.0) / packetcount; if ((sockbufsize < v*.4) || (sockbufsize > v*2)) { int sov; sov = v; printf("[b%d]",sov); setsockopt(net,SOL_SOCKET,SO_SNDBUF,&sov,sizeof(sov)); sockbufsize = sov; sov = v / 10; if (sov < 1) sov = 1; setsockopt(net,SOL_SOCKET,SO_SNDLOWAT,&sov,sizeof(sov)); } stats_clear(); lastadjust = sec; } static void do_something(void) { struct pollfd pfds[MAXPOLL]; short int pev[MAXPOLL]; void (*handler[MAXPOLL])(void); int npfd; int rv; int i; struct timeval now; void register_poll(int fd, short ev, void (*fn)(void)) { if (npfd >= MAXPOLL) abort(); pfds[npfd].fd = fd; pfds[npfd].events = ev; pev[npfd] = ev; handler[npfd] = fn; npfd ++; } gettimeofday(&now,0); maybe_adjust(now.tv_sec); fflush(0); npfd = 0; switch (encstate) { case ES_IDLE: break; case ES_RETURNING: case ES_RETENC: if (! fpkt1) break; /* fall through */ case ES_ENCODING: register_poll(encpipe,POLLIN|POLLRDNORM,&enc_read); break; default: abort(); break; } if (oqcur) register_poll(net,POLLOUT|POLLWRNORM,&write_net); register_poll(vfd,POLLIN|POLLRDNORM,&fetch_frame); if (echovideo) register_poll(disppipe,POLLIN|POLLRDNORM,&get_dispframe); rv = poll(&pfds[0],npfd,INFTIM); if (rv < 0) { if (errno == EINTR) return; fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno)); exit(1); } for (i=npfd-1;i>=0;i--) { if (pfds[i].revents & (pev[i]|POLLHUP|POLLERR|POLLNVAL)) { (*handler[i])(); return; } } } static void pkt_init(PKT *p) { p->b = 0; p->a = 0; p->l = 0; } static void setup_net(void) { int v; fcntl(net,F_SETFL,fcntl(net,F_GETFL,0)|O_NONBLOCK); v = 1; if (setsockopt(net,IPPROTO_TCP,TCP_NODELAY,&v,sizeof(v)) < 0) { fprintf(stderr,"%s: warning: setsockopt TCP_NODELAY: %s\n",__progname,strerror(errno)); exit(1); } sockbufsize = 0; } static void handleargs(int ac, char **av) { int skip; int errs; skip = 0; errs = 0; for (ac--,av++;ac;ac--,av++) { if (skip > 0) { skip --; continue; } if (**av != '-') { if (! arg1) { arg1 = *av; } else if (! arg2) { arg2 = *av; } else { fprintf(stderr,"%s: extra argument `%s'\n",__progname,*av); errs = 1; } continue; } if (0) { needarg:; fprintf(stderr,"%s: %s needs a following argument\n",__progname,*av); errs = 1; continue; } #define WANTARG() do { if (++skip >= ac) goto needarg; } while (0) if (!strcmp(*av,"-camera")) { WANTARG(); videodev = av[skip]; continue; } if (!strcmp(*av,"-thin")) { WANTARG(); thin = atoi(av[skip]); if (thin < 1) { fprintf(stderr,"%s: -thin value must be >= 1\n",__progname); errs = 1; } continue; } if (!strcmp(*av,"-adjust")) { WANTARG(); adjustrate = strtoul(av[skip],0,0); if (adjustrate < 1) { fprintf(stderr,"%s: -adjust value must be >= 1\n",__progname); errs = 1; } continue; } if (!strcmp(*av,"-echo")) { echovideo = 1; continue; } #undef WANTARG fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av); errs = 1; } if (errs) exit(1); } static void setup_dbufs(void) { void *mmrv; mmrv = mmap(0,EXPECTED_SIZE*2,PROT_READ|PROT_WRITE,MAP_ANON|MAP_SHARED,-1,0); if (mmrv == MAP_FAILED) { fprintf(stderr,"%s: buffer mmap (%u): %s\n",__progname,(unsigned int)(EXPECTED_SIZE*2),strerror(errno)); exit(1); } dbufs = mmrv; bzero(dbufs,EXPECTED_SIZE*2); } static void displayer_main(void) { unsigned char c; CVTPARAMS p; setup_X(); p.Kr = .299; p.Kb = .114; p.minY = 0; p.maxY = 255; p.maxCC = 127; reset_cvt(&p); wrt(disppipe,"\0\1",2); while (1) { rcvn(disppipe,&c,1); display_frame(dbufs+(c*EXPECTED_SIZE)); wrt(disppipe,&c,1); } } static void fork_displayer(void) { int p[2]; int pid; if (socketpair(AF_LOCAL,SOCK_STREAM,0,&p[0]) < 0) { fprintf(stderr,"%s: socketpair: %s\n",__progname,strerror(errno)); exit(1); } fflush(0); pid = fork(); if (pid == 0) { close(p[0]); disppipe = p[1]; displayer_main(); _exit(0); } close(p[1]); disppipe = p[0]; dbuffree = 0; } int main(int, char **); int main(int ac, char **av) { handleargs(ac,av); if (! arg1) { fprintf(stderr,"Usage: %s [options] [port | host port]\n",__progname); exit(1); } if (! arg2) { net_accept(arg1); } else { net_connect(arg1,arg2); } setup_net(); open_video(); setup_video(); if (echovideo) { setup_dbufs(); fork_displayer(); } fork_encoder(); start_video(); encstate = ES_RETURNING; pkt_init(&pkt1); pkt_init(&pkt2); fpkt1 = &pkt1; fpkt2 = &pkt2; stats_clear(); curquality = 15; while (1) do_something(); }