From b15d5b28af6ffb26c120dcac6c6d2fc30b277e37 Mon Sep 17 00:00:00 2001
From: Kamil Trzcinski <ayufan@ayufan.eu>
Date: Mon, 11 Apr 2022 16:10:30 +0200
Subject: [PATCH] Improve supported formats

---
 cmd/camera_stream.c        |  6 +++++
 device/camera/camera.c     |  9 ++++++-
 device/libcamera/buffer.cc | 51 +++++++++++++++++++++++++-------------
 device/v4l2/buffer_list.c  | 10 ++++----
 4 files changed, 53 insertions(+), 23 deletions(-)

diff --git a/cmd/camera_stream.c b/cmd/camera_stream.c
index 74f47b1..8029256 100644
--- a/cmd/camera_stream.c
+++ b/cmd/camera_stream.c
@@ -53,10 +53,16 @@ log_options_t log_options = {
 option_value_t camera_formats[] = {
   { "DEFAULT", 0 },
   { "YUYV", V4L2_PIX_FMT_YUYV },
+  { "YUV420", V4L2_PIX_FMT_YUV420 },
+  { "YUYV", V4L2_PIX_FMT_YUYV },
   { "MJPG", V4L2_PIX_FMT_MJPEG },
   { "MJPEG", V4L2_PIX_FMT_MJPEG },
   { "H264", V4L2_PIX_FMT_H264 },
   { "RG10", V4L2_PIX_FMT_SRGGB10P },
+  { "RGB565", V4L2_PIX_FMT_RGB565 },
+  { "RGBP", V4L2_PIX_FMT_RGB565 },
+  { "RGB24", V4L2_PIX_FMT_RGB24 },
+  { "RGB", V4L2_PIX_FMT_RGB24 },
   {}
 };
 
diff --git a/device/camera/camera.c b/device/camera/camera.c
index e90deba..2df0590 100644
--- a/device/camera/camera.c
+++ b/device/camera/camera.c
@@ -78,6 +78,12 @@ camera_t *camera_open(camera_options_t *options)
 
   switch (camera->camera->capture_list->fmt_format) {
   case V4L2_PIX_FMT_YUYV:
+  case V4L2_PIX_FMT_YVYU:
+  case V4L2_PIX_FMT_VYUY:
+  case V4L2_PIX_FMT_UYVY:
+  case V4L2_PIX_FMT_YUV420:
+  case V4L2_PIX_FMT_RGB565:
+  case V4L2_PIX_FMT_RGB24:
     if (camera_configure_direct(camera) < 0) {
       goto error;
     }
@@ -103,7 +109,8 @@ camera_t *camera_open(camera_options_t *options)
     break;
 
   default:
-    LOG_ERROR(camera, "Unsupported camera format=%s", fourcc_to_string(camera->options.format).buf);
+    LOG_ERROR(camera, "Unsupported camera format=%s",
+      fourcc_to_string(camera->camera->capture_list->fmt_format).buf);
     break;
   }
 
diff --git a/device/libcamera/buffer.cc b/device/libcamera/buffer.cc
index 1f954fb..80f40c4 100644
--- a/device/libcamera/buffer.cc
+++ b/device/libcamera/buffer.cc
@@ -18,24 +18,41 @@ int libcamera_buffer_open(buffer_t *buf)
     if (buf->libcamera->request->addBuffer(stream, buffer.get()) < 0) {
       LOG_ERROR(buf, "Can't set buffer for request");
     }
-
-    for (auto const &plane : buffer->planes()) {
-      if (buf->start) {
-        LOG_ERROR(buf, "Too many planes open.");
-      }
-
-      buf->start = mmap(NULL, plane.length, PROT_READ | PROT_WRITE, MAP_SHARED, plane.fd.get(), 0);
-      buf->length = plane.length;
-      buf->used = 0;
-      buf->dma_fd = plane.fd.get();
-
-      if (!buf->start) {
-        LOG_ERROR(buf, "Failed to mmap DMA buffer");
-      }
-
-      LOG_DEBUG(buf, "Mapped buffer: start=%p, length=%d, fd=%d",
-        buf->start, buf->length, buf->dma_fd);
+    if (buf->start) {
+      LOG_ERROR(buf, "Too many streams.");
     }
+
+    if (buffer->planes().empty()) {
+      LOG_ERROR(buf, "No planes allocated");
+    }
+
+    uint64_t offset = buffer->planes()[0].offset;
+    uint64_t length = 0;
+    libcamera::SharedFD dma_fd = buffer->planes()[0].fd;
+
+    // Require that planes are continuous
+    for (auto const &plane : buffer->planes()) {
+      if (plane.fd != dma_fd) {
+        LOG_ERROR(buf, "Plane does not share FD: fd=%d, expected=%d", plane.fd.get(), dma_fd.get());
+      }
+
+      if (offset + length != plane.offset) {
+        LOG_ERROR(buf, "Plane is not continuous: offset=%lld, expected=%lld", plane.offset, offset + length);
+      }
+
+      length += plane.length;
+    }
+
+    buf->start = mmap(NULL, length, PROT_READ | PROT_WRITE, MAP_SHARED, dma_fd.get(), offset);
+    if (!buf->start || buf->start == MAP_FAILED) {
+      LOG_ERROR(buf, "Failed to mmap DMA buffer");
+    }
+
+    buf->dma_fd = dma_fd.get();
+    buf->length = length;
+
+    LOG_DEBUG(buf, "Mapped buffer: start=%p, length=%d, fd=%d, planes=%d",
+      buf->start, buf->length, buf->dma_fd, buffer->planes().size());
   }
 
   return 0;
diff --git a/device/v4l2/buffer_list.c b/device/v4l2/buffer_list.c
index 51a0d44..ad265b2 100644
--- a/device/v4l2/buffer_list.c
+++ b/device/v4l2/buffer_list.c
@@ -102,11 +102,6 @@ int v4l2_buffer_list_open(buffer_list_t *buf_list, unsigned width, unsigned heig
     buf_list->fmt_bytesperline = fmt.fmt.pix.bytesperline;
   }
 
-  if (bytesperline > 0 && buf_list->fmt_bytesperline != bytesperline) {
-		LOG_ERROR(buf_list, "Requested bytesperline=%u. Got %u.",
-      bytesperline, buf_list->fmt_bytesperline);
-  }
-
   if (buf_list->fmt_width != width || buf_list->fmt_height != height) {
     if (bytesperline) {
       LOG_ERROR(buf_list, "Requested resolution=%ux%u is unavailable. Got %ux%u.",
@@ -123,6 +118,11 @@ int v4l2_buffer_list_open(buffer_list_t *buf_list, unsigned width, unsigned heig
 			fourcc_to_string(buf_list->fmt_format).buf);
 	}
 
+  if (bytesperline > 0 && buf_list->fmt_bytesperline != bytesperline) {
+		LOG_ERROR(buf_list, "Requested bytesperline=%u. Got %u.",
+      bytesperline, buf_list->fmt_bytesperline);
+  }
+
   // Some devices require setting pad size via media-controller
   if (buf_list->do_capture) {
     v4l2_device_set_pad_format(dev, width, height, format);