hwstub: enhance exit protocol and implement on stmp

Rename STOP command to EXIT, introduce ATEXIT, this gives better
control over the exit of the stub. Add stmp implementation.

Change-Id: I45442c8b88b9330d12ef439417ca5ffa1520477a

2 files changed, 71 insertions, 2 deletions

diff --git a/utils/hwstub/stub/stmp/target.c b/utils/hwstub/stub/stmp/target.c
index 11516e5492..da46d50779 100644
--- a/utils/hwstub/stub/stmp/target.c
+++ b/utils/hwstub/stub/stmp/target.c
@@ -49,7 +49,35 @@ enum stmp_family_t
     STMP3780
 };
 
-enum stmp_family_t g_stmp_family = UNKNOWN;
+static enum stmp_family_t g_stmp_family = UNKNOWN;
+static int g_atexit = HWSTUB_ATEXIT_OFF;
+
+/**
+ *
+ * Power
+ *
+ */
+
+#define HW_POWER_BASE       0x80044000
+
+void power_off(void)
+{
+    switch(g_stmp_family)
+    {
+        case STMP3600:
+            *(volatile uint32_t *)(HW_POWER_BASE + 0xc0) = 0x3e770014;
+            break;
+        case STMP3700:
+        case STMP3770:
+            *(volatile uint32_t *)(HW_POWER_BASE + 0xe0) = 0x3e770003;
+            break;
+        case STMP3780:
+            *(volatile uint32_t *)(HW_POWER_BASE + 0x100) = 0x3e770003;
+            break;
+        default:
+            break;
+    }
+}
 
 /**
  *
@@ -86,6 +114,25 @@ enum stmp_family_t g_stmp_family = UNKNOWN;
 #define HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK30M_GATE    (1 << 30)
 #define HW_CLKCTRL_UTMICLKCTRL__UTMI_CLK120M_GATE   (1 << 31)
 
+void clkctrl_reset(void)
+{
+    switch(g_stmp_family)
+    {
+        case STMP3600:
+            *(volatile uint32_t *)(HW_POWER_BASE + 0xc0) = 0x3e770002;
+            break;
+        case STMP3700:
+        case STMP3770:
+            *(volatile uint32_t *)(HW_CLKCTRL_BASE + 0xf0) = 0x1;
+            break;
+        case STMP3780:
+            *(volatile uint32_t *)(HW_CLKCTRL_BASE + 0x100) = 0x1;
+            break;
+        default:
+            break;
+    }
+}
+
 /**
  *
  * Digctl
@@ -205,6 +252,24 @@ int target_get_info(int info, void **buffer)
         return -1;
 }
 
+int target_atexit(int method)
+{
+    g_atexit = method;
+    return 0;
+}
+
 void target_exit(void)
 {
+    switch(g_atexit)
+    {
+        case HWSTUB_ATEXIT_OFF:
+            power_off();
+            // fallthrough in case of return
+        case HWSTUB_ATEXIT_REBOOT:
+            clkctrl_reset();
+            // fallthrough in case of return
+        case HWSTUB_ATEXIT_NOP:
+        default:
+            return;
+    }
 }
diff --git a/utils/hwstub/stub/target.h b/utils/hwstub/stub/target.h
index 3f1551c72d..56c960741f 100644
--- a/utils/hwstub/stub/target.h
+++ b/utils/hwstub/stub/target.h
@@ -23,9 +23,13 @@
 
 #include "protocol.h"
 
+/* do target specific init */
 void target_init(void);
+/* exit, performing the atexit action (default is target specific) */
 void target_exit(void);
-/* return actual size or -1 if error */
+/* get information, return actual size or -1 if error */
 int target_get_info(int info, void **buffer);
+/* set atexit action or return -1 on error */
+int target_atexit(int action);
 
 #endif /* __TARGET_H__ */