From a909eba31cf98988460aab7c8158849c90b49b39 Mon Sep 17 00:00:00 2001
From: mykola2312 <49044616+mykola2312@users.noreply.github.com>
Date: Thu, 21 Apr 2022 05:21:27 +0300
Subject: [PATCH] rewrite core.S

---
 core.S | 73 +++++++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 55 insertions(+), 18 deletions(-)

diff --git a/core.S b/core.S
index 01da618..7b7d0cc 100644
--- a/core.S
+++ b/core.S
@@ -1,25 +1,62 @@
 .syntax unified
 .cpu cortex-m4
-.fpu softvfp
 .thumb
 
-.global vtable
-.global reset_handler
+.global _estack
 
-.type vtable, %object
-vtable:
-    .word _estack
-    .word reset_handler
-.size vtable, .-vtable
+.equ SCR, 0xE000E000
+.equ STCSR, SCR+0x10
+.equ STRVR, SCR+0x14
+.equ STCVR, SCR+0x18
+.equ STCR, SCR+0x20
 
-.type reset_handler, %function
-reset_handler:
-  LDR  r0, =_estack
-  MOV  sp, r0
+ivt:
+  .word _estack
+  .word int0_reset
 
-  LDR  r7, =0xDEADBEEF
-  MOVS r0, #0
-  main_loop:
-    ADDS r0, r0, #1
-    B    main_loop
-.size reset_handler, .-reset_handler
+int0_reset:
+  ldr r0, =_estack
+  mov sp, r0
+
+  bl main
+
+  ldr r7, =0xDEADBEEF
+  mov r0, #0
+  reset_loop:
+    add r0, r0, #1
+    b reset_loop
+
+.equ CLOCK_1MS, 180000
+
+# r0 - delay
+systick_wait:
+  push {lr}
+
+  ldr r1, =CLOCK_1MS
+  mul r0, r0, r1
+  ldr r1, =STCVR
+  str r0, [r1]
+
+  mov r0, #0b101
+  ldr r1, =STCSR
+  str r0, [r1]
+
+  systick_loop:
+    ldr r0, [r1]
+    ands r0, #0b10000
+    beq systick_loop
+  
+  pop {lr}
+  bx lr
+
+main:
+  mov r0, #9000
+  bl systick_wait
+
+  ldr r6, =0xAABBCCDD
+  dbg_loop: b dbg_loop
+
+  mov r0, #9000
+  bl systick_wait
+
+  bx lr
\ No newline at end of file