Add setClock

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;
+}

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);

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;
+}

delay.h

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <stdint.h>
+
+uint64_t micros();

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

hello.c

@@ -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,10 +30,12 @@ int kernelmain(void)
     puts("Error getting videocore mem\n");
   }
 
+  printclock();
+
   unsigned char *ptr = fb.fbp;
 
-    for(int y=0; y<fb.height; y++) {
-        for(int x=0; x<fb.width; x++) {
+  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)

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;
 }

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");
+	}
+}

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();