printf.c

__asm__(".code16gcc");

unsigned char *vga = (unsigned char *) 0xB8000;
char attrib = 0x0F;

unsigned int getX() {

    unsigned short pos;

    // Get cursor position
    __asm__("mov $0x03, %%ah \n\
         mov $0x00, %%bh \n\
         int $0x10":"=d"(pos):);

    // Set x & y:
    int x = (pos>>0) & 0xFF; // column (DL)
    int y = (pos>>8) & 0xFF; // row    (DH)

    // return X
    return x;

}

unsigned int getY() {

    unsigned short pos;

    // Get cursor position
    __asm__("mov $0x03, %%ah \n\
         mov $0x00, %%bh \n\
         int $0x10":"=d"(pos):);

    // Set x & y:
    int x = (pos>>0) & 0xFF; // column (DL)
    int y = (pos>>8) & 0xFF; // row    (DH)

    // return Y
    return y;

}

void cursor(int x, int y) {

    unsigned short pos = 0;

    // Get x & y:
    pos |= x<<0;  // column (DL)
    pos |= y<<8;  // row    (DH)

    // Set cursor position
    __asm__("mov $0x02, %%ah \n\
         mov $0x00, %%bh \n\
         int $0x10"::"d"(pos));

}

void cls() {
    int i = 0;
    while(i < 80*25*2) {
        vga[i++] = ' ';
        vga[i++] = attrib;
    }
    cursor(0, 0);
}

void scroll() {
    unsigned int i;
    for (i=0; i< 80 * 24 * 2; i++)
        vga[i] = vga[i + 80 * 2];

    // clean last row:
    for (; i < 80 * 25 * 2; i+=2)
        vga[i] = ' ';
}

void putc(unsigned char chr) {

    int new_x, new_y;

    if (chr == '\n') {
        do putc(' '); while(getX());
        return;
    }

    int x = getX();
    int y = getY();

    vga[(y*80 + x)*2 + 0] = chr;
    vga[(y*80 + x)*2 + 1] = attrib;

    if (x != 79) {
        new_x = x+1;
        new_y = y;
    } else {
        new_x = 0;
        if (y != 24) {
            new_y = y+1;
        } else {
            scroll();
            new_y = y;
        }
    }

    cursor(new_x, new_y);
}

void printDec(unsigned int value) {
    unsigned int n = value / 10;
    int r = value % 10;
    if (value >= 10) printDec(n);
    putc(r+'0');
}

void printHex(unsigned int value) {
    int i;
    for(i = 7; i >= 0; i--)
        putc("0123456789ABCDEF"[(value>> (i*4)) & 0x0F]);
}

void printf(char *format, ...) {

    unsigned int arg_id = 0, i;
    int *addr = (int *) &format;

    for (i = 0; format[i] != 0; i++)
        if (format[i] == '%')
            switch (format[++i]) {
                case 'c':
                    putc(addr[++arg_id]);
                    break;

                case 'd':
                    printDec(addr[++arg_id]);
                    break;

                case 'x':
                    printHex(addr[++arg_id]);
                    break;

                case 's':
                    printf((char *) addr[++arg_id]);
                    break;

                default:
                    break;
            }
        else
            putc(format[i]);

}

void video_mode(unsigned char mode) {
    __asm__("int $0x10"::"a"(mode));
}