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

.equ COM_MAXWAIT                   = 200  ; maximum loop count when waiting for rising/falling clock (TODO: Make frequency-dependant)


.equ COM_BUFFER_FLAGS_DONE        = 0x80
.equ COM_BUFFER_FLAGS_RECEIVED    = 0x40
.equ COM_BUFFER_FLAGS_TOSEND      = 0x20
.equ COM_BUFFER_FLAGS_PRIO1       = 0x08
.equ COM_BUFFER_FLAGS_PRIO0       = 0x04
.equ COM_BUFFER_FLAGS_IFACE1      = 0x02
.equ COM_BUFFER_FLAGS_IFACE0      = 0x01

.equ COM_BUFFER_PRIO_INFO         = 0
.equ COM_BUFFER_PRIO_NORMAL       = COM_BUFFER_FLAGS_PRIO0
.equ COM_BUFFER_PRIO_IMPORTANT    = COM_BUFFER_FLAGS_PRIO1
.equ COM_BUFFER_PRIO_VITAL        = (COM_BUFFER_FLAGS_PRIO0 | COM_BUFFER_FLAGS_PRIO1)

.equ COM_REPEAT_INFO              = 3
.equ COM_REPEAT_NORMAL            = 32
.equ COM_REPEAT_IMPORTANT         = 64
.equ COM_REPEAT_VITAL             = 255

.equ COM_CMD_PING                 = 1


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

.dseg


comDataBegin:
  comFlags:           .byte 1
  comAddress:         .byte 1
  comRepeatCount:     .byte 1
  comReserved1:       .byte 1
  comStatsPacketsIn:  .byte 2
  comStatsPacketsOut: .byte 2
  comStatsRecvErrs:   .byte 2
  comStatsCollisions: .byte 2
  comStatsMissed:     .byte 2
  comStatsAborted:    .byte 2
  comStatsIgnored:    .byte 2
  comStatsHandled:    .byte 2
  comRingBuffer:      .byte RINGBUFFER_OFFS_DATA+COM_RINGBUFFER_SIZE
comDataEnd:



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

.cseg



; ---------------------------------------------------------------------------
; Com_Init
;
; IN:
; OUT:
; - CFLAG: set if okay, clear on error
; USED: R16, R17, X, Y

Com_Init:
  ; preset SRAM data area
  ldi xh, HIGH(comDataBegin)
  ldi xl, LOW(comDataBegin)
  clr r16
  ldi r17, (comDataEnd-comDataBegin)
  rcall Utils_FillSram

  ; init ringbuffer
  ldi r16, COM_RINGBUFFER_SIZE
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  rcall RingBuffer_Init

  ; setup pins and interrupts
  cbi COM_PORT_DATA, COM_PINNUM_DATA      ; disable internal pullup for DATA
  sbi COM_DDR_DATA, COM_PINNUM_DATA       ; set DATA port as input

  cbi COM_PORT_ATTN, COM_PINNUM_ATTN      ; disable internal pullup for ATTN
  sbi COM_DDR_ATTN, COM_PINNUM_ATTN       ; set ATTN port as input

  sbi COM_IRQ_ADDR_ATTN, COM_IRQ_BIT_ATTN ; enable pin change irq for ATTN line
  in r16, GIMSK                           ; enable pin change irq PCIE0 or PCIE1
  ori r16, (1<<COM_IRQ_GIMSK_ATTN)
  out GIMSK, R16
  in r16, GIFR                            ; clear pending irq
  andi r16, ~(1<<COM_IRQ_GIFR_ATTN)
  out GIFR, r16
  
  sec
  ret



; ---------------------------------------------------------------------------
; Com_Run
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if something done, can be called again immediately (otherwise: wait for timer interrupt and retry)
; REGS: (R1, R3, R16, R17, R18, R19, R22, X)

Com_Run:
  rcall comHandleNextPacketInRingbuffer ; nothing more than handling packages in ringbuffer
;  clc
  ret



; ---------------------------------------------------------------------------
; Mark a packet as enqueued.
;
; IN:
; - R18: pos pointer inside ring buffer to read from next
; - R20: priority
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R10, R11, R12, R15, R16, R20 (R3, R21, R22)

COM_EnqueuePacket:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  in r15, SREG
  cli
    ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
    ; read length
    rcall RingBuffer_Read             ; (R3, R22)
    cpi r16, 3                        ; at least 3 bytes?
    brcs COM_EnqueuePacket_error      ; too few bytes
    ; read flags
    push r18                          ; r18 points to flags
      rcall RingBuffer_Read           ; advances r18 behind flags
    pop r18                           ; r18 back to flags
    ; write flags
    andi r16, (~(COM_BUFFER_FLAGS_PRIO1 | COM_BUFFER_FLAGS_PRIO0 | COM_BUFFER_FLAGS_RECEIVED | COM_BUFFER_FLAGS_DONE)) & 0xff
    or r16, r20                       ; add priority
    ori r16, COM_BUFFER_FLAGS_TOSEND
    rcall RingBuffer_Write
  out SREG, r15                       ; restore IRQ flag
  sec
  ret
COM_EnqueuePacket_error:
  out SREG, r15                       ; restore IRQ flag
  clc
  ret


; ---------------------------------------------------------------------------
; Enqueue a PING packet.
;
; IN:
; - R17: destination address
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS:

COM_EnqueuePing:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  ldi r16, 3+1                 ; 3 bytes for buffer length, flags and destination address, cmd
  push r17
    rcall RingBuffer_Alloc     ; r16: pos of allocated buffer (r16, r17, r18, r19, r20, r21)
  pop r17
  brcc COM_EnqueuePing_error
  ; write header stuff
  mov r18, r16                  ; r18: start of packet in ring buffer
  mov r20, r16                  ; r20: start of packet in ring buffer
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
  ldi r16, 3+1
  rcall RingBuffer_Write        ; write length (r3, r22)
  clr r16
  rcall RingBuffer_Write        ; write flags
  mov r16, r17
  rcall RingBuffer_Write        ; write dest addr
  ; write payload
  ldi r16, COM_CMD_PING
  rcall RingBuffer_Write        ; write cmd
  mov r18, r20
  ldi r20, COM_REPEAT_INFO
  rcall COM_EnqueuePacket
  brcc COM_EnqueuePing_error
  sec
  ret
COM_EnqueuePing_error:
  clc
  ret



; ---------------------------------------------------------------------------
; comHandleNextPacketInRingbuffer
;
; Expects global IRQ disabled.
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if something done, can be called again immediately (otherwise: wait for timer interrupt and retry)
; MODIFIED REGS: R1, R16, R17, R18, R19, Y (R1, R3, R16, R17, R18, R19, R22, X)


comHandleNextPacketInRingbuffer:
  ; read current packet in ringbuffer (read pointer)
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  ldd r16, y+RINGBUFFER_OFFS_USED
  tst r16
  breq comHandleNextPacketInRingbuffer_retNc
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
  ldd r18, y+RINGBUFFER_OFFS_READPOS
  rcall RingBuffer_Read               ; packet length (R3, R22)
  cpi r16, 3                          ; at least 3 bytes?
  brcs comHandleNextPacketInRingbuffer_retNc
  mov r1, r16
  rcall RingBuffer_Read               ; read flags from packet
  ldd r18, y+RINGBUFFER_OFFS_READPOS  ; r18 back to read pointer
  mov r17, r16                        ; flags
  andi r17, COM_BUFFER_FLAGS_TOSEND   ; check for message to send
  brne comHandleNextPacketInRingbuffer_sendPacket
  mov r17, r16                        ; check for received message
  andi r17, COM_BUFFER_FLAGS_RECEIVED
  brne comHandleNextPacketInRingbuffer_receivedPacket
  rjmp comHandleNextPacketInRingbuffer_retNc
  
comHandleNextPacketInRingbuffer_sendPacket:
  rcall comSendPacketHandleRepeat     ; (R1, R3, R16, R17, R18, R19, R22, X)
  ret                                 ; use CFLAG from subroutine
  
comHandleNextPacketInRingbuffer_receivedPacket:
  clr r17
  sts comRepeatCount, r17               ; set comRepeatCount to zero
  rcall comHandleReceivedPacket
  sec                                   ; always return with set CFLAG
  ret

comHandleNextPacketInRingbuffer_retNc:
  clc
  ret



; ---------------------------------------------------------------------------
; comHandleReceivedPacket
;
; IN:
; - nothing
; OUT:
; - nothing
; MODIFIED REGS: X, (R3, R22)


comHandleReceivedPacket:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
  ldd r18, y+RINGBUFFER_OFFS_READPOS
  rcall onPacketReceived 
  brcs comHandleReceivedPacket_l1
  ldi xl, LOW(comStatsIgnored)
  ldi xh, HIGH(comStatsIgnored)
  rjmp comHandleReceivedPacket_l2
comHandleReceivedPacket_l1:
  ldi xl, LOW(comStatsHandled)
  ldi xh, HIGH(comStatsHandled)
comHandleReceivedPacket_l2:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
  ldd r18, y+RINGBUFFER_OFFS_READPOS
  rcall comDeallocReadBufAndIncrCounter ; (r16, r17, r18, r19, r20, r21)
  ret



; ---------------------------------------------------------------------------
; comSendPacketHandleRepeat
;
; IN:
; - R18: pos pointer inside ring buffer to read from next
; - R19: maximum size of the ringbuffer (for wrap-around)
; - Y  : ringbuffer
; OUT:
; - CFLAG: set if something done, can be called again immediately (otherwise: wait for timer interrupt and retry)
; MODIFIED REGS: R1, R15, R16, R17, R18, R19, X, (R3, R22)

comSendPacketHandleRepeat:
  in r15, SREG
  cli
    push r18
      rcall RingBuffer_Read                        ; packet length (R3, R22)
      mov r1, r16                                  ; r1: ringbuffer packet length
      rcall RingBuffer_Read                        ; flags
    pop r18  
    rcall comSetupRepeat                           ; setup comRepeatCount if not already done (R16, R17)
;    sbis COM_PIN_ATTN, COM_PINNUM_ATTN             ; check ATTN: low?
;      rjmp comSendPacketHandleRepeat_adjustRepeat  ; yes, line is busy, retry later
    push r18
      cbi COM_PORT_ATTN, COM_PINNUM_ATTN           ; set ATTN low
      sbi COM_DDR_ATTN, COM_PINNUM_ATTN            ; set ATTN as output
      Utils_WaitNanoSecs COM_BIT_LENGTH, 0, r22    ; wait for one bit duration
      rcall comSendPacketRaw                       ; (R3, R10, R11, R12, R16, R20 R21, R22)
      cbi COM_DDR_ATTN, COM_PINNUM_ATTN            ; release ATTN line
    pop r18
    brcc comSendPacketHandleRepeat_adjustRepeat
    ; packet sent, adjust stats
    mov r16, r1
    ldi xl, LOW(comStatsPacketsOut)
    ldi xh, HIGH(comStatsPacketsOut)
    rcall comDeallocReadBufAndIncrCounter          ; (r16, r17, r18)
    rjmp comSendPacketHandleRepeat_retC

comSendPacketHandleRepeat_adjustRepeat:
    lds r17, comRepeatCount
    cpi r17, COM_REPEAT_VITAL
    breq comSendPacketHandleRepeat_retNC             ; vital message, repeat forever
    dec r17
    sts comRepeatCount, r17
    brne comSendPacketHandleRepeat_retNC
  
    ; dealloc buffer, inc dismiss counter
    mov r16, r1
    ldi xl, LOW(comStatsAborted)
    ldi xh, HIGH(comStatsAborted)
    rcall comDeallocReadBufAndIncrCounter
    rjmp comSendPacketHandleRepeat_retC

comSendPacketHandleRepeat_retNC:
  out SREG, r15
  clc
  ret

comSendPacketHandleRepeat_retC:
  out SREG, r15
  sec
  ret



; ---------------------------------------------------------------------------
; comSetupRepeat
;
; IN:
; - R16: pos pointer inside ring buffer to read from next
; - R20: priority
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R17

comSetupRepeat:
  lds r17, comRepeatCount
  tst r17
  brne comSetupRepeat_l99          ; comRepeatCount already setup
  ; set comRepeatCount according to priority
  andi r16, (COM_BUFFER_FLAGS_PRIO1 | COM_BUFFER_FLAGS_PRIO0)  ; r16: flags
  cpi r16, COM_REPEAT_INFO
  brne comSetupRepeat_l1
  ldi r17, COM_REPEAT_INFO
  rjmp comSetupRepeat_l98
comSetupRepeat_l1:
  cpi r16, COM_REPEAT_NORMAL
  brne comSetupRepeat_l2
  ldi r17, COM_REPEAT_NORMAL
  rjmp comSetupRepeat_l98
comSetupRepeat_l2:
  cpi r16, COM_REPEAT_IMPORTANT
  brne comSetupRepeat_l3
  ldi r17, COM_REPEAT_IMPORTANT
  rjmp comSetupRepeat_l98
comSetupRepeat_l3:
  ldi r17, COM_REPEAT_VITAL
comSetupRepeat_l98:
  sts comRepeatCount, r17
comSetupRepeat_l99:
  ret



; ---------------------------------------------------------------------------
; comDeallocReadBufAndIncrCounter
;
; IN:
; - r16: num of bytes to dealloc
; - X  : pointer to counter
; - Y  : ring buffer
; OUT:
; - nothing
; REGS: r16, r17, r18 (r19, r20, r21)

comDeallocReadBufAndIncrCounter:
  rcall RingBuffer_DeallocRead                    ; (r17, r18, r19, r20, r21)
  clr r18
  ld r16, x+
  ld r17, x
  inc r16
  adc r17, r18
  st -x, r17
  st x, r16
  sts comRepeatCount, r18                         ; set comRepeatCount to zero
  ret



; ---------------------------------------------------------------------------
; comSendPacketRaw
;
; Send a packet.       (TODO: input buffer in x, length in r16)
;
; IN:
; - R18: pos pointer inside ring buffer to read from next
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R10, R11, R16 (R3, R21, R22)

comSendPacketRaw:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE
  rcall RingBuffer_Read          ; read entry length from buffer (advances r18) (R3, R22)
  cpi r16, 3                     ; at least 3 bytes?
  brcs comSendPacketRaw_error       ; too few bytes
  mov r10, r16                   ; r10: number of bytes at current ringbuffer pos
  dec r10
  dec r10
  dec r10                        ; r10: number of bytes in message
  clr r11                        ; r11: XOR checksum
  rcall RingBuffer_Read          ; read flags from buffer (ignore)
  rcall RingBuffer_Read          ; read dest address from buffer
  eor r11, r16                   ; calculate checksum
  rcall comSendByte              ; send destination address (R16, R21, R22)
  brcc comSendPacketRaw_error
  mov r16, r10                   ; send msg length
  eor r11, r16                   ; calculate checksum
  rcall comSendByte              ; send msg length
  brcc comSendPacketRaw_error
  tst r10                        ; payload?
  breq comSendPacketRaw_sendCrc     ; no payload, go send checksum
comSendPacketRaw_loop:
    rcall RingBuffer_Read        ; read byte from buffer
    eor r11, r16                 ; calculate checksum
    rcall comSendByte            ; send byte
    brcc comSendPacketRaw_error
    dec r10
  brne comSendPacketRaw_loop
comSendPacketRaw_sendCrc:
  mov r16, r11                   ; send checksum
  rcall comSendByte              ; send byte
  brcc comSendPacketRaw_error
  sec
  ret

comSendPacketRaw_error:
  clc
  ret



; ---------------------------------------------------------------------------
; comReceivePacket
;
; Receive a packet. If we are the recipient of the packet then space will be allocated inside the
; given ringbuffer and the received packet will be stored there.
;
; Packages are stored in the buffer like this:
; - 1 byte : buffer size
; - 1 byte: flags (e.g. COM_BUFFER_FLAGS_RECEIVED)
; - 1 byte : destination address
; - n bytes: packet (i.e. buffer size -3 bytes)
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R1, R2, R3, R16, R17, R18, R19 (R20, R21, R22, XL, XH)

comReceivePacket:
  ldi yl, LOW(comRingBuffer)
  ldi yh, HIGH(comRingBuffer)
  clr r1                                       ; r1: checksum
  ; read destination address
  rcall comReceiveByte                         ; read byte
  brcc comReceivePacket_error
  ; compare destination address (accept "0" and own address)
  tst r16
  breq comReceivePacket_acceptAddr
  lds r17, comAddress
  cp r16, r17
  breq comReceivePacket_acceptAddr
  clc                                          ; not for me
  ret
comReceivePacket_acceptAddr:
  mov r3, r16                                  ; r3: destination address (maybe we need to forward the packet)
  eor r1, r16
  rcall comReceiveByte                         ; read packet length (R16, R17, R20, R21, R22)
  brcc comReceivePacket_error
  ; alloc ring buffer
  mov r17, r16                                 ; r17: packet length
  inc r16                                      ; packet length bytes + length byte
  inc r16                                      ; + flags
  inc r16                                      ; + dest addr
  mov r2, r16                                  ; r2: number of bytes allocated in ringbuffer
  push r17
    rcall RingBuffer_Alloc                     ; (r16, r17, r18, r19, r20, r21)
  pop r17
  brcc comReceivePacket_error
  mov r18, r16                                 ; r18: pos in ring buffer
  ldd r19, y+RINGBUFFER_OFFS_MAXSIZE           ; max size
  mov r16, r2                                  ; write number of bytes allocated to ringbuffer
  ori r16, COM_BUFFER_FLAGS_RECEIVED
  rcall RingBuffer_Write                       ; (r3, r22)
  ldi r16, COM_BUFFER_FLAGS_RECEIVED           ; write flags to ringbuffer
  rcall RingBuffer_Write                       ; (r3, r22)
  mov r16, r3                                  ; write destination address for packet we saved earlier
  rcall RingBuffer_Write                       ; (r3, r22)
  ; read packet (except XOR byte, that will be read later)
comReceivePacket_loop1:
    rcall comReceiveByte                       ; (R16, R17, R20, R21, R22)
    brcc comReceivePacket_error_release_buffer
    eor r1, r16
    rcall RingBuffer_Write                     ; (r3, r22)
    dec r17
  brne comReceivePacket_loop1
  ; read XOR byte, don't store
  rcall comReceiveByte                         ; (R16, R17, R20, R21, R22)
  brcc comReceivePacket_error
  eor r1, r16
  brne comReceivePacket_error_release_buffer   ; XOR should be 0 here, otherwise error
  sec
  ret
comReceivePacket_error_release_buffer:
  mov r16, r2                                  ; number of bytes allocated in ringbuffer
  rcall RingBuffer_DeallocWrite                ; (r17, r18, r19, r20, r21)
comReceivePacket_error:
  clc
  ret




; ---------------------------------------------------------------------------
; comSendByte
;
; Send a byte.
; We only set the data pin to low at the beginning for the startbit. After that
; we only change the pin direction (e.g. input vs output):
; - for 0 bit: set DDR to output, forcing the data line low
; - for 1 bit: set DDR to input, letting the external pullup R pull the data line to HIGH
;              since the output pin is still set to 0 the internal pullup is disabled
; IN:
; - R16: byte to send
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGS: R16, R21, R22

comSendByte:
  ldi r21, 8                                   ; +1 bits left
  ; send startbit
  cbi COM_PORT_DATA, COM_PINNUM_DATA           ; +2 set DATA low
  sbi COM_DDR_DATA, COM_PINNUM_DATA            ; +2 set DATA as output
  Utils_WaitNanoSecs COM_BIT_LENGTH, 5, r22    ; wait for on bit duration
  ; send remaining bits
comSendByte_loop:
    lsr r16                                    ; 1+ bit to send -> CARRY
    brcs comSendByte_setHigh                   ; HI: +2, LO: +1
comSendByte_setLow:
    sbi COM_DDR_DATA, COM_PINNUM_DATA          ; +2 set DATA as output
    nop
    rjmp comSendByte_waitBit                   ; +2
comSendByte_setHigh:
    cbi COM_DDR_DATA, COM_PINNUM_DATA          ; +2 set DATA as input, pullup R makes it ONE
    nop                                        ; +1 (to make pin change available)
    sbis COM_PIN_DATA, COM_PINNUM_DATA         ; +1 if okay, +2 otherwise
      rjmp comSendByte_error                   ; +2 if error
comSendByte_waitBit:                           ; 7 cycles in this loop until now
    Utils_WaitNanoSecs COM_BIT_LENGTH, 10, r22
    dec r21                                    ; +1
  brne comSendByte_loop                        ; +2, sum per loop: 10 cycles
  ; send stopbit
  cbi COM_DDR_DATA, COM_PINNUM_DATA            ; +2 set DATA as input, pullup R makes it ONE
  Utils_WaitNanoSecs COM_BIT_LENGTH, 4, r22
  sec
  ret

comSendByte_error:
  clc
  ret



; ---------------------------------------------------------------------------
; comReceiveByte
;
; Receive a byte.
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if okay, clear otherwise
; - R16: byte read (if CFLAG set)
; MODIFIED REGS: R16, R20, R21, R22 (R17)

comReceiveByte:
  sbi COM_DDR_DATA, COM_PINNUM_DATA            ; set DATA port as input
  cbi COM_PORT_DATA, COM_PINNUM_DATA           ; disable internal pullup for DATA
  ldi r21, 8                                   ; bits left
  clr r20                                      ; byte currently receiving
  ; wait for startbit
  rcall comWaitForDataLow
  brcc comReceiveByte_error
  Utils_WaitNanoSecs COM_BIT_LENGTH/2, 5, r22  ; goto middle of startbit to maximize sync stability
comReceiveByte_loop:
    Utils_WaitNanoSecs COM_BIT_LENGTH, 8, r22  ; 8 cycles used in the complete loop between waits
    sec                                        ; +1
    sbic COM_PIN_DATA, COM_PINNUM_DATA         ; LOW: +2, HIGH: +1
      rjmp comReceiveByte_shiftIn              ; HIGH: +2, rjmp, use set CFLAG
    clc                                        ; LOW: +1
comReceiveByte_shiftIn:
    ror r20                                    ; +1
    dec r21                                    ; +1
  brne comReceiveByte_loop                     ; +2, sum per loop: 8 cycles
  rcall comWaitForDataHigh                     ; wait for start of stopbit
;  brcc comReceiveByte_error
  mov r16, r20
  sec
  ret
comReceiveByte_error:
  clc
  ret



; ---------------------------------------------------------------------------
; comWaitForDataLow
;
; Waits up to COM_MAXWAIT loops for low data line
; IN:
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGISTERS: r17, r22, X

comWaitForDataLow:
  ldi r17, COM_MAXWAIT

comWaitForDataLow_loop:
    sbis COM_PIN_DATA, COM_PINNUM_DATA
      rjmp comWaitForDataLow_done
    dec r17
  brne comWaitForDataLow_loop
  clc   ; timeout
  ret

comWaitForDataLow_done:
  sec   ; ok
  ret



; ---------------------------------------------------------------------------
; comWaitForDataHigh
;
; Waits up to COM_MAXWAIT loops for high data line
; IN:
; OUT:
; - CFLAG: set if okay, clear otherwise
; MODIFIED REGISTERS: r17, r22, X

comWaitForDataHigh:
  ldi r17, COM_MAXWAIT

comWaitForDataHigh_loop:
    sbic COM_PIN_DATA, COM_PINNUM_DATA
      rjmp comWaitForDataHigh_done
    dec r17
  brne comWaitForDataHigh_loop
  clc   ; timeout
  ret

comWaitForDataHigh_done:
  sec   ; ok
  ret