From 656cbdd7d406a69313ac261bdb1420a8c7bdf723 Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Sat, 28 Jan 2023 15:29:33 +0100
Subject: [PATCH] COM: Added COM_EnqueueDebug

---
 aqhome/libtest.c |  3 +++
 avr/com.asm      | 54 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 57 insertions(+)

diff --git a/aqhome/libtest.c b/aqhome/libtest.c
index 22deb46..2f1bf41 100644
--- a/aqhome/libtest.c
+++ b/aqhome/libtest.c
@@ -140,6 +140,9 @@ void _packetReceived(AQH_SERIAL *sr, const uint8_t *ptr, uint8_t len)
     availability=ptr[5];
     fprintf(stdout, "-> I2C DEVICE %02x: %s\n", i2cAddr, (availability==0)?"not available":"FOUND");
   }
+  else if (ptr[5]==1) {
+    fprintf(stdout, "-> Debug\n");
+  }
 }
 
 
diff --git a/avr/com.asm b/avr/com.asm
index d6f7dd4..7d913fb 100644
--- a/avr/com.asm
+++ b/avr/com.asm
@@ -41,6 +41,7 @@
 .equ COM_CMD_COMSENDSTATS         = 2
 .equ COM_CMD_COMRECVSTATS         = 3
 .equ COM_CMD_I2CBUSMEMBER         = 4
+.equ COM_CMD_DEBUG                = 5
 
 
 ; ***************************************************************************
@@ -396,6 +397,59 @@ COM_EnqueueI2cBusMember_error:
 
 
 
+; ---------------------------------------------------------------------------
+; Enqueue a DEBUG packet.
+;
+; IN:
+; - R16: destination address
+; - R1: debug value 1
+; - R2: debug value 2
+; OUT:
+; - CFLAG: set if okay, clear otherwise
+; MODIFIED REGS: R16, R17, R20, X (R15, Y)
+
+COM_EnqueueDebug:
+  push r16
+    rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
+  pop r16
+  brcc COM_EnqueueDebug_error
+  clr r17                         ; r17: XOR byte
+  ; write header (dest address, msg length)
+  st X+, r16                      ; destination address
+  eor r17, r16
+  ldi r16, 4                      ; 2 bytes payload
+  st X+, r16
+  eor r17, r16
+  ; write payload
+  ldi r16, COM_CMD_DEBUG
+  st X+, r16
+  eor r17, r16
+  lds r16, comAddress
+  st X+, r16
+  eor r17, r16
+  mov r16, r1 ; debug 1
+  st X+, r16
+  eor r17, r16
+  mov r16, r2 ; debug 2
+  st X+, r16
+  eor r17, r16
+  ; store XOR byte
+  st X+, r17
+  ; mark buffer as enqueued with PRIO "info" (limited amount of retries)
+  ldi r20, COM_BUFFER_PRIO_INFO
+  rcall COM_EnqueuePacket      ; (R15, R16)
+  brcc COM_EnqueueDebug_error
+  sec
+  ret
+COM_EnqueueDebug_error:
+  clc
+  ret
+
+
+
+
+
+
 ; ---------------------------------------------------------------------------
 ; comHandleNextPacketInQueue
 ;