Index: c/src/lib/libbsp/arm/tms570/Makefile.am
===================================================================
--- c/src/lib/libbsp/arm/tms570/Makefile.am	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/Makefile.am	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -40,4 +40,5 @@
 include_bsp_HEADERS += include/tms570-pom.h
 include_bsp_HEADERS += include/tms570-sci-driver.h
+include_bsp_HEADERS += include/system-clocks.h
 
 include_HEADERS += ../../shared/include/tm27.h
@@ -79,4 +80,5 @@
 libbsp_a_SOURCES += ../../shared/sbrk.c
 libbsp_a_SOURCES += ../../shared/src/stackalloc.c
+libbsp_a_SOURCES += ../../shared/cpucounterdiff.c
 
 # Startup
@@ -106,4 +108,5 @@
 libbsp_a_SOURCES += ../../shared/clockdrv_shell.h
 libbsp_a_SOURCES += clock/clock.c
+libbsp_a_SOURCES += clock/benchmark_timer.c
 
 # RTC
@@ -116,6 +119,12 @@
 
 # Misc
+libbsp_a_SOURCES += misc/cpucounterread.c
 
 # Watchdog
+
+# Cache
+libbsp_a_SOURCES += ../../../libcpu/shared/src/cache_manager.c
+libbsp_a_SOURCES += ../../../libcpu/arm/shared/include/cache_.h
+libbsp_a_CPPFLAGS += -I$(srcdir)/../../../libcpu/arm/shared/include
 
 # Start hooks
Index: c/src/lib/libbsp/arm/tms570/clock/benchmark_timer.c
===================================================================
--- c/src/lib/libbsp/arm/tms570/clock/benchmark_timer.c	(revision 46265063e3300ab613c03151d7aaade580b10554)
+++ c/src/lib/libbsp/arm/tms570/clock/benchmark_timer.c	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -0,0 +1,61 @@
+/**
+ * @file benchmark_timer.c
+ *
+ * @ingroup tms570
+ *
+ * @brief clock functions definitions.
+ */
+
+/*
+ * Copyright (c) 2014 Pavel Pisa <pisa@cmp.felk.cvut.cz>
+ *
+ * Czech Technical University in Prague
+ * Zikova 1903/4
+ * 166 36 Praha 6
+ * Czech Republic
+ *
+ * Based on LPC24xx and LPC1768 BSP
+ * by embedded brains GmbH and others
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <stdlib.h>
+
+#include <rtems.h>
+#include <bsp.h>
+#include <bsp/system-clocks.h>
+#include <rtems/btimer.h>
+#include <rtems/timerdrv.h>
+
+bool benchmark_timer_find_average_overhead = false;
+
+static uint32_t benchmark_timer_base;
+
+void benchmark_timer_initialize(void)
+{
+  benchmark_timer_base = _CPU_Counter_read();
+}
+
+uint32_t benchmark_timer_read(void)
+{
+  uint32_t delta = _CPU_Counter_read() - benchmark_timer_base;
+
+  if (benchmark_timer_find_average_overhead) {
+    return delta;
+  } else {
+    /* TODO check on hardware */
+    if (delta > 74) {
+      return delta - 74;
+    } else {
+      return 0;
+    }
+  }
+}
+
+void benchmark_timer_disable_subtracting_average_overhead(bool find_average_overhead )
+{
+  benchmark_timer_find_average_overhead = find_average_overhead;
+}
Index: c/src/lib/libbsp/arm/tms570/clock/clock.c
===================================================================
--- c/src/lib/libbsp/arm/tms570/clock/clock.c	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/clock/clock.c	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -30,4 +30,5 @@
 #include <bsp/irq.h>
 #include <bsp/tms570-rti.h>
+#include <rtems/counter.h>
 
 /**
@@ -49,4 +50,6 @@
 
   uint32_t microsec_per_tick = rtems_configuration_get_microseconds_per_tick();
+
+  rtems_counter_initialize_converter(BSP_PLL_OUT_CLOCK);
 
   /* Hardware specific initialize */
@@ -81,5 +84,4 @@
   TMS570_RTI.RTIINTFLAG = 0x00000001;
   tms570_rti_last_tick_fcr0 = TMS570_RTI.RTICOMP0 - TMS570_RTI.RTIUDCP0;
-  /* TMS570_RTI.RTICOMP0 += 1000; */
 }
 
Index: c/src/lib/libbsp/arm/tms570/include/system-clocks.h
===================================================================
--- c/src/lib/libbsp/arm/tms570/include/system-clocks.h	(revision 46265063e3300ab613c03151d7aaade580b10554)
+++ c/src/lib/libbsp/arm/tms570/include/system-clocks.h	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -0,0 +1,62 @@
+/**
+ * @file benchmark_timer.c
+ *
+ * @ingroup tms570
+ *
+ * @brief System clocks.
+ */
+
+/*
+ * Copyright (c) 2014 Pavel Pisa <pisa@cmp.felk.cvut.cz>
+ *
+ * Czech Technical University in Prague
+ * Zikova 1903/4
+ * 166 36 Praha 6
+ * Czech Republic
+ *
+ * Based on LPC24xx and LPC1768 BSP
+ * by embedded brains GmbH and others
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#ifndef LIBBSP_ARM_TMS570_SYSTEM_CLOCKS_H
+#define LIBBSP_ARM_TMS570_SYSTEM_CLOCKS_H
+
+#include <bsp/tms570-rti.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif /* __cplusplus */
+
+/**
+ * @defgroup tms570_clock System Clocks
+ *
+ * @ingroup tms570
+ *
+ * @brief System clocks.
+ *
+ * @{
+ */
+
+/**
+ * @brief Returns current standard timer value in microseconds.
+ *
+ * This function uses RTI module free running counter 0 used
+ * which is used as system tick timebase as well.
+ */
+static inline unsigned tms570_timer(void)
+{
+  uint32_t actual_fcr0 = TMS570_RTI.RTIFRC0;
+  return actual_fcr0;
+}
+
+/** @} */
+
+#ifdef __cplusplus
+}
+#endif /* __cplusplus */
+
+#endif /* LIBBSP_ARM_TMS570_SYSTEM_CLOCKS_H */
Index: c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk.cfg
===================================================================
--- c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk.cfg	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk.cfg	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -9,5 +9,5 @@
 CPU_CFLAGS = -march=armv7-r -mthumb -mbig-endian
 
-CFLAGS_OPTIMIZE_V = -O2 -ggdb3 -DNDEBUG
+CFLAGS_OPTIMIZE_V = -O2 -ggdb
 BINEXT?=.bin
 
Index: c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_intram.cfg
===================================================================
--- c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_intram.cfg	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_intram.cfg	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -9,5 +9,5 @@
 CPU_CFLAGS = -march=armv7-r -mthumb -mbig-endian
 
-CFLAGS_OPTIMIZE_V = -O2 -ggdb3 -DNDEBUG
+CFLAGS_OPTIMIZE_V = -O2 -ggdb
 BINEXT?=.bin
 
Index: c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_sdram.cfg
===================================================================
--- c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_sdram.cfg	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/make/custom/tms570ls3137_hdk_sdram.cfg	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -9,5 +9,5 @@
 CPU_CFLAGS = -march=armv7-r -mthumb -mbig-endian
 
-CFLAGS_OPTIMIZE_V = -O2 -ggdb3 -DNDEBUG
+CFLAGS_OPTIMIZE_V = -O2 -ggdb
 BINEXT?=.bin
 
Index: c/src/lib/libbsp/arm/tms570/misc/cpucounterread.c
===================================================================
--- c/src/lib/libbsp/arm/tms570/misc/cpucounterread.c	(revision 46265063e3300ab613c03151d7aaade580b10554)
+++ c/src/lib/libbsp/arm/tms570/misc/cpucounterread.c	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -0,0 +1,44 @@
+/**
+ * @file
+ *
+ * @ingroup tms570_clocks
+ *
+ * @brief System clocks.
+ */
+
+/*
+ * Copyright (c) 2014 Pavel Pisa <pisa@cmp.felk.cvut.cz>
+ *
+ * Czech Technical University in Prague
+ * Zikova 1903/4
+ * 166 36 Praha 6
+ * Czech Republic
+ *
+ * Based on LPC24xx and LPC1768 BSP
+ * by embedded brains GmbH and others
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#include <stdlib.h>
+
+#include <rtems.h>
+#include <bsp.h>
+
+
+/**
+ * @brief returns the actual value of Cortex-R cycle counter register
+ *
+ * The register is incremented at each core clock period
+ *
+ * @retval x actual core clock counter value
+ *
+ */
+CPU_Counter_ticks _CPU_Counter_read(void)
+{
+  uint32_t ticks;
+  asm volatile ("mrc p15, 0, %0, c9, c13, 0\n": "=r" (ticks));
+  return ticks;
+}
Index: c/src/lib/libbsp/arm/tms570/preinstall.am
===================================================================
--- c/src/lib/libbsp/arm/tms570/preinstall.am	(revision 4407ee675cb22e8bb870a76eafc590eb6e754315)
+++ c/src/lib/libbsp/arm/tms570/preinstall.am	(revision 46265063e3300ab613c03151d7aaade580b10554)
@@ -110,4 +110,8 @@
 PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/tms570-sci-driver.h
 
+$(PROJECT_INCLUDE)/bsp/system-clocks.h: include/system-clocks.h $(PROJECT_INCLUDE)/bsp/$(dirstamp)
+	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/bsp/system-clocks.h
+PREINSTALL_FILES += $(PROJECT_INCLUDE)/bsp/system-clocks.h
+
 $(PROJECT_INCLUDE)/tm27.h: ../../shared/include/tm27.h $(PROJECT_INCLUDE)/$(dirstamp)
 	$(INSTALL_DATA) $< $(PROJECT_INCLUDE)/tm27.h
