aedrix-kernel/kernel/print.c

/*
    Copyright (C) 2012, Aaron Lindsay <aaron@aclindsay.com>

    This file is part of Aedrix.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License along
    with this program; if not, write to the Free Software Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <init.h>
#include <list.h>
#include <kmalloc.h>
#include <print.h>
#include <vargs.h>

struct dlist_node print_funcs_list;

struct print_func {
	int (*putc)(char);
	struct dlist_node list;
};

/*
 * On startup, we'd like to have print capabilities before kmalloc is
 * initialized, so statically allocate one struct print_func to allow for the
 * registration of one print 'sink' before kmalloc is initialized.
 */
struct print_func print_func_early;
int print_register_func_early(int (*putc)(char)) {
	if (list_empty(&print_funcs_list)) {
		print_func_early.putc = putc;
		insert_before(&print_funcs_list, &print_func_early.list);
		return 0;
	}
	return -1;
}

int print_register_func(int (*putc)(char)) {
	struct print_func *pf;
	if (!(pf = kmalloc(sizeof(struct print_func))))
		return -1;

	pf->putc = putc;
	insert_before(&print_funcs_list, &pf->list);
	return 0;
}

int print_unregister_func(int (*putc)(char)) {
	struct print_func *it;

	if (list_empty(&print_funcs_list))
		return -1;

	for_each_list(it, &print_funcs_list, struct print_func, list) {
		if (it->putc == putc) {
			remove(&it->list);

			//only kfree if it was kmalloced
			if (it != &print_func_early)
				kfree(it);
			return 0;
		}
	}
	return -1;
}

int print_putc(char c) {
	struct print_func *it;

	if (list_empty(&print_funcs_list))
		return -1;

	for_each_list(it, &print_funcs_list, struct print_func, list)
		it->putc(c);
	return 0;
}

void puts(int (*putc)(char), const char *s)
{
	while (*s) putc(*s++);
}

void puti(int (*putc)(char), int i)
{
	unsigned int left;
	char buf[1 << (sizeof(int)*8) / 10];
	char *p = buf;
	unsigned char negative = i < 0;

	if (!i)
		putc('0');

	left = negative ? -1*i : i;

	while (left) {
		unsigned int remainder = left % 10;
		left /= 10;
		*p = ('0'-0) + remainder;
		p++;
	}

	if (negative)
		putc('-');

	while (p-- != buf)
		putc(*p);
}

void putx(int (*putc)(char), unsigned int i) {
	int j;

	puts(putc, "0x");

	for (j = 0; j < 8; j++) {
		unsigned int toprint = (i >> (4*(7-j))) & 0xf;
		if (toprint < 10)
			putc(('0'-0) + toprint);
		else
			putc(('a'-10) + toprint);
	}
}

void putb(int (*putc)(char), unsigned int i) {
	int j;

	puts(putc, "0b");

	for (j = 0; j < 32; j++) {
		putc((i>>(31-j)) & 1 ? '1' : '0');
	}
}

int _print(int (*putc)(char), char *fmt, va_list arg) {
	char *c;
	for (c = fmt; *c; c++) {
		if (*c == '%') {
			switch (*++c) {
			case '%':
				putc('%');
				break;
			case 's':
				puts(putc, va_arg(arg, char *));
				break;
			case 'c':
				putc(va_arg(arg, unsigned int));
				break;
			case 'd':
				puti(putc, va_arg(arg, int));
				break;
			case 'x':
				putx(putc, va_arg(arg, unsigned int));
				break;
			case 'b':
				putb(putc, va_arg(arg, unsigned int));
				break;
			default:
				puts(putc, "\nError: print(): Invalid formatting character: '");
				putc(*c);
				puts(putc, "'\n");
				return -1;
			}
		} else {
			putc(*c);
			if (*c == '\n')
				putc('\r');
		}
	}
	return 0;
}

int print(char *fmt, ...) {
	int ret;
	va_list arg;

	va_start(arg, fmt);
	ret = _print(&print_putc, fmt, arg);
	va_end(arg);

	return ret;
}


int print_func(int (*putc)(char), char *fmt, ...) {
	int ret;
	va_list arg;

	va_start(arg, fmt);
	ret = _print(putc, fmt, arg);
	va_end(arg);

	return ret;
}

void print_init() {
	init_list(&print_funcs_list);
}

early_initcall(print_init);