IMX6U-Game/src/Core/Platform/FBDisplay.cpp

#include "FBDisplay.h"
#include "FrameBuffer.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/select.h>
#include <unistd.h>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <ctime>

namespace Platform
{
	namespace
	{
		inline uint16_t rgba_to_rgb565(uint32_t rgba)
		{
			const uint32_t r = (rgba >> 24) & 0xFFu;
			const uint32_t g = (rgba >> 16) & 0xFFu;
			const uint32_t b = (rgba >> 8) & 0xFFu;
			return static_cast<uint16_t>(((r & 0xF8u) << 8) | ((g & 0xFCu) << 3) | (b >> 3));
		}

		inline uint32_t rgba_to_argb8888(uint32_t rgba)
		{
			return ((rgba >> 8) & 0x00FFFFFFu) | ((rgba & 0xFFu) << 24);
		}
	}

	bool FBDisplay::init(int w, int h)
	{
		width = w;
		height = h;

		fb_fd = open("/dev/fb0", O_RDWR);
		if (fb_fd < 0)
		{
			perror("open /dev/fb0");
			return false;
		}

		if (ioctl(fb_fd, FBIOGET_FSCREENINFO, &finfo) < 0 ||
		    ioctl(fb_fd, FBIOGET_VSCREENINFO, &vinfo) < 0)
		{
			perror("ioctl FBIOGET_FSCREENINFO / FBIOGET_VSCREENINFO");
			close(fb_fd);
			fb_fd = -1;
			return false;
		}

		printf("FB: %dx%d (virtual %dx%d), %dbpp, line_length=%d\n",
		       vinfo.xres, vinfo.yres, vinfo.xres_virtual, vinfo.yres_virtual,
		       vinfo.bits_per_pixel, finfo.line_length);
		printf("RGB offsets: R=%d/%d G=%d/%d B=%d/%d\n",
		       vinfo.red.offset, vinfo.red.length,
		       vinfo.green.offset, vinfo.green.length,
		       vinfo.blue.offset, vinfo.blue.length);

		fb_size = finfo.line_length * vinfo.yres;
		fb_mem = (uint8_t*)mmap(nullptr, fb_size, PROT_READ | PROT_WRITE, MAP_SHARED, fb_fd, 0);
		if (fb_mem == MAP_FAILED)
		{
			perror("mmap");
			close(fb_fd);
			fb_fd = -1;
			return false;
		}

		memset(fb_mem, 0, fb_size);
		return true;
	}

	uint32_t FBDisplay::convert_pixel(uint32_t rgba) const
	{
		uint8_t r = (rgba >> 24) & 0xFF;
		uint8_t g = (rgba >> 16) & 0xFF;
		uint8_t b = (rgba >> 8) & 0xFF;
		uint8_t a = rgba & 0xFF;

		uint32_t color = 0;
		if (vinfo.red.length > 0)
			color |= (uint32_t)(r >> (8 - vinfo.red.length)) << vinfo.red.offset;
		if (vinfo.green.length > 0)
			color |= (uint32_t)(g >> (8 - vinfo.green.length)) << vinfo.green.offset;
		if (vinfo.blue.length > 0)
			color |= (uint32_t)(b >> (8 - vinfo.blue.length)) << vinfo.blue.offset;
		if (vinfo.transp.length > 0)
			color |= (uint32_t)(a >> (8 - vinfo.transp.length)) << vinfo.transp.offset;
		return color;
	}

	void FBDisplay::present(const Core::FrameBuffer* framebuffer)
	{
		if (!fb_mem || !framebuffer)
			return;

		const uint32_t* src = static_cast<const uint32_t*>(framebuffer->get_buffer());
		const int src_width = framebuffer->get_width();
		const int dst_width = std::min(src_width, static_cast<int>(vinfo.xres));
		const int dst_height = std::min(framebuffer->get_height(), static_cast<int>(vinfo.yres));
		const int bytes_per_pixel = vinfo.bits_per_pixel / 8;
		const bool is_rgb565 =
			vinfo.bits_per_pixel == 16 &&
			vinfo.red.offset == 11 && vinfo.red.length == 5 &&
			vinfo.green.offset == 5 && vinfo.green.length == 6 &&
			vinfo.blue.offset == 0 && vinfo.blue.length == 5;
		const bool is_argb8888 =
			vinfo.bits_per_pixel == 32 &&
			vinfo.red.offset == 16 && vinfo.red.length == 8 &&
			vinfo.green.offset == 8 && vinfo.green.length == 8 &&
			vinfo.blue.offset == 0 && vinfo.blue.length == 8;
		const bool is_rgba8888 =
			vinfo.bits_per_pixel == 32 &&
			vinfo.red.offset == 24 && vinfo.red.length == 8 &&
			vinfo.green.offset == 16 && vinfo.green.length == 8 &&
			vinfo.blue.offset == 8 && vinfo.blue.length == 8;

		if (is_rgb565)
		{
			for (int y = 0; y < dst_height; ++y)
			{
				const uint32_t* src_row = src + y * src_width;
				uint16_t* dst_row = reinterpret_cast<uint16_t*>(fb_mem + y * finfo.line_length);
				for (int x = 0; x < dst_width; ++x)
				{
					dst_row[x] = rgba_to_rgb565(src_row[x]);
				}
			}
			return;
		}

		if (is_argb8888)
		{
			for (int y = 0; y < dst_height; ++y)
			{
				const uint32_t* src_row = src + y * src_width;
				uint32_t* dst_row = reinterpret_cast<uint32_t*>(fb_mem + y * finfo.line_length);
				for (int x = 0; x < dst_width; ++x)
				{
					dst_row[x] = rgba_to_argb8888(src_row[x]);
				}
			}
			return;
		}

		if (is_rgba8888 && dst_width == src_width && static_cast<int>(finfo.line_length) == dst_width * 4)
		{
			std::memcpy(fb_mem, src, static_cast<size_t>(dst_height) * static_cast<size_t>(dst_width) * sizeof(uint32_t));
			return;
		}

		for (int y = 0; y < dst_height; ++y)
		{
			for (int x = 0; x < dst_width; ++x)
			{
				uint32_t pixel = convert_pixel(src[y * src_width + x]);
				uint8_t* dst = fb_mem + y * finfo.line_length + x * bytes_per_pixel;
				if (vinfo.bits_per_pixel == 32)
				{
					*(uint32_t*)dst = pixel;
				}
				else if (vinfo.bits_per_pixel == 16)
				{
					*(uint16_t*)dst = static_cast<uint16_t>(pixel);
				}
			}
		}
	}

	void FBDisplay::poll_events(bool& should_quit)
	{
		fd_set fds;
		FD_ZERO(&fds);
		FD_SET(STDIN_FILENO, &fds);
		struct timeval tv = { 0, 0 };
		if (select(STDIN_FILENO + 1, &fds, nullptr, nullptr, &tv) > 0)
		{
			char c;
			if (read(STDIN_FILENO, &c, 1) == 1 && (c == 'q' || c == 'Q'))
			{
				should_quit = true;
			}
		}
	}

	void FBDisplay::shutdown()
	{
		if (fb_mem != nullptr)
		{
			memset(fb_mem, 0, fb_size);
			munmap(fb_mem, fb_size);
			fb_mem = nullptr;
		}
		if (fb_fd >= 0)
		{
			close(fb_fd);
			fb_fd = -1;
		}
	}
}