From 78cbbf334eba753576bc333493dd91853022e9a1 Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Sat, 19 Jul 2025 17:47:52 +0200
Subject: [PATCH] com2w: alloc buffer after receiving message. receiving works
 now.

---
 avr/modules/com2w/com2w.asm | 64 ++++++++++++++++++++++++++++++-------
 1 file changed, 53 insertions(+), 11 deletions(-)

diff --git a/avr/modules/com2w/com2w.asm b/avr/modules/com2w/com2w.asm
index d888f15..d2fe8b2 100644
--- a/avr/modules/com2w/com2w.asm
+++ b/avr/modules/com2w/com2w.asm
@@ -98,23 +98,64 @@ COM2W_Every100ms:
 ; @clobbers R16, R17, R18, R19, R20, R22, R24, R25, X
 
 com2wReceiveNextPkg:
+  mov xl, yl
+  mov xh, yh
+  adiw xh:xl, COM2W_IFACE_OFFS_BUFFER
+  ldd r18, Y+NET_IFACE_OFFS_ADDRESS
+  ldi r19, COM2W_BUFFER_SIZE
+  rcall com2wRecvMsg                              ; (r16, r17, r18, r19, r20, r22, r24, r25, X)
+  brcc com2wReceiveNextPkg_end
+  
+  mov xl, yl
+  mov xh, yh
+  adiw xh:xl, COM2W_IFACE_OFFS_BUFFER
+  rcall NETMSG_CheckMessageInBuffer               ; (R16, R17, R18, R19, R20, X)
+  brcc com2wReceiveNextPkg_eCrc
+
+  ; msg received, alloc buffer for it
   rcall NET_Buffer_Alloc                          ; R16=buffer num (R16, R17, X)
   brcs com2wReceiveNextPkg_gotBuffer
   ldi r16, NET_IFACE_OFFS_ERR_NOBUF_LOW
   rcall NET_Interface_IncCounter16                ; (R24, R25)
   rjmp com2wReceiveNextPkg_end
-com2wReceiveNextPkg_gotBuffer:
-  push r16                                        ; buffer number
-    rcall NET_Interface_SetIfaceNumInBuffer       ; (R16, R17)
-    adiw xh:xl, 1
-    ldd r18, Y+NET_IFACE_OFFS_ADDRESS
-    ldi r19, NET_BUFFERS_SIZE-1
-    rcall com2wReceiveAndCheckMsg                 ; (R16, R17, R18, R19, R20, R22, R24, R25)
-  pop r16
-  brcc com2wReceiveNextPkg_relBuffer
-  rcall NET_AddIncomingMsgNum                     ; (R17, R18, X)
-  brcs com2wReceiveNextPkg_end
 
+  ; copy received message into allocated buffer
+com2wReceiveNextPkg_gotBuffer:
+  mov r19, r16                               ; save buffer num
+  rcall NET_Interface_SetIfaceNumInBuffer    ; (R16, R17)
+  mov r16, r19                               ; restore buffer num
+  push zl
+    push zh
+      mov zl, yl
+      mov zh, yh
+      adiw zh:zl, COM2W_IFACE_OFFS_BUFFER
+      adiw xh:xl, 1
+      ldd r18, Z+NETMSG_OFFS_MSGLEN
+      inc r18
+      inc r18
+      inc r18
+com2wReceiveNextPkg_copyLoop:
+        ld r17, Z+
+        st X+, r17
+        dec r18
+      brne com2wReceiveNextPkg_copyLoop
+    pop zh
+  pop zl
+
+  ; add to incoming msg pool
+  rcall NET_AddIncomingMsgNum                     ; (R17, R18, X)
+  brcc com2wReceiveNextPkg_eMissed
+  ldi r16, NET_IFACE_OFFS_PACKETSIN_LOW
+  rcall NET_Interface_IncCounter16                ; (R24, R25)
+  sec
+  rjmp com2wReceiveNextPkg_end
+com2wReceiveNextPkg_eCrc:
+  push r16
+    ldi r16, NET_IFACE_OFFS_ERR_CONTENT_LOW
+    rcall NET_Interface_IncCounter16              ; (R24, R25)
+  pop r16
+  rjmp com2wReceiveNextPkg_relBuffer
+com2wReceiveNextPkg_eMissed:
   push r16
     ldi r16, NET_IFACE_OFFS_ERR_MISSED_LOW
     rcall NET_Interface_IncCounter16              ; (R24, R25)
@@ -122,6 +163,7 @@ com2wReceiveNextPkg_gotBuffer:
   ; fall-through to release buffer
 com2wReceiveNextPkg_relBuffer:
   rcall NET_Buffer_ReleaseByNum                   ; (R16, X)
+  clc
 com2wReceiveNextPkg_end:
   ret
 ; @end