#include <stdio.h>
#include <stdlib.h>
#include <linux/ioctl.h>
#include <unistd.h>
#include <string.h>
#include <poll.h>
#include <errno.h>
#include <syslog.h>
#include "CaptureThread.h"
#include "Capture.h"
#include "CaptureDebug.h"
#include "rgb133v4l2.h"
Functions | |
void * | CaptureThreadFunc (void *arg) |
int | CreateCaptureThread (psCapture pCapture, psCaptureThreadArgs pCaptureThreadArgs) |
void | JoinCaptureThread (psCapture pCapture) |
void | FreeCaptureThreadArgs (psCaptureThreadArgs *pCaptureThreadArgs) |
Variables | |
int | CaptureStop |
int | CaptureThreadStop = 0 |
void* CaptureThreadFunc | ( | void * | arg | ) |
Implementation of CaptureThreadFunc.
00042 { 00043 psCaptureThreadArgs pCaptureThreadArgs = (psCaptureThreadArgs)arg; 00044 psCapture pCapture = pCaptureThreadArgs->pCapture; 00045 eIOType IOType = pCapture->ioType; 00046 00047 struct v4l2_buffer buf; 00048 enum v4l2_buf_type buf_type; 00049 00050 int i = 0; 00051 int ret = 0; 00052 int frame = 0; 00053 00054 if(IOType == IO_MMAP) 00055 { 00056 CapError("TODO: Setup mmap options...\n"); 00057 } 00058 00059 switch(IOType) 00060 { 00061 case IO_READ: 00062 CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd); 00063 CapMessage("pData(%p), DataLength(%u)\n", pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength); 00064 while(!CaptureThreadStop) 00065 { 00066 ret = read( pCapture->fd, pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength ); 00067 if(ret < 0) 00068 { 00069 switch(errno) 00070 { 00071 case EWOULDBLOCK: 00072 //CapError("EWOULDBLOCK...\n"); 00073 break; 00074 case EIO: 00075 /* Could ignore EIO, see spec. */ 00076 /* fall through */ 00077 default: 00078 CapError("Failed to read from Capture Device - %d: %s\n", 00079 errno, pCapture->caps.driver); 00080 return 0; 00081 } 00082 } 00083 frame++; 00084 if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count)) 00085 { 00086 CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count); 00087 CaptureThreadStop = 1; 00088 } 00089 } 00090 break; 00091 case IO_MMAP: 00092 CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd); 00093 CapMessage("pData(%p), DataLength(%u)\n", pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength); 00094 00095 /* Initial queue of the buffers */ 00096 for(i=0; i<pCapture->buffers; ++i) 00097 { 00098 memset( &buf, 0, sizeof(buf) ); 00099 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00100 buf.memory = V4L2_MEMORY_MMAP; 00101 buf.index = i; 00102 00103 CapError("Q buffer[%d]\n", buf.index); 00104 if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0) 00105 { 00106 CapError("VIDIOC_QBUF failed: %d\n", ret); 00107 return 0; 00108 } 00109 } 00110 00111 buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00112 CapError("STREAMON\n"); 00113 if((ret = ioctl(pCapture->fd, VIDIOC_STREAMON, &buf_type)) < 0) 00114 { 00115 CapError("VIDIOC_STREAMON failed: %d\n", ret); 00116 return 0; 00117 } 00118 00119 openlog("CaptureApp", 0, LOG_LOCAL0); 00120 00121 while(!CaptureThreadStop) 00122 { 00123 struct pollfd fd; 00124 fd.fd = pCapture->fd; 00125 fd.events = POLLIN|POLLPRI; 00126 fd.revents = 0; 00127 00128 /* Wait for data, max 500ms */ 00129 if(poll(&fd, 1, 500) > 0) 00130 { 00131 if( fd.revents & (POLLIN|POLLPRI) ) 00132 { 00133 memset( &buf, 0, sizeof(buf) ); 00134 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00135 buf.memory = V4L2_MEMORY_MMAP; 00136 00137 /* dequeue frame */ 00138 if((ret = ioctl(pCapture->fd, VIDIOC_DQBUF, &buf)) < 0) 00139 { 00140 switch( errno ) 00141 { 00142 case EAGAIN: 00143 continue; 00144 case EIO: 00145 default: 00146 CapError("Failed to dequeue buffer[%d]: %d\n", buf.index, ret); 00147 return 0; 00148 } 00149 } 00150 00151 if(pCapture->timestamp) 00152 { 00153 static int prev = -1, this = -1; 00154 static struct timeval tv_this = {0, 0}, tv_prev = {0, 0}; 00155 00156 //CapError("DQ buffer[%d].seq(%d) ts(%d.%06d)\n", 00157 // buf.index, buf.sequence, 00158 // buf.timestamp.tv_sec, buf.timestamp.tv_usec); 00159 this = buf.sequence; 00160 tv_this.tv_sec = buf.timestamp.tv_sec; 00161 tv_this.tv_usec = buf.timestamp.tv_usec; 00162 00163 if(prev != -1) 00164 { 00165 if(this != (prev+1)) 00166 { 00167 CapError("DQ buffer[%d].seq(%d)(%d) mismatch\n", buf.index, this, prev); 00168 } 00169 } 00170 prev = this; 00171 00172 if(tv_prev.tv_sec != 0 && 00173 tv_prev.tv_usec != 0) 00174 { 00175 signed int diff_s = tv_this.tv_sec - tv_prev.tv_sec; 00176 signed int diff_us = tv_this.tv_usec - tv_prev.tv_usec; 00177 if(diff_us < 0) 00178 { 00179 diff_s--; 00180 diff_us += 1000000; 00181 } 00182 if(diff_s || 00183 (diff_us < 16600 || diff_us > 16700)) 00184 { 00185 CapError("DQ buffer[%d].ts(%d.%06d)(%d.%06d) validity(%d.%06d) frame(%d)\n", 00186 buf.index, (int)tv_this.tv_sec, (int)tv_this.tv_usec, 00187 (int)tv_prev.tv_sec, (int)tv_prev.tv_usec, 00188 diff_s, diff_us, frame); 00189 } 00190 } 00191 tv_prev.tv_sec = tv_this.tv_sec; 00192 tv_prev.tv_usec = tv_this.tv_usec; 00193 } 00194 00195 CapMessage("Dequeued buffer index: %d\n", buf.index); 00196 00197 if(buf.index >= pCapture->buffers ) { 00198 CapError("Invalid buffer index: %d\n", buf.index); 00199 return 0; 00200 } 00201 00202 frame++; 00203 if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count)) 00204 { 00205 CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count); 00206 CaptureThreadStop = 1; 00207 } 00208 00209 /* Requeue buffer */ 00210 //syslog(LOG_ERR, "*** Requeueing Buffer ***\n"); 00211 CapMessage("Requeuing buffer %d back to driver.\n", buf.index); 00212 //CapError("Q buffer[%d]\n", buf.index); 00213 if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0) 00214 { 00215 CapError("Failed to requeue buffer[%d]: %d\n", buf.index, ret); 00216 return 0; 00217 } 00218 } 00219 } 00220 if(pCapture->delay) 00221 usleep(pCapture->delay*1000); 00222 } 00223 00224 buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00225 CapError("STREAMOFF\n"); 00226 if((ret = ioctl(pCapture->fd, VIDIOC_STREAMOFF, &buf_type)) < 0) 00227 { 00228 CapError("VIDIOC_STREAMOFF failed: %d", ret); 00229 } 00230 00231 /* Make sure every buffer is dequeued */ 00232 for(i=0; i<pCapture->buffers; i++) 00233 { 00234 memset( &buf, 0, sizeof(buf) ); 00235 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00236 buf.memory = V4L2_MEMORY_MMAP; 00237 ioctl(pCapture->fd, VIDIOC_DQBUF, &buf); 00238 CapError("DQ buffer[%d]\n", buf.index); 00239 } 00240 00241 break; 00242 case IO_USERPTR: 00243 CapLog("Start capture on %s: %d\n", pCapture->caps.driver, pCapture->fd); 00244 CapMessage("pData[0](%p), DataLength[0](%u), pData[1](%p), DataLength[1](%u)\n" 00245 "pData[2](%p), DataLength[2](%u), pData[3](%p), DataLength[3](%u)\n", 00246 pCapture->pBuffers[0].pData, pCapture->pBuffers[0].DataLength, 00247 pCapture->pBuffers[1].pData, pCapture->pBuffers[1].DataLength, 00248 pCapture->pBuffers[2].pData, pCapture->pBuffers[2].DataLength, 00249 pCapture->pBuffers[3].pData, pCapture->pBuffers[3].DataLength); 00250 00251 /* Initial queue of the buffers */ 00252 for(i=0; i<pCapture->buffers; ++i) 00253 { 00254 memset( &buf, 0, sizeof(buf) ); 00255 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00256 buf.memory = V4L2_MEMORY_USERPTR; 00257 buf.index = i; 00258 buf.m.userptr = (unsigned long)pCapture->pBuffers[i].pData; 00259 buf.length = pCapture->pBuffers[i].DataLength; 00260 00261 CapError("Q buffer[%d]\n", buf.index); 00262 if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0) 00263 { 00264 CapError("VIDIOC_QBUF failed: %d\n", ret); 00265 return 0; 00266 } 00267 } 00268 00269 buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00270 CapError("STREAMON\n"); 00271 if((ret = ioctl(pCapture->fd, VIDIOC_STREAMON, &buf_type)) < 0) 00272 { 00273 CapError("VIDIOC_STREAMON failed: %d\n", ret); 00274 return 0; 00275 } 00276 00277 while(!CaptureThreadStop) 00278 { 00279 struct pollfd fd; 00280 fd.fd = pCapture->fd; 00281 fd.events = POLLIN|POLLPRI; 00282 fd.revents = 0; 00283 00284 /* Wait for data, max 500ms */ 00285 if(poll(&fd, 1, 500) > 0) 00286 { 00287 if( fd.revents & (POLLIN|POLLPRI) ) 00288 { 00289 memset( &buf, 0, sizeof(buf) ); 00290 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00291 buf.memory = V4L2_MEMORY_USERPTR; 00292 00293 /* dequeue frame */ 00294 if((ret = ioctl(pCapture->fd, VIDIOC_DQBUF, &buf)) < 0) 00295 { 00296 switch( errno ) 00297 { 00298 case EAGAIN: 00299 continue; 00300 case EIO: 00301 default: 00302 CapError("Failed to dequeue buffer[%d]: %d\n", buf.index, ret); 00303 return 0; 00304 } 00305 } 00306 00307 if(pCapture->timestamp) 00308 { 00309 static int prev = -1, this = -1; 00310 static struct timeval tv_this = {0, 0}, tv_prev = {0, 0}; 00311 00312 this = buf.sequence; 00313 tv_this.tv_sec = buf.timestamp.tv_sec; 00314 tv_this.tv_usec = buf.timestamp.tv_usec; 00315 00316 if(prev != -1) 00317 { 00318 if(this != (prev+1)) 00319 { 00320 CapError("DQ buffer[%d].seq(%d)(%d) mismatch\n", buf.index, this, prev); 00321 } 00322 } 00323 prev = this; 00324 00325 if(tv_prev.tv_sec != 0 && 00326 tv_prev.tv_usec != 0) 00327 { 00328 signed int diff_s = tv_this.tv_sec - tv_prev.tv_sec; 00329 signed int diff_us = tv_this.tv_usec - tv_prev.tv_usec; 00330 if(diff_us < 0) 00331 { 00332 diff_s--; 00333 diff_us += 1000000; 00334 } 00335 if(diff_s || 00336 (diff_us < 16600 || diff_us > 16700)) 00337 { 00338 CapError("DQ buffer[%d].ts(%d.%06d)(%d.%06d) validity(%d.%06d) frame(%d)\n", 00339 buf.index, (int)tv_this.tv_sec, (int)tv_this.tv_usec, 00340 (int)tv_prev.tv_sec, (int)tv_prev.tv_usec, 00341 diff_s, diff_us, frame); 00342 } 00343 } 00344 tv_prev.tv_sec = tv_this.tv_sec; 00345 tv_prev.tv_usec = tv_this.tv_usec; 00346 } 00347 00348 CapMessage("Dequeued buffer index: %d\n", buf.index); 00349 00350 if(buf.index >= pCapture->buffers ) { 00351 CapError("Invalid buffer index: %d\n", buf.index); 00352 return 0; 00353 } 00354 00355 frame++; 00356 if ((pCapture->capture_count > 0) && (frame >= pCapture->capture_count)) 00357 { 00358 CapMessage("Captured %d frames, stopping, as requested.\n", pCapture->capture_count); 00359 CaptureThreadStop = 1; 00360 } 00361 00362 /* Requeue buffer */ 00363 CapMessage("Requeuing buffer %d back to driver.\n", buf.index); 00364 if((ret = ioctl(pCapture->fd, VIDIOC_QBUF, &buf)) < 0) 00365 { 00366 CapError("Failed to requeue buffer[%d]: %d\n", buf.index, ret); 00367 return 0; 00368 } 00369 } 00370 } 00371 if(pCapture->delay) 00372 usleep(pCapture->delay*1000); 00373 } 00374 00375 buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00376 CapError("STREAMOFF\n"); 00377 if((ret = ioctl(pCapture->fd, VIDIOC_STREAMOFF, &buf_type)) < 0) 00378 { 00379 CapError("VIDIOC_STREAMOFF failed: %d", ret); 00380 } 00381 00382 /* Make sure every buffer is dequeued */ 00383 for(i=0; i<pCapture->buffers; i++) 00384 { 00385 memset( &buf, 0, sizeof(buf) ); 00386 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 00387 buf.memory = V4L2_MEMORY_USERPTR; 00388 ioctl(pCapture->fd, VIDIOC_DQBUF, &buf); 00389 CapError("DQ buffer[%d]\n", buf.index); 00390 } 00391 00392 break; 00393 default: 00394 CapError("Invalid IO method: %d\n", IOType); 00395 break; 00396 } 00397 00398 CapMessage("Capture Thread Finished\n"); 00399 00400 if(CaptureThreadStop) 00401 CaptureStop = 1; 00402 }
int CreateCaptureThread | ( | psCapture | pCapture, | |
psCaptureThreadArgs | pCaptureThreadArgs | |||
) |
Implementation of CreateCaptureThread.
00408 { 00409 int ret = 0; 00410 00411 if(pCaptureThreadArgs) 00412 { 00413 CapError("Capture Thread structure(%p) may be initialised, bailing out\n", pCaptureThreadArgs); 00414 return -1; 00415 } 00416 00417 pCaptureThreadArgs = malloc(sizeof(sCaptureThreadArgs)); 00418 if(pCaptureThreadArgs == 0) 00419 return -1; 00420 00421 memset(pCaptureThreadArgs, 0, sizeof(sCaptureThreadArgs)); 00422 pCaptureThreadArgs->pCapture = pCapture; 00423 ret = pthread_create(&pCapture->CaptureThread, NULL, CaptureThreadFunc, (void*)pCaptureThreadArgs); 00424 return 0; 00425 }
void FreeCaptureThreadArgs | ( | psCaptureThreadArgs * | pCaptureThreadArgs | ) |
void JoinCaptureThread | ( | psCapture | pCapture | ) |
Implementation of JoinCaptureThread.
00431 { 00432 CapMessage("Waiting for capture thread to join...\n"); 00433 pthread_join(pCapture->CaptureThread, 0); 00434 CapMessage("Capture thread has joined...\n"); 00435 }
int CaptureStop |
int CaptureThreadStop = 0 |