support enabling a secondary stream (#44)

Author: aler9

.dockerignore

@@ -6,3 +6,4 @@
 !/subprojects/x264.wrap
 !/subprojects/libpisp.wrap
 !/subprojects/openssl.wrap
+!/subprojects/libjpeg-turbo.wrap

.gitignore

@@ -6,3 +6,4 @@
 !/subprojects/x264.wrap
 !/subprojects/libpisp.wrap
 !/subprojects/openssl.wrap
+!/subprojects/libjpeg-turbo.wrap

camera.cpp

@@ -58,6 +58,20 @@ const char *camera_get_error() {
     return errbuf;
 }
 
+static long timespec_sub(struct timespec *a, struct timespec *b) {
+    return ((a->tv_sec * 1000000000L) + a->tv_nsec) -
+        ((b->tv_sec * 1000000000L) + b->tv_nsec);
+}
+
+static void timespec_add(struct timespec *a, long nanosecs) {
+    a->tv_nsec += nanosecs;
+
+    while (a->tv_nsec > 1000000000L) {
+        a->tv_sec++;
+        a->tv_nsec -= 1000000000L;
+    }
+};
+
 // https://github.com/raspberrypi/rpicam-apps/blob/6de1ab6a899df35f929b2a15c0831780bd8e750e/core/dma_heaps.cpp
 static int create_dma_allocator() {
     static const char *heap_positions[] = {
@@ -132,14 +146,17 @@ struct CameraPriv {
     const parameters_t *params;
     camera_frame_cb frame_cb;
     camera_error_cb error_cb;
+    long secondary_deltat;
     std::unique_ptr<CameraManager> camera_manager;
     std::shared_ptr<Camera> camera;
     Stream *video_stream;
+    Stream *secondary_stream;
     std::vector<std::unique_ptr<Request>> requests;
     std::mutex ctrls_mutex;
     std::unique_ptr<ControlList> ctrls;
     std::vector<std::unique_ptr<FrameBuffer>> frame_buffers;
     std::map<FrameBuffer *, uint8_t *> mapped_buffers;
+    struct timespec last_secondary_frame_time;
     bool in_error;
 };
 
@@ -194,14 +211,19 @@ bool camera_create(
     if (params->mode != NULL) {
         stream_roles.push_back(StreamRole::Raw);
     }
+    if (params->secondary_width != 0) {
+        stream_roles.push_back(StreamRole::Viewfinder);
+    }
 
     std::unique_ptr<CameraConfiguration> conf = camp->camera->generateConfiguration(stream_roles);
     if (conf == NULL) {
         set_error("Camera.generateConfiguration() failed");
         return false;
     }
 
-    StreamConfiguration &video_stream_conf = conf->at(0);
+    int cur_stream = 0;
+
+    StreamConfiguration &video_stream_conf = conf->at(cur_stream++);
     video_stream_conf.size = Size(params->width, params->height);
     video_stream_conf.pixelFormat = formats::YUV420;
     video_stream_conf.bufferCount = params->buffer_count;
@@ -212,12 +234,19 @@ bool camera_create(
     }
 
     if (params->mode != NULL) {
-        StreamConfiguration &raw_stream_conf = conf->at(1);
+        StreamConfiguration &raw_stream_conf = conf->at(cur_stream++);
         raw_stream_conf.size = Size(params->mode->width, params->mode->height);
         raw_stream_conf.pixelFormat = mode_to_pixel_format(params->mode);
         raw_stream_conf.bufferCount = video_stream_conf.bufferCount;
     }
 
+    if (params->secondary_width != 0) {
+        StreamConfiguration &secondary_stream_conf = conf->at(cur_stream++);
+        secondary_stream_conf.size = Size(params->secondary_width, params->secondary_height);
+        secondary_stream_conf.bufferCount = video_stream_conf.bufferCount;
+        secondary_stream_conf.pixelFormat = formats::YUV420;
+    }
+
     conf->orientation = Orientation::Rotate0;
     if (params->h_flip) {
         conf->orientation = conf->orientation * Transform::HFlip;
@@ -240,6 +269,11 @@ bool camera_create(
 
     camp->video_stream = video_stream_conf.stream();
 
+    if (params->secondary_width != 0) {
+        StreamConfiguration &secondary_stream_conf = conf->at(cur_stream - 1);
+        camp->secondary_stream = secondary_stream_conf.stream();
+    }
+
     for (unsigned int i = 0; i < params->buffer_count; i++) {
         std::unique_ptr<Request> request = camp->camera->createRequest((uint64_t)camp.get());
         if (request == NULL) {
@@ -282,8 +316,7 @@ bool camera_create(
             camp->frame_buffers.push_back(std::make_unique<FrameBuffer>(plane));
             FrameBuffer *fb = camp->frame_buffers.back().get();
 
-            // map buffers of the video stream only
-            if (stream == video_stream_conf.stream()) {
+            if (stream == camp->video_stream || stream == camp->secondary_stream) {
                 camp->mapped_buffers[fb] = (uint8_t*)mmap(NULL, stream_conf.frameSize, PROT_READ | PROT_WRITE, MAP_SHARED, plane[0].fd.get(), 0);
             }
 
@@ -300,6 +333,8 @@ bool camera_create(
     camp->params = params;
     camp->frame_cb = frame_cb;
     camp->error_cb = error_cb;
+    camp->secondary_deltat = (long)(1000000000.0 / params->secondary_fps);
+    clock_gettime(CLOCK_MONOTONIC, &camp->last_secondary_frame_time);
     *cam = camp.release();
 
     return true;
@@ -328,11 +363,26 @@ static void on_request_complete(Request *request) {
 
     FrameBuffer *buffer = request->buffers().at(camp->video_stream);
 
+    uint8_t *secondary_buffer_mapped = NULL;
+    if (camp->secondary_stream != NULL) {
+        struct timespec now;
+        clock_gettime(CLOCK_MONOTONIC, &now);
+
+        long diff = timespec_sub(&now, &camp->last_secondary_frame_time);
+
+        if (diff >= camp->secondary_deltat) {
+            timespec_add(&camp->last_secondary_frame_time, camp->secondary_deltat);
+            FrameBuffer *secondary_buffer = request->buffers().at(camp->secondary_stream);
+            secondary_buffer_mapped = camp->mapped_buffers.at(secondary_buffer);
+        }
+    }
+
     camp->frame_cb(
         camp->mapped_buffers.at(buffer),
         buffer->planes()[0].fd.get(),
         buffer_size(buffer->planes()),
-        buffer->metadata().timestamp / 1000);
+        buffer->metadata().timestamp / 1000,
+        secondary_buffer_mapped);
 
     request->reuse(Request::ReuseFlag::ReuseBuffers);
 
@@ -350,6 +400,11 @@ int camera_get_stride(camera_t *cam) {
     return camp->video_stream->configuration().stride;
 }
 
+int camera_get_secondary_stride(camera_t *cam) {
+    CameraPriv *camp = (CameraPriv *)cam;
+    return camp->secondary_stream->configuration().stride;
+}
+
 int camera_get_colorspace(camera_t *cam) {
     CameraPriv *camp = (CameraPriv *)cam;
     return get_v4l2_colorspace(camp->video_stream->configuration().colorSpace);

camera.h

@@ -9,7 +9,8 @@ typedef void (*camera_frame_cb)(
     uint8_t *buffer_mapped,
     int buffer_fd,
     uint64_t buffer_size,
-    uint64_t timestamp);
+    uint64_t timestamp,
+    uint8_t *secondary_buffer_mapped);
 
 typedef void (*camera_error_cb)();
 
@@ -24,6 +25,7 @@ bool camera_create(
     camera_error_cb error_cb,
     camera_t **cam);
 int camera_get_stride(camera_t *cam);
+int camera_get_secondary_stride(camera_t *cam);
 int camera_get_colorspace(camera_t *cam);
 bool camera_start(camera_t *cam);
 void camera_reload_params(camera_t *cam, const parameters_t *params);

encoder_jpeg.c

@@ -0,0 +1,154 @@
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include <pthread.h>
+
+#include <jpeglib.h>
+
+#include "encoder_jpeg.h"
+
+static char errbuf[256];
+
+const char *encoder_jpeg_get_error() {
+    return errbuf;
+}
+
+typedef struct {
+    int width;
+    int height;
+    int quality;
+    int stride;
+    pthread_mutex_t mutex;
+    pthread_cond_t cond;
+    pthread_t thread;
+    bool data_queued;
+    uint8_t *data_buffer;
+    uint64_t data_timestamp;
+    encoder_jpeg_output_cb output_cb;
+} encoder_jpeg_priv_t;
+
+static void save_as_jpeg(
+    unsigned int width,
+    unsigned int height,
+    unsigned int quality,
+    unsigned int stride,
+    uint8_t *in_buf,
+    uint8_t **out_buf,
+    unsigned long *out_size) {
+    struct jpeg_error_mgr jerr;
+    struct jpeg_compress_struct cinfo;
+    cinfo.err = jpeg_std_error(&jerr);
+    jpeg_create_compress(&cinfo);
+
+    cinfo.image_width = width;
+    cinfo.image_height = height;
+    cinfo.input_components = 3;
+    cinfo.in_color_space = JCS_YCbCr;
+    jpeg_set_defaults(&cinfo);
+    jpeg_set_quality(&cinfo, quality, TRUE);
+
+    jpeg_mem_dest(&cinfo, out_buf, out_size);
+
+    jpeg_start_compress(&cinfo, TRUE);
+
+    JSAMPROW row_pointer[1];
+    uint8_t *row_buf = malloc(width * 3);
+    row_pointer[0] = row_buf;
+
+    uint8_t *Y = in_buf;
+    uint8_t *U = Y + stride * height;
+    uint8_t *V = U + (stride / 2) * (height / 2);
+
+    while (cinfo.next_scanline < height) {
+        for (unsigned int j = 0; j < width; j++) {
+            int i1 = cinfo.next_scanline*stride + j;
+            int i2 = cinfo.next_scanline/2*stride/2 + j/2;
+
+            row_buf[j*3 + 0] = Y[i1];
+            row_buf[j*3 + 1] = U[i2];
+            row_buf[j*3 + 2] = V[i2];
+        }
+
+        jpeg_write_scanlines(&cinfo, row_pointer, 1);
+    }
+
+    free(row_buf);
+
+    jpeg_finish_compress(&cinfo);
+    jpeg_destroy_compress(&cinfo);
+}
+
+static void *thread_main(void *userdata) {
+    encoder_jpeg_priv_t *encp = (encoder_jpeg_priv_t *)userdata;
+
+    while (true) {
+        pthread_mutex_lock(&encp->mutex);
+
+        while (!encp->data_queued) {
+            pthread_cond_wait(&encp->cond, &encp->mutex);
+        }
+
+        uint8_t *buffer = encp->data_buffer;
+        uint64_t timestamp = encp->data_timestamp;
+        encp->data_queued = false;
+
+        pthread_cond_signal(&encp->cond);
+
+        pthread_mutex_unlock(&encp->mutex);
+
+        uint8_t *out_buf;
+        unsigned long out_size = 0;
+        save_as_jpeg(encp->width, encp->height, encp->quality, encp->stride, buffer, &out_buf, &out_size);
+
+        encp->output_cb(out_buf, out_size, timestamp);
+
+        free(out_buf);
+    }
+
+    return NULL;
+}
+
+bool encoder_jpeg_create(
+    int width,
+    int height,
+    int quality,
+    int stride,
+    encoder_jpeg_output_cb output_cb,
+    encoder_jpeg_t **enc) {
+    *enc = malloc(sizeof(encoder_jpeg_priv_t));
+    encoder_jpeg_priv_t *encp = (encoder_jpeg_priv_t *)(*enc);
+    memset(encp, 0, sizeof(encoder_jpeg_priv_t));
+
+    encp->width = width;
+    encp->height = height;
+    encp->quality = quality;
+    encp->stride = stride;
+    pthread_mutex_init(&encp->mutex, NULL);
+    pthread_cond_init(&encp->cond, NULL);
+    pthread_create(&encp->thread, NULL, thread_main, encp);
+    encp->output_cb = output_cb;
+
+    return true;
+}
+
+void encoder_jpeg_encode(encoder_jpeg_t *enc, uint8_t *buffer, uint64_t timestamp) {
+    encoder_jpeg_priv_t *encp = (encoder_jpeg_priv_t *)enc;
+
+    pthread_mutex_lock(&encp->mutex);
+
+    while (encp->data_queued) {
+        pthread_cond_wait(&encp->cond, &encp->mutex);
+    }
+
+    encp->data_queued = true;
+    encp->data_buffer = buffer;
+    encp->data_timestamp = timestamp;
+
+    pthread_cond_signal(&encp->cond);
+
+    pthread_mutex_unlock(&encp->mutex);
+}

encoder_jpeg.h

@@ -0,0 +1,20 @@
+#ifndef __ENCODER_JPEG_H__
+#define __ENCODER_JPEG_H__
+
+#include "parameters.h"
+
+typedef void encoder_jpeg_t;
+
+typedef void (*encoder_jpeg_output_cb)(const uint8_t *mapped, uint64_t size, uint64_t ts);
+
+const char *encoder_jpeg_get_error();
+bool encoder_jpeg_create(
+    int width,
+    int height,
+    int quality,
+    int stride,
+    encoder_jpeg_output_cb output_cb,
+    encoder_jpeg_t **enc);
+void encoder_jpeg_encode(encoder_jpeg_t *enc, uint8_t *mapped, uint64_t ts);
+
+#endif