Improve WFI and sleep implementation

Author: calc84maniac

core/arm/arm.c

@@ -25,7 +25,7 @@ static int arm_thrd(void *context) {
         uint16_t val;
         do {
             arm_cpu_execute(arm);
-        } while (++i);
+        } while (++i && !arm->sync.slp);
         peek = spsc_queue_peek(&arm->usart[0]);
         if (unlikely(peek != SPSC_QUEUE_INVALID_ENTRY &&
                      arm_mem_usart_recv(arm, 3, peek))) {
@@ -150,12 +150,16 @@ bool arm_usart_send(arm_t *arm, uint8_t val) {
     bool success = spsc_queue_enqueue(&arm->usart[0], val);
     if (likely(success)) {
         (void)spsc_queue_flush(&arm->usart[0]);
+        debug_char(false, val);
     }
     return success;
 }
 
 bool arm_usart_recv(arm_t *arm, uint8_t *val) {
     spsc_queue_entry_t entry = spsc_queue_dequeue(&arm->usart[1]);
     *val = entry;
+    if (entry != SPSC_QUEUE_INVALID_ENTRY) {
+        debug_char(true, *val);
+    }
     return entry != SPSC_QUEUE_INVALID_ENTRY;
 }

core/arm/armcpu.c

@@ -423,6 +423,11 @@ bool arm_cpu_exception(arm_t *arm, arm_exception_number_t exc) {
         }
         return false;
     }
+    cpu->wfi = false;
+    if (cpu->pm && exc > ARM_Exception_HardFault) {
+        return false;
+    }
+
     arm_cpu_tick(arm);
     cpu->exc = true;
     sp -= 0x20;
@@ -617,19 +622,17 @@ void arm_cpu_execute(arm_t *arm) {
         arm_cpu_exception(arm, ARM_Exception_HardFault);
         cpu->exc = false;
         return;
-    }
-    if (unlikely(!cpu->pm &&
-                 (icsr & (SCB_ICSR_NMIPENDSET_Msk |
-                          SCB_ICSR_PENDSVSET_Msk |
-                          SCB_ICSR_PENDSTSET_Msk) ||
-                  cpu->nvic.ipr & cpu->nvic.ier))) {
-        if (icsr & SCB_ICSR_NMIPENDSET_Msk) {
-            if (likely(arm_cpu_exception(arm, ARM_Exception_NMI))) {
-                cpu->scb.icsr &= ~SCB_ICSR_NMIPENDSET_Msk;
-                cpu->exc = false;
-                return;
-            }
-        } else if (icsr & SCB_ICSR_PENDSVSET_Msk) {
+    } else if (unlikely(icsr & SCB_ICSR_NMIPENDSET_Msk)) {
+        if (likely(arm_cpu_exception(arm, ARM_Exception_NMI))) {
+            cpu->scb.icsr &= ~SCB_ICSR_NMIPENDSET_Msk;
+            cpu->exc = false;
+            return;
+        }
+    } else if (unlikely((!cpu->pm || cpu->wfi) &&
+                        (icsr & (SCB_ICSR_PENDSVSET_Msk |
+                                 SCB_ICSR_PENDSTSET_Msk) ||
+                         cpu->nvic.ipr & cpu->nvic.ier))) {
+        if (icsr & SCB_ICSR_PENDSVSET_Msk) {
             if (likely(arm_cpu_exception(arm, ARM_Exception_PendSV))) {
                 cpu->scb.icsr &= ~SCB_ICSR_PENDSVSET_Msk;
                 cpu->exc = false;
@@ -654,6 +657,15 @@ void arm_cpu_execute(arm_t *arm) {
             return;
         }
     }
+    if (unlikely(cpu->wfi)) {
+        if ((arm->mem.pm.SLEEP.bit.IDLE == PM_SLEEP_IDLE_APB_Val) &&
+            (arm->cpu.scb.scr & SCB_SCR_SLEEPDEEP_Msk)) {
+            sync_sleep(&arm->sync);
+        } else {
+            arm_cpu_tick(arm);
+        }
+        return;
+    }
     arm_cpu_tick(arm);
     opc = arm_mem_load_half(arm, pc);
     if (unlikely(cpu->exc)) {
@@ -965,10 +977,7 @@ void arm_cpu_execute(arm_t *arm) {
                                         case 2: // Wait for Event hint
                                             break;
                                         case 3: // Wait for Interrupt hint
-                                            if ((arm->mem.pm.SLEEP.bit.IDLE == PM_SLEEP_IDLE_APB_Val) &&
-                                                (arm->cpu.scb.scr & SCB_SCR_SLEEPDEEP_Msk)) {
-                                                sync_sleep(&arm->sync);
-                                            }
+                                            cpu->wfi = true;
                                             break;
                                         case 4: // Send Event hint
                                             break;

core/arm/armcpu.h

@@ -35,7 +35,7 @@ typedef union arm_cpu {
     struct {
         uint32_t r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, ip, sp, lr, pc, altsp;
         uint64_t active;
-        bool v, c, z, n, pm, spsel, exc;
+        bool v, c, z, n, pm, spsel, exc, wfi;
         arm_systick_t systick;
         arm_nvic_t nvic;
         arm_scb_t scb;

core/arm/armmem.c

@@ -1128,6 +1128,10 @@ static void arm_mem_store_any(arm_t *arm, uint32_t val, uint32_t mask, uint32_t
             switch (offset) {
                 case (USB_CTRLA_OFFSET | USB_SYNCBUSY_OFFSET | USB_QOSCTRL_OFFSET) >> 2:
                     return;
+                case USB_DEVICE_EPSTATUSSET_OFFSET >> 2:
+                    return;
+                case USB_DEVICE_EPINTENSET_OFFSET >> 2:
+                    return;
             }
         } else if (addr < (uint32_t)SBMATRIX) { // MTB
             switch (offset) {