From 83c8b1da864e2ed9360649dd3482d775ace397e7 Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Tue, 22 Mar 2022 13:33:41 +0000
Subject: [PATCH] u/sink: Make u_sink_create_format_converter support L8

Co-authored-by: Moses Turner <moses@collabora.com>
---
 src/xrt/auxiliary/util/u_sink.h           |  1 +
 src/xrt/auxiliary/util/u_sink_converter.c | 54 ++++++++++++++++++++---
 2 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/src/xrt/auxiliary/util/u_sink.h b/src/xrt/auxiliary/util/u_sink.h
index 336d71e7b..1fa690c2d 100644
--- a/src/xrt/auxiliary/util/u_sink.h
+++ b/src/xrt/auxiliary/util/u_sink.h
@@ -170,6 +170,7 @@ u_sink_force_genlock_create(struct xrt_frame_context *xfctx,
                             struct xrt_frame_sink **out_left_xfs,
                             struct xrt_frame_sink **out_right_xfs);
 
+
 /*
  *
  * Debugging sink,
diff --git a/src/xrt/auxiliary/util/u_sink_converter.c b/src/xrt/auxiliary/util/u_sink_converter.c
index cb6c3f95e..0d1ae8bc8 100644
--- a/src/xrt/auxiliary/util/u_sink_converter.c
+++ b/src/xrt/auxiliary/util/u_sink_converter.c
@@ -232,6 +232,21 @@ from_YUYV422_to_R8G8B8(struct xrt_frame *dst_frame, uint32_t w, uint32_t h, size
 	}
 }
 
+static void
+from_YUYV422_to_L8(struct xrt_frame *dst_frame, uint32_t w, uint32_t h, size_t stride, const uint8_t *data)
+{
+	SINK_TRACE_MARKER();
+
+	for (uint32_t y = 0; y < h; y++) {
+		const uint8_t *src = data + (y * stride);
+		uint8_t *dst = dst_frame->data + (y * dst_frame->stride);
+
+		for (uint32_t x = 0; x < w; x++) {
+			dst[x] = src[x * 2];
+		}
+	}
+}
+
 static void
 from_UYVY422_to_R8G8B8(struct xrt_frame *dst_frame, uint32_t w, uint32_t h, size_t stride, const uint8_t *data)
 {
@@ -461,6 +476,32 @@ create_frame_with_format(struct xrt_frame *xf, enum xrt_format format, struct xr
 	return create_frame_with_format_of_size(xf, xf->width, xf->height, format, out_frame);
 }
 
+static void
+convert_frame_l8(struct xrt_frame_sink *xs, struct xrt_frame *xf)
+{
+	SINK_TRACE_MARKER();
+
+	struct u_sink_converter *s = (struct u_sink_converter *)xs;
+
+	struct xrt_frame *converted = NULL;
+
+	switch (xf->format) {
+	case XRT_FORMAT_L8: s->downstream->push_frame(s->downstream, xf); return;
+	case XRT_FORMAT_YUYV422:
+		if (!create_frame_with_format(xf, XRT_FORMAT_L8, &converted)) {
+			return;
+		}
+		from_YUYV422_to_L8(converted, xf->width, xf->height, xf->stride, xf->data);
+		break;
+	default: U_LOG_E("Can not convert from '%s' to L8!", u_format_str(xf->format)); return;
+	}
+
+	s->downstream->push_frame(s->downstream, converted);
+
+	// Refcount in case it's being held downstream.
+	xrt_frame_reference(&converted, NULL);
+}
+
 static void
 convert_frame_r8g8b8_or_l8(struct xrt_frame_sink *xs, struct xrt_frame *xf)
 {
@@ -801,13 +842,16 @@ destroy(struct xrt_frame_node *node)
 
 void
 u_sink_create_format_converter(struct xrt_frame_context *xfctx,
-                               enum xrt_format f,
+                               enum xrt_format format,
                                struct xrt_frame_sink *downstream,
                                struct xrt_frame_sink **out_xfs)
 {
-	if (f != XRT_FORMAT_R8G8B8) {
-		U_LOG_E("Format '%s' not supported", u_format_str(f));
-		return;
+	void (*func)(struct xrt_frame_sink *, struct xrt_frame *);
+
+	switch (format) {
+	case XRT_FORMAT_R8G8B8: func = convert_frame_r8g8b8; break;
+	case XRT_FORMAT_L8: func = convert_frame_l8; break;
+	default: U_LOG_E("Format '%s' not supported", u_format_str(format)); return;
 	}
 
 #ifdef USE_TABLE
@@ -815,7 +859,7 @@ u_sink_create_format_converter(struct xrt_frame_context *xfctx,
 #endif
 
 	struct u_sink_converter *s = U_TYPED_CALLOC(struct u_sink_converter);
-	s->base.push_frame = convert_frame_r8g8b8;
+	s->base.push_frame = func;
 	s->node.break_apart = break_apart;
 	s->node.destroy = destroy;
 	s->downstream = downstream;