7 files changed, 92 insertions, 25 deletions
diff --git a/bsp/env/entry.S b/bsp/env/entry.S
index cbf26eb..1f5de24 100644
--- a/bsp/env/entry.S
+++ b/bsp/env/entry.S
@@ -7,6 +7,7 @@
 #include "sifive/bits.h"
 
   .section      .text.entry	
+  .align 2
   .global trap_entry
 trap_entry:
   addi sp, sp, -32*REGBYTES
@@ -46,7 +47,7 @@ trap_entry:
   csrr a0, mcause
   csrr a1, mepc
   mv a2, sp
-  jal handle_trap
+  call handle_trap
   csrw mepc, a0
 
   # Remain in M-mode after mret
@@ -90,6 +91,7 @@ trap_entry:
 
 .weak handle_trap
 handle_trap:
-  j handle_trap
+1:
+  j 1b
 	
 #endif
diff --git a/bsp/env/freedom-e300-arty/init.c b/bsp/env/freedom-e300-arty/init.c
index c766e98..35b1104 100644
--- a/bsp/env/freedom-e300-arty/init.c
+++ b/bsp/env/freedom-e300-arty/init.c
@@ -9,11 +9,30 @@
 extern int main(int argc, char** argv);
 extern void trap_entry();
 
-uint32_t get_cpu_freq()
+static unsigned long get_cpu_freq()
 {
   return 65000000;
 }
 
+unsigned long get_timer_freq()
+{
+  return get_cpu_freq();
+}
+
+uint64_t get_timer_value()
+{
+#if __riscv_xlen == 32
+  while (1) {
+    uint32_t hi = read_csr(mcycleh);
+    uint32_t lo = read_csr(mcycle);
+    if (hi == read_csr(mcycleh))
+      return ((uint64_t)hi << 32) | lo;
+  }
+#else
+  return read_csr(mcycle);
+#endif
+}
+
 static void uart_init(size_t baud_rate)
 {
   GPIO_REG(GPIO_IOF_SEL) &= ~IOF0_UART0_MASK;
@@ -59,8 +78,6 @@ void _init()
   printf("core freq at %d Hz\n", get_cpu_freq());
 
   write_csr(mtvec, &trap_entry);
-  
-  //  _exit(main(0, NULL));
 }
 
 
diff --git a/bsp/env/freedom-e300-arty/platform.h b/bsp/env/freedom-e300-arty/platform.h
index 1f62956..d5d6dda 100644
--- a/bsp/env/freedom-e300-arty/platform.h
+++ b/bsp/env/freedom-e300-arty/platform.h
@@ -119,4 +119,7 @@
 #define HAS_BOARD_BUTTONS
 #include "hifive1.h"
 
+unsigned long get_timer_freq(void);
+uint64_t get_timer_value(void);
+
 #endif /* _SIFIVE_PLATFORM_H */
diff --git a/bsp/env/freedom-e300-hifive1/init.c b/bsp/env/freedom-e300-hifive1/init.c
index c088079..71e1659 100644
--- a/bsp/env/freedom-e300-hifive1/init.c
+++ b/bsp/env/freedom-e300-hifive1/init.c
@@ -5,21 +5,43 @@
 #include "platform.h"
 #include "encoding.h"
 
-uint32_t cpu_freq = 0;
-
 extern int main(int argc, char** argv);
 extern void trap_entry();
 
-uint32_t mtime_lo(void)
+static unsigned long mtime_lo(void)
+{
+  return *(volatile unsigned long *)(CLINT_BASE_ADDR + CLINT_MTIME);
+}
+
+#ifdef __riscv32
+
+static uint32_t mtime_hi(void)
 {
-  return *(volatile uint32_t *)(CLINT_BASE_ADDR + CLINT_MTIME);
+  return *(volatile uint32_t *)(CLINT_BASE_ADDR + CLINT_MTIME + 4);
 }
 
-uint32_t mcycle_lo(void)
+uint64_t get_timer_value()
 {
-  uint32_t t;
-  asm volatile ("csrr %0, mcycle" : "=r" (t));
-  return t;
+  while (1) {
+    uint32_t hi = mtime_hi();
+    uint32_t lo = mtime_lo();
+    if (hi == mtime_hi())
+      return ((uint64_t)hi << 32) | lo;
+  }
+}
+
+#else /* __riscv32 */
+
+uint64_t get_timer_value()
+{
+  return mtime_lo();
+}
+
+#endif
+
+unsigned long get_timer_freq()
+{
+  return 32768;
 }
 
 static void use_hfrosc(int div, int trim)
@@ -116,21 +138,40 @@ static void use_default_clocks()
   use_hfrosc(4, 16);
 }
 
-void measure_cpu_freq(size_t n, size_t mtime_freq)
+static unsigned long __attribute__((noinline)) measure_cpu_freq(size_t n)
 {
-  uint32_t start_mtime = mtime_lo();
-  uint32_t start_mcycle = mcycle_lo();
+  unsigned long start_mtime, delta_mtime;
+  unsigned long mtime_freq = get_timer_freq();
+
+  // Don't start measuruing until we see an mtime tick
+  unsigned long tmp = mtime_lo();
+  do {
+    start_mtime = mtime_lo();
+  } while (start_mtime == tmp);
 
-  while (mtime_lo() - start_mtime < n) ;
+  unsigned long start_mcycle = read_csr(mcycle);
 
-  uint32_t end_mtime = mtime_lo();
-  uint32_t end_mcycle = mcycle_lo();
+  do {
+    delta_mtime = mtime_lo() - start_mtime;
+  } while (delta_mtime < n);
 
-  cpu_freq = (end_mcycle-start_mcycle)/n*mtime_freq;
+  unsigned long delta_mcycle = read_csr(mcycle) - start_mcycle;
+
+  return (delta_mcycle / delta_mtime) * mtime_freq
+         + ((delta_mcycle % delta_mtime) * mtime_freq) / delta_mtime;
 }
 
-uint32_t get_cpu_freq()
+unsigned long get_cpu_freq()
 {
+  static uint32_t cpu_freq;
+
+  if (!cpu_freq) {
+    // warm up I$
+    measure_cpu_freq(1);
+    // measure for real
+    cpu_freq = measure_cpu_freq(10);
+  }
+
   return cpu_freq;
 }
 
@@ -178,7 +219,6 @@ void _init()
 {
   use_default_clocks();
   use_pll(0, 0, 1, 31, 1);
-  measure_cpu_freq(1000, 32768);
   uart_init(115200);
 
   printf("core freq at %d Hz\n", get_cpu_freq());
@@ -188,8 +228,6 @@ void _init()
     write_csr(mstatus, MSTATUS_FS); // allow FPU instructions without trapping
     write_csr(fcsr, 0); // initialize rounding mode, undefined at reset
   }
-
-  //_exit(main(0, NULL));
 }
 
 void _fini()
diff --git a/bsp/env/freedom-e300-hifive1/link.lds b/bsp/env/freedom-e300-hifive1/link.lds
index e224273..90e5c8f 100644
--- a/bsp/env/freedom-e300-hifive1/link.lds
+++ b/bsp/env/freedom-e300-hifive1/link.lds
@@ -26,6 +26,8 @@ SECTIONS
 
   .text           :
   {
+    *(.text.unlikely .text.unlikely.*)
+    *(.text.startup .text.startup.*)
     *(.text .text.*)
     *(.gnu.linkonce.t.*)
   } >flash AT>flash :flash
diff --git a/bsp/env/freedom-e300-hifive1/platform.h b/bsp/env/freedom-e300-hifive1/platform.h
index eca708e..63efc9e 100644
--- a/bsp/env/freedom-e300-hifive1/platform.h
+++ b/bsp/env/freedom-e300-hifive1/platform.h
@@ -126,6 +126,8 @@
 
 #include "hifive1.h"
 
-uint32_t get_cpu_freq();
+unsigned long get_cpu_freq(void);
+unsigned long get_timer_freq(void);
+uint64_t get_timer_value(void);
 
 #endif /* _SIFIVE_PLATFORM_H */
diff --git a/bsp/env/start.S b/bsp/env/start.S
index 77e223d..b526411 100644
--- a/bsp/env/start.S
+++ b/bsp/env/start.S
@@ -47,5 +47,8 @@ _start:
 1:
 #endif
 
+	/* argc = argv = 0 */
+	li a0, 0
+	li a1, 0
 	call main
 	tail exit