1 files changed, 37 insertions, 5 deletions
diff --git a/c/src/lib/libbsp/powerpc/mvme5500/include/tm27.h b/c/src/lib/libbsp/powerpc/mvme5500/include/tm27.h
index d4165949d9..f132fce9bb 100644
--- a/c/src/lib/libbsp/powerpc/mvme5500/include/tm27.h
+++ b/c/src/lib/libbsp/powerpc/mvme5500/include/tm27.h
@@ -15,18 +15,50 @@
 #ifndef __tm27_h
 #define __tm27_h
 
+#include <bsp/irq.h>
+
 /*
  *  Stuff for Time Test 27
  */
 
-#define MUST_WAIT_FOR_INTERRUPT 0
+#define MUST_WAIT_FOR_INTERRUPT 1
+
+void nullFunc() {}
+static rtems_irq_connect_data clockIrqData = {BSP_DECREMENTER,
+                                              0,
+                                              (rtems_irq_enable)nullFunc,
+                                              (rtems_irq_disable)nullFunc,
+                                              (rtems_irq_is_enabled) nullFunc};
+
+void Install_tm27_vector(void (*_handler)())
+{
+  clockIrqData.hdl = _handler;
+  if (!BSP_install_rtems_irq_handler (&clockIrqData)) {
+        printk("Error installing clock interrupt handler!\n");
+        rtems_fatal_error_occurred(1);
+  }
+}
 
-#define Install_tm27_vector( handler ) /* empty */
+#define Cause_tm27_intr()  \
+  do { \
+    uint32_t _clicks = 1; \
+    asm volatile( "mtdec %0" : "=r" ((_clicks)) : "r" ((_clicks)) ); \
+  } while (0)
 
-#define Cause_tm27_intr() /* empty */
 
-#define Clear_tm27_intr() /* empty */
+#define Clear_tm27_intr() \
+  do { \
+    uint32_t _clicks = 0xffffffff; \
+    asm volatile( "mtdec %0" : "=r" ((_clicks)) : "r" ((_clicks)) ); \
+  } while (0)
 
-#define Lower_tm27_intr() /* empty */
+#define Lower_tm27_intr() \
+  do { \
+    uint32_t _msr = 0; \
+    _ISR_Set_level( 0 ); \
+    asm volatile( "mfmsr %0 ;" : "=r" (_msr) : "r" (_msr) ); \
+    _msr |=  0x8002; \
+    asm volatile( "mtmsr %0 ;" : "=r" (_msr) : "r" (_msr) ); \
+  } while (0)
 
 #endif