Verzeichnisstruktur ver?ndertHEADmaster

6 files changed, 594 insertions, 0 deletions

diff --git a/driver/Makefile b/driver/Makefile
new file mode 100644
index 0000000..12d5789
--- /dev/null
+++ b/driver/Makefile
@@ -0,0 +1,6 @@
+DRIVER_FILES = driver/console.o driver/keyboard.o driver/floppy.o driver/dma.o
+
+driver/driver.o : $(DRIVER_FILES)
+	@echo Linking $*.o
+	@ld -r -o $*.o $(DRIVER_FILES)
+	@echo
diff --git a/driver/console.c b/driver/console.c
new file mode 100644
index 0000000..79f277c
--- /dev/null
+++ b/driver/console.c
@@ -0,0 +1,61 @@
+/* console_cur.c - console functions */
+
+#include <system.h>
+#include <console.h>
+
+unsigned char console_flags		= 0;
+unsigned short console_cur_pos		= 0;
+unsigned char console_std_attr		= 0x07;
+unsigned char console_akt_char_attr	= 0;
+unsigned char cur_blink_counter		= 0;
+
+
+void console_cursor_on() {
+	if(console_flags & FLAG_BLINK_ON) {console_flags &= ~FLAG_BLINK_ON; console_cur_blink();}
+}
+
+void console_cursor_off() {
+	if(console_flags & FLAG_BLINK_ON) {console_cur_blink(); console_flags |= FLAG_BLINK_ON;}
+}
+
+void console_cur_blink() {
+	if(console_flags & FLAG_BLINK_ON) {
+		console_flags &= ~FLAG_BLINK_ON;
+		asm (
+			"push %es\n"
+			"push %ebx\n"
+			"mov $32,%ax\n"
+			"mov %ax,%es\n"
+			"xor %ebx,%ebx\n"
+			"mov console_cur_pos,%bx\n"
+			"shl $1,%ebx\n"
+			"inc %ebx\n"
+			"mov console_akt_char_attr,%al\n"
+			"mov %al,%es:(%ebx)\n"
+			"pop %ebx\n"
+			"pop %es"
+		);
+	}
+	
+	else {
+		console_flags |= FLAG_BLINK_ON;
+		asm (
+			"push %es\n"
+			"push %ebx\n"
+			"mov $32,%ax\n"
+			"mov %ax,%es\n"
+			"xor %ebx,%ebx\n"
+			"mov console_cur_pos,%bx\n"
+			"shl $1,%ebx\n"
+			"inc %ebx\n"
+			"mov console_std_attr,%al\n"
+			"rol $4,%al\n"
+			"and $0b01110111,%al\n"
+			"mov %es:(%ebx),%ah\n"
+			"mov %ah,console_akt_char_attr\n"
+			"mov %al,%es:(%ebx)\n"
+			"pop %ebx\n"
+			"pop %es"
+		);
+	}
+}
diff --git a/driver/console_old.S b/driver/console_old.S
new file mode 100644
index 0000000..9e105c9
--- /dev/null
+++ b/driver/console_old.S
@@ -0,0 +1,14 @@
+.data
+
+.global console_cur_pos, console_std_attr, console_akt_char_attr, cur_blink_counter, console_flags
+
+console_cur_pos:	.word 0
+console_std_attr:	.byte 0b00000111
+console_akt_char_attr:	.byte 0
+
+cur_blink_counter:	.byte 0
+
+
+.global CUR_BLINK
+
+CUR_BLINK = 50
diff --git a/driver/dma.c b/driver/dma.c
new file mode 100644
index 0000000..dc48de2
--- /dev/null
+++ b/driver/dma.c
@@ -0,0 +1,31 @@
+/* dma.c - DMA functions */
+
+#include <system.h>
+#include <dma.h>
+
+void dma_enable(unsigned char dma_nr) {
+	OUT_PORT(dma_nr & 3, PORT_DMA_MASK);
+}
+
+void dma_disable(unsigned char dma_nr) {
+	OUT_PORT((dma_nr & 3) | 4, PORT_DMA_MASK);
+}
+
+void dma_set_base_addr(unsigned char dma_nr, void *addr) {
+	OUT_PORT((unsigned long)addr & 0xFF, (dma_nr & 3)*2);
+	OUT_PORT(((unsigned long)addr >> 8) & 0xFF, (dma_nr & 3)*2);
+	OUT_PORT(((unsigned long)addr >> 16) & 0xFF, dma_page_ports[dma_nr & 3]);
+}
+
+void dma_set_word_count(unsigned char dma_nr, unsigned short words) {
+	OUT_PORT(words & 0xFF, ((dma_nr & 3)*2)+1);
+	OUT_PORT((words >> 8) & 0xFF, ((dma_nr & 3)*2)+1);
+}
+
+void dma_set_mode(unsigned char dma_nr, unsigned char mode) {
+	OUT_PORT(mode | (dma_nr & 3), PORT_DMA_MODE);
+}
+
+void dma_clear_ff() {
+	OUT_PORT(0, PORT_DMA_FF);
+}
diff --git a/driver/floppy.c b/driver/floppy.c
new file mode 100644
index 0000000..6b5d2d9
--- /dev/null
+++ b/driver/floppy.c
@@ -0,0 +1,214 @@
+/* floppy.c - floppy disk driver */
+
+#include <system.h>
+#include <floppy.h>
+#include <dma.h>
+#include <timer.h>
+
+
+unsigned char floppy_flags = 0;
+unsigned char floppy_num_drives = 0;
+unsigned char floppy_drive_type;
+unsigned char floppy_state = FLOPPY_STATE_IDLE;
+unsigned char floppy_waiting = 0;
+unsigned char floppy_requests = 0;
+unsigned char ST0, ST1, ST2, ST3;
+
+struct floppy_request request;
+
+
+static inline void send_floppy_byte(unsigned char byte) {
+	unsigned char status;
+	do IN_PORT_PP(PORT_FLOPPY_STATUS, status); while((status & 0xC0) != 0x80);
+	OUT_PORT(byte, PORT_FLOPPY_COMMAND_DATA);
+}
+
+static inline unsigned char recv_floppy_byte() {
+	unsigned char status;
+	do IN_PORT_PP(PORT_FLOPPY_STATUS, status); while((status & 0xD0) != 0xD0);
+	IN_PORT_PP(PORT_FLOPPY_COMMAND_DATA, status);
+	return status;
+}
+
+static inline void recalibrate(unsigned char drive) {
+	floppy_state = FLOPPY_STATE_RECALIBRATE;
+	floppy_waiting = 1;
+	send_floppy_byte(0x07);
+	send_floppy_byte(drive);
+}
+
+void test() {
+	read_floppy_sectors(0, 0, 0, 1, 1);
+}
+
+void init_floppy() {
+	unsigned short info;
+	unsigned char i;
+	
+	
+	//Kill motor
+	OUT_PORT(0x0C, PORT_FLOPPY_DOR);
+	
+	//List drives
+	asm(
+		"mov $0x28,%%ax\n"
+		"mov %%ax,%%fs\n"
+		"mov %%fs:0x0410,%0"
+		: "=a" (info)
+	);
+	
+	if(info & 1) {
+		floppy_num_drives = ((info >> 6) & 3) + 1;
+		if(floppy_num_drives > 2) floppy_num_drives = 2;
+		if(floppy_num_drives == 1) print("1 floppy drive found\r\n");
+		else print("2 floppy drives found\r\n");
+	}
+	else {
+		print(" No floppy drives found...\r\n");
+		return;
+	}
+	
+	
+	OUT_PORT(0x10, 0x70);
+	IN_PORT(0x71, floppy_drive_type);
+	
+	if((floppy_drive_type >> 4) & 0x0F) {
+		print("   fd0: ");
+		if(((floppy_drive_type >> 4) & 0x0F) > 5) print("unknown");
+		else print(floppy_drive_types[(floppy_drive_type >> 4) & 0x0F]);
+		print("\r\n");
+	}
+	
+	if(floppy_drive_type & 0x0F) {
+		print("   fd1: ");
+		if((floppy_drive_type & 0x0F) > 5) print("unknown");
+		else print(floppy_drive_types[floppy_drive_type & 0x0F]);
+		print("\r\n");
+	}
+	
+	// configure
+	
+	/*OUT_PORT(0x13, PORT_FLOPPY_COMMAND_DATA);
+	OUT_PORT(0, PORT_FLOPPY_COMMAND_DATA);
+	OUT_PORT(0x1F, PORT_FLOPPY_COMMAND_DATA);
+	OUT_PORT(0, PORT_FLOPPY_COMMAND_DATA);
+	
+	// specify
+	
+	OUT_PORT(0x03, PORT_FLOPPY_COMMAND_DATA);
+	OUT_PORT(0xCF, PORT_FLOPPY_COMMAND_DATA);
+	OUT_PORT(0x06, PORT_FLOPPY_COMMAND_DATA);*/
+}
+
+int read_floppy_sectors(unsigned char drive, unsigned char cylinder, unsigned char head, unsigned char sector, unsigned char sector_count) {
+	unsigned char i;
+	
+	// test parameters
+	if(drive != 0 && drive != 1) return 1;
+	else if((drive == 0) && (((floppy_drive_type >> 4) & 0x0F) != 4)) return 2;
+	else if((drive == 1) && ((floppy_drive_type & 0x0F) != 4)) return 2;
+	
+	if(!sector_count) return 3;
+	
+	if(floppy_state == FLOPPY_STATE_IDLE) {
+		
+		//Activate drive motor
+		OUT_PORT((1 << (4 + drive)) | 0x0C, PORT_FLOPPY_DOR);
+		
+		print("TEST\n");
+		
+		sleep(5000);
+		
+		print("TEST\n");
+		
+		//Recalibrate
+		//recalibrate(drive);
+	}
+	
+	return 0;
+}
+
+void floppy_proc() {
+	unsigned char i;
+	
+	/*IN_PORT(PORT_FLOPPY_STATUS, i);
+	print_hex_byte(i);*/
+	//dma_clear_ff();
+	//IN_PORT(0x04, i);
+	//print_hex_byte(i);
+	//IN_PORT(0x04, i);
+	//print_hex_byte(i);
+	/*IN_PORT(0x81, i);
+	print_hex_byte(i);*/
+	//IN_PORT(0x05, i);
+	//print_hex_byte(i);
+	/*IN_PORT(0x05, i);
+	print_hex_byte(i);*/
+	
+	if(floppy_waiting) return;
+	
+	if(floppy_state == FLOPPY_STATE_RECALIBRATE) {
+		floppy_state = FLOPPY_STATE_READ;
+		floppy_waiting = 1;
+		 
+		dma_disable(2);
+		dma_clear_ff();
+		dma_set_mode(2, DMA_MODE_SINGLE | DMA_MODE_READ | DMA_MODE_INC_ADDR);
+		dma_set_base_addr(2, (void*)0x200000);
+		dma_set_word_count(2, 0xFFFF);
+		dma_enable(2);
+		
+		send_floppy_byte(0xE6);
+		send_floppy_byte(0x00);
+		send_floppy_byte(0x00);
+		send_floppy_byte(0x00);
+		send_floppy_byte(0x02);
+		send_floppy_byte(0x02);
+		send_floppy_byte(0x01);
+		send_floppy_byte(0x1B);
+		send_floppy_byte(0xFF);
+	}
+}
+
+void floppy_IRQ() {
+	//floppy_waiting = 0;
+	switch(floppy_state) {
+		case FLOPPY_STATE_RECALIBRATE:
+			print("Recalibrate FDC interrupt\n");
+			//IN_PORT(PORT_FLOPPY_STATUS, i);
+			//print_hex_byte(i);
+			//floppy_state = FLOPPY_STATE_READ;
+ 			//floppy_waiting = 0;
+			send_floppy_byte(0x08);
+			//print_hex_byte(recv_floppy_byte());
+			//print_hex_byte(recv_floppy_byte());
+			ST0 = recv_floppy_byte();
+			ST1 = recv_floppy_byte();
+			
+			/*dma_disable(2);
+			dma_clear_ff();
+			dma_set_mode(2, DMA_MODE_SINGLE | DMA_MODE_READ | DMA_MODE_INC_ADDR);
+			dma_set_base_addr(2, (void*)0x100000);
+			dma_set_word_count(2, 512);
+			dma_enable(2);
+			
+			send_floppy_byte(0xE6);
+			send_floppy_byte(0x00);
+			send_floppy_byte(0x00);
+			send_floppy_byte(0x00);
+			send_floppy_byte(0x01);
+			send_floppy_byte(0x02);
+			send_floppy_byte(0x01);
+			send_floppy_byte(0x1B);
+			send_floppy_byte(0xFF);*/
+	
+			break;
+		case FLOPPY_STATE_READ:
+			print("Read FDC interrupt\n");
+			floppy_state = FLOPPY_STATE_IDLE;
+			print_hex_long(*(unsigned long*)0x1FFB00);
+			break;
+		default:
+			print("Unexpected FDC interrupt!\n");
+	}
+}
diff --git a/driver/keyboard.c b/driver/keyboard.c
new file mode 100644
index 0000000..26a12f0
--- /dev/null
+++ b/driver/keyboard.c
@@ -0,0 +1,268 @@
+/* keyboard.c - keyboard functions */
+
+#include <system.h>
+#include <keyboard.h>
+#include <console.h>
+
+unsigned short keyflags = 0;
+unsigned short key_extended_buffer;
+unsigned char keybuffer[256];
+unsigned char keybuffer_pointer = 0, keybuffer_count = 0;
+
+unsigned char keydata[128] = {
+	0, 0, '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', 0xDF, '\'', '\b', '\t',
+	'q', 'w', 'e', 'r', 't', 'z', 'u', 'i', 'o', 'p', 0xFC, '+', '\n', 0, 'a', 's',
+	'd', 'f', 'g', 'h', 'j', 'k', 'l', 0xF6, 0xE4, '^', 0, '#', 'y', 'x', 'c', 'v',
+	'b', 'n', 'm', ',', '.', '-', 0, '*', 0, ' ', 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0x0B, 0, 0, '-', 0, 0, 0, '+', 0,
+	0, 0, 0, 0, 0, 0, '<', 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+unsigned char keydata_shift[128] = {
+	0, 0, '!', '"', 0xA7, '$', '%', '&', '/', '(', ')', '=', '?', '`', '\b', '\t',
+	'Q', 'W', 'E', 'R', 'T', 'Z', 'U', 'I', 'O', 'P', 0xDC, '*', '\n', 0, 'A', 'S',
+	'D', 'F', 'G', 'H', 'J', 'K', 'L', 0xD6, 0xC4, 0xB0, 0, '\'', 'Y', 'X', 'C', 'V',
+	'B', 'N', 'M', ';', ':', '_', 0, '*', 0, ' ', 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0x0B, 0, 0, '-', 0, 0, 0, '+', 0,
+	0, 0, 0, 0, 0, 0, '>', 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+unsigned char keydata_extended[128] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '\n', 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, '/', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0x0B, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+unsigned char keydata_altgr[128] = {
+	0, 0, 0, 0xB2, 0xB3, 0, 0, 0, '{', '[', ']', '}', '\\', 0, '\b', '\t',
+	'@', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, '~', '\n', 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0xB5, 0, 0, 0, 0, '*', 0, ' ', 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0x0B, 0, 0, '-', 0, 0, 0, '+', 0,
+	0, 0, 0, 0, 0, 0, '|', 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+unsigned char keydata_numlock[13] = {
+	'7', '8', '9', '-', '4', '5', '6', '+', '1', '2', '3', '0', ','
+};
+
+
+void update_leds() {
+	register unsigned char al asm("%al");
+	do {
+		asm (
+			"in $0x64,%al\n"
+			"and $0b00000010,%al\n"
+		);
+	} while (al);
+	asm (
+		"mov $0xED,%al\n"
+		"out %al,$0x60\n"
+	);
+	do {
+		asm (
+			"in $0x64,%al\n"
+			"and $0b00000010,%al\n"
+		);
+	} while (al);
+	asm (
+		"mov %0,%%eax\n"
+		"and $0b11100000,%%al\n"
+		"shr $5,%%al\n"
+		"out %%al,$0x60\n"
+		: : "m" (keyflags)
+	);
+}
+
+
+void kb_write_keycode() {
+	
+	unsigned char keycode;
+	unsigned long fullcode;
+	unsigned char ansicode = 0;
+	
+	unsigned char *keytable = keydata;
+	
+	asm ("mov %%al,%0" : "=m" (keycode));
+	
+	fullcode = (keycode & KEY_CODE);
+	
+	if(keycode==ACKNOWLEDGE_CODE) return;
+	
+	else if(keycode==EXTENDED0_CODE) {keyflags |= FLAG_EXTENDED_KEY; key_extended_buffer = EXTENDED0_CODE;}
+	
+	else if(keycode==EXTENDED1_CODE) {keyflags |= FLAG_EXTENDED_KEY; key_extended_buffer = EXTENDED1_CODE;}
+	
+	else {
+		
+		if(keyflags & FLAG_EXTENDED_KEY) {
+			if(key_extended_buffer==EXTENDED0_CODE) {
+				fullcode = ((keycode & KEY_CODE) | (EXTENDED0_CODE << 8));
+				keyflags &= ~FLAG_EXTENDED_KEY;
+			}
+			else if(key_extended_buffer==EXTENDED1_CODE) {
+				fullcode = ((keycode & KEY_CODE) | (EXTENDED1_CODE << 8));
+				key_extended_buffer = fullcode;
+			}
+			else {
+				fullcode = ((keycode & KEY_CODE) | (((unsigned long)key_extended_buffer) << 8));
+				keyflags &= ~FLAG_EXTENDED_KEY;
+			}
+		}
+		
+		
+		if(keycode & KEY_RELEASE) {
+			switch(fullcode) {
+				case NUMLOCK_CODE:
+					keyflags &= ~FLAG_NUMLOCK_PRESSED;
+					break;
+				case SCROLLLOCK_CODE:
+					keyflags &= ~FLAG_SCROLLLOCK_PRESSED;
+					break;
+				case CAPSLOCK_CODE:
+					keyflags &= ~FLAG_CAPSLOCK_PRESSED;
+					break;
+				case LSHIFT_CODE:
+					keyflags &= ~FLAG_LSHIFT_PRESSED;
+					break;
+				case RSHIFT_CODE:
+					keyflags &= ~FLAG_RSHIFT_PRESSED;
+					break;
+				case LALT_CODE:
+					keyflags &= ~FLAG_LALT_PRESSED;
+					break;
+				case RALT_CODE:
+					keyflags &= ~FLAG_RALT_PRESSED;
+					break;
+				case LCTRL_CODE:
+					keyflags &= ~FLAG_LCTRL_PRESSED;
+					break;
+				case RCTRL_CODE:
+					keyflags &= ~FLAG_RCTRL_PRESSED;
+					break;
+				default:
+					if((keyflags & FLAG_NUMLOCK_ACTIVE) && (0x47 <= (fullcode & ~KEY_RELEASE)) && ((fullcode & ~KEY_RELEASE) <= 0x53))
+						ansicode = keydata_numlock[(keycode & KEY_CODE) - 0x47];
+					else {
+						if((fullcode & (EXTENDED0_CODE << 8)) == (EXTENDED0_CODE << 8)) keytable = keydata_extended;
+						else if(keyflags & FLAG_RALT_PRESSED) keytable = keydata_altgr;
+						else if((keyflags & FLAG_LALT_PRESSED)/* && ((keyflags & FLAG_RCTRL_PRESSED)
+							|| (keyflags & FLAG_LCTRL_PRESSED))*/) keytable = keydata_altgr;
+						else if(keyflags & (FLAG_LSHIFT_PRESSED | FLAG_RSHIFT_PRESSED | FLAG_CAPSLOCK_ACTIVE))
+							keytable = keydata_shift;
+						ansicode = keytable[keycode & KEY_CODE];
+					}
+			}
+		}
+		else {
+			switch(fullcode) {
+				case NUMLOCK_CODE:
+					if(keyflags & FLAG_NUMLOCK_PRESSED) return;
+					else {
+						keyflags |= FLAG_NUMLOCK_PRESSED;
+						if(keyflags & FLAG_NUMLOCK_ACTIVE) keyflags &= ~FLAG_NUMLOCK_ACTIVE;
+						else keyflags |= FLAG_NUMLOCK_ACTIVE;
+						update_leds();
+					}
+					break;
+				case SCROLLLOCK_CODE:
+					if(keyflags & FLAG_SCROLLLOCK_PRESSED) return;
+					else {
+						keyflags |= FLAG_SCROLLLOCK_PRESSED;
+						if(keyflags & FLAG_SCROLLLOCK_ACTIVE) keyflags &= ~FLAG_SCROLLLOCK_ACTIVE;
+						else keyflags |= FLAG_SCROLLLOCK_ACTIVE;
+						update_leds();
+					}
+					break;
+				case CAPSLOCK_CODE:
+					if(keyflags & FLAG_CAPSLOCK_PRESSED) return;
+					else {
+						keyflags |= FLAG_CAPSLOCK_PRESSED;
+						if(keyflags & FLAG_CAPSLOCK_ACTIVE) keyflags &= ~FLAG_CAPSLOCK_ACTIVE;
+						else keyflags |= FLAG_CAPSLOCK_ACTIVE;
+						update_leds();
+					}
+					break;
+				case LSHIFT_CODE:
+					keyflags |= FLAG_LSHIFT_PRESSED;
+					break;
+				case RSHIFT_CODE:
+					keyflags |= FLAG_RSHIFT_PRESSED;
+					break;
+				case LALT_CODE:
+					keyflags |= FLAG_LALT_PRESSED;
+					break;
+				case RALT_CODE:
+					keyflags |= FLAG_RALT_PRESSED;
+					break;
+				case LCTRL_CODE:
+					keyflags |= FLAG_LCTRL_PRESSED;
+					break;
+				case RCTRL_CODE:
+					keyflags |= FLAG_RCTRL_PRESSED;
+					break;
+				default:
+					if((keyflags & FLAG_NUMLOCK_ACTIVE) && (0x47 <= fullcode) && (fullcode <= 0x53))
+						ansicode = keydata_numlock[keycode - 0x47];
+					else {
+						if((fullcode & (EXTENDED0_CODE << 8)) == (EXTENDED0_CODE << 8)) keytable = keydata_extended;
+						else if(keyflags & FLAG_RALT_PRESSED) keytable = keydata_altgr;
+						else if(keyflags & (FLAG_LSHIFT_PRESSED | FLAG_RSHIFT_PRESSED | FLAG_CAPSLOCK_ACTIVE))
+							keytable = keydata_shift;
+						ansicode = keytable[keycode];
+					}
+			}
+		}
+	}
+	if(keyflags & FLAG_BUFFER_FULL) return;
+	keybuffer[(keybuffer_pointer + keybuffer_count++) & 0xFF] = keycode;
+	keybuffer[(keybuffer_pointer + keybuffer_count++) & 0xFF] = ansicode;
+	if(keybuffer_count==0) keyflags |= FLAG_BUFFER_FULL;
+}
+
+
+void kb_read_keycode() {
+	unsigned long keycode = 0;
+	unsigned char ansicode = 0;
+	
+	if((keybuffer_count != 0) || (keyflags & FLAG_BUFFER_FULL)) {
+		keycode = keybuffer[keybuffer_pointer++];
+		ansicode = keybuffer[keybuffer_pointer++];
+		keybuffer_count-=2;
+		keyflags &= ~FLAG_BUFFER_FULL;
+		if(keycode == EXTENDED0_CODE) {
+			if(keybuffer_count != 0) {
+				keycode <<= 8;
+				keycode |= keybuffer[keybuffer_pointer++];
+				ansicode = keybuffer[keybuffer_pointer++];
+				keybuffer_count-=2;
+			}
+		}
+		else if(keycode == EXTENDED1_CODE) {
+			if(keybuffer_count != 0) {
+				keycode <<= 8;
+				keycode |= keybuffer[keybuffer_pointer++];
+				ansicode = keybuffer[keybuffer_pointer++];
+				keybuffer_count-=2;
+				if(keybuffer_count != 0) {
+					keycode <<= 8;
+					keycode |= keybuffer[keybuffer_pointer++];
+					ansicode = keybuffer[keybuffer_pointer++];
+					keybuffer_count-=2;
+				}
+			}
+		}
+	}
+	asm ("mov %0,%%eax; mov %1,%%dl" : : "m" (keycode), "m" (ansicode));
+}