[PATCH] libcamera: dma_heaps: Add support for using udmabuf to alloc DMA heaps

[Hans de Goede]

Add support for using udmabuf to alloc DMA heaps, this is basically:
https://patchwork.libcamera.org/patch/18922/

ported from the never merged HeapAllocator class to the current DmaHeap
class.

Co-developed-by: Harvey Yang <chenghaoyang at chromium.org>
Signed-off-by: Harvey Yang <chenghaoyang at chromium.org>
Signed-off-by: Hans de Goede <hdegoede at redhat.com>
---
 include/libcamera/internal/dma_heaps.h |   3 +
 src/libcamera/dma_heaps.cpp            | 127 +++++++++++++++++++++----
 2 files changed, 109 insertions(+), 21 deletions(-)

diff --git a/include/libcamera/internal/dma_heaps.h b/include/libcamera/internal/dma_heaps.h
index f0a8aa5d..7e1271ed 100644
--- a/include/libcamera/internal/dma_heaps.h
+++ b/include/libcamera/internal/dma_heaps.h
@@ -30,7 +30,10 @@ public:
 	UniqueFD alloc(const char *name, std::size_t size);
 
 private:
+	UniqueFD allocFromHeap(const char *name, std::size_t size);
+	UniqueFD allocFromUDmaBuf(const char *name, std::size_t size);
 	UniqueFD dmaHeapHandle_;
+	bool useUDmaBuf_;
 };
 
 LIBCAMERA_FLAGS_ENABLE_OPERATORS(DmaHeap::DmaHeapFlag)
diff --git a/src/libcamera/dma_heaps.cpp b/src/libcamera/dma_heaps.cpp
index d4cb880b..bb707786 100644
--- a/src/libcamera/dma_heaps.cpp
+++ b/src/libcamera/dma_heaps.cpp
@@ -10,10 +10,14 @@
 #include <array>
 #include <fcntl.h>
 #include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
 #include <unistd.h>
 
 #include <linux/dma-buf.h>
 #include <linux/dma-heap.h>
+#include <linux/udmabuf.h>
 
 #include <libcamera/base/log.h>
 
@@ -36,13 +40,15 @@ namespace libcamera {
 struct DmaHeapInfo {
 	DmaHeap::DmaHeapFlag type;
 	const char *deviceNodeName;
+	bool useUDmaBuf;
 };
 #endif
 
-static constexpr std::array<DmaHeapInfo, 3> heapInfos = { {
-	{ DmaHeap::DmaHeapFlag::Cma, "/dev/dma_heap/linux,cma" },
-	{ DmaHeap::DmaHeapFlag::Cma, "/dev/dma_heap/reserved" },
-	{ DmaHeap::DmaHeapFlag::System, "/dev/dma_heap/system" },
+static constexpr std::array<DmaHeapInfo, 4> heapInfos = { {
+	{ DmaHeap::DmaHeapFlag::Cma, "/dev/dma_heap/linux,cma", false },
+	{ DmaHeap::DmaHeapFlag::Cma, "/dev/dma_heap/reserved", false },
+	{ DmaHeap::DmaHeapFlag::System, "/dev/dma_heap/system", false },
+	{ DmaHeap::DmaHeapFlag::System, "/dev/udmabuf", true },
 } };
 
 LOG_DEFINE_CATEGORY(DmaHeap)
@@ -105,6 +111,7 @@ DmaHeap::DmaHeap(DmaHeapFlags type)
 
 		LOG(DmaHeap, Debug) << "Using " << info.deviceNodeName;
 		dmaHeapHandle_ = UniqueFD(ret);
+		useUDmaBuf_ = info.useUDmaBuf;
 		break;
 	}
 
@@ -123,25 +130,10 @@ DmaHeap::~DmaHeap() = default;
  * \return True if the DmaHeap is valid, false otherwise
  */
 
-/**
- * \brief Allocate a dma-buf from the DmaHeap
- * \param [in] name The name to set for the allocated buffer
- * \param [in] size The size of the buffer to allocate
- *
- * Allocates a dma-buf with read/write access.
- *
- * If the allocation fails, return an invalid UniqueFD.
- *
- * \return The UniqueFD of the allocated buffer
- */
-UniqueFD DmaHeap::alloc(const char *name, std::size_t size)
+UniqueFD DmaHeap::allocFromHeap(const char *name, std::size_t size)
 {
-	int ret;
-
-	if (!name)
-		return {};
-
 	struct dma_heap_allocation_data alloc = {};
+	int ret;
 
 	alloc.len = size;
 	alloc.fd_flags = O_CLOEXEC | O_RDWR;
@@ -162,4 +154,97 @@ UniqueFD DmaHeap::alloc(const char *name, std::size_t size)
 	return allocFd;
 }
 
+UniqueFD DmaHeap::allocFromUDmaBuf(const char *name, std::size_t size)
+{
+	/* size must be a multiple of the page-size round it up */
+	std::size_t pageMask = sysconf(_SC_PAGESIZE) - 1;
+	size = (size + pageMask) & ~pageMask;
+
+	int ret = memfd_create(name, MFD_ALLOW_SEALING);
+	if (ret < 0) {
+		ret = errno;
+		LOG(DmaHeap, Error)
+			<< "UdmaHeap failed to allocate memfd storage: "
+			<< strerror(ret);
+		return {};
+	}
+
+	UniqueFD memfd(ret);
+
+	ret = ftruncate(memfd.get(), size);
+	if (ret < 0) {
+		ret = errno;
+		LOG(DmaHeap, Error)
+			<< "UdmaHeap failed to set memfd size: " << strerror(ret);
+		return {};
+	}
+
+	/* UdmaHeap Buffers *must* have the F_SEAL_SHRINK seal */
+	ret = fcntl(memfd.get(), F_ADD_SEALS, F_SEAL_SHRINK);
+	if (ret < 0) {
+		ret = errno;
+		LOG(DmaHeap, Error)
+			<< "UdmaHeap failed to seal the memfd: " << strerror(ret);
+		return {};
+	}
+
+	struct udmabuf_create create;
+
+	create.memfd = memfd.get();
+	create.flags = UDMABUF_FLAGS_CLOEXEC;
+	create.offset = 0;
+	create.size = size;
+
+	ret = ::ioctl(dmaHeapHandle_.get(), UDMABUF_CREATE, &create);
+	if (ret < 0) {
+		ret = errno;
+		LOG(DmaHeap, Error)
+			<< "UdmaHeap failed to allocate " << size << " bytes: "
+			<< strerror(ret);
+		return {};
+	}
+
+	if (create.size < size) {
+		LOG(DmaHeap, Error)
+			<< "UdmaHeap allocated " << create.size << " bytes instead of "
+			<< size << " bytes";
+		return {};
+	}
+
+	if (create.size != size)
+		LOG(DmaHeap, Warning)
+			<< "UdmaHeap allocated " << create.size << " bytes, "
+			<< "which is greater than requested : " << size << " bytes";
+
+	/* Fail if not suitable, the allocation will be free'd by UniqueFD */
+	LOG(DmaHeap, Debug) << "UdmaHeap allocated " << create.size << " bytes";
+
+	/* The underlying memfd is kept as as a reference in the kernel */
+	UniqueFD uDma(ret);
+
+	return uDma;
+}
+
+/**
+ * \brief Allocate a dma-buf from the DmaHeap
+ * \param [in] name The name to set for the allocated buffer
+ * \param [in] size The size of the buffer to allocate
+ *
+ * Allocates a dma-buf with read/write access.
+ *
+ * If the allocation fails, return an invalid UniqueFD.
+ *
+ * \return The UniqueFD of the allocated buffer
+ */
+UniqueFD DmaHeap::alloc(const char *name, std::size_t size)
+{
+	if (!name)
+		return {};
+
+	if (useUDmaBuf_)
+		return allocFromUDmaBuf(name, size);
+	else
+		return allocFromHeap(name, size);
+}
+
 } /* namespace libcamera */
-- 
2.45.1