From 44bed4dc14651162d5155ac64355dad68c9b43eb Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Wed, 10 May 2023 21:09:08 +0200
Subject: [PATCH] avr: fixed some bugs in address handling code

- fixed bugs in jumptable code and calls
  NOTE to self: addresses in flash are WORD addresses, so we need
  to double the address difference when working with bytes!
- correctly set params for call to cproAddressSetTimer
---
 avr/modules/comproto/addr2.asm | 29 ++++++++++++++++++++---------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/avr/modules/comproto/addr2.asm b/avr/modules/comproto/addr2.asm
index b898523..50be3f1 100644
--- a/avr/modules/comproto/addr2.asm
+++ b/avr/modules/comproto/addr2.asm
@@ -38,14 +38,21 @@ CPRO_Address_OnPacketReceived:
   adiw xh:xl, COM2_MSG_OFFS_CMD
   ld r16, x
   sbiw xh:xl, COM2_MSG_OFFS_CMD
+
+  cpi r16, CPRO_CMD_NEED_ADDRESS
+  brcs CPRO_Address_OnPacketReceived_nc  ; smaller than 60
   
-  ldi r17, (cproPacketTypeTransTableEnd-cproPacketTypeTransTableBegin) & 0xff
+  cpi r16, (CPRO_CMD_ADDRESS_RANGE+1)
+  brcc CPRO_Address_OnPacketReceived_nc
+
+CPRO_Address_OnPacketReceived_debug2:
+  ldi r17, (cproPacketTypeTransTableEnd-cproPacketTypeTransTableBegin)*2
   ldi zl, LOW(cproPacketTypeTransTableBegin)
   ldi zh, HIGH(cproPacketTypeTransTableBegin)
   rcall Utils_FindBytePositionInTable
   brcc CPRO_Address_OnPacketReceived_nc
-  
-  ldi r17, (cproPacketTypeTransTableEnd-cproPacketTypeTransTableBegin) & 0xff
+
+  ldi r17, (cproPacketTypeTransTableEnd-cproPacketTypeTransTableBegin)*2
   ldi zl, LOW(cproPacketTypeHandleTable)
   ldi zh, HIGH(cproPacketTypeHandleTable)
   rcall Utils_TableJump
@@ -58,12 +65,12 @@ CPRO_Address_OnPacketReceived_nc:
 
 
 cproPacketTypeTransTableBegin:
- .db CPRO_CMD_PING, CPRO_CMD_NEED_ADDRESS, CPRO_CMD_HAVE_ADDRESS, CPRO_CMD_ADDRESS_RANGE, CPRO_CMD_DENY_ADDRESS, CPRO_CMD_CLAIM_ADDRESS
+ .db CPRO_CMD_NEED_ADDRESS, CPRO_CMD_HAVE_ADDRESS, CPRO_CMD_ADDRESS_RANGE, CPRO_CMD_DENY_ADDRESS, CPRO_CMD_CLAIM_ADDRESS, CPRO_CMD_PING
 cproPacketTypeTransTableEnd:
 
 ; position within table must be in same order as in table above!
 cproPacketTypeHandleTable:
-  .dw cproHandlePing, cproHandleNeedAddr, cproHandleHaveAddr, cproHandleAddrRange, cproHandleDenyAddr, cproHandleClaimAddr
+  .dw cproHandleNeedAddr, cproHandleHaveAddr, cproHandleAddrRange, cproHandleDenyAddr, cproHandleClaimAddr, cproHandlePing
 
 
 
@@ -80,6 +87,8 @@ cproHandleNeedAddr:
   lds r24, com2Address
   clr r25
   adiw r25:r24, 3
+  mov r18, r24
+  mov r19, r25
   rjmp cproAddressSetTimer
 cproHandleNeedAddr_done:
   sec
@@ -111,18 +120,20 @@ cproHandleHaveAddr_done:
 cproHandleClaimAddr:
   adiw xh:xl, CPRO_PACKET_CLAIMADDR_OFFS_ADDRESS
   ld r16, x
-  tst r16
+  sbiw xh:xl, CPRO_PACKET_CLAIMADDR_OFFS_ADDRESS
+  tst r16                        ; dont handle claim addr 0
   breq cproHandleClaimAddr_done
-  cpi r16, 0xff
+  cpi r16, 0xff                  ; dont handle claim addr 0
   breq cproHandleClaimAddr_done
   lds r17, com2Address
   tst r17
-  breq cproHandleClaimAddr_done
+  breq cproHandleClaimAddr_done  ; we have no address, yet
   cp r16, r17
-  brne cproHandleClaimAddr_done
+  brne cproHandleClaimAddr_done  ; not our address
   ldi r16, CPRO_MODE_SEND_DENY_ADDR
   sts cproMode, r16
   rcall cproAddressSetTimer1s
+  
 cproHandleClaimAddr_done:
   sec
   ret