[PATCH] TMS570: Add board reset code to bsp_reset

[Martin Galvan]

---
 c/src/lib/libbsp/arm/tms570/include/tms570.h   | 19 +++++++------
 c/src/lib/libbsp/arm/tms570/startup/bspreset.c | 37 ++++++++++++++++----------
 2 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/c/src/lib/libbsp/arm/tms570/include/tms570.h b/c/src/lib/libbsp/arm/tms570/include/tms570.h
index 2023a29..50f1315 100644
--- a/c/src/lib/libbsp/arm/tms570/include/tms570.h
+++ b/c/src/lib/libbsp/arm/tms570/include/tms570.h
@@ -7,15 +7,9 @@
  */
 
 /*
- * Copyright (c) 2014 Premysl Houdek <kom541000 at gmail.com>
+ * Copyright (c) 2015 Taller Technologies.
  *
- * Google Summer of Code 2014 at
- * Czech Technical University in Prague
- * Zikova 1903/4
- * 166 36 Praha 6
- * Czech Republic
- *
- * Based on LPC24xx and LPC1768 BSP
+ * @author Martin Galvan <martin.galvan at tallertechnologies.com>
  *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
@@ -25,4 +19,13 @@
 #ifndef LIBBSP_ARM_TMS570_H
 #define LIBBSP_ARM_TMS570_H
 
+#define SYSECR (*(uint32_t *)0xFFFFFFE0u) /* System Exception Control Register */
+#define ESMIOFFHR (*(uint32_t *)0xFFFFF528) /* ESM Interrupt Offset High Register */
+#define ESMSR1 (*(uint32_t *)0xFFFFF518u) /* ESM Status Register 1 */
+#define ESMSR2 (*(uint32_t *)0xFFFFF51Cu) /* ESM Status Register 2 */
+#define ESMSR3 (*(uint32_t *)0xFFFFF520u) /* ESM Status Register 3 */
+#define ESMSR4 (*(uint32_t *)0xFFFFF558u) /* ESM Status Register 4 */
+
+#define SYSECR_RESET 0x80000u
+
 #endif /* LIBBSP_ARM_TMS570_H */
diff --git a/c/src/lib/libbsp/arm/tms570/startup/bspreset.c b/c/src/lib/libbsp/arm/tms570/startup/bspreset.c
index d47920c..a4b6647 100644
--- a/c/src/lib/libbsp/arm/tms570/startup/bspreset.c
+++ b/c/src/lib/libbsp/arm/tms570/startup/bspreset.c
@@ -7,30 +7,39 @@
  */
 
 /*
- * Copyright (c) 2014 Premysl Houdek <kom541000 at gmail.com>
+ * Copyright (c) 2015 Taller Technologies.
  *
- * Google Summer of Code 2014 at
- * Czech Technical University in Prague
- * Zikova 1903/4
- * 166 36 Praha 6
- * Czech Republic
- *
- * Based on LPC24xx and LPC1768 BSP
+ * @author Martin Galvan <martin.galvan at tallertechnologies.com>
  *
  * 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 <rtems.h>
-
-#include <bsp/bootcard.h>
+#include <bsp.h>
 #include <bsp/tms570.h>
 #include <bsp/start.h>
 
-BSP_START_TEXT_SECTION __attribute__( ( flatten ) ) void bsp_reset( void )
+static void handle_esm_errors(uint32_t esm_irq_channel)
+{
+  /* ESMR3 errors don't generate interrupts. */
+  if (esm_irq_channel < 0x20u) {
+    ESMSR1 = 1 << esm_irq_channel;
+  } else if (esm_irq_channel < 0x40u) {
+    ESMSR2 = 1 << (esm_irq_channel - 32u);
+  } else if (esm_irq_channel < 0x60u) {
+    ESMSR4 = 1 << (esm_irq_channel - 64u);
+  }
+}
+
+void bsp_reset(void)
 {
-  while ( true ) {
-    /* Do nothing */
+  uint32_t esm_irq_channel = ESMIOFFHR - 1;
+
+  if (esm_irq_channel) {
+    handle_esm_errors(esm_irq_channel);
   }
+
+  /* Reset the board */
+  SYSECR = SYSECR_RESET;
 }
-- 
2.3.4