Mercurial > lcfOS

changeset 24:d8627924d40d

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Split up in more files and reboot command
author windel
date Fri, 02 Dec 2011 14:00:02 +0100
parents 5dd47d6eebac
children d3c4bf3720a3
files .hgignore cos/Makefile cos/bochsrc.txt cos/kernel/goto64.asm cos/kernel/handlers.c cos/kernel/kernel.c cos/kernel/kernel.h cos/kernel/keyboard.c cos/kernel/klib.c cos/kernel/malloc.c cos/kernel/timer.c cos/kernel/video.c
diffstat 12 files changed, 332 insertions(+), 256 deletions(-) [+]
line wrap: on
line diff
--- a/.hgignore	Thu Dec 01 21:42:59 2011 +0100
+++ b/.hgignore	Fri Dec 02 14:00:02 2011 +0100
@@ -3,5 +3,8 @@
 
 *.pyc
 *.o
+*.bin
 
 cos/python
+cos/bootdisk.img
+
--- a/cos/Makefile	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/Makefile	Fri Dec 02 14:00:02 2011 +0100
@@ -22,7 +22,11 @@
 	kernel/snprintf.o \
 	kernel/kernel.o \
 	kernel/asmcode.o \
-	kernel/handlers.o
+	kernel/handlers.o \
+	kernel/keyboard.o \
+	kernel/klib.o \
+	kernel/malloc.o \
+	kernel/timer.o
 
 lcfosc.bin: $(CRT0) $(OBJECTS) linker.ld
 	ld -T linker.ld -s -o lcfosc.bin $(CRT0) $(OBJECTS)
--- a/cos/bochsrc.txt	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/bochsrc.txt	Fri Dec 02 14:00:02 2011 +0100
@@ -1,4 +1,4 @@
-#display_library: x, options="gui_debug" # use GTK debugger gui
+display_library: x, options="gui_debug" # use GTK debugger gui
 romimage: file=$BXSHARE/BIOS-bochs-latest 
 cpu: count=1, ips=500000, reset_on_triple_fault=1, ignore_bad_msrs=1, msrs="msrs.def"
 cpuid: mmx=1, sep=1, sse=sse4_2, xapic=1, aes=1, movbe=1, xsave=1, cpuid_limit_winnt=0
--- a/cos/kernel/goto64.asm	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/kernel/goto64.asm	Fri Dec 02 14:00:02 2011 +0100
@@ -10,12 +10,22 @@
 ; Use 2 mbyte pages. Is this more efficient?
 
 ; Intended memory map (copied from pure64), at the end of this file:
-; 0x0 : IDT, 256 entries
+; MOVED TO 0x5000!! 0x0 : IDT, 256 entries
 ; 0x1000 - 0x2000 : PML4 (Page map level 4)
 ; 0x2000 - 0x3000 : PDPT (page directory pointer table)
 ; 0x3000 - 0x4000 : PDT (page directory table)
 ; 0x4000 - 0x5000 : PT (page table)
-; 0x5000 - 0xA000 : Stack
+; 0x5000 - 0x6000 : IDT entries
+; 0x6000 - 0xA000 : Stack
+
+; panic macro to debug on real hardware
+%macro DOPANIC 1
+mov al, %1
+mov edi, 0xb8000
+stosb
+xchg bx,bx
+hlt
+%endmacro
 
 bits 32 ; Start in 32 bits mode, as loaded by GRUB
 
@@ -109,7 +119,7 @@
 ; Print long mode not supported
 mov edi, 0xb8000
 mov esi, hltmessage
-xor eax,eax
+xor eax, eax
 loop1:
 lodsb
 mov dl, al
@@ -118,7 +128,6 @@
 stosb
 cmp dl, 0
 jne loop1
-
 hlt
 
 cpu_has_long_mode:
@@ -138,7 +147,7 @@
 ; Clear the paging tables 0x1000, 0x2000, 0x3000 and 0x4000:
 mov edi, 0x1000
 xor eax, eax
-mov ecx, 4096
+mov ecx, 0x1000
 rep stosd
 
 ; Create PML4 table:
@@ -180,8 +189,7 @@
 or eax, 1 << 5 ; PAE-bit is bit 5
 mov cr4, eax
 
-; Load the GDT:
-lgdt [gdt64pointer]
+lgdt [gdt64pointer]              ; Load the GDT
 
 ; Set LM-bit (Long Mode bit):
 mov ecx, 0xC0000080
@@ -214,14 +222,6 @@
 
 mov rsp, 0xA000      ; Setup stack pointer.
 
-# XCHG BX, BX ; bochs breakpoint
-
 extern kmain
 call kmain              ; Call kernel in C-code
 
-# Should we ever return, remain in endless loop:
-cli
-hang:
-  hlt
-  jmp hang
-
--- a/cos/kernel/handlers.c	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/kernel/handlers.c	Fri Dec 02 14:00:02 2011 +0100
@@ -1,12 +1,5 @@
 #include "kernel.h"
 
-void panic(char *msg) {
-  printf("Kernel panic: ");
-  printf(msg);
-  magicBochsBreak();
-  halt();
-}
-
 // Assembler wrapper prototypes:
 void INTDEF(void);
 void INT0(void);
@@ -35,7 +28,7 @@
 void INT34(void);
 
 // THE interrupt descriptor table:
-IDT_entry *idt = (IDT_entry*)0x0;
+IDT_entry *idt = (IDT_entry*)0x5000;
 volatile idtPointer idtP;
 
 void setIDTentry(int num, void (*handler)(), uint16_t selector, uint8_t flags)
@@ -46,6 +39,8 @@
   // Typecast the function pointer to a number:
   offset = (uint64_t)handler;
 
+  //panic("Almost setting an IDT entry");
+
   // Set offset:
   idt[num].baseLow = offset & 0xFFFF;
   idt[num].baseMid = (offset >> 16) & 0xFFFF;
@@ -62,10 +57,13 @@
 
 void setupIDT(void) {
   int i;
+  //panic("Just before filling IDT");
+  // After this line a crash occurs!
   // Fill all vectors with the default handler:
   for (i=0; i<256; i++) {
-    setIDTentry(i, &INTDEF, 0x08, 0x8E);
+    setIDTentry(i, INTDEF, 0x08, 0x8E);
   }
+  //panic("Just after setting defhandlers in IDT");
 
   // Now set other then default handler:
   setIDTentry(0, INT0, 0x08, 0x8E);
@@ -98,9 +96,11 @@
   idtP.base = (uint64_t)idt;
   idtP.limit = (sizeof(IDT_entry) * 256) - 1;
   // call load IDT asm function:
+  //panic("Just before LIDT");
   loadIDT();
 
   PICremap();
+  //panic("Just before sti");
   asm("sti");
 }
 
--- a/cos/kernel/kernel.c	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/kernel/kernel.c	Fri Dec 02 14:00:02 2011 +0100
@@ -1,211 +1,9 @@
 #include "kernel.h"
 
-static int shiftstate = 0;
-static volatile uint8_t charAvail = 0;
-static volatile char kbdchar = ' ';
-
-static char keymap[128] = {
-  '?','?','1','2', '3', '4', '5','6', '7', '8','9', '0', '-','=', 0xe, '?',
-  'q','w','e','r', 't', 'y', 'u','i', 'o', 'p','[', ']', '\n','?', 'a', 's',
-  'd','f','g','h', 'j', 'k', 'l',';', '\'', '?','?', '?', 'z','x', 'c', 'v',
-  'b','n','m',',', '.', '/', '?','?', '?', ' ','?', '?', '?','?', '?', '?',
-
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?'
-};
-
-static char keymapUPPER[128] = {
-  '?','?','!','@', '#', '$', '%','^', '&', '*','(', ')', '_','+', '?', '?',
-  'Q','W','E','R', 'T', 'Y', 'U','I', 'O', 'P','{', '}', '|','?', 'A', 'S',
-  'D','F','G','H', 'J', 'K', 'L',':', '"', '?','?', '?', 'Z','X', 'C', 'V',
-  'B','N','M','<', '>', '?', '?','?', '?', ' ','?', '?', '?','?', '?', '?',
-
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
-  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?'
-};
-
-// IO port helpers:
-void outb(uint16_t port, uint8_t value)
-{
-   asm volatile ("outb %1, %0" : : "dN" (port), "a" (value));
-}
-
-uint8_t inb(uint16_t port)
-{
-   uint8_t ret;
-   asm volatile ("inb %1, %0" : "=a" (ret) : "dN" (port));
-   return ret;
-}
-
-uint16_t inw(uint16_t port)
-{
-   uint16_t ret;
-   asm volatile ("inw %1, %0" : "=a" (ret) : "dN" (port));
-   return ret;
-}
-
-
-static uint64_t ticks = 0;
-void timerDriverUpdate()
-{
-  ticks++;
-}
-
-uint64_t getTimeMS()
-{
-  return 55*ticks;
-}
-
-int strncmp(const char* s1, const char* s2, int size) {
-  int i;
-  for (i=0; i<size; i++) {
-    if (s1[i] != s2[i]) return 0;
-  }
-  return 1;
-}
-
-void keyboardDriverUpdate(unsigned char scancode)
-{
-  switch(scancode) {
-    case 0x2a:
-      shiftstate = 1;
-      break;
-    case 0xaa:
-      shiftstate = 0;
-      break;
-    default:
-      if (scancode < 128) {
-        if (charAvail == 0) {
-          if (shiftstate == 0) {
-            kbdchar = keymap[scancode];
-          } else {
-            kbdchar = keymapUPPER[scancode];
-          }
-          
-          charAvail = 1;
-        }
-      } else {
-        // Key release
-        //printf("Unhandled scancode: 0x%x\n", scancode);
-      }
-      break;
-  }
-}
-
-char getChar() {
-  while (charAvail == 0);
-  char c = kbdchar;
-  charAvail = 0;
-  return c;
-}
-
-void getline(char *buffer, int len) {
-  char c;
-  int i = 0;
-  while (i < len-1) {
-    c = getChar();
-    if (c == '\n') {
-      // Enter
-      break;
-    }
-    if (c == 0x0e) {
-      if (i>0) {
-         printf(" ");
-         i--;
-      }
-      continue;
-    }
-    buffer[i] = c;
-    printf("%c", c);
-    i++;
-  }
-  buffer[i] = 0;
-}
-
-#define HEAP_MAGIC 0xc0ffee
-#define HEAP_START 0x400000
-#define HEAP_SIZE  0x200000
-#define HEAP_INUSE 1
-#define HEAP_FREE 0
-
-typedef struct {
-   uint64_t magic;
-   uint64_t state;
-   uint64_t size;
-} heap_t;
-
-/*
-  malloc and free divide the chunks of memory present at the heap
-  of the kernel into smaller parts.
-  The heap is located at: 0x
-*/
-static heap_t* kernel_heap = (heap_t*) 0x400000; // 4 MB - 6 MB is heap
-/* Allocates 'size' bytes and returns the pointer if succesfull.
-   Kernelpanic in case of failure..
-*/
-
-void* kmalloc(uint64_t size) {
-   // printf("Malloc %d bytes\n", size);
-
-   // Start at the beginning of our heap and search a free block:
-   heap_t *current = kernel_heap;
-   while (current->magic == HEAP_MAGIC)
-   {
-      if ((current->state == HEAP_FREE) && (current->size >= size))
-      {
-         // Mark block as used:
-         current->state = HEAP_INUSE;
-
-         // Insert a heap header if required:
-         if (current->size > size + sizeof(heap_t) + 1)
-         {
-            // Calculate location of the inserted header:
-            heap_t *newheader = (heap_t*) (((char*)current)+size+sizeof(heap_t));
-
-            // Set the new header fields:
-            newheader->size = current->size - size - sizeof(heap_t);
-            newheader->state = HEAP_FREE;
-            newheader->magic = HEAP_MAGIC;
-
-            // Set the size of this block
-            current->size = size; 
-         }
-         else
-         {
-            // We allocate this whole block
-         }
-         // Calculate the size of the block:
-         char *address = ((char*)current)+sizeof(heap_t);
-         return address;
-         
-      }
-      // Goto next heap block:
-      current = (heap_t*)(((char*) current) + current->size + sizeof(heap_t));
-   }
-   return 0x0;
-}
-
-void kfree(void* ptr) {
-  printf("Free address %x\n", ptr);
-}
-
-void init_heap(void)
-{
-   // Initialize the kernel heap:
-   kernel_heap->magic = HEAP_MAGIC;
-   kernel_heap->state = HEAP_FREE;
-   kernel_heap->size = HEAP_SIZE - sizeof(heap_t);
-}
-
 void startPython()
 {
    // TODO: connect to Py_Main
    //PyRun_SimpleString("print('hello world')");
-
 }
 
 void testMalloc()
@@ -222,10 +20,19 @@
    kfree(a);
 }
 
+void reboot()
+{
+   while ( (inb(0x64) & 0x02) == 0x02)
+   {
+      ;
+   }
+   outb(0x64, 0xFE);
+}
+
 void kmain()
 {
   init_screen();
-  setupIDT();
+  setupIDT(); // This causes error on real hardware
   init_heap();
 
   printf("Welcome!\n");
@@ -237,7 +44,6 @@
     getline(buffer, 70);
     // TODO: interpret this line with python :)
     printf("\n");
-    printf("Got line: '%s'\n", buffer);
     if (buffer[0] == 'x') 
     {
       printf("System time in ms: %d\n", getTimeMS());
@@ -251,6 +57,10 @@
       printf("Help\n Try one of these commands:\n");
       printf(" x: print system time in ms\n");
     }
+    if (strncmp(buffer, "r", 1))
+    {
+       reboot();
+    }
   }
 }
 
--- a/cos/kernel/kernel.h	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/kernel/kernel.h	Fri Dec 02 14:00:02 2011 +0100
@@ -27,6 +27,7 @@
 } __attribute__((packed)) idtPointer;
 
 // memory alloc functions:
+void init_heap();
 void* kmalloc(uint64_t size);
 void kfree(void* ptr);
 
@@ -34,11 +35,15 @@
 void printf(const char* fmt, ... );
 void memset(void* ptr, uint32_t value, uint32_t num);
 void memcpy(void* dst, void* src, uint32_t num);
+int strncmp(const char* s1, const char* s2, int size); 
 
 // Screen related:
 void clear_screen();
 void init_screen();
 void print_string(const char *);
+void set_cursor(int newrow, int newcol);
+void get_cursor(int *therow, int *thecol);
+void set_color(int forecolor, int backcolor);
 
 // For IO ports:
 uint8_t inb(uint16_t);
@@ -54,6 +59,7 @@
 
 // Panic exit:
 void halt(void);
+void panic(char *msg);
 
 // Bochs xchg bx,bx breakpoint:
 void magicBochsBreak();
@@ -63,7 +69,11 @@
 
 // Keyboard driver:
 void keyboardDriverUpdate(unsigned char scancode);
+void getline(char *buffer, int len);
+
+// Timer:
 void timerDriverUpdate(void);
+uint64_t getTimeMS();
 
 // Memory functions:
 void mappage(uint64_t address);
@@ -106,7 +116,8 @@
   uint32_t type;
 };
 
-typedef struct {
+typedef struct 
+{
   char name[32]; // Name of the console
   unsigned char screendata[80*25]; // All chars in the console!
 } console_t;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cos/kernel/keyboard.c	Fri Dec 02 14:00:02 2011 +0100
@@ -0,0 +1,98 @@
+#include "kernel.h"
+
+static int shiftstate = 0;
+static volatile uint8_t charAvail = 0;
+static volatile char kbdchar = ' ';
+
+static char keymap[128] = {
+  '?','?','1','2', '3', '4', '5','6', '7', '8','9', '0', '-','=', 0xe, '?',
+  'q','w','e','r', 't', 'y', 'u','i', 'o', 'p','[', ']', '\n','?', 'a', 's',
+  'd','f','g','h', 'j', 'k', 'l',';', '\'', '?','?', '?', 'z','x', 'c', 'v',
+  'b','n','m',',', '.', '/', '?','?', '?', ' ','?', '?', '?','?', '?', '?',
+
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?'
+};
+
+static char keymapUPPER[128] = {
+  '?','?','!','@', '#', '$', '%','^', '&', '*','(', ')', '_','+', '?', '?',
+  'Q','W','E','R', 'T', 'Y', 'U','I', 'O', 'P','{', '}', '|','?', 'A', 'S',
+  'D','F','G','H', 'J', 'K', 'L',':', '"', '?','?', '?', 'Z','X', 'C', 'V',
+  'B','N','M','<', '>', '?', '?','?', '?', ' ','?', '?', '?','?', '?', '?',
+
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?',
+  '?','?','?','?', '?', '?', '?','?', '?', '?','?', '?', '?','?', '?', '?'
+};
+
+void keyboardDriverUpdate(unsigned char scancode)
+{
+  switch(scancode) {
+    case 0x2a:
+      shiftstate = 1;
+      break;
+    case 0xaa:
+      shiftstate = 0;
+      break;
+    default:
+      if (scancode < 128) {
+        if (charAvail == 0) {
+          if (shiftstate == 0) {
+            kbdchar = keymap[scancode];
+          } else {
+            kbdchar = keymapUPPER[scancode];
+          }
+          
+          charAvail = 1;
+        }
+      } else {
+        // Key release
+        //printf("Unhandled scancode: 0x%x\n", scancode);
+      }
+      break;
+  }
+}
+
+char getChar() 
+{
+  while (charAvail == 0);
+  char c = kbdchar;
+  charAvail = 0;
+  return c;
+}
+
+void getline(char *buffer, int len) 
+{
+  char c;
+  int i = 0;
+  while (i < len-1) {
+    c = getChar();
+    //printf("%x", c);
+    if (c == '\n') {
+      // Enter
+      break;
+    }
+    if (c == 0x0e) 
+    {
+       // Backspace
+       if (i>0) 
+       {
+          int r, c;
+          get_cursor(&r, &c);
+          set_cursor(r, c - 1);
+          printf(" ");
+          set_cursor(r, c - 1);
+          i--;
+       }
+       continue;
+    }
+    buffer[i] = c;
+    printf("%c", c);
+    i++;
+  }
+  buffer[i] = 0;
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cos/kernel/klib.c	Fri Dec 02 14:00:02 2011 +0100
@@ -0,0 +1,41 @@
+#include "kernel.h"
+
+void panic(char *msg) 
+{
+   printf("Kernel panic: ");
+   printf(msg);
+   magicBochsBreak();
+   halt();
+}
+
+// IO port helpers:
+void outb(uint16_t port, uint8_t value)
+{
+   asm volatile ("outb %1, %0" : : "dN" (port), "a" (value));
+}
+
+uint8_t inb(uint16_t port)
+{
+   uint8_t ret;
+   asm volatile ("inb %1, %0" : "=a" (ret) : "dN" (port));
+   return ret;
+}
+
+uint16_t inw(uint16_t port)
+{
+   uint16_t ret;
+   asm volatile ("inw %1, %0" : "=a" (ret) : "dN" (port));
+   return ret;
+}
+
+// string functions:
+int strncmp(const char* s1, const char* s2, int size) 
+{
+  int i;
+  for (i=0; i<size; i++) 
+  {
+    if (s1[i] != s2[i]) return 0;
+  }
+  return 1;
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cos/kernel/malloc.c	Fri Dec 02 14:00:02 2011 +0100
@@ -0,0 +1,76 @@
+#include "kernel.h"
+
+#define HEAP_MAGIC 0xc0ffee
+#define HEAP_START 0x400000
+#define HEAP_SIZE  0x200000
+#define HEAP_INUSE 1
+#define HEAP_FREE 0
+
+typedef struct {
+   uint64_t magic;
+   uint64_t state;
+   uint64_t size;
+} heap_t;
+
+/*
+  malloc and free divide the chunks of memory present at the heap
+  of the kernel into smaller parts.
+  The heap is located at: 0x
+*/
+static heap_t* kernel_heap = (heap_t*) 0x400000; // 4 MB - 6 MB is heap
+/* Allocates 'size' bytes and returns the pointer if succesfull.
+   Kernelpanic in case of failure..
+*/
+
+void* kmalloc(uint64_t size) {
+   // printf("Malloc %d bytes\n", size);
+
+   // Start at the beginning of our heap and search a free block:
+   heap_t *current = kernel_heap;
+   while (current->magic == HEAP_MAGIC)
+   {
+      if ((current->state == HEAP_FREE) && (current->size >= size))
+      {
+         // Mark block as used:
+         current->state = HEAP_INUSE;
+
+         // Insert a heap header if required:
+         if (current->size > size + sizeof(heap_t) + 1)
+         {
+            // Calculate location of the inserted header:
+            heap_t *newheader = (heap_t*) (((char*)current)+size+sizeof(heap_t));
+
+            // Set the new header fields:
+            newheader->size = current->size - size - sizeof(heap_t);
+            newheader->state = HEAP_FREE;
+            newheader->magic = HEAP_MAGIC;
+
+            // Set the size of this block
+            current->size = size; 
+         }
+         else
+         {
+            // We allocate this whole block
+         }
+         // Calculate the size of the block:
+         char *address = ((char*)current)+sizeof(heap_t);
+         return address;
+         
+      }
+      // Goto next heap block:
+      current = (heap_t*)(((char*) current) + current->size + sizeof(heap_t));
+   }
+   return 0x0;
+}
+
+void kfree(void* ptr) {
+  printf("Free address %x\n", ptr);
+}
+
+void init_heap(void)
+{
+   // Initialize the kernel heap:
+   kernel_heap->magic = HEAP_MAGIC;
+   kernel_heap->state = HEAP_FREE;
+   kernel_heap->size = HEAP_SIZE - sizeof(heap_t);
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cos/kernel/timer.c	Fri Dec 02 14:00:02 2011 +0100
@@ -0,0 +1,14 @@
+#include "kernel.h"
+
+static uint64_t ticks = 0;
+
+void timerDriverUpdate()
+{
+  ticks++;
+}
+
+uint64_t getTimeMS()
+{
+  return 55*ticks;
+}
+
--- a/cos/kernel/video.c	Thu Dec 01 21:42:59 2011 +0100
+++ b/cos/kernel/video.c	Fri Dec 02 14:00:02 2011 +0100
@@ -18,8 +18,25 @@
    outb(0x3D5, cursorLocation & 0xFF);
 }
 
-void
-clear_screen()
+void set_cursor(int newrow, int newcol)
+{
+   row = newrow;
+   col = newcol;
+   move_cursor();
+}
+
+void get_cursor(int *therow, int *thecol)
+{
+   *therow = row;
+   *thecol = col;
+}
+
+void set_color(int forecolor, int backcolor)
+{
+   video_color = forecolor | (backcolor << 4);
+}
+
+void clear_screen()
 {
 	unsigned char *vidmem = (unsigned char *) VIDMEM;
 	int loop;
@@ -30,45 +47,48 @@
 	}
 }
 
-void
-init_screen()
+void init_screen()
 {
-	row = col = 0;
 	clear_screen();
-   move_cursor();
+   set_cursor(0, 0);
 }
 
-
-static void
-scroll_screen()
+static void scroll_screen()
 {
-	unsigned short* v;
+	uint8_t* v;
 	int i;
-	int n = SCREEN_SIZE;
 	
-	for (v = (unsigned short*) VIDMEM, i = 0; i < n; i++ ) {
-		*v = *(v + NUM_COLS);
+   // Shift all lines one up:
+	v = (uint8_t*)VIDMEM;
+	for (i = 0; i < SCREEN_SIZE; i++ )
+   {
+		*v = *(v + NUM_COLS*2);
+		++v;
+		*v = *(v + NUM_COLS*2);
 		++v;
 	}
 
-	for (v = (unsigned short*) VIDMEM + n, i = 0; i < NUM_COLS; i++) {
-		*v++ = (video_color << 8) & 0x20;
+   // Clear new line:
+   v = (uint8_t*)VIDMEM + SCREEN_SIZE * 2 - NUM_COLS * 2;
+	for (i = 0; i < NUM_COLS; i++) 
+   {
+      *v++ = ' ';
+		*v++ = video_color;
 	}
 }
 
-static void
-new_line()
+static void new_line()
 {
 	++row;
 	col = 0;
-	if (row == NUM_ROWS) {
+	if (row == NUM_ROWS) 
+   {
 		scroll_screen();
 		row = NUM_ROWS - 1;
 	}
 }
 
-static void
-clear_to_EOL()
+static void clear_to_EOL()
 {
 	int loop;
 	unsigned char *v = (unsigned char *) ((uint64_t)(VIDMEM + row * NUM_COLS * 2 + col * 2));
@@ -79,8 +99,7 @@
 	}
 }
 
-static void
-print_char(int c)
+static void print_char(int c)
 {
 	unsigned char *v = (unsigned char *) ((uint64_t)(VIDMEM + row * NUM_COLS * 2 + col * 2));
 
@@ -101,10 +120,10 @@
    move_cursor();
 }
 
-void
-print_string(const char *s)
+void print_string(const char *s)
 {
-	while (*s != '\0') {
+	while (*s != '\0') 
+   {
 		print_char(*s++);
 	}
 }