; ***************************************************************************
; copyright   : (C) 2023 by Martin Preuss
; email       : martin@libchipcard.de
;
; ***************************************************************************
; * This file is part of the project "AqHome".                              *
; * Please see toplevel file COPYING of that project for license details.   *
; ***************************************************************************



; ***************************************************************************
; This sub-module contains code for cooperative address management.
; Because the network of nodes doen't have a central control unit the nodes
; need to assign addresses to themselves with help from the community of existing
; nodes.
;
; Protocol A: Full Address Assignment Protocol
; --------------------------------------------
; This protocol is used when a node has no currently or previously assigned address.
; In this case we need to find a free address which no other node uses.
;
; 1) a node needs an address. It sends the packet "NEED_ADDRESS"
; 2) every node which already has an address sees this message and answers with
;    a "HAVE_ADDRESS" packet. The nodes must not answer all at once to avoid
;    congestions. Instead, the time it takes for a node to respond depends on
;    its own address (e.g. a node with address 10 will wait for 10+3s before answering)
; 3) the initial node collects all "HAVE_ADDRESS" packets and puts the addresses
;    received this way into a bitfield in which for every address received a bit is
;    set
; 4) after about 130s all nodes in the address range 1-127 should have answered.
;    So after this time the initial node looks at the bitfield of received addresses
;    and selects the first unused address (corresponding bit cleared in bitfield).
;    For this address the initial node sends a "CLAIM_ADDRESS" packet, three times
;    with about 30s between the packets.
; 5) if the to-be-claimed address is already in use by another node, that node will
;    send a "DENY_ADDRESS" packet. This is the second line of defense against address
;    collisions and should very rarely occur.
; 6) after no node denied the to-be-claimed address the initial node finally takes the
;    address and sends a "HAVE_ADDRESS" packet.
; 7) this concludes the address assignemt protocol
;
; Protocol B: Shortened Address Assignment Protocol
; -------------------------------------------------
; This protocol is used upon boot when a node previously got an address assigned to it.
; In this case the address is probably not used by an other node so we can shorten the
; procedure.
;
; 1) a node sends the packet CLAIM_ADDRESS with the previously assigned address
; 2) is any other node objects to this node using that address (e.g. because the other
;    node now uses that address) that node will send a DENY_ADDRESS packet.
; 3) if no DENY_ADDRESS has been received within 10s the address can be used.
; 4) Otherwise the node changes into protocol A (see above).




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

.cseg



CPRO_StartReclaimAddrProcedure:
  ldi xl, LOW(EEPROM_OFFS_COMADDR)
  ldi xh, HIGH(EEPROM_OFFS_COMADDR)
  in r15, SREG
  cli
  rcall Utils_ReadEepromIncr             ; (R16)
  tst r16
  breq CPRO_StartReclaimAddrProcedure_l1
  cpi r16, 0xff
  breq CPRO_StartReclaimAddrProcedure_l1
  sts cproAddrRangeBegin, r16            ; currently claimed address
  mov r19, r16
  rcall CPRO_EnqueueClaimAddress
  ldi r16, CPRO_WAITTIME_RECLAIMADDR
  sts cproAddressWaitCounter, r16
  ldi r16, CPRO_MODE_RECLAIMING_ADDR
  sts cproMode, r16
  sec
  ret
CPRO_StartReclaimAddrProcedure_l1:
  rcall CPRO_StartGetAddrProcedure
  ret



CPRO_StartGetAddrProcedure:
  ; reset bitfield of used addresses
  ldi xh, HIGH(cproUsedAddresses)
  ldi xl, LOW(cproUsedAddresses)
  clr r16
  ldi r17, 16
  rcall Utils_FillSram
  ; preset range
  ldi r16, 1
  sts cproAddrRangeBegin, r16
  ldi r16, 126
  sts cproAddrRangeEnd, r16
  ; send "NEED_ADDRESS" packet
  rcall CPRO_EnqueueNeedAddress
  brcc CPRO_StartGetAddrProcedure_error
  ; set waittimer for sampling of "HAVE_ADDRESS" packets
  ldi r16, CPRO_WAITTIME_GETADDR
  sts cproAddressWaitCounter, r16  
  ldi r16, CPRO_MODE_GETADDRSTARTED
  sts cproMode, r16
  sec
  ret
CPRO_StartGetAddrProcedure_error:
  clc
  ret




; ***************************************************************************
; onEverySecond handlers



cproHandle1sSendingHaveAddress:
  ; waiting for counter to reach zero to send a HAVE_ADDRESS packet
  lds r16, cproAddressWaitCounter
  dec r16
  sts cproAddressWaitCounter, r16
  brne cproHandle1sSendingHaveAddress_done     ; counter not 0, done for now
  rcall CPRO_EnqueueHaveAddress                ; counter is 0, send HAVE_ADDRESS
  ldi r16, CPRO_MODE_NORMAL
  sts cproMode, r16
cproHandle1sSendingHaveAddress_done:
  ret



cproHandle1sGetAddrStarted:
  lds r16, cproAddressWaitCounter
  dec r16
  sts cproAddressWaitCounter, r16
  brne cproHandle1sGetAddrStarted_done     ; counter not 0, done for now
  rcall cproClaimFirstFreeAddr             ; counter 0, find first free address and claim it
  brcs cproHandle1sGetAddrStarted_done
  ; no free address, abort  TODO: send an error message to bus ("bus full")
  ldi r16, CPRO_MODE_NORMAL
  sts cproMode, r16
cproHandle1sGetAddrStarted_done:
  ret



cproHandle1sClaimingAddr12:
  lds r16, cproAddressWaitCounter
  dec r16
  sts cproAddressWaitCounter, r16
  brne cproHandle1sClaimingAddr12_done     ; counter not 0, done for now
  push r17
    lds r19, cproAddrRangeBegin            ; currently claimed address
    rcall CPRO_EnqueueClaimAddress
  pop r17
  ldi r16, CPRO_WAITTIME_CLAIMADDR
  sts cproAddressWaitCounter, r16
  inc r17                                 ; next mode (CPRO_MODE_CLAIMING_ADDR1 and 2)
  sts cproMode, r17                       
cproHandle1sClaimingAddr12_done:
  ret


cproHandle1sClaimingAddr3:
  lds r16, cproAddressWaitCounter
  dec r16
  sts cproAddressWaitCounter, r16
  brne cproHandle1sClaimingAddr3_done     ; counter not 0, done for now
  ; claimed given address 3rd time, set address and enter "normal" mode
  lds r16, cproAddrRangeBegin             ; currently sent address is in cproAddrRangeBegin
  in r15, SREG
  cli
    sts comAddress, r16                   ; write address into eeprom
    ldi xl, LOW(EEPROM_OFFS_COMADDR)
    ldi xh, HIGH(EEPROM_OFFS_COMADDR)
    rcall Utils_WriteEepromIncr               ; write address to EEPROM
    ldi r16, CPRO_MODE_NORMAL             ; set mode to "normal"
    sts cproMode, r16
    rcall CPRO_EnqueueHaveAddress
  out SREG, r15
cproHandle1sClaimingAddr3_done:
  ret


cproHandle1sReclaimingAddr:
  lds r16, cproAddressWaitCounter
  dec r16
  sts cproAddressWaitCounter, r16
  brne cproHandle1sReclaimingAddr_done  ; counter not 0, done for now
  ; successfully claimed given address, set address and enter "normal" mode
  lds r16, cproAddrRangeBegin           ; currently sent address is in cproAddrRangeBegin
  sts comAddress, r16                   ; write address into eeprom
  ldi r16, CPRO_MODE_NORMAL             ; set mode to "normal"
  sts cproMode, r16
  rcall CPRO_EnqueueHaveAddress
cproHandle1sReclaimingAddr_done:
  ret







; ***************************************************************************
; onPacketReceived handlers



cproHandlePckNeedAddr:
  lds r17, cproMode
  cpi r17, CPRO_MODE_NORMAL
  brne cproHandlePckNeedAddr_done
  ; enter CPRO_MODE_SENDING_HAVE_ADDRESS mode
  lds r16, comAddress
  tst r16
  breq cproHandlePckNeedAddr_done         ; we have no address, don't handle
  rcall cproEnterSendingHaveAddressMode
cproHandlePckNeedAddr_done:
  sec
  ret



cproHandlePckHaveAddr:
  lds r17, cproMode
  cpi r17, CPRO_MODE_GETADDRSTARTED
  brne cproHandlePckHaveAddr_done
  ; validate address
  ldd r16, y+(COM_BUFFER_OFFS_DATA+CPRO_PACKET_HAVEADDR_OFFS_ADDRESS)
  tst r16
  breq cproHandlePckHaveAddr_done  ; invalid address, ignore
  cpi r16, 127
  brcc cproHandlePckHaveAddr_done  ; invalid address, ignore
  ; set bit corresponding to given address in bitfield of used addresses
  dec r16
  rcall cproSetBitInBitfield
cproHandlePckHaveAddr_done:
  sec
  ret



cproHandlePckClaimAddr:
  ldd r16, y+(COM_BUFFER_OFFS_DATA+CPRO_PACKET_CLAIMADDR_OFFS_ADDRESS)
  tst r16
  breq cproHandlePckClaimAddr_done
  cpi r16, 0xff
  breq cproHandlePckClaimAddr_done
  lds r17, comAddress
  cp r16, r17
  brne cproHandlePckClaimAddr_done
  rcall CPRO_EnqueueDenyAddress
cproHandlePckClaimAddr_done:
  sec
  ret



cproHandleDenyAddr:
  ; first check address
  ldd r16, y+(COM_BUFFER_OFFS_DATA+CPRO_PACKET_DENYADDR_OFFS_ADDRESS)
  lds r17, cproAddrRangeBegin
  cp r16, r17
  brne cproHandleDenyAddr_done            ; not our currently claimed address, ignore
  ; then check mode
  lds r17, cproMode
  cpi r17, CPRO_MODE_CLAIMING_ADDR1
  brcs cproHandleDenyAddr_l1
  cpi r17, CPRO_MODE_CLAIMING_ADDR3+1
  brcc cproHandleDenyAddr_l1
  ; we are in one of the three CLAIM_ADDRESS modes
  rcall cproClaimNextFreeAddr             ; claim next free address
  brcs cproHandleDenyAddr_done
  ; no free address, abort  TODO: send an error message to bus ("bus full")
  ldi r16, CPRO_MODE_NORMAL
  sts cproMode, r16
  rjmp cproHandleDenyAddr_done
cproHandleDenyAddr_l1:
  lds r17, cproMode
  cpi r17, CPRO_MODE_RECLAIMING_ADDR
  brne cproHandleDenyAddr_done
  ; reclaiming went wrong, go through full address assignment protocol
  rcall CPRO_StartGetAddrProcedure
  rjmp cproHandleDenyAddr_done
cproHandleDenyAddr_done:
  sec
  ret








cproClaimFirstFreeAddr:
  rjmp cproFindAndClaimFreeAddr



cproClaimNextFreeAddr:
  lds r16, cproAddrRangeBegin
  inc r16
  sts cproAddrRangeBegin, r16
  rjmp cproFindAndClaimFreeAddr


; IN:
; - R16: address to claim

cproFindAndClaimFreeAddr:
  lds r16, cproAddrRangeBegin
  lds r17, cproAddrRangeEnd
  cp r16, r17
  brge cproFindAndClaimFreeAddr_error
  rcall cproFindFreeAddr
  brcc cproFindAndClaimFreeAddr_error
  lds r19, cproAddrRangeBegin            ; currently claimed address
  rcall CPRO_EnqueueClaimAddress
  ldi r16, CPRO_WAITTIME_CLAIMADDR
  sts cproAddressWaitCounter, r16
  ldi r16, CPRO_MODE_CLAIMING_ADDR1
  sts cproMode, r16
  sec
  ret
cproFindAndClaimFreeAddr_error:
  clc
  ret



cproHandleAddrRange: ; not handled for now
  ; TODO
  clc
  ret







cproEnterSendingHaveAddressMode:
  ldi r17, 3
  add r16, r17
  sts cproAddressWaitCounter, r16             ; set counter to own address
  ldi r16, CPRO_MODE_SENDING_HAVE_ADDRESS
  sts cproMode, r16
  ret



; ---------------------------------------------------------------------------
; cproFindFreeAddr
;
; find a free address in the bitfield cproUsedAddresses
;
; IN:
; - nothing
; OUT:
; - CFLAG: set if handled, cleared otherwise
; - R16: free address (if CFLAG set)
; USED: R16, R17, R18, R19, R20, R21 X, (R1, R2, Z)

cproFindFreeAddr:
  ldi xl, LOW(cproUsedAddresses)
  ldi xh, HIGH(cproUsedAddresses)
  lds r16, cproAddrRangeBegin
  dec r16
  rcall CPRO_GetPosAndMaskInBitField ; r1=bit pos, r2=mask (r1, r2, r17, Z)
  clr r17
  add xl, r1
  adc xh, r17  ; X: pointer to byte
  mov r17, r2  ; mask
  lds r18, cproAddrRangeBegin
  dec r18
  lds r19, cproAddrRangeEnd
  inc r19      ; to make comparision easier
  ldi r20, 8
cproFindFreeAddr_byteLoop:
    ld r16, X+
    cpi r16, 0xff
    breq cproFindFreeAddr_nextByte ; byte full, skip
cproFindFreeAddr_bitLoop:
      mov r21, r16
      and r21, r17
      brne cproFindFreeAddr_nextBit
      ; found a clear bit, return
      inc r18
      sts cproAddrRangeBegin, r18
      mov r16, r18
      sec
      ret
cproFindFreeAddr_nextBit:
      inc r18 ; next address
      lsl r17 ; shift mask to the left
    brcc cproFindFreeAddr_bitLoop
    dec r18 ; take back inc
cproFindFreeAddr_nextByte:
    add r18, r20
    andi r18, 0xf8 ; clear lower 3 bits
    cp r18, r19   ; compare to end address+1
    brcc cproFindFreeAddr_allFull
    ldi r17, 0x01 ; mask
  rjmp cproFindFreeAddr_byteLoop
cproFindFreeAddr_allFull:
  clc
  ret



; ---------------------------------------------------------------------------
; cproSetBitInBitfield
;
; IN:
; - R16 : bit number to set (0-127)
; OUT:
; - nothing
; USED: R16, R17, X (r1, r2, Z)

cproSetBitInBitfield:
  ; set bit corresponding to given address in bitfield of used addresses
  rcall CPRO_GetPosAndMaskInBitField ; get offset into R1, mask into R2 (r1, r2, r17, Z)
  ldi xl, LOW(cproUsedAddresses)
  ldi xh, HIGH(cproUsedAddresses)
  clr r17
  add xl, r1
  adc xh, r17
  ld r16, X
  or r16, r2
  st X, r16
  ret



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



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



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



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



; ---------------------------------------------------------------------------
; 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 comCalcAndAddChecksumByte            ; (R16, R17, R18, R19, X)

  ; 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



; ---------------------------------------------------------------------------
; Get offset and mask for a given bit in a bitfield
; IN:
; - R16: bit to request position for
; OUT:
; - R1: offset into the bitfield to the byte containing the given bit
; - R2: mask for given id (apply to r1)
; USED REGISTERS: r1, r2, r17, Z

CPRO_GetPosAndMaskInBitField:
  mov r1, r16 ; divide by 8 to get the offset to the byte containing the given module id
  lsr r1
  lsr r1
  lsr r1       ; r1=offset of the byte holding the given bit
  mov r2, r16  ; get bit mask for bit position in table byte
  ldi r17, 7   ; keep lower 3 bits
  and r2, r17
  ldi zh, HIGH(cproModuleBitNumToMaskMap*2)
  ldi zl, LOW(cproModuleBitNumToMaskMap*2)
  add zl, r2
  brcc CPRO_GetPosAndMaskInBitField_noOverflow
  inc zh
CPRO_GetPosAndMaskInBitField_noOverflow:
  lpm r2, z    ; r2=mask for bit in byte from bitfield
  ret


cproModuleBitNumToMaskMap:
  .db 1, 2, 4, 8, 16, 32, 64, 128


