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