Enable MMU + D-cache: fix sustained host→device WRITE

agent/cobs.c

@@ -47,10 +47,5 @@ uint32_t cobs_decode(const uint8_t *input, uint32_t len, uint8_t *output) {
         }
     }
 
-    /* Remove trailing zero if present */
-    if (out_idx > 0 && output[out_idx - 1] == 0x00) {
-        out_idx--;
-    }
-
     return out_idx;
 }

agent/main.c

@@ -76,7 +76,8 @@ static void watchdog_disable(void) {
 }
 
 static uint32_t read_le32(const uint8_t *p) {
-    return p[0] | (p[1] << 8) | (p[2] << 16) | (p[3] << 24);
+    return (uint32_t)p[0] | ((uint32_t)p[1] << 8) |
+           ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
 }
 
 static void write_le32(uint8_t *p, uint32_t v) {
@@ -226,7 +227,9 @@ static void handle_write(const uint8_t *data, uint32_t len) {
             uint32_t chunk = pkt_len - 2;
             for (uint32_t i = 0; i < chunk && received < size; i++)
                 dest[received++] = pkt[2 + i];
-            /* No per-packet ACK — continuous streaming for throughput */
+            /* Backpressure: COBS-framed ACK after each DATA packet.
+             * Host waits for this before sending next packet. */
+            proto_send_ack(ACK_OK);
         } else if (cmd == 0) {
             uint8_t err[5];
             err[0] = ACK_FLASH_ERROR;
@@ -439,6 +442,8 @@ static void handle_selfupdate(const uint8_t *data, uint32_t len) {
             uint32_t chunk = pkt_len - 2;
             for (uint32_t i = 0; i < chunk && received < size; i++)
                 dest[received++] = pkt[2 + i];
+            /* Backpressure ACK — host must wait before sending next */
+            proto_send_ack(ACK_OK);
         } else if (cmd == 0) {
             proto_send_ack(ACK_FLASH_ERROR);
             return;
@@ -448,7 +453,7 @@ static void handle_selfupdate(const uint8_t *data, uint32_t len) {
     uint32_t actual_crc = crc32(0, dest, size);
     if (actual_crc != expected_crc) {
         proto_send_ack(ACK_CRC_ERROR);
-        return;
+        return;  /* Stay alive — don't jump to bad code */
     }
 
     proto_send_ack(ACK_OK);
@@ -653,7 +658,7 @@ int main(void) {
              * revert to 115200. Host may have disconnected. */
             if (!at_default_baud) {
                 baud_idle++;
-                if (baud_idle >= 20) {
+                if (baud_idle >= 60) {  /* ~30 seconds */
                     uart_set_baud(115200);
                     while (uart_readable()) uart_getc();
                     at_default_baud = 1;

agent/protocol.c

@@ -164,7 +164,8 @@ uint8_t proto_recv(uint8_t *data, uint32_t *len, uint32_t timeout_ms) {
     /* Verify CRC32 — read bytes individually to avoid unaligned access */
     uint32_t payload_len = raw_len - 4;
     volatile uint8_t *cp = &rx_raw[payload_len];
-    uint32_t expected_crc = cp[0] | (cp[1] << 8) | (cp[2] << 16) | (cp[3] << 24);
+    uint32_t expected_crc = (uint32_t)cp[0] | ((uint32_t)cp[1] << 8) |
+                           ((uint32_t)cp[2] << 16) | ((uint32_t)cp[3] << 24);
     uint32_t actual_crc = crc32(0, rx_raw, payload_len);
 
     /* Drain after CRC */
@@ -191,3 +192,10 @@ void proto_send_ack(uint8_t status) {
 void proto_drain_fifo(void) {
     soft_rx_drain();
 }
+
+void proto_reset_rx(void) {
+    /* Flush everything: software FIFO + hardware FIFO */
+    soft_rx_head = 0;
+    soft_rx_tail = 0;
+    uart_drain_rx();
+}

agent/protocol.h

@@ -54,4 +54,7 @@ uint32_t crc32(uint32_t crc, const uint8_t *buf, uint32_t len);
  * long computations to prevent 16-byte hardware FIFO overflow. */
 void proto_drain_fifo(void);
 
+/* Reset all RX buffers (software + hardware) */
+void proto_reset_rx(void);
+
 #endif /* PROTOCOL_H */

agent/startup.S

@@ -3,7 +3,8 @@
  *
  * Uploaded via HiSilicon boot protocol to DDR.
  * DDR and clocks are already initialized by the bootrom + DDR step.
- * We just need to set up the stack and jump to main().
+ * Sets up MMU with identity mapping + D-cache for fast COBS+CRC
+ * processing, then jumps to main().
  */
 
     .section .text.start
@@ -13,8 +14,6 @@
 /*
  * ARM exception vector table.
  * Must be at the entry point so VBAR points here.
- * All exceptions loop forever instead of resetting the SoC
- * (the SPL's handlers call reset_cpu which kills us).
  */
 _start:
 _vectors:
@@ -32,42 +31,33 @@ _exc_hang:
 
 /*
  * Data abort handler — prints fault info over UART before hanging.
- * On entry (ABT mode): LR_abt = faulting instruction + 8.
- * We must NOT use the stack (SP_abt is uninitialized).
- * Instead, read CP15 regs and call handler using SVC stack.
  */
     .global _data_abort
 _data_abort:
-    sub     lr, lr, #8              /* LR = faulting PC */
-
-    /* Read fault info into r0-r2 (caller-saved, safe to clobber) */
-    mrc     p15, 0, r0, c6, c0, 0  /* r0 = DFAR (fault address) */
-    mrc     p15, 0, r1, c5, c0, 0  /* r1 = DFSR (fault status) */
+    sub     lr, lr, #8
+    mrc     p15, 0, r0, c6, c0, 0  /* r0 = DFAR */
+    mrc     p15, 0, r1, c5, c0, 0  /* r1 = DFSR */
     mov     r2, lr                  /* r2 = faulting PC */
-
-    /* Switch to SVC mode (which has a valid stack) with IRQ/FIQ disabled */
-    cps     #0x13                   /* SVC mode */
-
+    cps     #0x13                   /* SVC mode (valid stack) */
     push    {r0-r3, lr}
     bl      data_abort_handler
     pop     {r0-r3, lr}
-
     b       _exc_hang
 
 _start_code:
-    /* Set VBAR to our vector table (use ADR for PC-relative) */
+    /* Set VBAR to our vector table */
     adr     r0, _start
-    mcr     p15, 0, r0, c12, c0, 0  /* Write VBAR */
+    mcr     p15, 0, r0, c12, c0, 0
     isb
 
     /* Disable interrupts */
     mrs     r0, cpsr
-    orr     r0, r0, #0xC0      /* Disable IRQ and FIQ */
+    orr     r0, r0, #0xC0
     msr     cpsr_c, r0
 
-    /* Invalidate TLBs, caches, then disable MMU */
+    /* Disable MMU/caches first (clean state for setup) */
     mov     r0, #0
-    mcr     p15, 0, r0, c8, c7, 0  /* Invalidate entire TLB */
+    mcr     p15, 0, r0, c8, c7, 0  /* Invalidate TLB */
     mcr     p15, 0, r0, c7, c5, 0  /* Invalidate I-cache */
     mcr     p15, 0, r0, c7, c5, 6  /* Invalidate branch predictor */
     dsb
@@ -77,15 +67,15 @@ _start_code:
     bic     r0, r0, #0x1           /* Disable MMU */
     bic     r0, r0, #0x4           /* Disable D-cache */
     bic     r0, r0, #0x1000        /* Disable I-cache */
-    mcr     p15, 0, r0, c1, c0, 0  /* Write SCTLR */
+    mcr     p15, 0, r0, c1, c0, 0
     dsb
     isb
 
     /* Set up stack pointer (16KB below _start) */
     ldr     sp, =_start
-    sub     sp, sp, #0x4000     /* 16KB stack below code */
+    sub     sp, sp, #0x4000
 
-    /* Clear BSS */
+    /* Clear BSS (includes page table) */
     ldr     r0, =__bss_start
     ldr     r1, =__bss_end
     mov     r2, #0
@@ -94,10 +84,100 @@ bss_loop:
     strlt   r2, [r0], #4
     blt     bss_loop
 
+    /* ===== MMU PAGE TABLE SETUP =====
+     *
+     * ARMv7 short-descriptor, 1MB sections.
+     * Identity mapping: virtual == physical.
+     *
+     * Section descriptor bits:
+     *   [1:0]  = 0b10  (section)
+     *   [2]    = B (bufferable)
+     *   [3]    = C (cacheable)
+     *   [4]    = XN (execute never)
+     *   [5:8]  = Domain 0
+     *   [11:10]= AP = 0b11 (full access)
+     *   [14:12]= TEX
+     *   [31:20]= Physical base (section number)
+     *
+     * DEVICE (uncached, strongly-ordered):
+     *   AP=11, Domain=0, XN=1, B=0, C=0, TEX=000, section
+     *   = (3<<10) | (1<<4) | (0b10) = 0x00000C12
+     *
+     * CACHED (write-back, write-allocate):
+     *   AP=11, Domain=0, XN=0, B=1, C=1, TEX=001, section
+     *   = (3<<10) | (1<<12) | (1<<3) | (1<<2) | (0b10) = 0x00001C0E
+     */
+
+    /* r4 = page table base, r5 = DEVICE descriptor template,
+     * r6 = CACHED descriptor template, r7 = loop counter */
+    ldr     r4, =_page_table
+
+    /* Fill all 4096 entries as DEVICE (uncached) */
+    ldr     r5, =0x00000C12        /* DEVICE descriptor */
+    mov     r0, r4                 /* r0 = current entry pointer */
+    mov     r1, r5                 /* r1 = descriptor for section 0 */
+    mov     r7, #4096
+pt_fill_device:
+    str     r1, [r0], #4
+    add     r1, r1, #0x100000     /* Next 1MB section */
+    subs    r7, r7, #1
+    bne     pt_fill_device
+
+    /* Mark DDR sections as CACHED.
+     * RAM_BASE is set at compile time (0x40000000 or 0x80000000).
+     * We cache 128MB = 128 sections starting at RAM_BASE >> 20. */
+    ldr     r6, =0x00001C0E        /* CACHED descriptor */
+    ldr     r1, =RAM_BASE
+    lsr     r2, r1, #20           /* r2 = first DDR section number */
+    add     r0, r4, r2, lsl #2   /* r0 = &page_table[first_section] */
+    orr     r1, r6, r2, lsl #20  /* r1 = descriptor for first DDR section */
+    mov     r7, #128              /* 128 sections = 128MB */
+pt_fill_ddr:
+    str     r1, [r0], #4
+    add     r1, r1, #0x100000
+    subs    r7, r7, #1
+    bne     pt_fill_ddr
+
+    /* ===== ENABLE MMU + CACHES ===== */
+
+    /* Invalidate TLB again (page table just written) */
+    mov     r0, #0
+    mcr     p15, 0, r0, c8, c7, 0
+    dsb
+    isb
+
+    /* Set TTBR0 = page_table | inner WB/WA | outer WB/WA */
+    ldr     r0, =_page_table
+    orr     r0, r0, #(1 << 0)     /* IRGN bit 0 (inner WB) */
+    orr     r0, r0, #(1 << 6)     /* IRGN bit 6 */
+    orr     r0, r0, #(1 << 3)     /* RGN = outer WB */
+    mcr     p15, 0, r0, c2, c0, 0
+    isb
+
+    /* DACR = all domains manager (no permission checks) */
+    mvn     r0, #0
+    mcr     p15, 0, r0, c3, c0, 0
+    isb
+
+    /* Enable MMU + D-cache + I-cache via SCTLR */
+    mrc     p15, 0, r0, c1, c0, 0
+    orr     r0, r0, #(1 << 0)     /* M = MMU enable */
+    orr     r0, r0, #(1 << 2)     /* C = D-cache enable */
+    orr     r0, r0, #(1 << 12)    /* I = I-cache enable */
+    mcr     p15, 0, r0, c1, c0, 0
+    dsb
+    isb
+
     /* Jump to C main */
     bl      main
 
-    /* If main returns, loop forever */
 hang:
     wfi
     b       hang
+
+/* ===== PAGE TABLE (in BSS, 16KB aligned) ===== */
+    .section .bss
+    .align 14
+    .global _page_table
+_page_table:
+    .space 16384

agent/test_agent.c

@@ -46,19 +46,17 @@ static void test_cobs_all_zeros(void) {
         ASSERT(enc[i] != 0, "cobs all-zeros: no zeros in output");
 
     uint32_t dec_len = cobs_decode(enc, enc_len, dec);
-    /* COBS strips trailing zero — [0,0,0,0] roundtrips to [0,0,0] */
-    ASSERT(dec_len == 3, "cobs all-zeros: length (trailing zero stripped)");
-    for (uint32_t i = 0; i < dec_len; i++)
-        ASSERT(dec[i] == 0, "cobs all-zeros: data");
+    ASSERT(dec_len == 4, "cobs all-zeros: length");
+    ASSERT(memcmp(in, dec, 4) == 0, "cobs all-zeros: data");
 }
 
 static void test_cobs_single_zero(void) {
     uint8_t in[] = {0};
     uint8_t enc[8], dec[8];
     uint32_t enc_len = cobs_encode(in, 1, enc);
     uint32_t dec_len = cobs_decode(enc, enc_len, dec);
-    /* Single zero roundtrips to empty — trailing zero stripped */
-    ASSERT(dec_len == 0, "cobs single zero: stripped to empty");
+    ASSERT(dec_len == 1, "cobs single zero: length");
+    ASSERT(dec[0] == 0, "cobs single zero: value");
 }
 
 static void test_cobs_empty(void) {

agent/test_cobs_detail.c

@@ -0,0 +1,77 @@
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include "cobs.h"
+
+extern uint32_t crc32(uint32_t, const uint8_t*, uint32_t);
+
+
+int main(int argc, char *argv[]) {
+    FILE *f = fopen(argv[1], "rb");
+    fseek(f, 0, SEEK_END); long sz = ftell(f); fseek(f, 0, SEEK_SET);
+    uint8_t *fw = malloc(sz); fread(fw, 1, sz, f); fclose(f);
+
+    /* Block 3 (offset 49152), packet 29 (offset 49152 + 29*512 = 63,616) */
+    uint32_t off = 49152 + 29 * 512;
+    uint8_t chunk[512];
+    memcpy(chunk, &fw[off], 512);
+
+    /* Build DATA packet: cmd(1) + seq(2) + data(512) + crc(4) = 519 bytes */
+    uint8_t raw[520];
+    raw[0] = 0x82; /* RSP_DATA */
+    raw[1] = 29; raw[2] = 0; /* seq=29 LE */
+    memcpy(&raw[3], chunk, 512);
+    uint32_t raw_len = 515;
+    uint32_t c = crc32(0, raw, raw_len);
+    raw[raw_len+0] = c & 0xFF;
+    raw[raw_len+1] = (c >> 8) & 0xFF;
+    raw[raw_len+2] = (c >> 16) & 0xFF;
+    raw[raw_len+3] = (c >> 24) & 0xFF;
+    raw_len += 4; /* 519 */
+
+    printf("Raw payload: %u bytes, CRC=0x%08x\n", raw_len, c);
+    printf("  raw[515..518] (CRC bytes): %02x %02x %02x %02x\n",
+           raw[515], raw[516], raw[517], raw[518]);
+
+    /* COBS encode */
+    uint8_t encoded[600];
+    uint32_t enc_len = cobs_encode(raw, raw_len, encoded);
+    printf("COBS encoded: %u bytes\n", enc_len);
+
+    /* COBS decode */
+    uint8_t decoded[600];
+    uint32_t dec_len = cobs_decode(encoded, enc_len, decoded);
+    printf("COBS decoded: %u bytes\n", dec_len);
+
+    /* Compare */
+    if (dec_len != raw_len) {
+        printf("LENGTH MISMATCH: decoded=%u raw=%u\n", dec_len, raw_len);
+    }
+
+    int match = (dec_len == raw_len) && (memcmp(decoded, raw, raw_len) == 0);
+    printf("Roundtrip match: %s\n", match ? "YES" : "NO");
+
+    if (!match) {
+        for (uint32_t i = 0; i < raw_len && i < dec_len; i++) {
+            if (decoded[i] != raw[i]) {
+                printf("  First diff at byte %u: decoded=0x%02x raw=0x%02x\n",
+                       i, decoded[i], raw[i]);
+                break;
+            }
+        }
+    }
+
+    /* Extract CRC from decoded */
+    if (dec_len >= 5) {
+        uint32_t plen = dec_len - 4;
+        uint32_t exp = (uint32_t)decoded[plen] | ((uint32_t)decoded[plen+1]<<8) |
+                       ((uint32_t)decoded[plen+2]<<16) | ((uint32_t)decoded[plen+3]<<24);
+        uint32_t act = crc32(0, decoded, plen);
+        printf("CRC check: actual=0x%08x expected=0x%08x %s\n",
+               act, exp, act == exp ? "OK" : "MISMATCH");
+    }
+
+    free(fw);
+    return match ? 0 : 1;
+}