1 files changed, 30 insertions, 19 deletions

diff --git a/c/src/lib/libbsp/m68k/mcf5329/clock/clock.c b/c/src/lib/libbsp/m68k/mcf5329/clock/clock.c
index ed11320bba..94c5bb0b04 100644
--- a/c/src/lib/libbsp/m68k/mcf5329/clock/clock.c
+++ b/c/src/lib/libbsp/m68k/mcf5329/clock/clock.c
@@ -3,6 +3,7 @@
  */
 
 #include <rtems.h>
+#include <rtems/timecounter.h>
 #include <bsp.h>
 
 /*
@@ -10,36 +11,40 @@
  */
 #define CLOCK_VECTOR (128+46)
 
-static uint32_t s_pcntrAtTick = 0;
-static uint32_t s_nanoScale = 0;
+static rtems_timecounter_simple mcf5329_tc;
 
-/*
- * Provide nanosecond extension
- */
-static uint32_t bsp_clock_nanoseconds_since_last_tick(void)
+static uint32_t mcf5329_tc_get(rtems_timecounter_simple *tc)
 {
-  uint32_t i;
+  return MCF_PIT3_PCNTR;
+}
 
-  if (MCF_PIT3_PCSR & MCF_PIT_PCSR_PIF) {
-    i = s_pcntrAtTick + (MCF_PIT3_PMR - MCF_PIT3_PCNTR);
-  } else {
-    i = s_pcntrAtTick - MCF_PIT3_PCNTR;
-  }
-  return i * s_nanoScale;
+static bool mcf5329_tc_is_pending(rtems_timecounter_simple *tc)
+{
+  return (MCF_PIT3_PCSR & MCF_PIT_PCSR_PIF) != 0;
+}
+
+static uint32_t mcf5329_tc_get_timecount(struct timecounter *tc)
+{
+  return rtems_timecounter_simple_downcounter_get(
+    tc,
+    mcf5329_tc_get,
+    mcf5329_tc_is_pending
+  );
 }
 
-#define Clock_driver_nanoseconds_since_last_tick bsp_clock_nanoseconds_since_last_tick
+static void mcf5329_tc_tick(void)
+{
+  rtems_timecounter_simple_downcounter_tick(&mcf5329_tc, mcf5329_tc_get);
+}
 
 /*
  * Periodic interval timer interrupt handler
  */
 #define Clock_driver_support_at_tick()             \
     do {                                           \
-        s_pcntrAtTick = MCF_PIT3_PCNTR;            \
         MCF_PIT3_PCSR |= MCF_PIT_PCSR_PIF;         \
     } while (0)                                    \
 
-
 /*
  * Attach clock interrupt handler
  */
@@ -75,8 +80,6 @@ static void Clock_driver_support_initialize_hardware(void)
       break;
     preScaleCode++;
   }
-  s_nanoScale = 1000000000 / (clk >> preScaleCode);
-
   MCF_INTC1_ICR46 = MCF_INTC_ICR_IL(PIT3_IRQ_LEVEL);
 
   rtems_interrupt_disable(level);
@@ -89,7 +92,15 @@ static void Clock_driver_support_initialize_hardware(void)
   MCF_PIT3_PMR = pmr;
   MCF_PIT3_PCSR = MCF_PIT_PCSR_PRE(preScaleCode) |
     MCF_PIT_PCSR_PIE | MCF_PIT_PCSR_RLD | MCF_PIT_PCSR_EN;
-  s_pcntrAtTick = MCF_PIT3_PCNTR;
+
+  rtems_timecounter_simple_install(
+    &mcf5329_tc,
+    clk >> preScaleCode,
+    pmr,
+    mcf5329_tc_get_timecount
+  );
 }
 
+#define Clock_driver_timecounter_tick() mcf5329_tc_tick()
+
 #include "../../../shared/clockdrv_shell.h"