00001 #include <errno.h>
00002 #include <sys/types.h>
00003 #include <sys/stat.h>
00004 #include <sys/fcntl.h>
00005 #include <sys/ioctl.h>
00006 #include <poll.h>
00007
00008 #include "FCam/Time.h"
00009 #include "FCam/Frame.h"
00010 #include "FCam/Action.h"
00011
00012 #include "../Debug.h"
00013 #include "Daemon.h"
00014 #include "linux/omap34xxcam-fcam.h"
00015
00016 namespace FCam { namespace N900 {
00017
00018 void *daemon_setter_thread_(void *arg) {
00019 Daemon *d = (Daemon *)arg;
00020 d->runSetter();
00021 d->setterRunning = false;
00022 close(d->daemon_fd);
00023 pthread_exit(NULL);
00024 }
00025
00026 void *daemon_handler_thread_(void *arg) {
00027 Daemon *d = (Daemon *)arg;
00028 d->runHandler();
00029 d->handlerRunning = false;
00030 pthread_exit(NULL);
00031 }
00032
00033 void *daemon_action_thread_(void *arg) {
00034 Daemon *d = (Daemon *)arg;
00035 d->runAction();
00036 d->actionRunning = false;
00037 pthread_exit(NULL);
00038 }
00039
00040 Daemon::Daemon(Sensor *sensor) :
00041 sensor(sensor),
00042 stop(false),
00043 frameLimit(128),
00044 dropPolicy(Sensor::DropNewest),
00045 setterRunning(false),
00046 handlerRunning(false),
00047 actionRunning(false),
00048 threadsLaunched(false) {
00049
00050
00051 v4l2Sensor = V4L2Sensor::instance("/dev/video0");
00052
00053
00054 if ((errno =
00055 -(pthread_mutex_init(&actionQueueMutex, NULL) ||
00056 pthread_mutex_init(&cameraMutex, NULL)))) {
00057 error(Event::InternalError, sensor, "Error creating mutexes: %d", errno);
00058 }
00059
00060
00061 sem_init(&actionQueueSemaphore, 0, 0);
00062
00063 lastGoodShot.wanted = false;
00064
00065 pipelineFlush = true;
00066 }
00067
00068 void Daemon::launchThreads() {
00069 if (threadsLaunched) return;
00070 threadsLaunched = true;
00071
00072
00073 pthread_attr_t attr;
00074 struct sched_param param;
00075
00076
00077 daemon_fd = open("/dev/video0", O_RDWR, 0);
00078
00079 if (daemon_fd < 0) {
00080 error(Event::InternalError, sensor, "Error opening /dev/video0: %d", errno);
00081 return;
00082 }
00083
00084
00085 if (ioctl(daemon_fd, VIDIOC_FCAM_INSTALL, NULL)) {
00086 if (errno == EBUSY) {
00087 error(Event::DriverLockedError, sensor,
00088 "An FCam program is already running");
00089 } else {
00090 error(Event::DriverMissingError, sensor,
00091 "Error %d calling VIDIOC_FCAM_INSTALL: Are the FCam drivers installed?", errno);
00092 }
00093 return;
00094 }
00095
00096
00097
00098 param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;
00099
00100 pthread_attr_init(&attr);
00101
00102 if ((errno =
00103 -(pthread_attr_setschedparam(&attr, ¶m) ||
00104 pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00105 pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00106 pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
00107 error(Event::InternalError, sensor, "Error creating daemon setter thread: %d", errno);
00108 return;
00109 } else {
00110 setterRunning = true;
00111 }
00112
00113
00114 param.sched_priority = sched_get_priority_min(SCHED_FIFO);
00115
00116 if ((errno =
00117 -(pthread_attr_setschedparam(&attr, ¶m) ||
00118 pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00119 pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00120 pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
00121 error(Event::InternalError, sensor, "Error creating daemon handler thread: %d", errno);
00122 return;
00123 } else {
00124 handlerRunning = true;
00125 }
00126
00127
00128 param.sched_priority = sched_get_priority_max(SCHED_FIFO);
00129
00130 if ((errno =
00131 -(pthread_attr_setschedparam(&attr, ¶m) ||
00132 pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
00133 pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
00134 pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
00135 error(Event::InternalError, sensor, "Error creating daemon action thread: %d", errno);
00136 return;
00137 } else {
00138 actionRunning = true;
00139 }
00140
00141 pthread_attr_destroy(&attr);
00142 }
00143
00144 Daemon::~Daemon() {
00145 stop = true;
00146
00147
00148 sem_post(&actionQueueSemaphore);
00149
00150 if (setterRunning)
00151 pthread_join(setterThread, NULL);
00152
00153 if (handlerRunning)
00154 pthread_join(handlerThread, NULL);
00155
00156 if (actionRunning)
00157 pthread_join(actionThread, NULL);
00158
00159 pthread_mutex_destroy(&actionQueueMutex);
00160 pthread_mutex_destroy(&cameraMutex);
00161
00162
00163 while (inFlightQueue.size()) delete inFlightQueue.pull();
00164 while (requestQueue.size()) delete requestQueue.pull();
00165 while (frameQueue.size()) delete frameQueue.pull();
00166 while (actionQueue.size()) {
00167 delete actionQueue.top().action;
00168 actionQueue.pop();
00169 }
00170
00171 v4l2Sensor->stopStreaming();
00172
00173 v4l2Sensor->close();
00174 }
00175
00176
00177 void Daemon::setDropPolicy(Sensor::DropPolicy p, int f) {
00178 dropPolicy = p;
00179 frameLimit = f;
00180 enforceDropPolicy();
00181 }
00182
00183 void Daemon::enforceDropPolicy() {
00184 if (frameQueue.size() > frameLimit) {
00185 warning(Event::FrameLimitHit, sensor,
00186 "WARNING: frame limit hit (%d), silently dropping %d frames.\n"
00187 "You're not draining the frame queue quickly enough. Use longer \n"
00188 "frame times or drain the frame queue until empty every time you \n"
00189 "call getFrame()\n", frameLimit, frameQueue.size() - frameLimit);
00190 if (dropPolicy == Sensor::DropOldest) {
00191 while (frameQueue.size() >= frameLimit) {
00192 sensor->decShotsPending();
00193 delete frameQueue.pull();
00194 }
00195 } else if (dropPolicy == Sensor::DropNewest) {
00196 while (frameQueue.size() >= frameLimit) {
00197 sensor->decShotsPending();
00198 delete frameQueue.pullBack();
00199 }
00200 } else {
00201 error(Event::InternalError, sensor,
00202 "Unknown drop policy! Not dropping frames.\n");
00203 }
00204 }
00205 }
00206
00207 void Daemon::runSetter() {
00208 dprintf(2, "Running setter...\n"); fflush(stdout);
00209 tickSetter(Time::now());
00210 while (!stop) {
00211 struct timeval t;
00212 if (ioctl(daemon_fd, VIDIOC_FCAM_WAIT_FOR_HS_VS, &t)) {
00213 if (stop) break;
00214 error(Event::DriverError, sensor,
00215 "error in VIDIOC_FCAM_WAIT_FOR_HS_VS: %s", strerror(errno));
00216 usleep(100000);
00217 continue;
00218 }
00219 tickSetter(Time(t));
00220 }
00221
00222
00223
00224
00225 if (lastGoodShot.wanted == true) {
00226 v4l2Sensor->setFrameTime(lastGoodShot.frameTime);
00227 v4l2Sensor->setExposure(lastGoodShot.exposure);
00228 v4l2Sensor->setGain(lastGoodShot.gain);
00229 }
00230 }
00231
00232 void Daemon::tickSetter(Time hs_vs) {
00233 static _Frame *req = NULL;
00234
00235 dprintf(3, "Current hs_vs was at %d %d\n", hs_vs.s(), hs_vs.us());
00236
00237
00238 static int lastReadout = 33000;
00239
00240 static bool ignoreNextHSVS = false;
00241
00242 dprintf(4, "Setter: got HS_VS\n");
00243
00244 if (ignoreNextHSVS) {
00245 dprintf(4, "Setter: ignoring it\n");
00246 ignoreNextHSVS = false;
00247 return;
00248 }
00249
00250
00251 if (req) {
00252 dprintf(4, "Setter: setting gain and WB\n");
00253
00254
00255
00256 if (req->shot().wanted == true) {
00257 if (req->shot().gain != current._shot.gain) {
00258 v4l2Sensor->setGain(req->shot().gain);
00259 current._shot.gain = req->shot().gain;
00260 }
00261 current.gain = req->gain = v4l2Sensor->getGain();
00262
00263 if (req->shot().colorMatrix().size() != 12) {
00264 if (req->shot().whiteBalance != current._shot.whiteBalance) {
00265 int wb = req->shot().whiteBalance;
00266
00267
00268 if (wb < 0) wb = 0;
00269 if (wb > 25000) wb = 25000;
00270
00271 float matrix[12];
00272 sensor->platform().rawToRGBColorMatrix(wb, matrix);
00273 v4l2Sensor->setWhiteBalance(matrix);
00274
00275 current._shot.whiteBalance = req->shot().whiteBalance;
00276 current.whiteBalance = wb;
00277 }
00278 req->whiteBalance = current.whiteBalance;
00279 } else {
00280
00281 v4l2Sensor->setWhiteBalance(&req->shot().colorMatrix()[0]);
00282 current._shot.setColorMatrix(&req->shot().colorMatrix()[0]);
00283 }
00284 }
00285
00286
00287
00288
00289
00290 req->processingDoneTime = hs_vs;
00291
00292
00293 req->processingDoneTime += lastReadout;
00294
00295
00296 req->processingDoneTime += req->frameTime;
00297
00298
00299
00300 int ispTime = 0;
00301 if (req->image.type() == UYVY) {
00302 if (req->image.height() > 1024 && req->image.width() > 1024) {
00303 ispTime = 65000;
00304 } else {
00305 ispTime = 10000;
00306 }
00307 }
00308 req->processingDoneTime += ispTime;
00309
00310
00311 req->exposureStartTime = hs_vs + req->frameTime - req->exposure;
00312
00313
00314
00315 lastReadout = (current.image.height() > 1008) ? 76000 : 33000;
00316
00317 req->exposureEndTime = req->exposureStartTime + req->exposure + lastReadout;
00318
00319
00320 pthread_mutex_lock(&actionQueueMutex);
00321 for (std::set<FCam::Action*>::const_iterator i = req->shot().actions().begin();
00322 i != req->shot().actions().end();
00323 i++) {
00324 Action a;
00325 a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
00326 a.action = (*i)->copy();
00327 actionQueue.push(a);
00328 }
00329 pthread_mutex_unlock(&actionQueueMutex);
00330 for (size_t i = 0; i < req->shot().actions().size(); i++) {
00331 sem_post(&actionQueueSemaphore);
00332 }
00333
00334
00335
00336 inFlightQueue.push(req);
00337 req = NULL;
00338 } else {
00339
00340 lastReadout = (current.image.height() > 1008) ? 76000 : 33000;
00341 }
00342
00343
00344
00345 if (!requestQueue.size()) {
00346 sensor->generateRequest();
00347 }
00348
00349
00350
00351 if (requestQueue.size()) {
00352 dprintf(4, "Setter: grabbing next request\n");
00353
00354 req = requestQueue.front();
00355
00356
00357
00358 if (req->_shot.wanted == true) {
00359 lastGoodShot.wanted = true;
00360 lastGoodShot.exposure = req->_shot.exposure;
00361 lastGoodShot.gain = req->_shot.gain;
00362 lastGoodShot.frameTime = req->_shot.frameTime;
00363 }
00364 } else {
00365 dprintf(4, "Setter: inserting a bubble\n");
00366
00367
00368
00369
00370 req = new _Frame;
00371 req->_shot.wanted = false;
00372
00373
00374
00375
00376
00377 req->_shot.image = Image(current._shot.image.size(), current._shot.image.type(), Image::Discard);
00378
00379
00380
00381 req->_shot.histogram = current._shot.histogram;
00382 req->_shot.sharpness = current._shot.sharpness;
00383
00384
00385 requestQueue.pushFront(req);
00386 }
00387
00388
00389 if (req->shot().image.size() != current._shot.image.size() ||
00390 req->shot().image.type() != current._shot.image.type() ||
00391 req->shot().histogram != current._shot.histogram ||
00392 req->shot().sharpness != current._shot.sharpness) {
00393
00394
00395 dprintf(3, "Setter: Mode switch required - flushing pipe\n");
00396 pipelineFlush = true;
00397
00398 pthread_mutex_lock(&cameraMutex);
00399 dprintf(3, "Setter: Handler done flushing pipe, passing control back to setter\n");
00400
00401
00402
00403 if (current.image.width() > 0) {
00404 dprintf(3, "Setter: Shutting down camera\n");
00405 v4l2Sensor->stopStreaming();
00406 v4l2Sensor->close();
00407 }
00408 dprintf(3, "Setter: Starting up camera in new mode\n");
00409 v4l2Sensor->open();
00410
00411
00412 V4L2Sensor::Mode m;
00413 m.width = req->shot().image.width();
00414 m.height = req->shot().image.height();
00415
00416 m.type = req->shot().image.type() == RAW ? RAW : UYVY;
00417 v4l2Sensor->startStreaming(m, req->shot().histogram, req->shot().sharpness);
00418 v4l2Sensor->setFrameTime(0);
00419
00420 dprintf(3, "Setter: Setter done bringing up camera, passing control back "
00421 "to handler. Expect two mystery frames.\n");
00422 pipelineFlush = false;
00423 pthread_mutex_unlock(&cameraMutex);
00424
00425 m = v4l2Sensor->getMode();
00426
00427 req->image = Image(m.width, m.height, m.type, Image::Discard);
00428
00429 current._shot.image = Image(req->shot().image.size(), req->shot().image.type(), Image::Discard);
00430 current._shot.histogram = req->shot().histogram;
00431 current._shot.sharpness = req->shot().sharpness;
00432
00433
00434
00435 current._shot.frameTime = -1;
00436 current._shot.exposure = -1;
00437 current._shot.gain = -1.0;
00438 current._shot.whiteBalance = -1;
00439 current.image = Image(m.width, m.height, m.type, Image::Discard);
00440
00441 req = NULL;
00442
00443
00444 ignoreNextHSVS = true;
00445
00446 return;
00447 } else {
00448
00449 }
00450
00451
00452 requestQueue.pop();
00453
00454 Time next = hs_vs + current.frameTime;
00455 dprintf(3, "The current %d x %d frame has a frametime of %d\n",
00456 current.image.width(), current.image.height(), current.frameTime);
00457 dprintf(3, "Predicting that the next HS_VS will be at %d %d\n",
00458 next.s(), next.us());
00459
00460 int exposure = req->shot().exposure;
00461 int frameTime = req->shot().frameTime;
00462
00463 if (frameTime < exposure + 400) {
00464 frameTime = exposure + 400;
00465 }
00466
00467 dprintf(4, "Setter: setting frametime and exposure\n");
00468
00469 v4l2Sensor->setFrameTime(frameTime);
00470 v4l2Sensor->setExposure(exposure);
00471
00472
00473
00474 current._shot.frameTime = frameTime;
00475 current._shot.exposure = exposure;
00476 current.exposure = req->exposure = v4l2Sensor->getExposure();
00477 current.frameTime = req->frameTime = v4l2Sensor->getFrameTime();
00478 req->image = current.image;
00479
00480
00481
00482
00483 dprintf(4, "Setter: Done with this HS_VS, waiting for the next one\n");
00484
00485 }
00486
00487
00488 void Daemon::runHandler() {
00489 _Frame *req = NULL;
00490 V4L2Sensor::V4L2Frame *f = NULL;
00491
00492 pthread_mutex_lock(&cameraMutex);
00493
00494 while (!stop) {
00495
00496
00497
00498 if (!req && pipelineFlush && inFlightQueue.size() == 0) {
00499 dprintf(3, "Handler: Handler done flushing pipe, passing control back to setter\n");
00500 while (pipelineFlush) {
00501 pthread_mutex_unlock(&cameraMutex);
00502
00503
00504
00505 usleep(10000);
00506 pthread_mutex_lock(&cameraMutex);
00507 }
00508 dprintf(3, "Handler: Setter done bringing up camera, passing control back to handler\n");
00509
00510 }
00511
00512 if (pipelineFlush) {
00513 dprintf(3, "Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
00514 }
00515
00516
00517 if (!f)
00518 f = v4l2Sensor->acquireFrame(true);
00519
00520 if (!f) {
00521 error(Event::InternalError, "Handler got a NULL frame\n");
00522 usleep(300000);
00523 continue;
00524 }
00525
00526
00527 if (!req) {
00528 if (inFlightQueue.size()) {
00529 dprintf(4, "Handler: popping a frame request\n");
00530 req = inFlightQueue.pull();
00531 } else {
00532
00533
00534 dprintf(3, "Handler: Got a frame without an outstanding request,"
00535 " dropping it.\n");
00536 v4l2Sensor->releaseFrame(f);
00537 f = NULL;
00538 continue;
00539 }
00540 }
00541
00542
00543
00544 int dt = req->processingDoneTime - f->processingDoneTime;
00545
00546 dprintf(4, "Handler dt = %d\n", dt);
00547
00548 if (dt < -25000) {
00549 dprintf(3, "Handler: Expected a frame at %d %d, but one didn't arrive until %d %d\n",
00550 req->processingDoneTime.s(), req->processingDoneTime.us(),
00551 f->processingDoneTime.s(), f->processingDoneTime.us());
00552 error(Event::ImageDroppedError, sensor,
00553 "Expected image data not returned from V4L2. Likely caused by excess"
00554 " page faults (thrashing).");
00555 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00556 if (!req->shot().wanted) {
00557 delete req;
00558 } else {
00559
00560 req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime, req->shot().histogram);
00561 req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime, req->shot().sharpness);
00562 frameQueue.push(req);
00563 enforceDropPolicy();
00564 }
00565 req = NULL;
00566 } else if (dt < 10000) {
00567
00568 if (!req->shot().wanted) {
00569
00570 dprintf(4, "Handler: discarding a bubble\n");
00571 delete req;
00572 v4l2Sensor->releaseFrame(f);
00573 } else {
00574
00575
00576 req->processingDoneTime = f->processingDoneTime;
00577
00578 size_t bytes = req->image.width()*req->image.height()*2;
00579 if (f->length < bytes) bytes = f->length;
00580
00581 if (req->shot().image.autoAllocate()) {
00582 req->image = Image(req->image.size(), req->image.type(), f->data).copy();
00583 } else if (req->shot().image.discard()) {
00584 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00585 } else {
00586 if (req->image.size() != req->shot().image.size()) {
00587 error(Event::ResolutionMismatch, sensor,
00588 "Requested image size (%d x %d) "
00589 "on an already allocated image does not "
00590 "match actual image size (%d x %d). Dropping image data.",
00591 req->shot().image.width(), req->shot().image.height(),
00592 req->image.width(), req->image.height());
00593 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00594
00595 } else if (req->image.type() != req->shot().image.type()) {
00596 error(Event::FormatMismatch, sensor,
00597 "Requested unsupported image format %d "
00598 "for an already allocated image.",
00599 req->shot().image.type());
00600 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00601 } else {
00602 req->image = req->shot().image;
00603
00604
00605 Time lockStart = Time::now();
00606 if (req->image.lock(10000)) {
00607 req->image.copyFrom(Image(req->image.size(), req->image.type(), f->data));
00608 req->image.unlock();
00609 } else {
00610 warning(Event::ImageTargetLocked, sensor,
00611 "Daemon discarding image data (target is still locked, "
00612 "waited for %d us)\n", Time::now() - lockStart);
00613 req->image = Image(req->image.size(), req->image.type(), Image::Discard);
00614 }
00615 }
00616 }
00617
00618 v4l2Sensor->releaseFrame(f);
00619 req->histogram = v4l2Sensor->getHistogram(req->exposureEndTime, req->shot().histogram);
00620 req->sharpness = v4l2Sensor->getSharpnessMap(req->exposureEndTime, req->shot().sharpness);
00621
00622 frameQueue.push(req);
00623 enforceDropPolicy();
00624
00625 }
00626
00627 req = NULL;
00628 f = NULL;
00629
00630 } else {
00631 dprintf(3, "Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
00632 f->processingDoneTime.s(), f->processingDoneTime.us(),
00633 req->processingDoneTime.s(), req->processingDoneTime.us());
00634 v4l2Sensor->releaseFrame(f);
00635 f = NULL;
00636 }
00637
00638 }
00639 pthread_mutex_unlock(&cameraMutex);
00640
00641 }
00642
00643
00644 void Daemon::runAction() {
00645 dprintf(2, "Action thread running...\n");
00646 while (1) {
00647 sem_wait(&actionQueueSemaphore);
00648 if (stop) break;
00649
00650 pthread_mutex_lock(&actionQueueMutex);
00651 Action a = actionQueue.top();
00652 actionQueue.pop();
00653 pthread_mutex_unlock(&actionQueueMutex);
00654
00655 Time t = Time::now();
00656 int delay = (a.time - t) - 500;
00657 if (delay > 0) usleep(delay);
00658 Time before = Time::now();
00659
00660 while (a.time > before) before = Time::now();
00661 a.action->doAction();
00662
00663 dprintf(3, "Action thread: Initiated action %d us after scheduled time\n", before - a.time);
00664 delete a.action;
00665 }
00666 }
00667
00668 }}
00669