1 files changed, 25 insertions, 1 deletions
diff --git a/src/soc/imgtec/pistachio/monotonic_timer.c b/src/soc/imgtec/pistachio/monotonic_timer.c
index a8fe27c9ccbc..99c147b7c212 100644
--- a/src/soc/imgtec/pistachio/monotonic_timer.c
+++ b/src/soc/imgtec/pistachio/monotonic_timer.c
@@ -17,9 +17,33 @@
  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  */
 
+#include <stdint.h>
 #include <timer.h>
+#include <timestamp.h>
+#include <arch/cpu.h>
+#include <soc/cpu.h>
+
+static int get_count_mhz_freq(void)
+{
+	static unsigned count_mhz_freq;
+
+	if (!count_mhz_freq) {
+		if (IMG_PLATFORM_ID() != IMG_PLATFORM_ID_SILICON)
+			count_mhz_freq = 25; /* FPGA board */
+			/*
+			 * Will need some means of finding out the counter
+			 * frequency on a real SOC
+			 */
+	}
+	return count_mhz_freq;
+}
 
 void timer_monotonic_get(struct mono_time *mt)
 {
-	/* to be defined */
+	mt->microseconds = (long)timestamp_get();
+}
+
+uint64_t timestamp_get(void)
+{
+	return read_c0_count()/get_count_mhz_freq();
 }