From a45d5e560498cdab22ebb6e27a0da826fc658ddc Mon Sep 17 00:00:00 2001
From: occheung <dc@m-labs.hk>
Date: Thu, 10 Sep 2020 10:28:51 +0800
Subject: [PATCH] logger: init

---
 examples/util/logger.rs | 96 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 96 insertions(+)
 create mode 100644 examples/util/logger.rs

diff --git a/examples/util/logger.rs b/examples/util/logger.rs
new file mode 100644
index 0000000..bc756ab
--- /dev/null
+++ b/examples/util/logger.rs
@@ -0,0 +1,96 @@
+// Enables ITM
+pub unsafe fn enable_itm(
+    dbgmcu: &stm32h7xx_hal::stm32::DBGMCU,
+    dcb: &mut cortex_m::peripheral::DCB,
+    itm: &mut cortex_m::peripheral::ITM
+) {
+    // ARMv7-M DEMCR: Set TRCENA. Enables DWT and ITM units
+    //unsafe { *(0xE000_EDFC as *mut u32) |= 1 << 24 };
+    dcb.enable_trace();
+
+    // Ensure debug blocks are clocked before interacting with them
+    dbgmcu.cr.modify(|_, w| {
+        w.d1dbgcken()
+            .set_bit()
+            .d3dbgcken()
+            .set_bit()
+            .traceclken()
+            .set_bit()
+            .dbgsleep_d1()
+            .set_bit()
+    });
+
+    // SWO: Unlock
+    *(0x5c00_3fb0 as *mut u32) = 0xC5ACCE55;
+    // SWTF: Unlock
+    *(0x5c00_4fb0 as *mut u32) = 0xC5ACCE55;
+
+    // SWO CODR Register: Set SWO speed
+    *(0x5c00_3010 as *mut _) = 400 - 1;
+
+    // SWO SPPR Register:
+    // 1 = Manchester
+    // 2 = NRZ
+    *(0x5c00_30f0 as *mut _) = 2;
+
+    // SWTF Trace Funnel: Enable for CM7
+    *(0x5c00_4000 as *mut u32) |= 1;
+
+    // ITM: Unlock
+    itm.lar.write(0xC5ACCE55);
+    // ITM Trace Enable Register: Enable lower 8 stimulus ports
+    itm.ter[0].write(1);
+    // ITM Trace Control Register: Enable ITM
+    itm.tcr.write(
+        (0b000001 << 16) | // TraceBusID
+        (1 << 3) | // enable SWO output
+        (1 << 0), // enable the ITM
+    );
+}
+
+// use panic_itm as _;
+
+use lazy_static::lazy_static;
+use log::LevelFilter;
+
+pub use cortex_m_log::log::Logger;
+
+use cortex_m_log::{
+    destination::Itm as ItmDest,
+    printer::itm::InterruptSync,
+    modes::InterruptFree,
+    printer::itm::ItmSync
+};
+
+lazy_static! {
+    static ref LOGGER: Logger<ItmSync<InterruptFree>> = Logger {
+        level: LevelFilter::Debug,
+        inner: unsafe {
+            InterruptSync::new(
+                ItmDest::new(cortex_m::Peripherals::steal().ITM)
+            )
+        },
+    };
+}
+
+pub fn init() {
+    cortex_m_log::log::init(&LOGGER).unwrap();
+}
+
+use panic_semihosting as _;
+
+use cortex_m_log::printer::semihosting;
+use cortex_m_log::printer::semihosting::Semihosting;
+use cortex_m_log::modes::InterruptOk;
+use cortex_m_semihosting::hio::HStdout;
+
+lazy_static! {
+    static ref HLOGGER: Logger<Semihosting<InterruptOk, HStdout>> = Logger {
+        level: LevelFilter::Debug,
+        inner: semihosting::InterruptOk::<_>::stdout().expect("Get Semihosting stdout"),
+    };
+}
+
+pub fn semihosting_init() {
+    cortex_m_log::log::init(&HLOGGER).unwrap();
+}
\ No newline at end of file