13 files changed, 930 insertions, 0 deletions
diff --git a/libkms++util/CMakeLists.txt b/libkms++util/CMakeLists.txt
new file mode 100644
index 0000000..d08ec84
--- /dev/null
+++ b/libkms++util/CMakeLists.txt
@@ -0,0 +1,5 @@
+file(GLOB SRCS "*.cpp" "*.h")
+add_library(kms++util ${SRCS})
+
+target_link_libraries(kms++util kms++)
+target_include_directories(kms++util PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/libkms++util/color.cpp b/libkms++util/color.cpp
new file mode 100644
index 0000000..490ff64
--- /dev/null
+++ b/libkms++util/color.cpp
@@ -0,0 +1,93 @@
+#include "color.h"
+
+namespace kms
+{
+RGB::RGB()
+{
+	r = g = b = 0;
+	a = 255;
+}
+
+RGB::RGB(uint8_t r, uint8_t g, uint8_t b)
+	:RGB(255, r, g, b)
+{
+}
+
+RGB::RGB(uint8_t a, uint8_t r, uint8_t g, uint8_t b)
+{
+	this->r = r;
+	this->g = g;
+	this->b = b;
+	this->a = a;
+}
+
+RGB::RGB(uint32_t argb)
+{
+	this->b = (argb >> 0) & 0xff;
+	this->g = (argb >> 8) & 0xff;
+	this->r = (argb >> 16) & 0xff;
+	this->a = (argb >> 24) & 0xff;
+}
+
+uint32_t RGB::rgb888() const
+{
+	return (r << 16) | (g << 8) | (b << 0);
+}
+
+uint32_t RGB::argb8888() const
+{
+	return (a << 24) | (r << 16) | (g << 8) | (b << 0);
+}
+
+uint32_t RGB::abgr8888() const
+{
+	return (a << 24) | (b << 16) | (g << 8) | (r << 0);
+}
+
+uint16_t RGB::rgb565() const
+{
+	return ((r >> 3) << 11) | ((g >> 2) << 5) | ((b >> 3) << 0);
+}
+
+YUV RGB::yuv() const
+{
+	return YUV(*this);
+}
+
+
+YUV::YUV()
+{
+	y = u = v = a = 0;
+}
+
+YUV::YUV(uint8_t y, uint8_t u, uint8_t v)
+{
+	this->y = y;
+	this->u = u;
+	this->v = v;
+	this->a = 0;
+}
+
+static inline uint8_t MAKE_YUV_601_Y(uint8_t r, uint8_t g, uint8_t b)
+{
+	return (((66 * r + 129 * g +  25 * b + 128) >> 8) + 16);
+}
+
+static inline uint8_t MAKE_YUV_601_U(uint8_t r, uint8_t g, uint8_t b)
+{
+	return (((-38 * r -  74 * g + 112 * b + 128) >> 8) + 128);
+}
+
+static inline uint8_t MAKE_YUV_601_V(uint8_t r, uint8_t g, uint8_t b)
+{
+	return (((112 * r -  94 * g -  18 * b + 128) >> 8) + 128);
+}
+
+YUV::YUV(const RGB& rgb)
+{
+	this->y = MAKE_YUV_601_Y(rgb.r, rgb.g, rgb.b);
+	this->u = MAKE_YUV_601_U(rgb.r, rgb.g, rgb.b);
+	this->v = MAKE_YUV_601_V(rgb.r, rgb.g, rgb.b);
+	this->a = rgb.a;
+}
+}
diff --git a/libkms++util/color.h b/libkms++util/color.h
new file mode 100644
index 0000000..ef85a67
--- /dev/null
+++ b/libkms++util/color.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include <cstdint>
+
+namespace kms
+{
+struct YUV;
+
+struct RGB
+{
+	RGB();
+	RGB(uint8_t r, uint8_t g, uint8_t b);
+	RGB(uint8_t a, uint8_t r, uint8_t g, uint8_t b);
+	RGB(uint32_t argb);
+
+	uint32_t rgb888() const;
+	uint32_t argb8888() const;
+	uint32_t abgr8888() const;
+	uint16_t rgb565() const;
+	YUV yuv() const;
+
+	uint8_t b;
+	uint8_t g;
+	uint8_t r;
+	uint8_t a;
+};
+
+struct YUV
+{
+	YUV();
+	YUV(uint8_t y, uint8_t u, uint8_t v);
+	YUV(const RGB& rgb);
+
+	uint8_t v;
+	uint8_t u;
+	uint8_t y;
+	uint8_t a;
+};
+}
diff --git a/libkms++util/colorbar.cpp b/libkms++util/colorbar.cpp
new file mode 100644
index 0000000..9fbec28
--- /dev/null
+++ b/libkms++util/colorbar.cpp
@@ -0,0 +1,128 @@
+#include <cstdint>
+
+#include <kms++.h>
+
+#include "test.h"
+
+namespace kms
+{
+static const RGB colors32[] = {
+	RGB(255, 255, 255),
+	RGB(255, 0, 0),
+	RGB(255, 255, 255),
+	RGB(0, 255, 0),
+	RGB(255, 255, 255),
+	RGB(0, 0, 255),
+	RGB(255, 255, 255),
+	RGB(200, 200, 200),
+	RGB(255, 255, 255),
+	RGB(100, 100, 100),
+	RGB(255, 255, 255),
+	RGB(50, 50, 50),
+};
+
+static const uint16_t colors16[] = {
+	colors32[0].rgb565(),
+	colors32[1].rgb565(),
+	colors32[2].rgb565(),
+	colors32[3].rgb565(),
+	colors32[4].rgb565(),
+	colors32[5].rgb565(),
+	colors32[6].rgb565(),
+	colors32[7].rgb565(),
+	colors32[8].rgb565(),
+	colors32[9].rgb565(),
+	colors32[10].rgb565(),
+	colors32[11].rgb565(),
+};
+
+static void drm_draw_color_bar_rgb888(IMappedFramebuffer& buf, int old_xpos, int xpos, int width)
+{
+	for (unsigned y = 0; y < buf.height(); ++y) {
+		RGB bcol = colors32[y * ARRAY_SIZE(colors32) / buf.height()];
+		uint32_t *line = (uint32_t*)(buf.map(0) + buf.stride(0) * y);
+
+		if (old_xpos >= 0) {
+			for (int x = old_xpos; x < old_xpos + width; ++x)
+				line[x] = 0;
+		}
+
+		for (int x = xpos; x < xpos + width; ++x)
+			line[x] = bcol.argb8888();
+	}
+}
+
+static void drm_draw_color_bar_rgb565(IMappedFramebuffer& buf, int old_xpos, int xpos, int width)
+{
+	static_assert(ARRAY_SIZE(colors32) == ARRAY_SIZE(colors16), "bad colors arrays");
+
+	for (unsigned y = 0; y < buf.height(); ++y) {
+		uint16_t bcol = colors16[y * ARRAY_SIZE(colors16) / buf.height()];
+		uint16_t *line = (uint16_t*)(buf.map(0) + buf.stride(0) * y);
+
+		if (old_xpos >= 0) {
+			for (int x = old_xpos; x < old_xpos + width; ++x)
+				line[x] = 0;
+		}
+
+		for (int x = xpos; x < xpos + width; ++x)
+			line[x] = bcol;
+	}
+}
+
+static void drm_draw_color_bar_semiplanar_yuv(IMappedFramebuffer& buf, int old_xpos, int xpos, int width)
+{
+	const uint8_t colors[] = {
+		0xff,
+		0x00,
+		0xff,
+		0x20,
+		0xff,
+		0x40,
+		0xff,
+		0x80,
+		0xff,
+	};
+
+	for (unsigned y = 0; y < buf.height(); ++y) {
+		unsigned int bcol = colors[y * ARRAY_SIZE(colors) / buf.height()];
+		uint8_t *line = (uint8_t*)(buf.map(0) + buf.stride(0) * y);
+
+		if (old_xpos >= 0) {
+			for (int x = old_xpos; x < old_xpos + width; ++x)
+				line[x] = 0;
+		}
+
+		for (int x = xpos; x < xpos + width; ++x)
+			line[x] = bcol;
+	}
+}
+
+void draw_color_bar(IMappedFramebuffer& buf, int old_xpos, int xpos, int width)
+{
+	switch (buf.format()) {
+	case PixelFormat::NV12:
+	case PixelFormat::NV21:
+		// XXX not right but gets something on the screen
+		drm_draw_color_bar_semiplanar_yuv(buf, old_xpos, xpos, width);
+		break;
+
+	case PixelFormat::YUYV:
+	case PixelFormat::UYVY:
+		// XXX not right but gets something on the screen
+		drm_draw_color_bar_rgb565(buf, old_xpos, xpos, width);
+		break;
+
+	case PixelFormat::RGB565:
+		drm_draw_color_bar_rgb565(buf, old_xpos, xpos, width);
+		break;
+
+	case PixelFormat::XRGB8888:
+		drm_draw_color_bar_rgb888(buf, old_xpos, xpos, width);
+		break;
+
+	default:
+		ASSERT(false);
+	}
+}
+}
diff --git a/libkms++util/cpuframebuffer.cpp b/libkms++util/cpuframebuffer.cpp
new file mode 100644
index 0000000..1f14ddc
--- /dev/null
+++ b/libkms++util/cpuframebuffer.cpp
@@ -0,0 +1,36 @@
+#include <map>
+
+#include "cpuframebuffer.h"
+
+using namespace std;
+
+namespace kms {
+
+CPUFramebuffer::CPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format)
+	: m_width(width), m_height(height), m_format(format)
+{
+	const PixelFormatInfo& format_info = get_pixel_format_info(m_format);
+
+	m_num_planes = format_info.num_planes;
+
+	for (unsigned i = 0; i < format_info.num_planes; ++i) {
+		const PixelFormatPlaneInfo& pi = format_info.planes[i];
+		FramebufferPlane& plane = m_planes[i];
+
+		plane.stride = width * pi.bitspp / 8;
+		plane.size = plane.stride * height/ pi.ysub;
+		plane.offset = 0;
+		plane.map = new uint8_t[plane.size];
+	}
+}
+
+CPUFramebuffer::~CPUFramebuffer()
+{
+	for (unsigned i = 0; i < m_num_planes; ++i) {
+		FramebufferPlane& plane = m_planes[i];
+
+		delete plane.map;
+	}
+}
+
+}
diff --git a/libkms++util/cpuframebuffer.h b/libkms++util/cpuframebuffer.h
new file mode 100644
index 0000000..d2073bc
--- /dev/null
+++ b/libkms++util/cpuframebuffer.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include "kms++.h"
+
+namespace kms
+{
+
+class CPUFramebuffer : public IMappedFramebuffer {
+public:
+	CPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format);
+
+	virtual ~CPUFramebuffer();
+
+	CPUFramebuffer(const CPUFramebuffer& other) = delete;
+	CPUFramebuffer& operator=(const CPUFramebuffer& other) = delete;
+
+	uint32_t width() const { return m_width; }
+	uint32_t height() const { return m_height; }
+
+	PixelFormat format() const { return m_format; }
+	unsigned num_planes() const { return m_num_planes; }
+
+	uint32_t stride(unsigned plane) const { return m_planes[plane].stride; }
+	uint32_t size(unsigned plane) const { return m_planes[plane].size; }
+	uint32_t offset(unsigned plane) const { return m_planes[plane].offset; }
+	uint8_t* map(unsigned plane) { return m_planes[plane].map; }
+
+private:
+	struct FramebufferPlane {
+		uint32_t size;
+		uint32_t stride;
+		uint32_t offset;
+		uint8_t *map;
+	};
+
+	uint32_t m_width;
+	uint32_t m_height;
+	PixelFormat m_format;
+
+	unsigned m_num_planes;
+	struct FramebufferPlane m_planes[4];
+};
+
+}
diff --git a/libkms++util/extcpuframebuffer.cpp b/libkms++util/extcpuframebuffer.cpp
new file mode 100644
index 0000000..145168f
--- /dev/null
+++ b/libkms++util/extcpuframebuffer.cpp
@@ -0,0 +1,50 @@
+
+#include "extcpuframebuffer.h"
+#include "test.h"
+
+using namespace std;
+
+namespace kms
+{
+
+ExtCPUFramebuffer::ExtCPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format,
+				     uint8_t* buffer, uint32_t pitch)
+	: m_width(width), m_height(height), m_format(format)
+{
+	const PixelFormatInfo& format_info = get_pixel_format_info(m_format);
+
+	m_num_planes = format_info.num_planes;
+
+	ASSERT(m_num_planes == 1);
+
+	const PixelFormatPlaneInfo& pi = format_info.planes[0];
+	FramebufferPlane& plane = m_planes[0];
+
+	plane.stride = pitch;
+	plane.size = plane.stride * height / pi.ysub;
+	plane.map = buffer;
+}
+
+ExtCPUFramebuffer::ExtCPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format,
+				     uint8_t* buffers[4], uint32_t pitches[4])
+	: m_width(width), m_height(height), m_format(format)
+{
+	const PixelFormatInfo& format_info = get_pixel_format_info(m_format);
+
+	m_num_planes = format_info.num_planes;
+
+	for (unsigned i = 0; i < format_info.num_planes; ++i) {
+		const PixelFormatPlaneInfo& pi = format_info.planes[i];
+		FramebufferPlane& plane = m_planes[i];
+
+		plane.stride = pitches[i];
+		plane.size = plane.stride * height / pi.ysub;
+		plane.map = buffers[i];
+	}
+}
+
+ExtCPUFramebuffer::~ExtCPUFramebuffer()
+{
+}
+
+}
diff --git a/libkms++util/extcpuframebuffer.h b/libkms++util/extcpuframebuffer.h
new file mode 100644
index 0000000..172841f
--- /dev/null
+++ b/libkms++util/extcpuframebuffer.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include "kms++.h"
+
+namespace kms
+{
+
+class ExtCPUFramebuffer : public IMappedFramebuffer
+{
+public:
+	ExtCPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format,
+			  uint8_t* buffer, uint32_t pitch);
+	ExtCPUFramebuffer(uint32_t width, uint32_t height, PixelFormat format,
+			  uint8_t* buffers[4], uint32_t pitches[4]);
+	virtual ~ExtCPUFramebuffer();
+
+	uint32_t width() const { return m_width; }
+	uint32_t height() const { return m_height; }
+
+	PixelFormat format() const { return m_format; }
+	unsigned num_planes() const { return m_num_planes; }
+
+	uint32_t stride(unsigned plane) const { return m_planes[plane].stride; }
+	uint32_t size(unsigned plane) const { return m_planes[plane].size; }
+	uint32_t offset(unsigned plane) const { return 0; }
+	uint8_t* map(unsigned plane) { return m_planes[plane].map; }
+
+private:
+	struct FramebufferPlane {
+		uint32_t size;
+		uint32_t stride;
+		uint8_t *map;
+	};
+
+	uint32_t m_width;
+	uint32_t m_height;
+	PixelFormat m_format;
+
+	unsigned m_num_planes;
+	struct FramebufferPlane m_planes[4];
+};
+}
diff --git a/libkms++util/kmstest.h b/libkms++util/kmstest.h
new file mode 100644
index 0000000..aed40fe
--- /dev/null
+++ b/libkms++util/kmstest.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "color.h"
+#include "framebuffer.h"
+
+namespace kms
+{
+class IMappedFramebuffer;
+
+void draw_color_bar(IMappedFramebuffer& buf, int old_xpos, int xpos, int width);
+
+void draw_test_pattern(IMappedFramebuffer &fb);
+
+void draw_rect(IMappedFramebuffer &fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h, RGB color);
+}
diff --git a/libkms++util/opts.cpp b/libkms++util/opts.cpp
new file mode 100644
index 0000000..4ea31f8
--- /dev/null
+++ b/libkms++util/opts.cpp
@@ -0,0 +1,117 @@
+#include <algorithm>
+
+#include <unistd.h>
+#include <getopt.h>
+
+#include "opts.h"
+
+using namespace std;
+
+Option::Option(const string& str, function<void()> func)
+	: m_void_func(func)
+{
+	parse(str);
+}
+
+Option::Option(const string& str, function<void(const string)> func)
+	: m_func(func)
+{
+	parse(str);
+}
+
+void Option::parse(const string& str)
+{
+	auto iend = str.end();
+	if (*(iend - 1) == '=') {
+		iend--;
+		m_has_arg = 1;
+	} else if (*(iend - 1) == '?') {
+		iend--;
+		m_has_arg = 2;
+	} else {
+		m_has_arg = 0;
+	}
+
+	auto isplit = find(str.begin(), iend, '|');
+
+	if (isplit != str.begin())
+		m_short = str[0];
+	else
+		m_short = 0;
+
+	if (isplit != iend)
+		m_long = string(isplit + 1, iend);
+}
+
+OptionSet::OptionSet(initializer_list<Option> il)
+	: m_opts(il)
+{
+}
+
+void OptionSet::parse(int argc, char** argv)
+{
+	string shortopts = ":";
+	vector<struct option> longopts;
+
+	for (unsigned opt_idx = 0; opt_idx < m_opts.size(); ++opt_idx) {
+		const Option& o = m_opts[opt_idx];
+
+		if (o.m_short != 0) {
+			shortopts.push_back(o.m_short);
+			if (o.m_has_arg == 1)
+				shortopts.push_back(':');
+			else if (o.m_has_arg == 2)
+				shortopts.append("::");
+		}
+
+		if (!o.m_long.empty()) {
+			struct option copt;
+			copt.name = o.m_long.c_str();
+			copt.has_arg = o.m_has_arg;
+			copt.flag = 0;
+			copt.val = opt_idx + 1000;
+			longopts.push_back(copt);
+		}
+	}
+
+	longopts.push_back(option {});
+
+	while (1) {
+		int long_idx = 0;
+		int c = getopt_long(argc, argv, shortopts.c_str(),
+				    longopts.data(), &long_idx);
+		if (c == -1)
+			break;
+
+		if (c == '?')
+			throw std::invalid_argument(string("Unrecognized option ") + argv[optind - 1]);
+
+		if (c == ':') {
+			const Option& o = find_opt(optopt);
+			if (optopt < 256)
+				throw std::invalid_argument(string("Missing argument to -") + o.m_short);
+			else
+				throw std::invalid_argument(string("Missing argument to --") + o.m_long);
+		}
+
+		string sarg = { optarg ?: "" };
+
+		const Option& opt = find_opt(c);
+
+		if (opt.m_func)
+			opt.m_func(sarg);
+		else
+			opt.m_void_func();
+	}
+
+	for (int i = optind; i < argc; ++i)
+		m_params.push_back(argv[i]);
+}
+
+const Option& OptionSet::find_opt(int c)
+{
+	if (c < 256)
+		return *find_if(m_opts.begin(), m_opts.end(), [c](const Option& o) { return o.m_short == c; });
+	else
+		return m_opts[c - 1000];
+}
diff --git a/libkms++util/opts.h b/libkms++util/opts.h
new file mode 100644
index 0000000..1b0fd22
--- /dev/null
+++ b/libkms++util/opts.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <functional>
+
+class Option
+{
+	friend class OptionSet;
+public:
+	Option(const std::string& str, std::function<void()> func);
+	Option(const std::string& str, std::function<void(const std::string)> func);
+
+private:
+	void parse(const std::string& str);
+
+	char m_short;
+	std::string m_long;
+	int m_has_arg;
+	std::function<void()> m_void_func;
+	std::function<void(const std::string)> m_func;
+};
+
+class OptionSet
+{
+public:
+	OptionSet(std::initializer_list<Option> il);
+
+	void parse(int argc, char** argv);
+
+	const std::vector<std::string> params() const { return m_params; }
+
+private:
+	const Option& find_opt(int c);
+
+	const std::vector<Option> m_opts;
+	std::vector<std::string> m_params;
+};
diff --git a/libkms++util/test.h b/libkms++util/test.h
new file mode 100644
index 0000000..3631f2c
--- /dev/null
+++ b/libkms++util/test.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "color.h"
+#include "kmstest.h"
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+
+#define unlikely(x) __builtin_expect(!!(x), 0)
+
+#define ASSERT(x) \
+	if (unlikely(!(x))) { \
+		fprintf(stderr, "%s:%d: %s: ASSERT(%s) failed\n", __FILE__, __LINE__, __PRETTY_FUNCTION__, __STRING(x)); \
+		abort(); \
+	}
+
+#define FAIL(fmt, ...) \
+	do { \
+		fprintf(stderr, "%s:%d: %s:\n" fmt "\n", __FILE__, __LINE__, __PRETTY_FUNCTION__, ##__VA_ARGS__); \
+		abort(); \
+	} while(0)
+
+#define FAIL_IF(x, fmt, ...) \
+	if (unlikely(x)) { \
+		fprintf(stderr, "%s:%d: %s:\n" fmt "\n", __FILE__, __LINE__, __PRETTY_FUNCTION__, ##__VA_ARGS__); \
+		abort(); \
+	}
+
+#define EXIT(fmt, ...) \
+	do { \
+		fprintf(stderr, fmt "\n", ##__VA_ARGS__); \
+		exit(-1); \
+	} while(0)
diff --git a/libkms++util/testpat.cpp b/libkms++util/testpat.cpp
new file mode 100644
index 0000000..68f4385
--- /dev/null
+++ b/libkms++util/testpat.cpp
@@ -0,0 +1,291 @@
+
+//#define DRAW_PERF_PRINT
+
+#include <chrono>
+#include <cstring>
+#include <cassert>
+
+#include "kms++.h"
+#include "test.h"
+#include "cpuframebuffer.h"
+
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
+
+namespace kms
+{
+static void draw_rgb_pixel(IMappedFramebuffer& buf, unsigned x, unsigned y, RGB color)
+{
+	switch (buf.format()) {
+	case PixelFormat::XRGB8888:
+	case PixelFormat::ARGB8888:
+	{
+		uint32_t *p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
+		*p = color.argb8888();
+		break;
+	}
+	case PixelFormat::XBGR8888:
+	case PixelFormat::ABGR8888:
+	{
+		uint32_t *p = (uint32_t*)(buf.map(0) + buf.stride(0) * y + x * 4);
+		*p = color.abgr8888();
+		break;
+	}
+	case PixelFormat::RGB565:
+	{
+		uint16_t *p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
+		*p = color.rgb565();
+		break;
+	}
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
+static void draw_yuv422_macropixel(IMappedFramebuffer& buf, unsigned x, unsigned y, YUV yuv1, YUV yuv2)
+{
+	ASSERT((x & 1) == 0);
+
+	uint8_t *p = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x * 2);
+
+	uint8_t y0 = yuv1.y;
+	uint8_t y1 = yuv2.y;
+	uint8_t u = (yuv1.u + yuv2.u) / 2;
+	uint8_t v = (yuv1.v + yuv2.v) / 2;
+
+	switch (buf.format()) {
+	case PixelFormat::UYVY:
+		p[0] = u;
+		p[1] = y0;
+		p[2] = v;
+		p[3] = y1;
+		break;
+
+	case PixelFormat::YUYV:
+		p[0] = y0;
+		p[1] = u;
+		p[2] = y1;
+		p[3] = v;
+		break;
+
+	case PixelFormat::YVYU:
+		p[0] = y0;
+		p[1] = v;
+		p[2] = y1;
+		p[3] = u;
+		break;
+
+	case PixelFormat::VYUY:
+		p[0] = v;
+		p[1] = y0;
+		p[2] = u;
+		p[3] = y1;
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
+static void draw_yuv420_macropixel(IMappedFramebuffer& buf, unsigned x, unsigned y,
+				   YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
+{
+	ASSERT((x & 1) == 0);
+	ASSERT((y & 1) == 0);
+
+	uint8_t *py1 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 0) + x);
+	uint8_t *py2 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 1) + x);
+
+	uint8_t *puv = (uint8_t*)(buf.map(1) + buf.stride(1) * (y / 2) + x);
+
+	uint8_t y0 = yuv1.y;
+	uint8_t y1 = yuv2.y;
+	uint8_t y2 = yuv3.y;
+	uint8_t y3 = yuv4.y;
+	uint8_t u = (yuv1.u + yuv2.u + yuv3.u + yuv4.u) / 4;
+	uint8_t v = (yuv1.v + yuv2.v + yuv3.v + yuv4.v) / 4;
+
+	switch (buf.format()) {
+	case PixelFormat::NV12:
+		py1[0] = y0;
+		py1[1] = y1;
+		py2[0] = y2;
+		py2[1] = y3;
+		puv[0] = u;
+		puv[1] = v;
+		break;
+
+	case PixelFormat::NV21:
+		py1[0] = y0;
+		py1[1] = y1;
+		py2[0] = y2;
+		py2[1] = y3;
+		puv[0] = v;
+		puv[1] = u;
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
+static RGB get_test_pattern_pixel(IMappedFramebuffer& fb, unsigned x, unsigned y)
+{
+	const unsigned w = fb.width();
+	const unsigned h = fb.height();
+
+	const unsigned mw = 20;
+
+	const unsigned xm1 = mw;
+	const unsigned xm2 = w - mw - 1;
+	const unsigned ym1 = mw;
+	const unsigned ym2 = h - mw - 1;
+
+	// white margin lines
+	if (x == xm1 || x == xm2 || y == ym1 || y == ym2)
+		return RGB(255, 255, 255);
+	// white box outlines to corners
+	else if ((x == 0 || x == w - 1) && (y < ym1 || y > ym2))
+		return RGB(255, 255, 255);
+	// white box outlines to corners
+	else if ((y == 0 || y == h - 1) && (x < xm1 || x > xm2))
+		return RGB(255, 255, 255);
+	// blue bar on the left
+	else if (x < xm1 && (y > ym1 && y < ym2))
+		return RGB(0, 0, 255);
+	// blue bar on the top
+	else if (y < ym1 && (x > xm1 && x < xm2))
+		return RGB(0, 0, 255);
+	// red bar on the right
+	else if (x > xm2 && (y > ym1 && y < ym2))
+		return RGB(255, 0, 0);
+	// red bar on the bottom
+	else if (y > ym2 && (x > xm1 && x < xm2))
+		return RGB(255, 0, 0);
+	// inside the margins
+	else if (x > xm1 && x < xm2 && y > ym1 && y < ym2) {
+		// diagonal line
+		if (x == y || w - x == h - y)
+			return RGB(255, 255, 255);
+		// diagonal line
+		else if (w - x == y || x == h - y)
+			return RGB(255, 255, 255);
+		else {
+			int t = (x - xm1 - 1) * 8 / (xm2 - xm1 - 1);
+			unsigned r = 0, g = 0, b = 0;
+
+			unsigned c = (y - ym1 - 1) % 256;
+
+			switch (t) {
+			case 0:
+				r = c;
+				break;
+			case 1:
+				g = c;
+				break;
+			case 2:
+				b = c;
+				break;
+			case 3:
+				g = b = c;
+				break;
+			case 4:
+				r = b = c;
+				break;
+			case 5:
+				r = g = c;
+				break;
+			case 6:
+				r = g = b = c;
+				break;
+			case 7:
+				break;
+			}
+
+			return RGB(r, g, b);
+		}
+	} else {
+		// black corners
+		return RGB(0, 0, 0);
+	}
+}
+
+static void draw_test_pattern_impl(IMappedFramebuffer& fb)
+{
+	unsigned x, y;
+	unsigned w = fb.width();
+	unsigned h = fb.height();
+
+	switch (fb.format()) {
+	case PixelFormat::XRGB8888:
+	case PixelFormat::XBGR8888:
+	case PixelFormat::ARGB8888:
+	case PixelFormat::ABGR8888:
+	case PixelFormat::RGB565:
+		for (y = 0; y < h; y++) {
+			for (x = 0; x < w; x++) {
+				RGB pixel = get_test_pattern_pixel(fb, x, y);
+				draw_rgb_pixel(fb, x, y, pixel);
+			}
+		}
+		break;
+
+	case PixelFormat::UYVY:
+	case PixelFormat::YUYV:
+	case PixelFormat::YVYU:
+	case PixelFormat::VYUY:
+		for (y = 0; y < h; y++) {
+			for (x = 0; x < w; x += 2) {
+				RGB pixel1 = get_test_pattern_pixel(fb, x, y);
+				RGB pixel2 = get_test_pattern_pixel(fb, x + 1, y);
+				draw_yuv422_macropixel(fb, x, y, pixel1.yuv(), pixel2.yuv());
+			}
+		}
+		break;
+
+	case PixelFormat::NV12:
+	case PixelFormat::NV21:
+		for (y = 0; y < h; y += 2) {
+			for (x = 0; x < w; x += 2) {
+				RGB pixel00 = get_test_pattern_pixel(fb, x, y);
+				RGB pixel10 = get_test_pattern_pixel(fb, x + 1, y);
+				RGB pixel01 = get_test_pattern_pixel(fb, x, y + 1);
+				RGB pixel11 = get_test_pattern_pixel(fb, x + 1, y + 1);
+				draw_yuv420_macropixel(fb, x, y,
+						       pixel00.yuv(), pixel10.yuv(),
+						       pixel01.yuv(), pixel11.yuv());
+			}
+		}
+		break;
+	default:
+		throw std::invalid_argument("unknown pixelformat");
+	}
+}
+
+void draw_test_pattern(IMappedFramebuffer &fb)
+{
+#ifdef DRAW_PERF_PRINT
+	using namespace std::chrono;
+
+	auto t1 = high_resolution_clock::now();
+#endif
+
+	draw_test_pattern_impl(fb);
+
+#ifdef DRAW_PERF_PRINT
+	auto t2 = high_resolution_clock::now();
+	auto time_span = duration_cast<microseconds>(t2 - t1);
+
+	printf("draw took %u us\n", (unsigned)time_span.count());
+#endif
+}
+
+void draw_rect(IMappedFramebuffer &fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h, RGB color)
+{
+	for (unsigned i = x; i < x + w; ++i) {
+		for (unsigned j = y; j < y + h; ++j) {
+			draw_rgb_pixel(fb, i, j, color);
+		}
+	}
+}
+
+}