lib/mjpeg_encoder: Add an MJPEG encoder for YUV420 data

MJPEG is an extremely useful format given its compression allows
high framerates even over limited bandwith USB connections. Add
an MJPEG encoder class that converts YUV420 data into MJPEG data.

Where a libcamera-source does not support MJPEG natively, convert
YUV420 into MJPEG if the user tries to set MJPEG format.

Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
Signed-off-by: Daniel Scally <dan.scally@ideasonboard.com>

3 files changed, 330 insertions, 4 deletions

diff --git a/lib/libcamera-source.cpp b/lib/libcamera-source.cpp
index 4e22688..30b67f0 100644
--- a/lib/libcamera-source.cpp
+++ b/lib/libcamera-source.cpp
@@ -16,10 +16,15 @@
 #include <string>
 #include <string.h>
 #include <unistd.h>
+#include <map>
+#include <sys/mman.h>
 
 #include <libcamera/libcamera.h>
 #include <linux/videodev2.h>
 
+#include "config.h"
+#include "mjpeg_encoder.hpp"
+
 extern "C" {
 #include "events.h"
 #include "libcamera-source.h"
@@ -28,6 +33,7 @@ extern "C" {
 }
 
 using namespace libcamera;
+using namespace std::placeholders;
 
 #define to_libcamera_source(s) container_of(s, struct libcamera_source, src)
 
@@ -44,11 +50,35 @@ struct libcamera_source {
 	std::queue<Request *> completed_requests;
 	int pfds[2];
 
+	MjpegEncoder *encoder;
+	std::unordered_map<FrameBuffer *, Span<uint8_t>> mapped_buffers_;
+
 	struct video_buffer_set buffers;
 
+	void mapBuffer(const std::unique_ptr<FrameBuffer> &buffer);
 	void requestComplete(Request *request);
+	void outputReady(void *mem, size_t bytesused, int64_t timestamp, unsigned int cookie);
 };
 
+void libcamera_source::mapBuffer(const std::unique_ptr<FrameBuffer> &buffer)
+{
+	size_t buffer_size = 0;
+
+	for (unsigned int i = 0; i < buffer->planes().size(); i++) {
+		const FrameBuffer::Plane &plane = buffer->planes()[i];
+		buffer_size += plane.length;
+
+		if (i == buffer->planes().size() - 1 ||
+			plane.fd.get() != buffer->planes()[i + 1].fd.get()) {
+			void *memory = mmap(NULL, buffer_size, PROT_READ | PROT_WRITE,
+						MAP_SHARED, plane.fd.get(), 0);
+			mapped_buffers_[buffer.get()] =
+				Span<uint8_t>(static_cast<uint8_t *>(memory), buffer_size);
+			buffer_size = 0;
+		}
+	}
+}
+
 void libcamera_source::requestComplete(Request *request)
 {
 	if (request->status() == Request::RequestCancelled)
@@ -65,9 +95,22 @@ void libcamera_source::requestComplete(Request *request)
 	write(pfds[1], "x", 1);
 };
 
+void libcamera_source::outputReady(void *mem, size_t bytesused, int64_t timestamp, unsigned int cookie)
+{
+	struct video_buffer buffer;
+
+	buffer.index = cookie;
+	buffer.mem = mem;
+	buffer.bytesused = bytesused;
+	buffer.timestamp = { timestamp / 1000000, timestamp % 1000000 };
+
+	src.handler(src.handler_data, &src, &buffer);
+}
+
 static void libcamera_source_video_process(void *d)
 {
 	struct libcamera_source *src = (struct libcamera_source *)d;
+	Stream *stream = src->config->at(0).stream();
 	struct video_buffer buffer;
 	Request *request;
 
@@ -80,6 +123,25 @@ static void libcamera_source_video_process(void *d)
 	/* We have only a single buffer per request, so just pick the first */
 	FrameBuffer *framebuf = request->buffers().begin()->second;
 
+	/*
+	 * If we have an encoder, then rather than simply detailing the buffer
+	 * here and passing it back to the sink we need to queue it to the
+	 * encoder. The encoder will queue that buffer to the sink after
+	 * compression.
+	 */
+	if (src->src.type == VIDEO_SOURCE_ENCODED) {
+		int64_t timestamp_ns = framebuf->metadata().timestamp;
+		StreamInfo info = src->encoder->getStreamInfo(stream);
+		auto span = src->mapped_buffers_.find(framebuf);
+		void *mem = span->second.data();
+		void *dest = src->buffers.buffers[request->cookie()].mem;
+		unsigned int size = span->second.size();
+
+		src->encoder->EncodeBuffer(mem, dest, size, info, timestamp_ns / 1000, request->cookie());
+
+		return;
+	}
+
 	buffer.index = request->cookie();
 
 	/* TODO: Correct this for formats libcamera treats as multiplanar */
@@ -113,13 +175,33 @@ static int libcamera_source_set_format(struct video_source *s,
 {
 	struct libcamera_source *src = to_libcamera_source(s);
 	StreamConfiguration &streamConfig = src->config->at(0);
+	__u32 chosen_pixelformat = fmt->pixelformat;
 
 	streamConfig.size.width = fmt->width;
 	streamConfig.size.height = fmt->height;
-	streamConfig.pixelFormat = PixelFormat(fmt->pixelformat);
+	streamConfig.pixelFormat = PixelFormat(chosen_pixelformat);
 
 	src->config->validate();
 
+#ifdef CONFIG_CAN_ENCODE
+	/*
+	 * If the user requests MJPEG but the camera can't supply it, try again
+	 * with YUV420 and initialise an MjpegEncoder to compress the data.
+	 */
+	if (chosen_pixelformat == V4L2_PIX_FMT_MJPEG &&
+	    streamConfig.pixelFormat.fourcc() != chosen_pixelformat) {
+		std::cout << "MJPEG format not natively supported; encoding YUV420" << std::endl;
+
+		src->encoder = new MjpegEncoder();
+		src->encoder->SetOutputReadyCallback(std::bind(&libcamera_source::outputReady, src, _1, _2, _3, _4));
+
+		streamConfig.pixelFormat = PixelFormat(V4L2_PIX_FMT_YUV420);
+		src->src.type = VIDEO_SOURCE_ENCODED;
+
+		src->config->validate();
+	}
+#endif
+
 	if (fmt->pixelformat != streamConfig.pixelFormat.fourcc())
 		std::cerr << "Warning: set_format: Requested format unavailable" << std::endl;
 
@@ -132,7 +214,7 @@ static int libcamera_source_set_format(struct video_source *s,
 
 	fmt->width = streamConfig.size.width;
 	fmt->height = streamConfig.size.height;
-	fmt->pixelformat = streamConfig.pixelFormat.fourcc();
+	fmt->pixelformat = src->encoder ? V4L2_PIX_FMT_MJPEG : streamConfig.pixelFormat.fourcc();
 	fmt->field = V4L2_FIELD_ANY;
 
 	/* TODO: Can we use libcamera helpers to get image size / stride? */
@@ -181,6 +263,11 @@ static int libcamera_source_alloc_buffers(struct video_source *s, unsigned int n
 	const std::vector<std::unique_ptr<FrameBuffer>> &buffers = allocator->buffers(stream);
 	src->buffers.nbufs = buffers.size();
 
+	if (src->src.type == VIDEO_SOURCE_ENCODED) {
+		for (const std::unique_ptr<FrameBuffer> &buffer : buffers)
+			src->mapBuffer(buffer);
+	}
+
 	src->buffers.buffers = (video_buffer *)calloc(src->buffers.nbufs, sizeof(*src->buffers.buffers));
 	if (!src->buffers.buffers) {
 		std::cerr << "failed to allocate buffers" << std::endl;
@@ -243,6 +330,11 @@ static int libcamera_source_free_buffers(struct video_source *s)
 	struct libcamera_source *src = to_libcamera_source(s);
 	Stream *stream = src->config->at(0).stream();
 
+	for (auto &[buf, span] : src->mapped_buffers_)
+		munmap(span.data(), span.size());
+
+	src->mapped_buffers_.clear();
+
 	src->allocator->free(stream);
 	delete src->allocator;
 	free(src->buffers.buffers);
@@ -313,6 +405,18 @@ static int libcamera_source_stream_off(struct video_source *s)
 	while (!src->completed_requests.empty())
 		src->completed_requests.pop();
 
+	if (src->src.type == VIDEO_SOURCE_ENCODED) {
+		delete src->encoder;
+		src->encoder = nullptr;
+	}
+
+	/*
+	 * We need to reinitialise this here, as if the user selected an
+	 * unsupported MJPEG format the encoding routine will have overriden
+	 * this setting.
+	 */
+	src->src.type = VIDEO_SOURCE_DMABUF;
+
 	return 0;
 }
 
@@ -403,6 +507,7 @@ struct video_source *libcamera_source_create(const char *devname)
 
 	src->src.ops = &libcamera_source_ops;
 	src->src.type = VIDEO_SOURCE_DMABUF;
+
 	src->cm = std::make_unique<CameraManager>();
 	src->cm->start();
 
diff --git a/lib/meson.build b/lib/meson.build
index 0931eef..f2626fa 100644
--- a/lib/meson.build
+++ b/lib/meson.build
@@ -19,12 +19,16 @@ if libcamera.found()
   libuvcgadget_sources += files(['libcamera-source.cpp'])
 endif
 
+if libjpeg.found() and threads.found()
+  libuvcgadget_sources += files(['mjpeg_encoder.cpp'])
+endif
+
 libuvcgadget = shared_library('uvcgadget',
                               libuvcgadget_sources,
-                              dependencies : libcamera,
+                              dependencies : [libcamera, libjpeg, threads],
                               version : uvc_gadget_version,
                               install : true,
-                              include_directories : includes)
+                              include_directories : [includes, config_includes])
 
 libuvcgadget_dep = declare_dependency(sources : [
                                           uvcgadget_public_headers,
diff --git a/lib/mjpeg_encoder.cpp b/lib/mjpeg_encoder.cpp
new file mode 100644
index 0000000..41a13d5
--- /dev/null
+++ b/lib/mjpeg_encoder.cpp
@@ -0,0 +1,217 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, Raspberry Pi (Trading) Ltd.
+ *
+ * mjpeg_encoder.cpp - mjpeg video encoder.
+ */
+
+#include <chrono>
+#include <iostream>
+#include <pthread.h>
+
+#include <jpeglib.h>
+
+#include <libcamera/libcamera.h>
+
+#include "mjpeg_encoder.hpp"
+
+#if JPEG_LIB_VERSION_MAJOR > 9 || (JPEG_LIB_VERSION_MAJOR == 9 && JPEG_LIB_VERSION_MINOR >= 4)
+typedef size_t jpeg_mem_len_t;
+#else
+typedef unsigned long jpeg_mem_len_t;
+#endif
+
+MjpegEncoder::MjpegEncoder()
+	: abortEncode_(false), abortOutput_(false), index_(0)
+{
+	output_thread_ = std::thread(&MjpegEncoder::outputThread, this);
+	for (int i = 0; i < NUM_ENC_THREADS; i++)
+		encode_thread_[i] = std::thread(std::bind(&MjpegEncoder::encodeThread, this, i));
+}
+
+MjpegEncoder::~MjpegEncoder()
+{
+	abortEncode_ = true;
+	for (int i = 0; i < NUM_ENC_THREADS; i++)
+		encode_thread_[i].join();
+	abortOutput_ = true;
+	output_thread_.join();
+}
+
+void MjpegEncoder::EncodeBuffer(void *mem, void *dest, unsigned int size,
+				StreamInfo const &info, int64_t timestamp_us,
+				unsigned int cookie)
+{
+	std::lock_guard<std::mutex> lock(encode_mutex_);
+	EncodeItem item = { mem, dest, size, info, timestamp_us, index_++, cookie };
+
+	encode_queue_.push(item);
+	encode_cond_var_.notify_all();
+}
+
+void MjpegEncoder::encodeJPEG(struct jpeg_compress_struct &cinfo, EncodeItem &item,
+			      uint8_t *&encoded_buffer, size_t &buffer_len)
+{
+	cinfo.image_width = item.info.width;
+	cinfo.image_height = item.info.height;
+	cinfo.input_components = 3;
+	cinfo.in_color_space = JCS_YCbCr;
+	cinfo.restart_interval = 0;
+
+	jpeg_set_defaults(&cinfo);
+	cinfo.raw_data_in = TRUE;
+	jpeg_set_quality(&cinfo, 50, TRUE);
+
+	jpeg_mem_len_t jpeg_mem_len = buffer_len;
+	jpeg_mem_dest(&cinfo, &encoded_buffer, &jpeg_mem_len);
+	jpeg_start_compress(&cinfo, TRUE);
+
+	int stride2 = item.info.stride / 2;
+	uint8_t *Y = (uint8_t *)item.mem;
+	uint8_t *U = (uint8_t *)Y + item.info.stride * item.info.height;
+	uint8_t *V = (uint8_t *)U + stride2 * (item.info.height / 2);
+	uint8_t *Y_max = U - item.info.stride;
+	uint8_t *U_max = V - stride2;
+	uint8_t *V_max = U_max + stride2 * (item.info.height / 2);
+
+	JSAMPROW y_rows[16];
+	JSAMPROW u_rows[8];
+	JSAMPROW v_rows[8];
+
+	for (uint8_t *Y_row = Y, *U_row = U, *V_row = V; cinfo.next_scanline < item.info.height;)
+	{
+		for (int i = 0; i < 16; i++, Y_row += item.info.stride)
+			y_rows[i] = std::min(Y_row, Y_max);
+		for (int i = 0; i < 8; i++, U_row += stride2, V_row += stride2) {
+			u_rows[i] = std::min(U_row, U_max);
+			v_rows[i] = std::min(V_row, V_max);
+		}
+
+		JSAMPARRAY rows[] = { y_rows, u_rows, v_rows };
+		jpeg_write_raw_data(&cinfo, rows, 16);
+	}
+
+	jpeg_finish_compress(&cinfo);
+
+	buffer_len = jpeg_mem_len;
+}
+
+void MjpegEncoder::encodeThread(int num)
+{
+	struct jpeg_compress_struct cinfo;
+	struct jpeg_error_mgr jerr;
+	EncodeItem encode_item;
+	uint32_t frames = 0;
+
+	cinfo.err = jpeg_std_error(&jerr);
+	jpeg_create_compress(&cinfo);
+
+	while (true)
+	{
+		{
+			std::unique_lock<std::mutex> lock(encode_mutex_);
+			while (true)
+			{
+				using namespace std::chrono_literals;
+				if (abortEncode_ && encode_queue_.empty())
+				{
+					jpeg_destroy_compress(&cinfo);
+					return;
+				}
+				if (!encode_queue_.empty())
+				{
+					encode_item = encode_queue_.front();
+					encode_queue_.pop();
+					break;
+				}
+				else
+					encode_cond_var_.wait_for(lock, 200ms);
+			}
+		}
+
+		uint8_t *encoded_buffer = (uint8_t *)encode_item.dest;
+		size_t buffer_len = encode_item.size;
+
+		encodeJPEG(cinfo, encode_item, encoded_buffer, buffer_len);
+
+		frames++;
+
+		/*
+		 * Don't return buffers until the output thread as that's where
+		 * they're in order again.
+		 *
+		 * We push this encoded buffer to another thread so that our
+		 * application can take its time with the data without blocking
+		 * the encode process.
+		 */
+		OutputItem output_item = {
+			encoded_buffer,
+			buffer_len,
+			encode_item.timestamp_us,
+			encode_item.index,
+			encode_item.cookie
+		};
+		std::lock_guard<std::mutex> lock(output_mutex_);
+		output_queue_[num].push(output_item);
+		output_cond_var_.notify_one();
+	}
+}
+
+void MjpegEncoder::outputThread()
+{
+	OutputItem item;
+	uint64_t index = 0;
+	while (true)
+	{
+		{
+			std::unique_lock<std::mutex> lock(output_mutex_);
+			while (true)
+			{
+				using namespace std::chrono_literals;
+
+				/*
+				 * We look for the thread that's completed the
+				 * frame we want next. If we don't find it, we
+				 * wait.
+				 *
+				 * Must also check for an abort signal and if
+				 * set, all queues must be empty. This is done
+				 * first to ensure all frame callbacks have a
+				 * chance to run.
+				 */
+				bool abort = abortOutput_ ? true : false;
+				for (auto &q : output_queue_)
+				{
+					if (abort && !q.empty())
+						abort = false;
+
+					if (!q.empty() && q.front().index == index)
+					{
+						item = q.front();
+						q.pop();
+						goto got_item;
+					}
+				}
+				if (abort)
+					return;
+
+				output_cond_var_.wait_for(lock, 200ms);
+			}
+		}
+	got_item:
+		output_ready_callback_(item.mem, item.bytes_used, item.timestamp_us, item.cookie);
+		index++;
+	}
+}
+
+StreamInfo MjpegEncoder::getStreamInfo(libcamera::Stream *stream)
+{
+	libcamera::StreamConfiguration const &cfg = stream->configuration();
+	StreamInfo info;
+	info.width = cfg.size.width;
+	info.height = cfg.size.height;
+	info.stride = cfg.stride;
+	info.pixel_format = cfg.pixelFormat;
+	info.colour_space = cfg.colorSpace;
+	return info;
+}
\ No newline at end of file