From f28dda75420fd4a92934613135e920f75fe2637f Mon Sep 17 00:00:00 2001
From: imi415 <imi415@imi.moe>
Date: Sat, 25 Jun 2022 16:29:45 +0800
Subject: [PATCH] We have main.

---
 src/main.c        | 50 +++++++++++++++++++++++++++++++++++------------
 startup_stx7105.S | 33 ++++++++++++++++++++-----------
 stx7105.ld        |  1 +
 3 files changed, 60 insertions(+), 24 deletions(-)

diff --git a/src/main.c b/src/main.c
index ec03b08..490cbb7 100644
--- a/src/main.c
+++ b/src/main.c
@@ -14,8 +14,22 @@
 #define ST_GPIO0_OFFSET_SET_PC2  0x44U
 #define ST_GPIO0_OFFSET_CLR_PC2  0x48U
 
-#define LED_RED_PIN  4U
-#define LED_BLUE_PIN 5U
+#define SH_TMU_BASE_ADDR    (0xFFD80000)
+#define SH_TMU_OFFSET_TOCR  0x00U
+#define SH_TMU_OFFSET_TSTR  0x04U
+#define SH_TMU_OFFSET_TCOR0 0x08U
+#define SH_TMU_OFFSET_TCNT0 0x0CU
+#define SH_TMU_OFFSET_TCR0  0x10U
+#define SH_TMU_OFFSET_TCOR1 0x14U
+#define SH_TMU_OFFSET_TCNT1 0x18U
+#define SH_TMU_OFFSET_TCR1  0x1CU
+#define SH_TMU_OFFSET_TCOR2 0x20U
+#define SH_TMU_OFFSET_TCNT2 0x24U
+#define SH_TMU_OFFSET_TCR2  0x28U
+#define SH_TMU_OFFSET_TCPR2 0x2CU
+
+#define LED_RED_PIN  5U
+#define LED_BLUE_PIN 4U
 
 static void init_led(uint8_t pin) {
     *(uint32_t *)(ST_GPIO0_BASE_ADDR + ST_GPIO0_OFFSET_CLR_PC0) = (1 << pin);
@@ -32,27 +46,37 @@ static void set_led(uint8_t pin, uint8_t val) {
     }
 }
 
-static void delay(uint32_t msec) {
-    volatile uint32_t loop = 0;
-    for (uint32_t i = 0; i < msec; i++) {
-        for (uint32_t j = 0; j < 40000; i++) {
-            loop++;
-        }
-    }
+static void delay_ms(uint32_t msec) {
+    /* Initialize TMU and count to zero */
+    /* TMU clock is from Peripheral clock, approx. 66MHz */
+    /* Prescale to 450kHz for convenience (TMUs can only divide by max. 1024) */
+
+    *(uint8_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TSTR) &= ~1U;          /* Stop counter */
+    *(uint16_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TCR0)  = 0x04U;       /* 1024 prescale */
+    *(uint32_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TCNT0) = (msec * 66); /* 66kHz */
+    *(uint8_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TSTR) |= 1U;           /* Start counter */
+
+    /* Wait until underflow occurs */
+    uint16_t tcr0 = 0U;
+    do {
+        tcr0 = *(uint16_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TCR0);
+    } while ((tcr0 & 0x100) == 0);
+
+    *(uint8_t *)(SH_TMU_BASE_ADDR + SH_TMU_OFFSET_TSTR) &= ~1U; /* Stop counter */
 }
 
 int main(void) {
     init_led(LED_RED_PIN);
     init_led(LED_BLUE_PIN);
 
-    set_led(LED_RED_PIN, 1);
-    set_led(LED_BLUE_PIN, 1);
+    set_led(LED_RED_PIN, 0);
+    set_led(LED_BLUE_PIN, 0);
 
     for (;;) {
         /* Dead loop */
         set_led(LED_BLUE_PIN, 1);
-        delay(1000);
+        delay_ms(1);
         set_led(LED_BLUE_PIN, 0);
-        delay(1000);
+        delay_ms(1);
     }
 }
\ No newline at end of file
diff --git a/startup_stx7105.S b/startup_stx7105.S
index c0cd74d..0650adf 100644
--- a/startup_stx7105.S
+++ b/startup_stx7105.S
@@ -34,6 +34,11 @@ _start:
     ldc r0, sr
     mov.l _stack_k, sp /* Setup R15(SP) */
 
+_disable_wdt:
+    mov.l _cpg_wdt_wtcsr_k, r0
+    mov.l _cpg_wdt_wtcsr_value_k, r1
+    mov.w r1, @r0
+
 _configure_spinor:
     mov.l _emi_spinor_config_data_k, r0
     mov.l _emi_spinor_config_data_value_k, r1
@@ -154,18 +159,7 @@ _copy_data:
     mov.l _edata_k, r2
 
 _loop_copy_data:
-/* Turn on Red LED */
-    mov #32, r7
-    mov.l _gpio_set_k, r8
-    mov.l r7, @r8
-
     mov.l @r0+, r3 /* Load a word to r3 from [sidata], with post-increment of 4 */
-
-/* Turn on Blue LED */
-    mov #16, r7
-    mov.l _gpio_set_k, r8
-    mov.l r7, @r8
-
     mov.l r3, @r1  /* Store the word in r3 to [sdata] */
     add #4, r1     /* Increment sdata pointer */
     cmp/ge r1, r2
@@ -182,12 +176,23 @@ _loop_zero_bss:
     cmp/ge r0, r1
     bt _loop_zero_bss
 
+/* Turn on Blue LED */
+    mov #16, r7
+    mov.l _gpio_set_k, r8
+    mov.l r7, @r8
+
 _setup_fpu:
     mov.l _set_fpscr_k, r1
     jsr @r1
     mov #0, r4
     lds r3, fpscr
 
+/* Turn on Red LED */
+    mov #32, r7
+    mov.l _gpio_set_k, r8
+    mov.l r7, @r8
+
+_main_entry:
     mov.l _main_k, r0
     jsr @r0
     or r0, r0
@@ -251,6 +256,12 @@ _ccn_pascr_value_k:
 _sr_k:
     .long 0x400000F0
 
+/* WDT */
+_cpg_wdt_wtcsr_k:
+    .long 0xFFC0000C
+_cpg_wdt_wtcsr_value_k:
+    .long 0x0000A500
+
 /* EMI SPI NOR configuration registers */
 _emi_spinor_config_data_k:
     .long 0xFE702020
diff --git a/stx7105.ld b/stx7105.ld
index 4f87f6c..2e12aab 100644
--- a/stx7105.ld
+++ b/stx7105.ld
@@ -19,6 +19,7 @@ SECTIONS {
         *(.text*)
         *(.rodata)
         *(.rodata*)
+        . = ALIGN(4);
     } >EMI
 
     .data : {