From 5f837e12b91a029aea22e45cde1daa06ee4328f4 Mon Sep 17 00:00:00 2001
From: Ondrej Mikle <ondrej.mikle@gmail.com>
Date: Mon, 7 Dec 2020 04:45:29 +0100
Subject: [PATCH] feat(core): send messaged through systemview

---
 core/embed/firmware/main.c       |  6 ++++++
 core/embed/firmware/mphalport.c  | 14 ++++++++++++++
 core/embed/firmware/systemview.c | 24 +++++++++++++++++-------
 core/embed/firmware/systemview.h | 11 +++++++++--
 core/embed/trezorhal/usbd_conf.c |  5 +++++
 5 files changed, 51 insertions(+), 9 deletions(-)

diff --git a/core/embed/firmware/main.c b/core/embed/firmware/main.c
index 691a9f1774..9ef85834a2 100644
--- a/core/embed/firmware/main.c
+++ b/core/embed/firmware/main.c
@@ -51,6 +51,7 @@
 #include "supervise.h"
 #include "touch.h"
 
+
 int main(void) {
   // initialize pseudo-random number generator
   drbg_init();
@@ -176,6 +177,11 @@ void SVC_C_Handler(uint32_t *stack) {
     case SVC_SET_PRIORITY:
       NVIC_SetPriority(stack[0], stack[1]);
       break;
+#ifdef SYSTEM_VIEW
+    case SVC_GET_DWT_CYCCNT:
+      cyccnt_cycles = *DWT_CYCCNT_ADDR;
+      break;
+#endif
     default:
       stack[0] = 0xffffffff;
       break;
diff --git a/core/embed/firmware/mphalport.c b/core/embed/firmware/mphalport.c
index 9c5c9f1411..493f475e79 100644
--- a/core/embed/firmware/mphalport.c
+++ b/core/embed/firmware/mphalport.c
@@ -20,6 +20,8 @@
 #include "common.h"
 #include "py/mphal.h"
 #include "usb.h"
+#include "systemview.h"
+#include "string.h"
 
 static int vcp_iface_num = -1;
 
@@ -41,3 +43,15 @@ void mp_hal_stdout_tx_strn(const char *str, size_t len) {
 }
 
 void mp_hal_set_vcp_iface(int iface_num) { vcp_iface_num = iface_num; }
+
+#ifdef SYSTEM_VIEW
+int segger_print(const char* str, size_t len) {
+    static char str_copy[1024];
+    size_t copylen = len > 1023 ? 1023 : len;
+    memcpy(str_copy, str, copylen);
+    str_copy[copylen] = 0;
+    SEGGER_SYSVIEW_Print(str_copy);
+    return len;
+}
+#endif
+
diff --git a/core/embed/firmware/systemview.c b/core/embed/firmware/systemview.c
index 275adf1e7f..e14c319466 100644
--- a/core/embed/firmware/systemview.c
+++ b/core/embed/firmware/systemview.c
@@ -1,6 +1,7 @@
 #ifdef SYSTEM_VIEW
 
 #include <stdint.h>
+#include "systemview.h"
 
 #include "SEGGER_SYSVIEW_Conf.h"
 #include "SEGGER_SYSVIEW.h"
@@ -8,6 +9,9 @@
 #define SYSTICK ((SYSTICK_REGS*)0xE000E010)
 #define SCS     ((SCS_REGS*)0xE000ED00)
 
+// for storing DWT CYCCNT from SVC call
+volatile uint32_t cyccnt_cycles;
+
 typedef struct {
     volatile unsigned int CSR;
     volatile unsigned int RVR;
@@ -42,17 +46,23 @@ typedef struct {
 
 extern uint32_t SystemCoreClock;
 
-U32 SEGGER_SYSVIEW_X_GetTimestamp ()
-{
-//    static U32 i = 0;
-//    return i++;
-    return (*(U32 *)(0xE0001004));
+static inline uint32_t is_mode_unprivileged(void) {
+    uint32_t r0;
+    __asm__ volatile("mrs %0, control" : "=r"(r0));
+    return r0 & 1;
+}
+
+uint32_t svc_get_dwt_cyccnt() {
+    if (is_mode_unprivileged()) {
+        __asm__ __volatile__("svc %0" ::"i"(SVC_GET_DWT_CYCCNT));
+        } else {
+        cyccnt_cycles = *DWT_CYCCNT_ADDR;
+    }
+    return cyccnt_cycles;
 }
 
 U32 SEGGER_SYSVIEW_X_GetInterruptId()
 {
-//    static U32 i = 0;
-//    return (i++) % 200;
     return ((*(U32*)(0xE000ED04)) & 0x1FF);
 }
 
diff --git a/core/embed/firmware/systemview.h b/core/embed/firmware/systemview.h
index 6169ee5512..0806067cf7 100644
--- a/core/embed/firmware/systemview.h
+++ b/core/embed/firmware/systemview.h
@@ -4,13 +4,20 @@
 
 #ifdef SYSTEM_VIEW
 
+#include <stdint.h>
 #include "SEGGER_SYSVIEW.h"
 
+#define DWT_CYCCNT_ADDR ((uint32_t*)(0xE0001004));
+#define SVC_GET_DWT_CYCCNT 3
+
+extern volatile uint32_t cyccnt_cycles;
+
 void enable_systemview();
+uint32_t svc_get_dwt_cyccnt();
 
 #else
-#define SEGGER_SYSVIEW_RecordEnterISR() do {} while(0)
-#define SEGGER_SYSVIEW_RecordExitISR() do {} while(0)
+#define SEGGER_SYSVIEW_RecordEnterISR()
+#define SEGGER_SYSVIEW_RecordExitISR()
 #endif
 
 #endif //CORE_SYSTEMVIEW_H
diff --git a/core/embed/trezorhal/usbd_conf.c b/core/embed/trezorhal/usbd_conf.c
index 989d89c74b..9122618ce8 100644
--- a/core/embed/trezorhal/usbd_conf.c
+++ b/core/embed/trezorhal/usbd_conf.c
@@ -56,6 +56,7 @@
 #include "usb.h"
 #include "irq.h"
 #include "supervise.h"
+#include "systemview.h"
 
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
@@ -702,20 +703,24 @@ void  USBD_LL_Delay(uint32_t Delay)
   */
 #if defined(USE_USB_FS)
 void OTG_FS_IRQHandler(void) {
+    SEGGER_SYSVIEW_RecordEnterISR();
     IRQ_ENTER(OTG_FS_IRQn);
     if (pcd_fs_handle.Instance) {
         HAL_PCD_IRQHandler(&pcd_fs_handle);
     }
     IRQ_EXIT(OTG_FS_IRQn);
+    SEGGER_SYSVIEW_RecordExitISR();
 }
 #endif
 #if defined(USE_USB_HS)
 void OTG_HS_IRQHandler(void) {
+    SEGGER_SYSVIEW_RecordEnterISR();
     IRQ_ENTER(OTG_HS_IRQn);
     if (pcd_hs_handle.Instance) {
         HAL_PCD_IRQHandler(&pcd_hs_handle);
     }
     IRQ_EXIT(OTG_HS_IRQn);
+    SEGGER_SYSVIEW_RecordExitISR();
 }
 #endif