5 files changed, 47 insertions, 23 deletions
diff --git a/src/soc/imgtec/pistachio/Makefile.inc b/src/soc/imgtec/pistachio/Makefile.inc
index e955968ddc..f7a0848e86 100644
--- a/src/soc/imgtec/pistachio/Makefile.inc
+++ b/src/soc/imgtec/pistachio/Makefile.inc
@@ -31,16 +31,13 @@ ramstage-y += uart.c
 endif
 
 bootblock-y += monotonic_timer.c
-bootblock-y += timestamp.c
 
 ramstage-y += cbmem.c
 ramstage-y += monotonic_timer.c
-ramstage-y += timestamp.c
 
 romstage-y += cbmem.c
 romstage-y += romstage.c
 romstage-y += monotonic_timer.c
-romstage-y += timestamp.c
 
 CPPFLAGS_common += -Isrc/soc/imgtec/pistachio/include/
 
diff --git a/src/soc/imgtec/pistachio/bootblock.c b/src/soc/imgtec/pistachio/bootblock.c
index f6cc76b0b9..90112646c4 100644
--- a/src/soc/imgtec/pistachio/bootblock.c
+++ b/src/soc/imgtec/pistachio/bootblock.c
@@ -19,6 +19,21 @@
  * MA 02110-1301 USA
  */
 
+#include <stdint.h>
+#include <arch/cpu.h>
+
 static void bootblock_cpu_init(void)
 {
+	uint32_t cause;
+
+	/*
+	 * Make sure the count register is counting by clearing the "Disable
+	 * Counter" bit, in case it is set.
+	 */
+	cause = read_c0_cause();
+	if (cause & C0_CAUSE_DC)
+		write_c0_cause(cause & ~(C0_CAUSE_DC));
+
+	/* And make sure that it starts from zero. */
+	write_c0_count(0);
 }
diff --git a/src/soc/imgtec/pistachio/include/soc/cpu.h b/src/soc/imgtec/pistachio/include/soc/cpu.h
index c61c58dfca..72775b5cbe 100644
--- a/src/soc/imgtec/pistachio/include/soc/cpu.h
+++ b/src/soc/imgtec/pistachio/include/soc/cpu.h
@@ -21,4 +21,11 @@
 #define IMG_SPIM0_BASE_ADDRESS	0xB8100F00
 #define IMG_SPIM1_BASE_ADDRESS	0xB8101000
 
+/*
+ * Reading at this address allows to identify the platform the code is running
+ * on.
+ */
+#define IMG_PLATFORM_ID()	(*((unsigned *)0xB8149060))
+#define IMG_PLATFORM_ID_SILICON 0xF00D0006
+
 #endif
diff --git a/src/soc/imgtec/pistachio/monotonic_timer.c b/src/soc/imgtec/pistachio/monotonic_timer.c
index a8fe27c9cc..99c147b7c2 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();
 }
diff --git a/src/soc/imgtec/pistachio/timestamp.c b/src/soc/imgtec/pistachio/timestamp.c
deleted file mode 100644
index f0dc5ad9cb..0000000000
--- a/src/soc/imgtec/pistachio/timestamp.c
+++ /dev/null
@@ -1,19 +0,0 @@
-/*
- * Copyright (C) 2014 Google, Inc.
- *
- * This software is licensed under the terms of the GNU General Public
- * License version 2, as published by the Free Software Foundation, and
- * may be copied, distributed, and modified under those terms.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <timestamp.h>
-
-uint64_t timestamp_get(void)
-{
-	return 0;
-}