N900/Daemon.cpp

#include "Daemon.h"
#include "linux/omap34xxcam-daemon.h"

#include "../Time.h"
#include "../Frame.h"
#include "../Action.h"

#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <poll.h>

namespace FCam { namespace N900 {

void *daemon_setter_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runSetter();    
    d->setterRunning = false;    
    close(d->daemon_fd);
    pthread_exit(NULL);
} 

void *daemon_handler_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runHandler();
    d->handlerRunning = false;
    pthread_exit(NULL);    
}

void *daemon_action_thread_(void *arg) {
    Daemon *d = (Daemon *)arg;
    d->runAction();
    d->actionRunning = false;
    pthread_exit(NULL);
}

Daemon::Daemon() :
    stop(false), 
    generator(NULL),
    frameLimit(128),
    dropPolicy(Sensor::DropNewest),
    setterRunning(false), 
    handlerRunning(false), 
    actionRunning(false),
    waitingForFirstRequest(true) {

    // add a dummy shot to the current request
    current.request = new Shot;
    
    // tie ourselves to the correct sensor
    sensor = V4L2Sensor::instance("/dev/video0");

    // launch the daemon thread with realtime priority

    // check I'm root
    if (getuid()) {
    printf("Camera Daemon can only run as root. Aborting.\n");
    return;
    }
    
    // enable real-time scheduling modes
    FILE *f = fopen("/proc/sys/kernel/sched_rt_runtime_us", "w");
    if (f) {
    fprintf(f, "-1");
    fclose(f);
    } else {
    printf("Could not enable real-time scheduling modes, daemon thread creating may fail\n");
    }

    // kill the built-in camera program
    if (!fork()) {
    // on success, this function never returns
    execl("/usr/sbin/dsmetool", "/usr/sbin/dsmetool", "-k", "/usr/bin/camera-ui", (char *)NULL);
    // it returned, so there was a failure
    perror("Failed to kill camera-ui");
    exit(0);
    }

    // make the mutexes for the producer-consumer queues
    if ((errno = 
     -(pthread_mutex_init(&requestQueueMutex, NULL) ||
       pthread_mutex_init(&frameQueueMutex, NULL) ||
       pthread_mutex_init(&actionQueueMutex, NULL) ||
       pthread_mutex_init(&inFlightQueueMutex, NULL) ||
       pthread_mutex_init(&cameraMutex, NULL)))) {
    perror("Error creating mutexes");
    }
    
    // make the semaphores
    sem_init(&frameQueueSemaphore, 0, 0);
    sem_init(&actionQueueSemaphore, 0, 0);

    // make the semaphore for pipeline flushes
    pipelineFlush = true;
    
}

void Daemon::launchThreads() {

    pthread_attr_t attr;
    struct sched_param param;

    // make the setter thread

    param.sched_priority = sched_get_priority_min(SCHED_FIFO)+1;

    pthread_attr_init(&attr);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&setterThread, &attr, daemon_setter_thread_, this)))) {
    perror("Error creating daemon setter thread");
    return;
    } else {
    setterRunning = true;
    }

    // make the handler thread
    param.sched_priority = sched_get_priority_min(SCHED_FIFO);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&handlerThread, &attr, daemon_handler_thread_, this)))) {
    perror("Error creating daemon handler thread");
    return;
    } else {
    handlerRunning = true;
    }

    // make the actions thread
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);

    if ((errno =
     -(pthread_attr_setschedparam(&attr, &param) ||
       pthread_attr_setschedpolicy(&attr, SCHED_FIFO) ||
       pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED) ||
       pthread_create(&actionThread, &attr, daemon_action_thread_, this)))) {
    perror("Error creating daemon action thread");
    return;
    } else {
    actionRunning = true;
    }

    pthread_attr_destroy(&attr);
}

Daemon::~Daemon() {
    stop = true;

    // post a wakeup call to the action thread
    sem_post(&actionQueueSemaphore);

    if (setterRunning) 
    pthread_join(setterThread, NULL);
    
    if (handlerRunning)
    pthread_join(handlerThread, NULL);

    if (actionRunning)
    pthread_join(actionThread, NULL);

    pthread_mutex_destroy(&requestQueueMutex);    
    pthread_mutex_destroy(&frameQueueMutex);
    pthread_mutex_destroy(&inFlightQueueMutex);
    pthread_mutex_destroy(&actionQueueMutex);

    sensor->stopStreaming();

    sensor->close();
}


void Daemon::setRequestGenerator(Daemon::RequestGenerator *gen) {
    generator = gen;
    if (waitingForFirstRequest && generator) {
    // get things started
    printf("Calling generator for the first time\n");
    generator->generateRequest();
    }
}

Daemon::RequestGenerator *Daemon::getRequestGenerator() {
    return generator;
}


int Daemon::requestFrame(Frame *req) {
    // enqueue the request
    pthread_mutex_lock(&requestQueueMutex);
    requestQueue.push(req);
    pthread_mutex_unlock(&requestQueueMutex);    

    if (waitingForFirstRequest) {
    printf("First frame requested\n");
    tickSetter(Time::now());
    waitingForFirstRequest = false;
    launchThreads();
    }
    return 0;
}

Frame *Daemon::getNextFrame() {
    //.printf("frameQueue: %d\n", frameQueue.size());
    sem_wait(&frameQueueSemaphore);
    pthread_mutex_lock(&frameQueueMutex);
    Frame *f = frameQueue.front();
    frameQueue.pop();
    pthread_mutex_unlock(&frameQueueMutex);
    return f;
}


void Daemon::setDropPolicy(Sensor::DropPolicy p, int f) {
    dropPolicy = p;
    frameLimit = f;
    enforceDropPolicy();
}

void Daemon::enforceDropPolicy() {
    if (frameQueue.size() > frameLimit) {
    pthread_mutex_lock(&frameQueueMutex);
    int drops = frameQueue.size() - frameLimit;
    if (drops > 0) {
        printf("WARNING: frame limit hit (%d), silently dropping %d frames.\n"
           "You're not draining the frame queue quickly enough. Use longer \n"
           "frame times or drain the frame queue until empty every time you \n"
           "call getFrame()\n", frameLimit, drops);
        if (dropPolicy == Sensor::DropOldest) {
        for (int i = 0; i < drops; i++) {
            sem_wait(&frameQueueSemaphore);
            delete frameQueue.front();
            frameQueue.pop();
        }
        } else if (dropPolicy == Sensor::DropNewest) {
        // recycle the first bunch
        for (size_t i = 0; i < frameQueue.size() - drops; i++) {
            frameQueue.push(frameQueue.front());
            frameQueue.pop();
        }
        for (int i = 0; i < drops; i++) {
            sem_wait(&frameQueueSemaphore);
            delete frameQueue.front();
            frameQueue.pop();
        }
        } else {
        printf("Unknown drop policy! Not dropping frames.\n");
        }
    }
    pthread_mutex_unlock(&frameQueueMutex);
    }
}

void Daemon::runSetter() {
    // open the device as a daemon
    daemon_fd = open("/dev/video0", O_RDWR, 0);

    if (daemon_fd < 0) {
    perror("opening /dev/video0 as daemon");
    return;
    }

    // try to register myself as the omap camera daemon
    if (ioctl(daemon_fd, VIDIOC_DAEMON_INSTALL, NULL)) {
    perror("VIDIOC_DAEMON_INSTALL");
    return;
    }    

    unsigned int event_mask = 8; // just HS_VS please
    if (ioctl(daemon_fd, VIDIOC_DAEMON_SET_EVENTS, &event_mask)) {
    perror("VIDIOC_DAEMON_SET_EVENTS");
    return;
    }    


    struct pollfd p = {daemon_fd, POLLIN, 0};

    struct omap34xxcam_daemon_daemon_req req;

    req.req.blob = malloc(1<<10);
    req.req.max_size = 1<<10;

    while (!stop) {
    //printf("Daemon polling...\n"); fflush(stdout);
    poll(&p, 1, 1000);
    if (p.revents & POLLIN) {
        // something happened!

        req.u.sync = 0;
        req.req.size = 0;
        req.req.type = 0;

        if (ioctl(daemon_fd, VIDIOC_DAEMON_DAEMON_REQ_GET, &req)) {
        perror("VIDIOC_DAEMON_DAEMON_REQ_GET");
    
        // This bugs out in older versions of the kernel
        // module (it was doing a copy_to_user inside a spin
        // lock - naughty!
        usleep(10000);
        continue;
        }    

        if (req.req.type == 1000) {
        // let's pretend to complete it
        
        if (ioctl(daemon_fd, VIDIOC_DAEMON_DAEMON_REQ_COMPLETE, &req)) {
            perror("VIDIOC_DAEMON_DAEMON_REQ_COMPLETE");
            //return NULL;
        }    
        

        } else if (req.req.type == 10) { // an interrupt

        struct omap34xxcam_daemon_event *event = 
            (struct omap34xxcam_daemon_event *)req.req.blob;
        //.printf("Daemon interrupt: %d\n", event->mask);

        if (event->mask == 8) { // HS_VS
            tickSetter(Time(event->hs_vs_stamp));
        }
        }

    } else {
        // why I am awake!
        //.printf("poll returned with %d\n", p.revents);
    }
    }

}

void Daemon::tickSetter(Time hs_vs) {
    static Frame *req = NULL;

    // how long will the previous frame take to readout
    static int lastReadout = 33000;

    static bool ignoreNextHSVS = false;

    if (ignoreNextHSVS) {
    ignoreNextHSVS = false;
    return;
    }

    if (req) {
    // set the gain and predicted done time on the pending request
    // and then push it onto the handler's input queue and the v4l2 buffer queue

    if (req->request->gain != current.request->gain) {
        sensor->setGain(req->request->gain);
        current.request->gain = req->request->gain;
    }
    current.gain = req->gain = sensor->getGain();

    // Predict when this frame will be done. It should be HS_VS +
    // the readout time for the previous frame + the frame time
    // for this request + however long the ISP takes to process
    // this frame.
    req->processingDoneTime = hs_vs;
    
    // first there's the readout time for the previous frame
    req->processingDoneTime += lastReadout;
    
    // then there's the time to expose and read out this frame
    req->processingDoneTime += req->frameTime;

    // then there's some significant time inside the ISP if it's YUV and large
    // (this may be N900 specific)
    int ispTime = 0;
    if (req->image.type == UYVY) {
        if (req->image.size.height > 1024 && req->image.size.width > 1024) {
        ispTime = 65000;
        } else {
        ispTime = 10000;
        }
    }
    req->processingDoneTime += ispTime;         
    
    // Also compute when the exposure starts and finishes
    req->exposureStartTime = hs_vs + req->frameTime - req->exposure;

    // Now updated the readout time for this frame. This formula
    // is specific to the toshiba et8ek8 sensor on the n900
    lastReadout = (current.image.size.height > 1008) ? 76000 : 33000;

    req->exposureEndTime  = req->exposureStartTime + req->exposure + lastReadout;

    //printf("Predicted time taken to read out previous frame: %d\n", lastReadout);
    //printf("Predicted time to expose and read out this frame: %d\n", req->frameTime);
    //printf("Predicted time to take in the ISP: %d\n", ispTime);

    // now queue up this request's actions
    pthread_mutex_lock(&actionQueueMutex);
    for (std::set<FCam::Action*>::iterator i = req->request->actions.begin();
         i != req->request->actions.end();
         i++) {
        Action a;
        a.time = req->exposureStartTime + (*i)->time - (*i)->latency;
        a.action = (*i)->copy();
        actionQueue.push(a);
    }
    pthread_mutex_unlock(&actionQueueMutex);
    for (size_t i = 0; i < req->request->actions.size(); i++) {
        sem_post(&actionQueueSemaphore);
    }

    pthread_mutex_lock(&inFlightQueueMutex);
    inFlightQueue.push(req);
    pthread_mutex_unlock(&inFlightQueueMutex);
    req = NULL;
    } else {
    // a bubble!
    lastReadout = (current.image.size.height > 1008) ? 76000 : 33000;
    }


    // grab a new request and set an appropriate exposure and frame time for it
    // make sure there's a request ready for us
    if (generator && !requestQueue.size()) {
    generator->generateRequest();
    }

    pthread_mutex_lock(&requestQueueMutex); 
    if (requestQueue.size()) {
    // peek at the next request
    req = requestQueue.front();
    } else {
    //printf("Setter pushing a bubble into the pipeline\n");
    
    // There are no pending requests, push a bubble into the
    // pipeline. The default parameters for a frame work nicely as
    // a bubble (as short as possible, no stats generated, output
    // unwanted).
    req = new Frame;
    req->request = new Shot;
    req->request->wanted = false;

    // bubbles should just run at whatever resolution is going. If
    // fast mode switches were possible, it might be nice to run
    // at the minimum resolution to go even faster, but they're
    // not.
    req->request->image.size = current.request->image.size;
    req->request->image.type = current.request->image.type;

    // generate histograms and sharpness maps if they're going,
    // but drop the data.
    req->request->histogram  = current.request->histogram;
    req->request->sharpness  = current.request->sharpness;
    req->histogram.data = NULL;
    req->sharpness.data = NULL;

    // push the bubble into the pipe
    requestQueue.push(req);
    }
    pthread_mutex_unlock(&requestQueueMutex);

    // Check if the next request requires a mode switch
    if (req->request->image.size != current.request->image.size ||
    req->request->image.type != current.request->image.type ||
    req->request->histogram  != current.request->histogram  ||
    req->request->sharpness  != current.request->sharpness) {

    printf("Setter: Request %d %d\n", req->request->image.size.width, req->request->image.size.height);
    printf("Setter: Current %d %d\n", current.request->image.size.width, current.request->image.size.height);
    
    // flush the pipeline
    printf("Setter: Mode switch required - flushing pipe\n");
    pipelineFlush = true;

    pthread_mutex_lock(&cameraMutex);
    printf("Setter: Handler done flushing pipe, passing control back to setter\n");
    
    // do the mode switch
    
    if (current.image.size.width > 0) {
        printf("Setter: Shutting down camera\n");
        sensor->stopStreaming();
        sensor->close();
    }
    printf("Setter: Starting up camera in new mode\n");
    sensor->open();
    
    // set all the params for the new frame
    V4L2Sensor::Mode m;
    m.width  = req->request->image.size.width;
    m.height = req->request->image.size.height;
    m.type   = req->request->image.type;
    sensor->startStreaming(m, &req->request->histogram, &req->request->sharpness);
    sensor->setFrameTime(0);
    
    printf("Setter: Setter done bringing up camera, passing control back "
           "to handler. Expect two mystery frames.\n");
    pipelineFlush = false;
    pthread_mutex_unlock(&cameraMutex);
    
    m = sensor->getMode();
    req->image.size.width  = m.width;
    req->image.size.height = m.height;
    req->image.type        = m.type;
    
    current.request->image.type = req->request->image.type;
    current.request->image.size = req->request->image.size;
    current.request->histogram  = req->request->histogram;
    current.request->sharpness  = req->request->sharpness;
    
    // make sure we set everything else for the next frame,
    // because restarting streaming will have nuked it
    current.request->frameTime = -1;
    current.request->exposure = -1;
    current.request->gain = -1.0;
    
    current.image.size.width = m.width;
    current.image.size.height = m.height;
    current.image.type = m.type;
    
    req = NULL;
    
    // Wait for the second HS_VS before proceeding
    ignoreNextHSVS = true;     
    
    return;
    } else {
    //printf("Setter: no mode switch required\n");
    }

    // pop the request 
    pthread_mutex_lock(&requestQueueMutex);
    requestQueue.pop();
    pthread_mutex_unlock(&requestQueueMutex);    

    int exposure = req->request->exposure;
    int frameTime = req->request->frameTime;

    if (frameTime < exposure + 400) {
    frameTime = exposure + 400;
    }

    // Set the exposure
    sensor->setFrameTime(frameTime);
    sensor->setExposure(exposure);
    
    // Tag the request with the actual params. Also store them in
    // current to avoid unnecessary I2C.
    current.request->frameTime = frameTime;
    current.request->exposure  = exposure;
    current.exposure  = req->exposure  = sensor->getExposure();
    current.frameTime = req->frameTime = sensor->getFrameTime();
    req->image.size  = current.image.size;
    req->image.type  = current.image.type;
    //req->histogram = req->request->histogram;
    //req->sharpness = req->request->sharpness;

    // Exposure and frame time are set. Return and wait for the next
    // HS_VS before setting gain for this request-> (and pulling the next request).
    
}


void Daemon::runHandler() {
    Frame *req = NULL;       
    V4L2Sensor::V4L2Frame *f = NULL;

    pthread_mutex_lock(&cameraMutex);

    while (!stop) {

    // the setter may be waiting for me to finish processing
    // outstanding requests
    if (!req && pipelineFlush && inFlightQueue.size() == 0) {   
        printf("Handler: Handler done flushing pipe, passing control back to setter\n");
        while (pipelineFlush) {
        pthread_mutex_unlock(&cameraMutex);
        // let the setter grab the mutex. It has higher priority,
        // so it should happen instantly. We put this in a while
        // loop just to be sure.
        usleep(10000);
        pthread_mutex_lock(&cameraMutex);
        }
        printf("Handler: Setter done bringing up camera, passing control back to handler\n");
        
    }
    
    if (pipelineFlush) {
        printf("Handler: Setter would like me to flush the pipeline, but I have requests in flight\n");
    }
    
    // wait for a frame
    if (!f)
        f = sensor->acquireFrame(true);

    if (!f) {
        printf("Handler got a NULL frame\n");
        usleep(300000);
        continue;
    } 
    
    // grab a request to match to it
    if (!req) {
        if (inFlightQueue.size()) {
        //.printf("Handler: popping a frame request\n");
        pthread_mutex_lock(&inFlightQueueMutex);
        req = inFlightQueue.front();
        inFlightQueue.pop();
        pthread_mutex_unlock(&inFlightQueueMutex);
        } else {
        // there's no request for this frame - probably coming up
        // from a mode switch or starting up
        printf("Handler: Got a frame without an outstanding request, "
               "dropping it.\n");
        sensor->releaseFrame(f);
        f = NULL;
        continue;
        }
    }
        
    // at this point we have a frame and a request, now look at
    // the time delta between them to see if they're a match
    int dt = req->processingDoneTime - f->processingDoneTime;
    
    //printf("Handler dt = %d\n", dt);
    
    if (dt < -25000) { // more than 25 ms late
        printf("Handler: Expected a frame at %d %d, but one didn't arrive until %d %d\n",
           req->processingDoneTime.s(), req->processingDoneTime.us(),
           f->processingDoneTime.s(), f->processingDoneTime.us());
        req->image.setData(Image::Discard);
        if (!req->request->wanted) {
        delete req;
        } else {
        // the histogram and sharpness map may still have appeared
        sensor->getHistogram(req->exposureEndTime, &req->histogram, &req->request->histogram);
        sensor->getSharpnessMap(req->exposureEndTime, &req->sharpness, &req->request->sharpness);              
        pthread_mutex_lock(&frameQueueMutex);
        frameQueue.push(req);
        pthread_mutex_unlock(&frameQueueMutex);
        sem_post(&frameQueueSemaphore);     
        enforceDropPolicy();
        }
        req = NULL;
    } else if (dt < 10000) {
        // Is this frame wanted or a bubble?
        if (!req->request->wanted) {
        // it's a bubble - drop it
        //printf("Handler: discarding a bubble\n");
        delete req;
        sensor->releaseFrame(f);
        } else {
        
        // this looks like a match - bag and tag it
        req->processingDoneTime = f->processingDoneTime;

        size_t bytes = req->image.size.width*req->image.size.height*2;
        if (f->length < bytes) bytes = f->length;

        if (req->shot().image.data == Image::AutoAllocate) {
            req->image = Image(req->image.size, req->image.type);
            memcpy(req->image.data, f->data, bytes);
        } else if (req->shot().image.data == Image::Discard) {
            req->image.setData(Image::Discard);
        } else {
            if (req->image.size != req->shot().image.size) {
            printf("ERROR: Requested image size (%d x %d) "
                   "on an already allocated image does not "
                   "match actual image size (%d x %d). Dropping image data.\n",
                   req->shot().image.size.width, req->shot().image.size.height,
                   req->image.size.width, req->image.size.height);
            req->image.setData(Image::Discard);
            } else { // the size matches
            req->image = req->shot().image;
            // figure out how long I can afford to wait
            // For now, 10000 us should be safe
            Time lockStart = Time::now();
            if (req->image.lock(10000)) {
                memcpy(req->image.data, f->data, bytes);
                req->image.unlock();
            } else {
                printf("WARNING: Daemon discarding image data (target is still locked, "
                   "waited for %d us)\n", Time::now() - lockStart);
                req->image.setData(Image::Discard);
            }
            }
        }

        sensor->releaseFrame(f);
        sensor->getHistogram(req->exposureEndTime, &req->histogram, &req->request->histogram);
        sensor->getSharpnessMap(req->exposureEndTime, &req->sharpness, &req->request->sharpness);
        
        pthread_mutex_lock(&frameQueueMutex);
        frameQueue.push(req);
        pthread_mutex_unlock(&frameQueueMutex);
        sem_post(&frameQueueSemaphore);
        enforceDropPolicy();

        /*
        printf("Handler: finalized a request for a %d x %d frame\n", req->image.size.width, req->image.size.height);
        printf("         frame was expected at %d %d and arrived at %d %d\n", 
               req->processingDoneTime.s(), req->processingDoneTime.us(),
               f->processingDoneTime.s(), f->processingDoneTime.us());
        */
        }
        
        req = NULL;
        f = NULL;

    } else { // more than 10ms early... something weird's going on
        printf("Handler: Received an early mystery frame (%d %d) vs (%d %d), dropping it.\n",
           f->processingDoneTime.s(), f->processingDoneTime.us(),
           req->processingDoneTime.s(), req->processingDoneTime.us());
        sensor->releaseFrame(f);
        f = NULL;
    }
    
    }
    pthread_mutex_unlock(&cameraMutex);
    
}


void Daemon::runAction() {
    printf("Action thread running...\n");
    while (1) {       
    sem_wait(&actionQueueSemaphore);
    if (stop) break;
    // priority inversion :(
    pthread_mutex_lock(&actionQueueMutex);
    Action a = actionQueue.top();
    actionQueue.pop();
    pthread_mutex_unlock(&actionQueueMutex);

    Time t = Time::now();
    int delay = (a.time - t) - 500;
    if (delay > 0) usleep(delay);
    Time before = Time::now();
    // busy wait until go time
    while (a.time > before) before = Time::now();
    a.action->doAction();
    //Time after = Time::now();
    printf("Action thread: Initiated action %d us after scheduled time\n", before - a.time);
    delete a.action;
    }
}

}}
 

---