From 29be8034518c0babbf6334d3ef6fc9ffe30e6b70 Mon Sep 17 00:00:00 2001 From: Daniel Scally Date: Tue, 10 Jan 2023 08:49:30 +0000 Subject: 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 Signed-off-by: Daniel Scally --- lib/libcamera-source.cpp | 109 +++++++++++++++++++++++- lib/meson.build | 8 +- lib/mjpeg_encoder.cpp | 217 +++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 330 insertions(+), 4 deletions(-) create mode 100644 lib/mjpeg_encoder.cpp (limited to 'lib') 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 #include #include +#include +#include #include #include +#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 completed_requests; int pfds[2]; + MjpegEncoder *encoder; + std::unordered_map> mapped_buffers_; + struct video_buffer_set buffers; + void mapBuffer(const std::unique_ptr &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 &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(static_cast(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> &buffers = allocator->buffers(stream); src->buffers.nbufs = buffers.size(); + if (src->src.type == VIDEO_SOURCE_ENCODED) { + for (const std::unique_ptr &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(); 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 +#include +#include + +#include + +#include + +#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 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 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 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 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 -- cgit v1.2.3