mango/kexts/drivers/tty/fbcon/vgacon.c

#include <socks/tty.h>
#include <arch/serial.h>
#include <socks/libc/string.h>
#include <socks/libc/ctype.h>
#include <arch/ports.h>
#include <socks/console.h>
#include <socks/vm.h>
#include <socks/printk.h>
#include <stdint.h>
#include "fbcon.h"

#define VGA_PORT_CMD		0x3D4
#define VGA_PORT_DATA		0x3D5

#define VGA_CHAR(ch, attrib)	((uint16_t)(ch) | ((uint16_t)(attrib) << 8))
#define DEFAULT_ATTRIB		0x07

static uint16_t *g_console_fb = (uint16_t *)(VM_KERNEL_VOFFSET + 0xb8000);
static const unsigned int k_console_width = 80;
static const unsigned int k_console_height = 25;
static unsigned int g_console_cursor_xpos = 0;
static unsigned int g_console_cursor_ypos = 0;

static void init_vga_cursor(void)
{
	unsigned int start = 0, end = 15;
	outportb(VGA_PORT_CMD, 0x0A);
	outportb(VGA_PORT_DATA, (inportb(VGA_PORT_DATA) & 0xC0) | start);

	outportb(VGA_PORT_CMD, 0x0B);
	outportb(VGA_PORT_DATA, (inportb(VGA_PORT_DATA) & 0xE0) | end);
}

static void move_vga_cursor(unsigned int x, unsigned int y)
{
	unsigned int offset = y * k_console_width + x;

	outportb(VGA_PORT_CMD, 0x0F);
	outportb(VGA_PORT_DATA, (uint8_t)(offset & 0xFF));

	outportb(VGA_PORT_CMD, 0x0E);
	outportb(VGA_PORT_DATA, (uint8_t)((offset >> 8) & 0xFF));
}

static void scroll_display()
{
	uint16_t *src = g_console_fb + k_console_width;
	uint16_t *dst = g_console_fb;
	size_t n = k_console_width * (k_console_height - 1) * 2;

	memmove(dst, src, n);

	dst = g_console_fb + ((k_console_height - 1) * k_console_width);

	for (int i = 0; i < k_console_width; i++) {
		dst[i] = VGA_CHAR(0, DEFAULT_ATTRIB);
	}
}

static void handle_ctrl(int c)
{
	switch (c) {
	case '\n':
		g_console_cursor_xpos = 0;
		g_console_cursor_ypos++;

		if (g_console_cursor_ypos >= k_console_height) {
			scroll_display();
			g_console_cursor_ypos = k_console_height - 1;
		}
		break;
	default:
		break;
	}

	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);
}

static void vgacon_putchar(int c)
{
	//serial_putchar(SERIAL_PORT_A, c);

	if (iscntrl(c)) {
		handle_ctrl(c);
		return;
	}

	g_console_fb[(g_console_cursor_ypos * k_console_width) + g_console_cursor_xpos] = VGA_CHAR(c, DEFAULT_ATTRIB);

	g_console_cursor_xpos++;

	if (g_console_cursor_xpos >= k_console_width) {
		g_console_cursor_xpos = 0;
		g_console_cursor_ypos++;
	}

	if (g_console_cursor_ypos >= k_console_height) {
		scroll_display();
		g_console_cursor_ypos = k_console_height - 1;
		g_console_cursor_xpos = 0;
	}

	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);
}

static void vgacon_write(struct console *con, const char *s, unsigned int len)
{
	for (unsigned int i = 0; i < len; i++) {
		vgacon_putchar(s[i]);
	}
}

static struct console early_vgacon = {
	.c_name = "vgacon",
	.c_flags = CON_BOOT,
	.c_write = vgacon_write,
	.c_lock = SPIN_LOCK_INIT,
};

void early_vgacon_init(void)
{
	g_console_cursor_xpos = 0;
	g_console_cursor_ypos = 0;

	for (int i = 0; i < k_console_width * k_console_height; i++) {
		g_console_fb[i] = DEFAULT_ATTRIB << 8;
	}

	init_vga_cursor();
	move_vga_cursor(g_console_cursor_xpos, g_console_cursor_ypos);

	console_register(&early_vgacon);
	early_printk_init(&early_vgacon);
}

static void vgacon_init(struct device *dev)
{

}

static void vgacon_deinit(struct device *dev)
{

}

static void vgacon_clear(struct device *dev, int x, int y, int width, int height)
{

}

static void vgacon_putc(struct device *dev, int c, int xpos, int ypos, tty_attrib_t attrib)
{
	struct tty_device *ttydev = TTY_DEVICE(dev);
	struct fbcon_priv *priv = dev->dev_priv;
	priv->fb_cells[(ypos * ttydev->tty_xcells) + xpos] = VGA_CHAR(c, attrib);
}

static void vgacon_set_cursor(struct device *dev, enum tty_cursor cur)
{

}

static void vgacon_move_cursor(struct device *dev, int x, int y)
{
	move_vga_cursor(x, y);
}

static void vgacon_scroll(struct device *dev, enum tty_scroll_dir dir, int lines)
{

}


static struct tty_driver_ops vgacon_ops = {
	.tty_init = vgacon_init,
	.tty_deinit = vgacon_deinit,
	.tty_clear = vgacon_clear,
	.tty_putc = vgacon_putc,
	.tty_set_cursor = vgacon_set_cursor,
	.tty_move_cursor = vgacon_move_cursor,
	.tty_scroll = vgacon_scroll,
};

kern_status_t init_vgacon_console(struct device *tty, struct device *fb)
{
	struct char_device *cdev = CHAR_DEVICE(tty);
	struct framebuffer_varinfo fb_mode;
	struct framebuffer_fixedinfo fixedinfo;
	struct tty_device *ttydev = cdev->c_tty;

	struct fbcon_priv *priv = kzalloc(sizeof *priv, VM_NORMAL);
	if (!priv) {
		return KERN_NO_MEMORY;
	}

	kern_status_t status = framebuffer_get_varinfo(fb, &fb_mode);

	if (status != KERN_OK) {
		kfree(priv);
		return status;
	}

	status = framebuffer_get_fixedinfo(fb, &fixedinfo);

	if (status != KERN_OK) {
		kfree(priv);
		return status;
	}

	ttydev->tty_xcells = fb_mode.fb_xcells;
	ttydev->tty_ycells = fb_mode.fb_ycells;
	ttydev->tty_xcur = g_console_cursor_xpos;
	ttydev->tty_ycur = g_console_cursor_ypos;
	ttydev->tty_curattrib = DEFAULT_ATTRIB;

	priv->fbdev = fb;
	priv->fb_pitch = fb_mode.fb_stride;
	priv->fb_cells = vm_phys_to_virt(fixedinfo.fb_baseptr);
	priv->tty_ops = &vgacon_ops;
	tty->dev_priv = priv;

	return KERN_OK;
}