diff --git a/clock.c b/clock.c
new file mode 100644
index 0000000..1d80dee
--- /dev/null
+++ b/clock.c
@@ -0,0 +1,51 @@
+#include "clock.h"
+#include "mbox.h"
+#include "uart.h"
+
+unsigned int getclock(uint32_t clkid, uint8_t max) {
+  mbox[0] = 8 * 4;
+  mbox[1] = MBOX_REQUEST;
+  mbox[2] = max > 0 ? MBOX_TAG_GETMAXCLK : MBOX_TAG_GETCLK;
+  mbox[3] = 8;
+  mbox[4] = 0;
+  mbox[5] = clkid;
+  mbox[6] = 0;
+  mbox[7] = MBOX_TAG_LAST;
+
+  if (mbox_call(MBOX_CH_PROP, mbox)) {
+    puts("Failed getting clock\n");
+    return 0;
+  } else {
+    return mbox[6];
+  }
+}
+
+int setmaxclock(uint32_t clkid)
+{
+  uint32_t maxfreq = getclock(clkid, 1);
+  if (maxfreq != 0) {
+    return setclock(clkid, maxfreq);
+  }
+
+  return -1;
+}
+
+int setclock(uint32_t clkid, uint32_t freq)
+{
+  mbox[0] = 9 * 4;
+  mbox[1] = MBOX_REQUEST;
+  mbox[2] = MBOX_TAG_SETCLK;
+  mbox[3] = 12;
+  mbox[4] = 0;
+  mbox[5] = clkid;
+  mbox[6] = freq;
+  mbox[7] = 0;
+  mbox[8] = MBOX_TAG_LAST;
+
+  if (mbox_call(MBOX_CH_PROP, mbox)) {
+    puts("Failed setting clock\n");
+    return -1;
+  }
+
+  return 0;
+}
diff --git a/clock.h b/clock.h
new file mode 100644
index 0000000..e4ace3b
--- /dev/null
+++ b/clock.h
@@ -0,0 +1,21 @@
+#pragma once
+#include <stdint.h>
+
+#define CLK_EMMC  0x00000001
+#define CLK_UART  0x00000002
+#define CLK_ARM   0x00000003
+#define CLK_CORE  0x00000004
+#define CLK_V3D   0x00000005
+#define CLK_H264  0x00000006
+#define CLK_ISP   0x00000007
+#define CLK_SDRAM 0x00000008
+#define CLK_PIXEL 0x00000009
+#define CLK_PWM   0x0000000a
+#define CLK_HEVC  0x0000000b
+#define CLK_EMMC2 0x0000000c
+#define CLK_M2MC  0x0000000d
+#define CLK_PIXEL_BVB 0x0000000e
+
+unsigned int getclock(uint32_t clkid, uint8_t max);
+int setmaxclock(uint32_t clkid);
+int setclock(uint32_t clkid, uint32_t freq);
diff --git a/delay.c b/delay.c
new file mode 100644
index 0000000..4f84672
--- /dev/null
+++ b/delay.c
@@ -0,0 +1,16 @@
+#include "delay.h"
+#include "mmio.h"
+
+#define STC_LOW (*(volatile unsigned *)(IO_BASE + 0x3004))
+#define STC_HIGH (*(volatile unsigned *)(IO_BASE + 0x3008))
+
+uint64_t micros() {
+  uint32_t high, low;
+  high = STC_HIGH;
+  low = STC_LOW;
+  if (high != STC_HIGH) {
+    high = STC_HIGH;
+    low = STC_LOW;
+  }
+  return ((uint64_t)high) << 32 | low;
+}
diff --git a/delay.h b/delay.h
new file mode 100644
index 0000000..92268aa
--- /dev/null
+++ b/delay.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include <stdint.h>
+
+uint64_t micros();
diff --git a/fb.c b/fb.c
index 841954d..3cd0aec 100644
--- a/fb.c
+++ b/fb.c
@@ -50,7 +50,7 @@ unsigned char *init_fb(struct fbst *fb) {
   mbox[33] = 0; // FrameBufferInfo.pitch
   mbox[34] = MBOX_TAG_LAST;
 
-  if (mbox_call(MBOX_CH_PROP, mbox) && mbox[20] == 32 && mbox[28] != 0) {
+  if (mbox_call(MBOX_CH_PROP, mbox) == 0 && mbox[20] == 32 && mbox[28] != 0) {
     mbox[28] &= 0x3FFFFFFF; // convert GPU address to ARM address
     fb->width = mbox[5];    // get actual physical width
     fb->height = mbox[6];   // get actual physical height
diff --git a/hello.c b/hello.c
index 7b75ad0..acfe3a9 100644
--- a/hello.c
+++ b/hello.c
@@ -7,7 +7,7 @@ struct fbst fb;
 int kernelmain(void)
 {
   init_uart();
-  if (init_fb(&fb) == 0){
+  if (init_fb(&fb) == 0) {
     puts("Fail to init framebuffer");
   }
   mbox[0] = 8 * 4;
@@ -19,7 +19,7 @@ int kernelmain(void)
   mbox[6] = 0;
   mbox[7] = MBOX_TAG_LAST;
 
-  if (mbox_call(MBOX_CH_PROP, mbox)) {
+  if (!mbox_call(MBOX_CH_PROP, mbox)) {
     puts("videocore start at 0x");
     printhex(mbox[5]);
     puts(" - ");
@@ -30,20 +30,22 @@ int kernelmain(void)
     puts("Error getting videocore mem\n");
   }
 
-    unsigned char *ptr=fb.fbp;
+  printclock();
 
-    for(int y=0; y<fb.height; y++) {
-        for(int x=0; x<fb.width; x++) {
-            if(y%60==0)
-                *((unsigned int*)ptr)=0x00FF0000;
-            else if (y%40==0)
-                *((unsigned int*)ptr)=0x0000FF00;
-            else if (y%20==0)
-                *((unsigned int*)ptr)=0x000000FF;
-            else
-                *((unsigned int*)ptr)=0x00000000;
-            ptr+=4;
-        }
+  unsigned char *ptr = fb.fbp;
+
+  for (unsigned int y = 0; y < fb.height; y++) {
+    for (unsigned int x = 0; x < fb.width; x++) {
+      if (y % 60 == 0)
+        *((unsigned int *)ptr) = 0x00FF0000;
+      else if (y % 40 == 0)
+        *((unsigned int *)ptr) = 0x0000FF00;
+      else if (y % 20 == 0)
+        *((unsigned int *)ptr) = 0x000000FF;
+      else
+        *((unsigned int *)ptr) = 0x00000000;
+      ptr += 4;
     }
+  }
   return 0;
 }
diff --git a/mbox.c b/mbox.c
index cec796a..730cc7b 100644
--- a/mbox.c
+++ b/mbox.c
@@ -23,7 +23,8 @@
 // Should be allocated in the first 2^32bytes
 volatile uint32_t __attribute__((aligned(16))) mbox[36];
 
-int mbox_call(uint8_t ch, volatile uint32_t *box) {
+int mbox_call(uint8_t ch, volatile uint32_t *box)
+{
   // mbox is 16 aligned
   uint32_t r = (uint64_t)box | (ch & 0xF);
   while (*MBOX_STATUS & MBOX_FULL) {
@@ -36,8 +37,8 @@ int mbox_call(uint8_t ch, volatile uint32_t *box) {
     }
     if (r == *MBOX_READ)
       /* is it a valid successful response? */
-      return box[1] == MBOX_RESPONSE;
+      return box[1] == MBOX_RESPONSE ? 0 : -1;
   }
 
-  return 0;
+  return -1;
 }
diff --git a/mbox.h b/mbox.h
index 4ca0017..8de9099 100644
--- a/mbox.h
+++ b/mbox.h
@@ -27,14 +27,14 @@
 #define MBOX_CH_PROP    8 // Used for ARM to GPU
 
 /* tags */
-#define MBOX_TAG_GETSERIAL		0x00010004
-#define MBOX_TAG_GETARMMEM		0x00010005
-#define MBOX_TAG_GETVCOREMEM	0x00010006
-#define MBOX_TAG_GETCORETEMP	0x00030006
-#define MBOX_TAG_GETMAXCLK    0x00030004
-#define MBOX_TAG_GETCLK       0x00030047
-#define MBOX_TAG_SETCLK       0x00038002
-#define MBOX_TAG_LAST			    0
+#define MBOX_TAG_GETSERIAL 0x00010004
+#define MBOX_TAG_GETARMMEM 0x00010005
+#define MBOX_TAG_GETVCOREMEM 0x00010006
+#define MBOX_TAG_GETCORETEMP 0x00030006
+#define MBOX_TAG_GETMAXCLK 0x00030004
+#define MBOX_TAG_GETCLK 0x00030047
+#define MBOX_TAG_SETCLK 0x00038002
+#define MBOX_TAG_LAST 0
 
 extern volatile unsigned int mbox[36];
 //box should be 16 aligned.
diff --git a/utils.c b/utils.c
index d8158f7..cd173fb 100644
--- a/utils.c
+++ b/utils.c
@@ -1,5 +1,6 @@
 #include <stdint.h>
 #include "uart.h"
+#include "clock.h"
 
 // K&R inspirated
 
@@ -52,3 +53,21 @@ void printdec(unsigned int d)
 		putc(s[i]);
 	}
 }
+
+void printclock()
+{
+	static const char *clocktxts[] = {"reserved", "EMMC", "UART",  "ARM",   "CORE",
+					  "V3D",      "H264", "ISP",   "SDRAM", "PIXEL",
+					  "PWM",      "HEVC", "EMMC2", "M2MC",  "PIXEL_BVB"};
+
+	puts("Clocks:\n");
+	for(int i =1; i <= CLK_PIXEL_BVB; i++ ){
+		puts(" ");
+		puts(clocktxts[i]);
+		puts(": ");
+		printdec(getclock(i, 0));
+		puts("/");
+		printdec(getclock(i, 1));
+		puts(" Hz\n");
+	}
+}
diff --git a/utils.h b/utils.h
index d3eda2f..4dfed5d 100644
--- a/utils.h
+++ b/utils.h
@@ -5,3 +5,4 @@ int strlen(char s[]);
 void reverse(char s[]);
 void itoa(unsigned int n, char s[]);
 void printdec(unsigned int d);
+void printclock();