mango/kexts/drivers/tty/serialcon/main.c

#include <socks/printk.h>
#include <socks/device.h>
#include <socks/kext.h>
#include <socks/tty.h>
#include <socks/libc/stdio.h>
#include <arch/ports.h>
#include <arch/irq.h>
#include <socks/serialcon.h>

#define COM1 0x3F8
#define COM2 0x2F8
#define COM3 0x3E8
#define COM4 0x2E8

static struct tty_driver *serialcon_driver = NULL;

static int transmit_empty(int device)
{
	return inportb(device + 5) & 0x20;
}

static int serial_received(int device)
{
	return inportb(device + 5) & 0x1;
}

void serial_send_byte(int device, char out)
{
	volatile unsigned int _count = 0;
	while (!transmit_empty(device)) {
		_count++;
	}

	outportb(device, out);

	while (!transmit_empty(device)) {
		_count++;
	}
}

char serial_recv_byte(int device)
{
	volatile unsigned int _count = 0;
	while (!serial_received(device)) {
		_count++;
	}

	char c = inportb(device);

	outportb(device + 5, inportb(device + 5) & ~0x1);

	return c;
}

void serial_putchar(int port, char ch)
{
	if (ch == '\n') {
		serial_send_byte(port, '\r');
	}

	serial_send_byte(port, ch);
}

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

static int get_baud_divisor(int baud)
{
	int freq = 115200;
	int best_baud = -1;
	int best_div = -1;

	for (int i = 1; i < 254; i++) {
		int this_baud = freq / i;

		if (this_baud == baud) {
			return i;
		}

		if (best_baud == -1) {
			best_baud = this_baud;
			best_div = i;
			continue;
		}

		if (this_baud < baud && best_baud > baud) {
			/* TODO pick divisor that gives the closest baud rate */
			return best_div;
		}

		best_baud = this_baud;
		best_div = i;
	}

	return best_div;
}

static void init_serial_port(int port, int baud)
{
	int baud_div = get_baud_divisor(baud);
	outportb(port + 1, 0x00);     // Disable all interrupts
	outportb(port + 3, 0x80);     // Enable DLAB (set baud rate divisor)
	outportb(port + 0, baud_div); // Set divisor
	outportb(port + 1, 0x00);
	outportb(port + 3, 0x03);     // 8 bits, no parity, one stop bit
	outportb(port + 2, 0xC7);     // Enable FIFO, clear them, with 14-byte threshold
	outportb(port + 4, 0x0B);     // IRQs enabled, RTS/DSR set
	outportb(port + 4, 0x1E);     // Set in loopback mode, test the serial chip
	outportb(port + 0, 0xAE);     // Test serial chip (send byte 0xAE and check if serial returns same byte)

	volatile unsigned int q = 0;
	while (!serial_received(port)) {
		q++;
	}

	// Check if serial is faulty (i.e: not same byte as sent)
	if(inportb(port + 0) != 0xAE) {
		return;
	}

	// If serial is not faulty set it in normal operation mode
	// (not-loopback with IRQs enabled and OUT#1 and OUT#2 bits enabled)
	outportb(port + 1, 0x01);
	outportb(port + 4, 0x0F);
	printk("serial: port %x initialised", port);
}

static struct console serialcon = {
	.c_name = "serialcon",
	.c_flags = CON_BOOT,
	.c_write = serialcon_write,
	.c_lock = SPIN_LOCK_INIT,
};

static int serial_irq1(void)
{
	if (serial_received(COM1)) {
		unsigned char c = serial_recv_byte(COM1);
		printk("serial: COM1 received %c", c);
	}

	if (serial_received(COM3)) {
		unsigned char c = serial_recv_byte(COM3);
		printk("serial: COM3 received %c", c);
	}

	return 0;
}

static struct irq_hook irq1_hook = {
	.irq_callback = serial_irq1,
};

void early_serialcon_init(int baud)
{
	hook_irq(IRQ4, &irq1_hook);
	init_serial_port(COM1, baud);
	console_register(&serialcon);
}

static void serialcon_putc(struct device *dev, int c, int xpos, int ypos, tty_attrib_t attrib)
{
	unsigned int port = (uintptr_t)dev->dev_priv;
	serial_putchar(port, c);
}

static struct tty_driver_ops serialcon_ops = {
	.tty_putc = serialcon_putc,
};

static kern_status_t online(struct kext *self)
{
	serialcon_driver = tty_driver_create(self, "ttyS");
	if (!serialcon_driver) {
		return KERN_NO_MEMORY;
	}

	serialcon_driver->tty_ops = &serialcon_ops;

	tty_driver_register(serialcon_driver);

	struct device *ttyS0 = tty_device_create();
	struct device *ttyS1 = tty_device_create();
	struct device *ttyS2 = tty_device_create();
	struct device *ttyS3 = tty_device_create();

	ttyS0->dev_priv = (void *)COM1;
	ttyS1->dev_priv = (void *)COM2;
	ttyS2->dev_priv = (void *)COM3;
	ttyS3->dev_priv = (void *)COM4;

	snprintf(ttyS0->dev_name, sizeof ttyS0->dev_name, "ttyS0");
	snprintf(ttyS1->dev_name, sizeof ttyS1->dev_name, "ttyS1");
	snprintf(ttyS2->dev_name, sizeof ttyS2->dev_name, "ttyS2");
	snprintf(ttyS3->dev_name, sizeof ttyS3->dev_name, "ttyS3");

	init_serial_port(COM1, 115200);
	init_serial_port(COM2, 115200);
	init_serial_port(COM3, 115200);
	init_serial_port(COM4, 115200);

	tty_device_register(ttyS0, serialcon_driver, misc_device());
	tty_device_register(ttyS1, serialcon_driver, misc_device());
	tty_device_register(ttyS2, serialcon_driver, misc_device());
	tty_device_register(ttyS3, serialcon_driver, misc_device());

	return KERN_OK;
}

DEFINE_KEXT("net.doorstuck.socks.serialcon",
	    online, NULL,
	    KEXT_NO_DEPENDENCIES);