From b3199681de718a8f07261eb927904b2c1da162c7 Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Sun, 20 Oct 2024 18:46:34 +0200
Subject: [PATCH] avr: optimized value functions.

---
 avr/modules/comproto/msg_value.asm | 52 ++++++++++++++++++++++++------
 1 file changed, 43 insertions(+), 9 deletions(-)

diff --git a/avr/modules/comproto/msg_value.asm b/avr/modules/comproto/msg_value.asm
index 477ddd1..1aa6022 100644
--- a/avr/modules/comproto/msg_value.asm
+++ b/avr/modules/comproto/msg_value.asm
@@ -30,6 +30,44 @@
 ; @clobbers       R16 (R17, R18, R19, R20, R21)
 
 CPRO_WriteReportValue:
+  rcall CPRO_PrepareReportValue              ; (R16, R17, R18, R19, R20, R21)
+
+  ; set msg id
+  adiw xh:xl, CPRO_PACKET_VALUE_OFFS_MSGID
+  ldi r18, 1
+  lds r16, com2LastMsgId
+  lds r17, com2LastMsgId+1
+  add r16, r18
+  dec r18
+  adc r17, r18
+  sts com2LastMsgId, r16
+  sts com2LastMsgId+1, r17
+  st X+, r16
+  st X+, r17
+  sbiw xh:xl, CPRO_PACKET_VALUE_OFFS_MSGID+2 ; go back to begin  of msg
+  
+  ; calc and add checksum
+  rcall com2CalcAndAddChecksumByte           ; (R16, R17, R18, R19, X)
+  sbiw xh:xl, 17                             ; go back to beginning of message
+  ret
+; @end
+
+
+
+; ---------------------------------------------------------------------------
+; @routine CPRO_PrepareReportValue @global
+; Write a REPORT_VALUE packet (except the checksum byte).
+;
+; @return nothing
+; @param  R16     destination address
+; @param  R17     value id
+; @param  R19:R18 value
+; @param  R21:R20 denom (e.g. 100, meaning value must be divided by 100)
+; @param  R22     value type
+; @param  X       buffer to write to
+; @clobbers       R16 (R17, R18, R19, R20, R21)
+
+CPRO_PrepareReportValue:
   st X+, r16     ; dest address
   ldi r16, 14    ; msg code+src address+12 payload bytes
   st X+, r16     ; msg len
@@ -37,11 +75,7 @@ CPRO_WriteReportValue:
   st X+, r16     ; msg code
   lds r16, com2Address
   st X+, r16     ; src address
-  clr r16
-  push r17
-    ldi r17, 6   ; uid (4 bytes), msg/ref id (2 bytes)
-    rcall Utils_FillSram ; (R17)
-  pop r17
+  adiw xh:xl, 6  ; skip uid (4 bytes), msg/ref id (2 bytes)
   st X+, r17     ; value id
   st X+, r22     ; value type
   st X+, r18     ; value (low)
@@ -51,10 +85,10 @@ CPRO_WriteReportValue:
 
   sbiw xh:xl, 12 ; go back to UID (12=16 back, 4 forward)
   rcall COM2_AddUidToBuffer ; (r16, r18, r19, r20, r21)
-  sbiw xh:xl, 8 ; go back to beginning of message
-  
-  rcall com2CalcAndAddChecksumByte            ; (R16, R17, R18, R19, X)
-  sbiw xh:xl, 16 ; go back to beginning of message
+  clr r16
+  st X+, r16     ; msg id (low)
+  st X+, r16     ; msg id (high)
+  sbiw xh:xl, 10 ; go back to beginning of message
   ret
 ; @end