; ***************************************************************************
; defines


.equ CPRO_CMD_PING                 = 10
.equ CPRO_CMD_PONG                 = 11
.equ CPRO_CMD_COMSENDSTATS         = 20
.equ CPRO_CMD_COMRECVSTATS         = 21
.equ CPRO_CMD_TWIBUSMEMBER         = 30
.equ CPRO_CMD_DEBUG                = 40
.equ CPRO_CMD_VALUE                = 50
.equ CPRO_CMD_NEED_ADDRESS         = 60
.equ CPRO_CMD_HAVE_ADDRESS         = 61
.equ CPRO_CMD_CLAIM_ADDRESS        = 62
.equ CPRO_CMD_DENY_ADDRESS         = 63
.equ CPRO_CMD_ADDRESS_RANGE        = 64

.equ CPRO_CMD_FLASH_START          = 70
.equ CPRO_CMD_FLASH_END            = 71
.equ CPRO_CMD_FLASH_ADDR           = 72
.equ CPRO_CMD_FLASH_DATA           = 73
.equ CPRO_CMD_FLASH_RSP            = 74


; flags for variable payload enqueue function
.equ CPRO_PAYLOAD_FLAGS_SECONDS    = 0x01
.equ CPRO_PAYLOAD_FLAGS_UID        = 0x02
.equ CPRO_PAYLOAD_FLAGS_RESERVED1  = 0x04
.equ CPRO_PAYLOAD_FLAGS_RESERVED2  = 0x08
.equ CPRO_PAYLOAD_FLAGS_RESERVED3  = 0x10
.equ CPRO_PAYLOAD_FLAGS_NUM0       = 0x20
.equ CPRO_PAYLOAD_FLAGS_NUM1       = 0x40
.equ CPRO_PAYLOAD_FLAGS_NUM3       = 0x80
.equ CPRO_PAYLOAD_FLAGS_SHIFT_NUM  = 5


.equ CPRO_PACKET_HAVEADDR_OFFS_ADDRESS  = COM_MSG_OFFS_PAYLOAD+4
.equ CPRO_PACKET_CLAIMADDR_OFFS_ADDRESS = COM_MSG_OFFS_PAYLOAD+4
.equ CPRO_PACKET_DENYADDR_OFFS_ADDRESS  = COM_MSG_OFFS_PAYLOAD+4

.equ CPRO_PACKET_FLASH_START_OFFS_MSGNUM = COM_MSG_OFFS_PAYLOAD+0 ; 2 bytes
.equ CPRO_PACKET_FLASH_START_OFFS_ADDR   = COM_MSG_OFFS_PAYLOAD+2 ; 2 bytes

.equ CPRO_PACKET_FLASH_ADDR_OFFS_MSGNUM  = COM_MSG_OFFS_PAYLOAD+0 ; 2 bytes
.equ CPRO_PACKET_FLASH_ADDR_OFFS_ADDR    = COM_MSG_OFFS_PAYLOAD+2 ; 2 bytes

.equ CPRO_PACKET_FLASH_DATA_OFFS_MSGNUM  = COM_MSG_OFFS_PAYLOAD+0 ; 2 bytes
.equ CPRO_PACKET_FLASH_DATA_OFFS_ADDR    = COM_MSG_OFFS_PAYLOAD+2 ; 2 bytes
.equ CPRO_PACKET_FLASH_DATA_OFFS_DATA    = COM_MSG_OFFS_PAYLOAD+4 ; 2 bytes


.equ CPRO_WAITTIME_GETADDR       = 130
.equ CPRO_WAITTIME_CLAIMADDR     = 17
.equ CPRO_WAITTIME_RECLAIMADDR   = 10


; current mode of operation
.equ CPRO_MODE_NORMAL               =  0 ; normal operation
.equ CPRO_MODE_GETADDRSTARTED       = 10 ; waiting for HAVE_ADDRESS and ADDRESS_RANGE packets to arrive
.equ CPRO_MODE_CLAIMING_ADDR1       = 20 ; CLAIM_ADDRESS sent, waiting for HAVE_ADDRESS packet to reject the claim
.equ CPRO_MODE_CLAIMING_ADDR2       = 21 ; CLAIM_ADDRESS sent, 2nd try
.equ CPRO_MODE_CLAIMING_ADDR3       = 22 ; CLAIM_ADDRESS sent, 3rd try
.equ CPRO_MODE_SENDING_HAVE_ADDRESS = 30 ; waiting for our turn to send HAVE_ADDRESS packet
.equ CPRO_MODE_RECLAIMING_ADDR      = 40 ; CLAIM_ADDRESS with the previously used address sent after bootup



; ***************************************************************************
; data

.dseg


cproDataBegin:
#ifndef BASE_SYSTEM
  cproMode:                .byte 1  ; "normal", "waitForHaveAddress", "samplingAddresses", "claimAddress"
  cproAddrRangeBegin:      .byte 1
  cproAddrRangeEnd:        .byte 1
  cproAddressWaitCounter:  .byte 1  ; counter for seconds to wait for all nodes to respond
  cproUsedAddresses:       .byte 16 ; one bit per address known to b in use
#endif
cproDataEnd:



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

.cseg



CPRO_Init:
  ; preset SRAM data area
  ldi xh, HIGH(cproDataBegin)
  ldi xl, LOW(cproDataBegin)
  clr r16
  ldi r17, (cproDataEnd-cproDataBegin)
  rcall Utils_FillSram
  sec
  ret



CPRO_OnEverySecond:
#ifndef BASE_SYSTEM
  lds r17, cproMode
  cpi r17, CPRO_MODE_NORMAL
  breq CPRO_OnEverySecond_done
CPRO_OnEverySecond_l1:
  cpi r17, CPRO_MODE_GETADDRSTARTED
  brne CPRO_OnEverySecond_l2
  rjmp cproHandle1sGetAddrStarted
CPRO_OnEverySecond_l2:
  cpi r17, CPRO_MODE_SENDING_HAVE_ADDRESS
  brne CPRO_OnEverySecond_l3
  rjmp cproHandle1sSendingHaveAddress
CPRO_OnEverySecond_l3:
  cpi r17, CPRO_MODE_CLAIMING_ADDR1
  brne CPRO_OnEverySecond_l4
  rjmp cproHandle1sClaimingAddr12
CPRO_OnEverySecond_l4:
  cpi r17, CPRO_MODE_CLAIMING_ADDR2
  brne CPRO_OnEverySecond_l5
  rjmp cproHandle1sClaimingAddr12
CPRO_OnEverySecond_l5:
  cpi r17, CPRO_MODE_CLAIMING_ADDR3
  brne CPRO_OnEverySecond_l6
  rjmp cproHandle1sClaimingAddr3
CPRO_OnEverySecond_l6:
  cpi r17, CPRO_MODE_RECLAIMING_ADDR
  brne CPRO_OnEverySecond_done
  rjmp cproHandle1sReclaimingAddr
CPRO_OnEverySecond_done:
#endif
  ret



; ---------------------------------------------------------------------------
; CPRO_OnPacketReceived:
;
; Try to handle the given packet.
;
; IN:
; - Y  : pointer to received buffer
; OUT:
; - CFLAG: set if handled, cleared otherwise
; USED: depending on called routines

CPRO_OnPacketReceived:
  ldd r16, y+(COM_BUFFER_OFFS_DATA+COM_MSG_OFFS_CMD)
  cpi r16, CPRO_CMD_PING
  brne CPRO_OnPacketReceived_l1
  rjmp cproHandlePing
CPRO_OnPacketReceived_l1:
#ifndef BASE_SYSTEM
  cpi r16, CPRO_CMD_NEED_ADDRESS
  brne CPRO_OnPacketReceived_l2
  rjmp cproHandlePckNeedAddr
CPRO_OnPacketReceived_l2:
  cpi r16, CPRO_CMD_HAVE_ADDRESS
  brne CPRO_OnPacketReceived_l3
  rjmp cproHandlePckHaveAddr
CPRO_OnPacketReceived_l3:
  cpi r16, CPRO_CMD_ADDRESS_RANGE
  brne CPRO_OnPacketReceived_l4
  rjmp cproHandleAddrRange
CPRO_OnPacketReceived_l4:
  cpi r16, CPRO_CMD_DENY_ADDRESS
  brne CPRO_OnPacketReceived_l5
  rjmp cproHandleDenyAddr
CPRO_OnPacketReceived_l5:
  cpi r16, CPRO_CMD_CLAIM_ADDRESS
  brne CPRO_OnPacketReceived_l6
  rjmp cproHandlePckClaimAddr
CPRO_OnPacketReceived_l6:
#endif
  clc
  ret


cproHandlePing:
  ldd r16, y+(COM_BUFFER_OFFS_DATA+COM_MSG_OFFS_SRCADDR)
  tst r16
  breq cproHandlePing_notHandled
  rcall CPRO_EnqueuePong
  ret                       ; use carry flag from previous call
cproHandlePing_notHandled:
  clc
  ret







; ---------------------------------------------------------------------------
; Enqueue a PING packet.
;
; IN:
; - R16: destination address
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18, R20 (R15, R16, R17, X, Y)

CPRO_EnqueuePing:
  ldi r18, CPRO_CMD_PING
  ldi r20, COM_BUFFER_PRIO_INFO
  rjmp cproEnqueueMsgWithCmdAndSrcAddr



; ---------------------------------------------------------------------------
; Enqueue a PONG packet.
;
; IN:
; - R16: destination address
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18, R20 (R15, R16, R17, X, Y)

CPRO_EnqueuePong:
  ldi r18, CPRO_CMD_PONG
  ldi r20, COM_BUFFER_PRIO_INFO
  rjmp cproEnqueueMsgWithCmdAndSrcAddr



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a COM Send stats packet.
;
; IN:
; - R16: destination address
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17, R20, X (R15, Y)

CPRO_EnqueueComSendStats:
  push r15
    in r15, SREG
    cli
    push r16
      rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
    pop r16
    brcc CPRO_EnqueueComSendStats_error

    ldi r17, CPRO_PAYLOAD_FLAGS_SECONDS | (6<<CPRO_PAYLOAD_FLAGS_SHIFT_NUM) ; seconds + 6 bytes payload
    ldi r18, CPRO_CMD_COMSENDSTATS
    push xh
      push xl
        rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
        lds r16, comStatsPacketsOut            ; 6: packets out
        st X+, r16
        lds r16, comStatsPacketsOut+1
        st X+, r16
        lds r16, comStatsCollisions            ; 8: collisions
        st X+, r16
        lds r16, comStatsCollisions+1
        st X+, r16
        lds r16, comStatsAborted               ; 10: aborted
        st X+, r16
        lds r16, comStatsAborted+1
        st X+, r16
      pop xl
    pop xh
    rcall cproCalcAndAddChecksumByte
  
    ; mark buffer as enqueued with PRIO "important" (higher retry count)
    ldi r20, COM_BUFFER_PRIO_IMPORTANT
    rcall COM_EnqueuePacket      ; (R15, R16)
    brcc CPRO_EnqueueComSendStats_error
  pop r15
  out SREG, r15
  sec
  ret
CPRO_EnqueueComSendStats_error:
  pop r15
  out SREG, r15
  clc
  ret
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a TWI Bus Member packet.
;
; IN:
; - R16: destination address
; - R1 : Address of the bus member
; - R2 : availability (0=not available, 1=available)
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17, R20, X (R15, Y)

CPRO_EnqueueTwiBusMember:
  push r16
    rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
  pop r16
  brcc CPRO_EnqueueTwiBusMember_error
  clr r17                         ; r17: XOR byte
  ; write header (dest address, msg length)
  st X+, r16                      ; destination address
  eor r17, r16
  ldi r16, 4                      ; 4 bytes payload
  st X+, r16
  eor r17, r16
  ; write payload
  ldi r16, CPRO_CMD_TWIBUSMEMBER   ; send command
  st X+, r16
  eor r17, r16
  lds r16, comAddress             ; send source address
  st X+, r16
  eor r17, r16
  mov r16, r1                     ; send i2c bus member address
  st X+, r16
  eor r17, r16
  mov r16, r2                     ; send i2c bus member availability
  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_EnqueueTwiBusMember_error
  sec
  ret
CPRO_EnqueueTwiBusMember_error:
  clc
  ret
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; 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
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a VALUE packet.
;
; IN:
; - R16: destination address
; - R17: value id
; - R19:R18: value
; - R21:R20: denom (e.g. 100, meaning value must be divided by 100)
; - R22: value type
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R6, R7, R8, R9, R10, R11, R12, R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)

CPRO_EnqueueValue:
  mov r6, r16
  mov r7, r17
  mov r8, r18
  mov r9, r19
  mov r10, r20
  mov r11, r21
  mov r12, r22

  rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
  brcc CPRO_EnqueueValue_error

  push xh
    push xl
      mov r16, r6
      ldi r17, CPRO_PAYLOAD_FLAGS_SECONDS | (6<<CPRO_PAYLOAD_FLAGS_SHIFT_NUM)
      ldi r18, CPRO_CMD_VALUE
      rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
      st X+, r7                              ; 6: value id
      st X+, r12                             ; 7: value type
      st X+, r8                              ; 8: low value
      st X+, r9                              ; 9: high value
      st X+, r10                             ; 10: low denom
      st X+, r11                             ; 11: high denom
    pop xl
  pop xh
  rcall cproCalcAndAddChecksumByte

  ; mark buffer as enqueued with PRIO "normal" (slightly limited number of retries)
  ldi r20, COM_BUFFER_PRIO_NORMAL
  rcall COM_EnqueuePacket      ; (R15, R16)
  brcc CPRO_EnqueueValue_error
  sec
  ret
CPRO_EnqueueValue_error:
  clc
  ret
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a NEEDADDRESS packet.
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18 (R3, R4, R15, R16, R17, R20, R21, X, Y (R18, R19)

CPRO_EnqueueNeedAddress:
  ldi r18, CPRO_CMD_NEED_ADDRESS
  rjmp cproEnqueueAddressPacket
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a HAVE_ADDRESS packet.
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18 (R3, R4, R15, R16, R17, R18, R19, R20, R21, X, Y)

CPRO_EnqueueHaveAddress:
  ldi r18, CPRO_CMD_HAVE_ADDRESS
  lds r19, comAddress
  rjmp cproEnqueueAddressPacket
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a CLAIM_ADDRESS packet.
;
; IN:
; - R19: claimed address
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18 (R3, R4, R15, R16, R17, R18, R19, R20, R21, X, Y)

CPRO_EnqueueClaimAddress:
  ldi r18, CPRO_CMD_CLAIM_ADDRESS
  rjmp cproEnqueueAddressPacket
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; Enqueue a DENY_ADDRESS packet.
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R18, R19 (R3, R4, R15, R16, R17, R20, R21, X, Y)

CPRO_EnqueueDenyAddress:
  ldi r18, CPRO_CMD_DENY_ADDRESS
  lds r19, comAddress
  rjmp cproEnqueueAddressPacket
#endif



#ifndef BASE_SYSTEM
; ---------------------------------------------------------------------------
; cproEnqueueAddressPacket
; Enqueue a NEED/HAVE/CLAIM ADDRESS packet.
;
; IN:
; - R18: command (either CPRO_CMD_NEED_ADDRESS, CPRO_CMD_HAVE_ADDRESS or CPRO_CMD_CLAIM_ADDRESS)
; - R19: address to send (claim, have)
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17, R20, R21, X, Y (R3, R4, R15, R16, R17, R18, R19, R21, X)

cproEnqueueAddressPacket:
  mov r6, r19
  rcall COM_AllocBufferAndGetXY            ; (r16, r17, r21)
  brcc cproEnqueueAddressPacket_error
  ldi r16, 0xff
  ldi r17, CPRO_PAYLOAD_FLAGS_UID | (1<<CPRO_PAYLOAD_FLAGS_SHIFT_NUM)
  push xh
    push xl
      rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
      st X+, r6                              ; 5: value id
    pop xl
  pop xh
  rcall cproCalcAndAddChecksumByte

  ; mark buffer as enqueued with PRIO "important" (higher retry count)
  ldi r20, COM_BUFFER_PRIO_IMPORTANT
  rcall COM_EnqueuePacket      ; (R15, R16)
  brcc cproEnqueueAddressPacket_error
  sec
  ret
cproEnqueueAddressPacket_error:
  clc
  ret
#endif



; ---------------------------------------------------------------------------
; Enqueue a simple packet with payload only CMD and source address.
;
; IN:
; - R16: destination address
; - R18: command (e.g. CPRO_CMD_PING or CPRO_CMD_PONG)
; - R20: priority of the message
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)

cproEnqueueMsgWithCmdAndSrcAddr:
  push r16
    rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
  pop r16
  brcc cproEnqueueMsgWithCmdAndSrcAddr_error

  ldi r17, CPRO_PAYLOAD_FLAGS_SECONDS
  push xh
    push xl
      push r20
        rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
      pop r20
    pop xl
  pop xh
  rcall cproCalcAndAddChecksumByte

  ; mark buffer as enqueued with PRIO given in R20
  rcall COM_EnqueuePacket      ; (R15, R16)
  brcc cproEnqueueMsgWithCmdAndSrcAddr_error
  sec
  ret
cproEnqueueMsgWithCmdAndSrcAddr_error:
  clc
  ret



; ---------------------------------------------------------------------------
; begin packet with variable payload.
;
; IN:
; - R16: destination address
; - R17: flags
; - R18: command (e.g. CPRO_CMD_PING)
; - X  : pointer to packet buffer
; OUT:
; - X  : points to end of packet as it was written so far
; MODIFIED REGS: R3, R16, R17, R18, R19, R20, R21, X (R4)

cproBeginMsgWithVariablePayload:
  ; write header (dest address, msg length)
  st X+, r16                          ; destination address
  mov r16, r17                        ; calculate payload size
  mov r3, r17
  rcall cproCalcPayloadSize           ; (R4, R16, R17)
  inc r16                             ; add CMD byte
  inc r16                             ; add source address byte
  st X+, r16
  ; write payload
  st X+, r18                          ; 0: CMD
  lds r16, comAddress                 ; 1: source address
  st X+, r16
  lsr r3                             ; shift out CPRO_PAYLOAD_FLAGS_SECONDS
  brcc cproBeginMsgWithVariablePayload_l1
  lds r16, timerModuleCounterSecs     ; adding of current seconds counter requested
  st X+, r16
  lds r16, timerModuleCounterSecs+1
  st X+, r16
  lds r16, timerModuleCounterSecs+2
  st X+, r16
  lds r16, timerModuleCounterSecs+3
  st X+, r16
cproBeginMsgWithVariablePayload_l1:
  lsr r3                             ; shift out shift out CPRO_PAYLOAD_FLAGS_UID
  brcc cproBeginMsgWithVariablePayload_l2
  push xh
    push xl
      rcall Utils_ReadUid             ; (R16, X)
    pop xl
  pop xh
  st X+, r18
  st X+, r19
  st X+, r20
  st X+, r21
cproBeginMsgWithVariablePayload_l2:
  ret



; ---------------------------------------------------------------------------
; add checksum byte to buffer
;
; IN:
; - X  : pointer to packet buffer
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17, R18, X

cproCalcAndAddChecksumByte:
  clr r17
  ld r16, X+        ; dest addr
  eor r17, r16
  ld r18, X+        ; msg len
  eor r17, r18
cproCalcAndAddChecksumByte_loop:
    ld r16, X+
    eor r17, r16
    dec r18
  brne cproCalcAndAddChecksumByte_loop
  st X+, r17
  ret



; ---------------------------------------------------------------------------
; cproCalcPayloadSize
;
; Calculate payload size from given flags
;
; IN:
; - R16: flags
; OUT:
; - R16: payload size
; MODIFIED REGS: R4, R16, R17

cproCalcPayloadSize:
  clr r4
  ldi r17, 4
  lsr r16                      ; shift out CPRO_PAYLOAD_FLAGS_SECONDS
  brcc cproCalcPayloadSize_l1
  add r4, r17                   ; add 4 bytes
cproCalcPayloadSize_l1:
  lsr r16                      ; shift out CPRO_PAYLOAD_FLAGS_UID
  brcc cproCalcPayloadSize_l2
  add r4, r17                   ; add 4 bytes
cproCalcPayloadSize_l2:
  lsr r16
  lsr r16
  lsr r16                      ; after this R16 contains CPRO_PAYLOAD_FLAGS_NUM0-3
  add r16, r4                  ; add previous bytes to R16
  ret
  



#ifdef WITH_FLASH
; ---------------------------------------------------------------------------
; Enqueue a FLASHDATA_ACK packet.
;
; IN:
; - R16: destination address
; - R17: ACK(1) or NAK(0)
; - R19:R18: msg num
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R6, R7, R8, R9, R10, R11, R12, R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)

CPRO_EnqueueFlashRsp:
  mov r6, r16
  mov r7, r17
  mov r8, r18
  mov r9, r19

  rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
  brcc CPRO_EnqueueFlashRsp_error

  push xh
    push xl
      mov r16, r6
      ldi r17, CPRO_PAYLOAD_FLAGS_SECONDS | (3<<CPRO_PAYLOAD_FLAGS_SHIFT_NUM)
      ldi r18, CPRO_CMD_FLASH_RSP
      rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
      st X+, r8                              ; 4: msg num (low)
      st X+, r9                              ; 5: msg num (high)
      st X+, r7                              ; 6: ACK or NAK
    pop xl
  pop xh
  rcall cproCalcAndAddChecksumByte

  ; mark buffer as enqueued with PRIO "important" (higher number of retries)
  ldi r20, COM_BUFFER_PRIO_IMPORTANT
  rcall COM_EnqueuePacket      ; (R15, R16)
  brcc CPRO_EnqueueFlashRsp_error
  sec
  ret
CPRO_EnqueueFlashRsp_error:
  clc
  ret
#endif