kernel_samsung_a34x-permissive/drivers/gpu/drm/mediatek/mtk_drm_fbconsole.c

/* SPDX-License-Identifier: GPL-2.0 */
/*
 * Copyright (c) 2019 MediaTek Inc.
*/

#include <linux/font.h>
#include <linux/string.h>
#include <linux/semaphore.h>
#include <linux/slab.h>
#include <linux/font.h>

#include "mtk_drm_fbconsole.h"

/* ------------------------------------------------------------------------- */

#define MFC_WIDTH (ctxt->fb_width)
#define MFC_HEIGHT (ctxt->fb_height)
#define MFC_BPP (ctxt->fb_bpp)
#define MFC_PITCH (MFC_WIDTH * MFC_BPP)

#define MFC_FG_COLOR (ctxt->fg_color)
#define MFC_BG_COLOR (ctxt->bg_color)

#define MFC_FONT font_vga_8x16
#define MFC_FONT_WIDTH (MFC_FONT.width)
#define MFC_FONT_HEIGHT (MFC_FONT.height)
#define MFC_FONT_DATA (MFC_FONT.data)

#define MFC_ROW_SIZE (MFC_FONT_HEIGHT * MFC_PITCH * ctxt->scale)
#define MFC_ROW_FIRST ((BYTE *)(ctxt->fb_addr))
#define MFC_ROW_SECOND (MFC_ROW_FIRST + MFC_ROW_SIZE)
#define MFC_ROW_LAST (MFC_ROW_FIRST + MFC_SIZE - MFC_ROW_SIZE)
#define MFC_SIZE (MFC_ROW_SIZE * ctxt->rows)
#define MFC_SCROLL_SIZE (MFC_SIZE - MFC_ROW_SIZE)

#define MAKE_TWO_RGB565_COLOR(high, low) (((low) << 16) | (high))

/* ------------------------------------------------------------------------- */
UINT32 MFC_Get_Cursor_Offset(MFC_HANDLE handle)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;

	UINT32 offset = ctxt->cursor_col * MFC_FONT_WIDTH * MFC_BPP +
			ctxt->cursor_row * MFC_FONT_HEIGHT * MFC_PITCH;

	offset *= ctxt->scale;
	return offset;
}

static void _mfc_draw_row(struct MFC_CONTEXT *ctxt, BYTE *dest, BYTE raw_color)
{
	UINT32 pixel_row, i;
	uint16_t color;
	int cols, index;

	for (pixel_row = 0; pixel_row < ctxt->scale; pixel_row++) {
		for (cols = 7; cols >= 0; cols--) {
			if (raw_color >> (unsigned int)cols & 1)
				color = MFC_FG_COLOR;
			else
				color = MFC_BG_COLOR;

			for (i = 0; i < ctxt->scale; i++) {
				index = i + (7 - cols) * ctxt->scale;
				((uint16_t *)dest)[index] = color;
			}
		}
		dest += MFC_PITCH;
	}
}

static void _mfc_draw_char(struct MFC_CONTEXT *ctxt, UINT32 x, UINT32 y, char c)
{
	BYTE ch = *((BYTE *)&c);
	const BYTE *cdat = (const BYTE *)MFC_FONT_DATA + ch * MFC_FONT_HEIGHT;
	BYTE *dest = NULL;
	INT32 rows, cols, offset;

	int font_draw_table16[4];

	if (x > (MFC_WIDTH - MFC_FONT_WIDTH)) {
		pr_info("draw width too large,x=%d\n", x);
		return;
	}
	if (y > (MFC_HEIGHT - MFC_FONT_HEIGHT)) {
		pr_info("draw hight too large,y=%d\n", y);
		return;
	}

	offset = y * MFC_PITCH + x * MFC_BPP;
	offset *= ctxt->scale;
	dest = (MFC_ROW_FIRST + offset);

	switch (MFC_BPP) {
	case 2:
		font_draw_table16[0] =
			MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_BG_COLOR);
		font_draw_table16[1] =
			MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_FG_COLOR);
		font_draw_table16[2] =
			MAKE_TWO_RGB565_COLOR(MFC_FG_COLOR, MFC_BG_COLOR);
		font_draw_table16[3] =
			MAKE_TWO_RGB565_COLOR(MFC_FG_COLOR, MFC_FG_COLOR);

		for (rows = MFC_FONT_HEIGHT; rows--;) {
			BYTE bits = *cdat++;

			if (ctxt->scale >= 2) {
				_mfc_draw_row(ctxt, dest, bits);
				dest += (MFC_PITCH * ctxt->scale);
			} else {
				((UINT32 *)dest)[0] =
					font_draw_table16[bits >> 6];
				((UINT32 *)dest)[1] =
					font_draw_table16[bits >> 4 & 3];
				((UINT32 *)dest)[2] =
					font_draw_table16[bits >> 2 & 3];
				((UINT32 *)dest)[3] =
					font_draw_table16[bits & 3];
				dest += MFC_PITCH;
			}
		}
		break;
	case 3:
		for (rows = MFC_FONT_HEIGHT; rows--; dest += MFC_PITCH) {
			BYTE bits = *cdat++;
			BYTE *tmp = dest;

			for (cols = 0; cols < 8; ++cols) {
				UINT32 color = (((unsigned int)bits >>
						       (unsigned int)(7 -
						       cols)) & 0x1)
						       ? MFC_FG_COLOR
						       : MFC_BG_COLOR;
				((BYTE *)tmp)[0] = color & 0xff;
				((BYTE *)tmp)[1] = (color >> 8) & 0xff;
				((BYTE *)tmp)[2] = (color >> 16) & 0xff;
				tmp += 3;
			}
		}
		break;
	case 4:
		for (rows = MFC_FONT_HEIGHT; rows--; dest += MFC_PITCH) {
			BYTE bits = *cdat++;
			BYTE *tmp = dest;

			for (cols = 0; cols < 8; ++cols) {
				UINT32 color = ((bits >> (7 - cols)) & 0x1)
						       ? MFC_FG_COLOR
						       : MFC_BG_COLOR;
				((BYTE *)tmp)[1] = color & 0xff;
				((BYTE *)tmp)[2] = (color >> 8) & 0xff;
				((BYTE *)tmp)[3] = (color >> 16) & 0xff;
				((BYTE *)tmp)[0] = (color >> 16) & 0xff;
				tmp += 4;
			}
		}
		break;
	default:
		pr_info("draw char fail,MFC_BPP=%d\n", MFC_BPP);
		break;
	}
}

static void _mfc_scroll_up(struct MFC_CONTEXT *ctxt)
{
	const UINT32 bg_color =
		MAKE_TWO_RGB565_COLOR(MFC_BG_COLOR, MFC_BG_COLOR);
	UINT32 *ptr = (UINT32 *)MFC_ROW_LAST;
	int i = MFC_ROW_SIZE / sizeof(UINT32);

	memcpy(MFC_ROW_FIRST, MFC_ROW_SECOND, MFC_SCROLL_SIZE);

	while (--i >= 0)
		*ptr++ = bg_color;
}

static void _mfc_newline(struct MFC_CONTEXT *ctxt)
{
	/* Bin:add for filling the color for the blank of this column */
	while (ctxt->cursor_col < ctxt->cols) {
		_mfc_draw_char(ctxt, ctxt->cursor_col * MFC_FONT_WIDTH,
			       ctxt->cursor_row * MFC_FONT_HEIGHT, ' ');

		++ctxt->cursor_col;
	}
	++ctxt->cursor_row;
	ctxt->cursor_col = 0;

	/* check if we need to scroll the terminal */
	if (ctxt->cursor_row >= ctxt->rows) {
		/* Scroll everything up */
		_mfc_scroll_up(ctxt);

		/* decrement row number */
		--ctxt->cursor_row;
	}
}

#define check_newline()                                                        \
	do {                                                                   \
		if (ctxt->cursor_col >= ctxt->cols)                            \
			_mfc_newline(ctxt);                                    \
	} while (0)

static void _mfc_putc(struct MFC_CONTEXT *ctxt, const char c)
{
	check_newline();

	switch (c) {
	case '\n': /* next line */
		_mfc_newline(ctxt);
		break;

	case '\r': /* carriage return */
		ctxt->cursor_col = 0;
		break;

	case '\t': /* tab 8 */
		ctxt->cursor_col += 8;
		ctxt->cursor_col &= ~0x0007;
		check_newline();
		break;

	default: /* draw the char */
		_mfc_draw_char(ctxt, ctxt->cursor_col * MFC_FONT_WIDTH,
			       ctxt->cursor_row * MFC_FONT_HEIGHT, c);
		++ctxt->cursor_col;
		check_newline();
		break;
	}
}

static int MFC_GetScale(unsigned int fb_width, unsigned int fb_height,
			unsigned int fb_bpp)
{
	if (fb_bpp == 2 && fb_width * fb_height >= 1080 * 1920)
		return 2;

	return 1;
}

enum MFC_STATUS MFC_Open(MFC_HANDLE *handle, void *fb_addr,
			 unsigned int fb_width, unsigned int fb_height,
			 unsigned int fb_bpp, unsigned int fg_color,
			 unsigned int bg_color, struct file *filp)
{
	struct MFC_CONTEXT *ctxt = NULL;

	if (!handle || !fb_addr)
		return MFC_STATUS_INVALID_ARGUMENT;

	/* if (fb_bpp != 2) */
	/* return MFC_STATUS_NOT_IMPLEMENTED; */

	ctxt = kzalloc(sizeof(struct MFC_CONTEXT), GFP_KERNEL);
	if (!ctxt)
		return MFC_STATUS_OUT_OF_MEMORY;

	ctxt->scale = MFC_GetScale(fb_width, fb_height, fb_bpp);
	if (ctxt->scale == 0)
		ctxt->scale = 1;

	sema_init(&ctxt->sem, 1);
	ctxt->fb_addr = fb_addr;
	ctxt->fb_width = fb_width;
	ctxt->fb_height = fb_height;
	ctxt->fb_bpp = fb_bpp;
	ctxt->fg_color = fg_color;
	ctxt->bg_color = bg_color;
	ctxt->rows = fb_height / (MFC_FONT_HEIGHT * ctxt->scale);
	ctxt->cols = fb_width / (MFC_FONT_WIDTH * ctxt->scale);
	ctxt->font_width = MFC_FONT_WIDTH;
	ctxt->font_height = MFC_FONT_HEIGHT;
	ctxt->filp = filp;

	*handle = ctxt;

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_Open_Ex(MFC_HANDLE *handle, void *fb_addr,
			    unsigned int fb_width, unsigned int fb_height,
			    unsigned int fb_pitch, unsigned int fb_bpp,
			    unsigned int fg_color, unsigned int bg_color,
			    struct file *filp)
{
	struct MFC_CONTEXT *ctxt = NULL;

	if (!handle || !fb_addr)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (fb_bpp != 2)
		return MFC_STATUS_NOT_IMPLEMENTED; /* only support RGB565 */

	ctxt = kzalloc(sizeof(struct MFC_CONTEXT), GFP_KERNEL);
	if (!ctxt)
		return MFC_STATUS_OUT_OF_MEMORY;

	ctxt->scale = MFC_GetScale(fb_width, fb_height, fb_bpp);
	if (ctxt->scale == 0)
		ctxt->scale = 1;

	sema_init(&ctxt->sem, 1);
	ctxt->fb_addr = fb_addr;
	ctxt->fb_width = fb_pitch;
	ctxt->fb_height = fb_height;
	ctxt->fb_bpp = fb_bpp;
	ctxt->fg_color = fg_color;
	ctxt->bg_color = bg_color;
	ctxt->rows = fb_height / (MFC_FONT_HEIGHT * ctxt->scale);
	ctxt->cols = fb_width / (MFC_FONT_WIDTH * ctxt->scale);
	ctxt->font_width = MFC_FONT_WIDTH;
	ctxt->font_height = MFC_FONT_HEIGHT;
	ctxt->filp = filp;

	*handle = ctxt;

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_Close(MFC_HANDLE handle)
{
	if (!handle)
		return MFC_STATUS_INVALID_ARGUMENT;

	kfree(handle);

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_SetColor(MFC_HANDLE handle, unsigned int fg_color,
			     unsigned int bg_color)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;

	if (!ctxt)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (down_interruptible(&ctxt->sem)) {
		pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
		return MFC_STATUS_LOCK_FAIL;
	}

	ctxt->fg_color = fg_color;
	ctxt->bg_color = bg_color;

	up(&ctxt->sem);

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_ResetCursor(MFC_HANDLE handle)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;

	if (!ctxt)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (down_interruptible(&ctxt->sem)) {
		pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
		return MFC_STATUS_LOCK_FAIL;
	}

	ctxt->cursor_row = ctxt->cursor_col = 0;

	up(&ctxt->sem);

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_Print(MFC_HANDLE handle, const char *str)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
	int count = 0;

	if (!ctxt || !str)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (down_interruptible(&ctxt->sem)) {
		pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
		return MFC_STATUS_LOCK_FAIL;
	}

	count = strlen(str);

	while (count--)
		_mfc_putc(ctxt, *str++);

	up(&ctxt->sem);

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_SetMem(MFC_HANDLE handle, const char *str, UINT32 color)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
	int count = 0;
	int i, j;
	UINT32 *ptr = NULL;

	if (!ctxt || !str)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (down_interruptible(&ctxt->sem)) {
		pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
		return MFC_STATUS_LOCK_FAIL;
	}

	count = strlen(str);
	count = count * MFC_FONT_WIDTH;

	for (j = 0; j < MFC_FONT_HEIGHT; j++) {
		ptr = (UINT32 *)(ctxt->fb_addr + (j + 1) * MFC_PITCH -
				 count * ctxt->fb_bpp);
		for (i = 0; i < count * ctxt->fb_bpp / sizeof(UINT32); i++)
			*ptr++ = color;
	}

	up(&ctxt->sem);

	return MFC_STATUS_OK;
}

enum MFC_STATUS MFC_LowMemory_Printf(MFC_HANDLE handle, const char *str,
				     UINT32 fg_color, UINT32 bg_color)
{
	struct MFC_CONTEXT *ctxt = (struct MFC_CONTEXT *)handle;
	int count = 0;
	unsigned int col, row, fg_color_mfc, bg_color_mfc;

	if (!ctxt || !str)
		return MFC_STATUS_INVALID_ARGUMENT;

	if (down_interruptible(&ctxt->sem)) {
		pr_err("[MFC] ERROR: Can't get semaphore in %s()\n", __func__);
		return MFC_STATUS_LOCK_FAIL;
	}

	count = strlen(str);
	/* store cursor_col and row for printf low memory char temply */
	row = ctxt->cursor_row;
	col = ctxt->cursor_col;
	ctxt->cursor_row = 0;
	ctxt->cursor_col = ctxt->cols - count;
	fg_color_mfc = ctxt->fg_color;
	bg_color_mfc = ctxt->bg_color;

	ctxt->fg_color = fg_color;
	ctxt->bg_color = bg_color;
	while (count--)
		_mfc_putc(ctxt, *str++);

	/* restore cursor_col and row for printf low memory char temply */
	ctxt->cursor_row = row;
	ctxt->cursor_col = col;
	ctxt->fg_color = fg_color_mfc;
	ctxt->bg_color = bg_color_mfc;

	up(&ctxt->sem);

	return MFC_STATUS_OK;
}

/* ---------- screen logger ---------- */
static struct screen_logger logger_head;
static int screen_logger_inited;

void screen_logger_init(void)
{
	if (screen_logger_inited == 0) {
		INIT_LIST_HEAD(&logger_head.list);
		screen_logger_inited = 1;
	}
}

void screen_logger_add_message(char *obj, enum message_mode mode, char *message)
{
	struct screen_logger *p = NULL;
	int add_new = 1;
	char *new = NULL, *old = NULL;
	unsigned int len = 0;

	screen_logger_init();
	list_for_each_entry(p, &logger_head.list, list) {
		if (strcmp(p->obj, obj) != 0)
			continue;

		switch (mode) {
		case MESSAGE_REPLACE:
			old = p->message;
			new = kstrdup(message, GFP_KERNEL);
			p->message = new;
			kfree(old);
			break;
		case MESSAGE_APPEND:
			len = strlen(p->message) + strlen(message);
			new = kmalloc(sizeof(char) * (len + 1), GFP_KERNEL);
			if (!new)
				break;
			strncpy(new, p->message, strlen(p->message));
			if (strlen(message) + strlen(p->message) < len)
				strncat(new, message, strlen(message));
			else
				strncat(new, message, len - strlen(p->message));
			old = p->message;
			p->message = new;
			kfree(old);
			break;
		default:
			break;
		}

		add_new = 0;
	}

	if (add_new == 1) {
		struct screen_logger *logger =
			kmalloc(sizeof(struct screen_logger), GFP_KERNEL);
		if (logger == NULL)
			return;

		logger->obj = kstrdup(obj, GFP_KERNEL);
		logger->message = kstrdup(message, GFP_KERNEL);
		list_add_tail(&logger->list, &logger_head.list);
	}
}

void screen_logger_remove_message(const char *obj)
{
	struct screen_logger *p = NULL;

	list_for_each_entry(p, &logger_head.list, list) {
		if (strcmp(p->obj, obj) == 0) {
			kfree(p->obj);
			kfree(p->message);
			/* memory leak. TODO: delete completely. */
			list_del(&p->list);
			kfree(p);
			break;
		}
	}
	p = NULL;
}

void screen_logger_print(MFC_HANDLE handle)
{
	struct screen_logger *p = NULL;

	list_for_each_entry(p, &logger_head.list, list)
		MFC_Print(handle, p->message);
}

void screen_logger_empty(void)
{
	struct screen_logger *p = NULL;

	p = list_entry(logger_head.list.prev, typeof(*p), list);
	while (p != &logger_head) {
		screen_logger_remove_message(p->obj);
		p = list_entry(logger_head.list.prev, typeof(*p), list);
	}
}

void screen_logger_test_case(MFC_HANDLE handle)
{
	screen_logger_add_message("message1", MESSAGE_REPLACE,
				  "print message1\n");
	screen_logger_add_message("message1", MESSAGE_REPLACE,
				  "print message2\n");
	screen_logger_print(handle);
	screen_logger_empty();
}