; ***************************************************************************
; code

.cseg

; ---------------------------------------------------------------------------
; 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)

CPRO_EnqueueDebug:
  push r16
    rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
  pop r16
  brcc CPRO_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, CPRO_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 CPRO_EnqueueDebug_error
  sec
  ret
CPRO_EnqueueDebug_error:
  clc
  ret