1 files changed, 39 insertions, 6 deletions

diff --git a/src/main.c b/src/main.c
index 7c1172c..68fdb66 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,6 +1,7 @@
 #define NB_COLS 80
 #define NB_ROWS 25
 #define BPC     2
+#define BPR     (BPC * NB_COLS)
 
 #define VIDEO_BUFFER 0xB8000
 
@@ -24,6 +25,19 @@ static void my_memset(void *_dest, int c, int n)
     }
 }
 
+static void clear_line(void *_addr)
+{
+    char *addr = _addr;
+
+    for (int i = 0; i < NB_COLS; ++i) {
+        if (i % BPC == 0) {
+            *addr++ = ' ';
+        } else {
+            *addr++ = 0x7;
+        }
+    }
+}
+
 int printk(const char *str)
 {
     static const long int offset = VIDEO_BUFFER;
@@ -33,15 +47,34 @@ int printk(const char *str)
     for (int i = 0; str[i]; ++i) {
         /* TODO add the serial port handling */
 
-        if ((long)addr >= offset + NB_COLS * NB_ROWS * BPC) {
+        char chr;
+        char *new_addr = addr;
+        char *addr_line = (char *)(((long)(addr - offset) / BPR) * BPR) + offset;
+
+        if ((long)addr >= offset + NB_ROWS * BPR) {
             /* shift up */
-            addr -= NB_COLS * BPC;
-            my_memmove((void *)offset, (void *)offset + NB_COLS * BPC, NB_COLS * (NB_ROWS - 1) * BPC);
-            my_memset((void *)offset + NB_COLS * (NB_ROWS - 1) * BPC, ' ', NB_COLS * BPC);
+            addr -= BPR;
+            addr_line -= BPR;
+            my_memmove((void *)offset, (void *)offset + BPR, BPR * (NB_ROWS - 1));
+            clear_line(addr_line);
         }
 
-        *addr = str[i];
-        addr += BPC;
+
+        switch (str[i]) {
+            case '\n':
+                new_addr = addr_line + BPR;
+                chr = '\0';
+                break;
+            default:
+                new_addr = addr + BPC;
+                chr = str[i];
+                break;
+        }
+
+        if (chr)
+            *addr = chr;
+
+        addr = new_addr;
         ++count;
     }