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


; 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



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

.dseg




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

.cseg



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



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



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



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



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



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

CPRO_EnqueueNeedAddress:
  push r16
    rcall COM_AllocBufferAndGetXY            ; (r16, r17, r21)
  pop r16
  brcc CPRO_EnqueueNeedAddress_error
  ldi r17, CPRO_PAYLOAD_FLAGS_UID
  ldi r18, CPRO_CMD_NEED_ADDRESS
  push xh
    push xl
      rcall cproBeginMsgWithVariablePayload  ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
    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_EnqueueNeedAddress_error
  sec
  ret
CPRO_EnqueueNeedAddress_error:
  clc
  ret



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