diff --git a/include/lib/simplefb.h b/include/lib/simplefb.h
index dfa1182..34509cc 100644
--- a/include/lib/simplefb.h
+++ b/include/lib/simplefb.h
@@ -9,9 +9,9 @@
 #define SIMPLEFB_H_
 
 typedef struct _color {
-	int r;
-	int g;
-	int b;
+	unsigned char r;
+	unsigned char g;
+	unsigned char b;
 } color;
 
 typedef struct _font_params {
@@ -21,15 +21,4 @@ typedef struct _font_params {
 
 font_params get_font_params(void);
 
-long unsigned int strlen(const char *p) {
-	unsigned int i = 0;
-
-	while(*p != '\0') {
-		i++;
-		p++;
-	}
-
-    return i;
-}
-
 #endif
diff --git a/include/main.h b/include/main.h
index 493831b..8a773ed 100644
--- a/include/main.h
+++ b/include/main.h
@@ -17,13 +17,5 @@ extern void clean_fb(volatile char *fb, int width, int height, int stride);
 extern void printk(char *text);
 
 extern void writel(unsigned int value, void* address);
-#ifdef __aarch64__
-	/* Define our own 128 bit memcpy */
-	extern void memcpy(void *dest, void *src, size_t size);
-#endif
-
-#ifdef __arm__
-	extern void memcpy (char* src, char* dest, size_t len);
-#endif
 
 #endif // MAIN_H_
diff --git a/lib/debug/debug.c b/lib/debug/debug.c
index f206818..74d0db8 100644
--- a/lib/debug/debug.c
+++ b/lib/debug/debug.c
@@ -26,22 +26,3 @@ void printk(char *text) {
 	debug_linecount++;
 #endif
 }
-/*
-	#ifdef __aarch64__
-		Define our own 128 bit memcpy *
-		void memcpy(void *dest, void *src, size_t size)
-		{
-			unsigned __int128 *src2 = src;
-			unsigned __int128 *dest2 = dest;
-
-			for (size_t i = 0; i < size / 16; i++)
-				dest2[i] = src2[i];
-		}
-	#endif
-
-	#ifdef __arm__
-		void memcpy (char* src, char* dest, size_t len) {
-			for (size_t i = 0; i < len; i++)
-				dest[i] = src[i];
-		}
-	#endif*/
diff --git a/lib/simplefb/simplefb.c b/lib/simplefb/simplefb.c
index 0f47219..45cb595 100644
--- a/lib/simplefb/simplefb.c
+++ b/lib/simplefb/simplefb.c
@@ -7,29 +7,31 @@
 
 #include <lib/simplefb.h>
 #include <lib/font.h>
+#include <string.h>
 
 void clean_fb(volatile char *fb, int width, int height, int stride) {
 	for (volatile char *addr = fb; addr < fb + (width * height * stride); addr += stride)
-		*(int*) (addr) = 0x0;
+		*(int*)(addr) = 0x0;
 }
 
-void draw_pixel(volatile char *fb, int x, int y, int width, int stride) {
-	long int location = (x * stride) + (y * width * stride);
+/* RGB888 format */
+/* Unlike ARGB8888, we explicitly use 3 bytes to represent each pixel, making sure no extra padding byte is added. */
+void draw_pixel(volatile char *fb, int x, int y, int width, int stride, color c) {
+	long int location = (x * 3) + (y * width * 3);
 
-	*(fb + location) = 255;	// Blue
-	*(fb + location + 1) = 255;	// Green
-	*(fb + location + 2) = 255;	// Red
-	*(fb + location + 3) = 255;	// Full opacity
+	*(fb + location) = c.r;
+	*(fb + location + 1) = c.g;
+	*(fb + location + 2) = c.b;
 }
 
 void draw_horizontal_line(volatile char *fb, int x1, int x2, int y, color c, int width, int stride) {
 	for (int i = x1; i < x2; i++)
-		draw_pixel(fb, i, y, width, stride);
+		draw_pixel(fb, i, y, width, stride, c);
 }
 
 void draw_vertical_line(volatile char *fb, int x, int y1, int y2, color c, int width, int stride) {
 	for (int i = y1; i < y2; i++)
-		draw_pixel(fb, x, i, width, stride);
+		draw_pixel(fb, x, i, width, stride, c);
 }
 
 void draw_filled_rectangle(volatile char *fb, int x1, int y1, int w, int h, color c, int width, int stride) {
@@ -42,7 +44,7 @@ void draw_text(volatile char *fb, char *text, int textX, int textY, int width, i
 	int l = strlen(text);
 
 	for (int i = 0; i < l; i++) {
-		if(text[i] < 32)
+		if (text[i] < 32)
 			continue;
 
 		int ix = font_index(text[i]);
@@ -53,15 +55,8 @@ void draw_text(volatile char *fb, char *text, int textX, int textY, int width, i
 
 			for (int x = 0; x < FONTW; x++) {
 				if (((b << x) & 0b10000000) > 0)
-					draw_pixel(fb, textX + i * FONTW + x, textY + y, width, stride);
+					draw_pixel(fb, textX + i * FONTW + x, textY + y, width, stride, (color){255, 255, 255});
 			}
 		}
 	}
 }
-
-/* Helper functions */ 
-font_params get_font_params() {
-	font_params params = {.width=FONTW, .height=FONTH};
-
-	return params;
-}
diff --git a/main/main.c b/main/main.c
index f965adb..bd9428a 100644
--- a/main/main.c
+++ b/main/main.c
@@ -4,6 +4,7 @@
  */
 
 #include <main.h>
+#include <string.h>
 
 void main(void* dt, void* kernel) {
 	/* Initialize SoC and Board specific peripherals/quirks */