From 0b1e0ea4d5d5aed754f57a94fcf4ef3b26283ba1 Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Sun, 5 Feb 2023 23:42:59 +0100
Subject: [PATCH] Implemented address assignment protocol.

---
 avr/att84_temp1.asm   |  17 +-
 avr/com.asm           |  11 +-
 avr/comproto.asm      | 215 +++++++++++++++++++++++--
 avr/comproto_addr.asm | 357 ++++++++++++++++++++++++++++++++++++++++++
 avr/main.asm          |  61 ++++----
 avr/utils.asm         |  40 ++++-
 6 files changed, 655 insertions(+), 46 deletions(-)
 create mode 100644 avr/comproto_addr.asm

diff --git a/avr/att84_temp1.asm b/avr/att84_temp1.asm
index c33e37d..f19a1cb 100644
--- a/avr/att84_temp1.asm
+++ b/avr/att84_temp1.asm
@@ -190,6 +190,7 @@ realInterruptTable:
 .include "led.asm"
 .include "com.asm"
 .include "comproto.asm"
+.include "comproto_addr.asm"
 .include "twimaster.asm"
 .include "lcd.asm"
 ;.include "bmp280.asm"
@@ -246,8 +247,6 @@ onSystemStart:
 #ifdef MODULES_COM
   rcall Utils_SetupUid
 ;  brcc onSystemStart_l1
-  ldi r16, 0xff
-  rcall CPRO_EnqueueNeedAddress
 onSystemStart_l1:
 #endif
 
@@ -295,6 +294,7 @@ onEvery100ms:
 
 onEverySecond:
 ;  rcall TWI_Master_ScanNext
+  rcall CPRO_OnEverySecond
   ret
 
 
@@ -331,6 +331,16 @@ onEvery30s:
 #ifdef MODULES_SI7021
   rcall SI7021_PeriodicMeasurement
   rcall Main_SendValueMsg
+
+    lds r16, comAddress
+    tst r16
+    brne onEvery30s_l1
+    lds r16, cproMode
+    cpi r16, CPRO_MODE_NORMAL
+    brne onEvery30s_l1
+    rcall CPRO_StartGetAddrProcedure
+onEvery30s_l1:
+
 #endif
   ret
 
@@ -379,7 +389,8 @@ onEveryMinute:
 ; USED: depending on called routines
 
 onPacketReceived:
-  clc               ; not handled
+  rcall CPRO_OnPacketReceived
   ret
 
 
+
diff --git a/avr/com.asm b/avr/com.asm
index 9baf299..1f0314b 100644
--- a/avr/com.asm
+++ b/avr/com.asm
@@ -132,7 +132,14 @@ Com_Init:
   ldi r17, (comDataEnd-comDataBegin)
   rcall Utils_FillSram
 
-  ldi r16, 1          ; debug: set fixed address "1"
+  ; read address from EEPROM
+  ldi xl, LOW(EEPROM_OFFS_COMADDR)
+  ldi xh, HIGH(EEPROM_OFFS_COMADDR)
+  rcall Utils_ReadEeprom                  ; (R16)
+  cpi r16, 0xff
+  brne Com_Init_l1
+  clr r16
+Com_Init_l1:
   sts comAddress, r16
 
   ; setup pins and interrupts
@@ -564,6 +571,8 @@ comReceivePacketToSram:
   ; compare destination address (accept "0" and own address)
   tst r16
   breq comReceivePacketToSram_acceptAddr
+  cpi r16, 0xff
+  breq comReceivePacketToSram_acceptAddr
   lds r17, comAddress
   cp r16, r17
   breq comReceivePacketToSram_acceptAddr
diff --git a/avr/comproto.asm b/avr/comproto.asm
index ae1b957..6d03a82 100644
--- a/avr/comproto.asm
+++ b/avr/comproto.asm
@@ -14,6 +14,9 @@
 .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
 
 
 ; flags for variable payload enqueue function
@@ -28,6 +31,28 @@
 .equ CPRO_PAYLOAD_FLAGS_SHIFT_NUM  = 5
 
 
+.equ CPRO_PACKET_OFFS_DESTADDR = 0
+.equ CPRO_PACKET_OFFS_MSGLEN   = 1
+.equ CPRO_PACKET_OFFS_CMD      = 2
+.equ CPRO_PACKET_OFFS_SRCADDR  = 3
+.equ CPRO_PACKET_OFFS_PAYLOAD  = 4
+
+.equ CPRO_PACKET_HAVEADDR_OFFS_ADDRESS  = 8
+.equ CPRO_PACKET_CLAIMADDR_OFFS_ADDRESS = 8
+.equ CPRO_PACKET_DENYADDR_OFFS_ADDRESS  = 8
+
+.equ CPRO_WAITTIME_GETADDR       = 130
+.equ CPRO_WAITTIME_CLAIMADDR     = 30
+
+; 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
+
+
 
 ; ***************************************************************************
 ; data
@@ -35,6 +60,14 @@
 .dseg
 
 
+cproDataBegin:
+  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
+cproDataEnd:
+
 
 
 ; ***************************************************************************
@@ -44,6 +77,104 @@
 
 
 
+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:
+  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_done
+  rjmp cproHandle1sClaimingAddr3
+CPRO_OnEverySecond_done:
+  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+CPRO_PACKET_OFFS_CMD)
+  cpi r16, CPRO_CMD_PING
+  brne CPRO_OnPacketReceived_l1
+  rjmp cproHandlePing
+CPRO_OnPacketReceived_l1:
+  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:
+  clc
+  ret
+
+
+cproHandlePing:
+  ldd r16, y+(COM_BUFFER_OFFS_DATA+CPRO_PACKET_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.
 ;
@@ -286,21 +417,85 @@ CPRO_EnqueueValue_error:
 ; Enqueue a NEEDADDRESS packet.
 ;
 ; IN:
-; - R16: destination address
+; - 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)
 
-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
+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
@@ -308,10 +503,10 @@ CPRO_EnqueueNeedAddress:
   ; 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
+  brcc cproEnqueueAddressPacket_error
   sec
   ret
-CPRO_EnqueueNeedAddress_error:
+cproEnqueueAddressPacket_error:
   clc
   ret
 
diff --git a/avr/comproto_addr.asm b/avr/comproto_addr.asm
new file mode 100644
index 0000000..0cc3688
--- /dev/null
+++ b/avr/comproto_addr.asm
@@ -0,0 +1,357 @@
+; ***************************************************************************
+; 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.
+;
+; The protocol is as follows:
+; 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 10s 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
+
+
+
+
+; ***************************************************************************
+; code
+
+.cseg
+
+
+
+
+; ***************************************************************************
+; 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)
+; TODO    rcall Utils_WriteEeprom
+    ldi r16, CPRO_MODE_NORMAL             ; set mode to "normal"
+    sts cproMode, r16
+    rcall CPRO_EnqueueHaveAddress
+  out SREG, r15
+cproHandle1sClaimingAddr3_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:
+  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
+  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
+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:
+  sts cproAddressWaitCounter, r16             ; set counter to own address
+  ldi r16, CPRO_MODE_SENDING_HAVE_ADDRESS
+  sts cproMode, r16
+  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
+
+
+
+; ---------------------------------------------------------------------------
+; 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 Utils_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 Utils_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
+
+
+
+
+
diff --git a/avr/main.asm b/avr/main.asm
index 87d6faf..4381b93 100644
--- a/avr/main.asm
+++ b/avr/main.asm
@@ -73,7 +73,8 @@ initModules:
 #endif
   
 #ifdef MODULES_COM
-  rcall Com_Init
+  rcall Com_Init   ; init COM module
+  rcall CPRO_Init  ; init COM protocol module
 #endif
 #ifdef MODULES_TWI_MASTER
   rcall TWI_Master_Init
@@ -181,21 +182,40 @@ printSendStats:
     ldi r18, 0
     ldi r19, 3
     rcall LCD_SetCursor
-    ldi zl, LOW(textRandom)
-    ldi zh, HIGH(textRandom)
+    ldi zl, LOW(textAddress)
+    ldi zh, HIGH(textAddress)
     rcall LCD_PrintFromFlash
-    rcall Utils_PseudoRandom
+    lds r16, comAddress
     rcall LCD_PrintHexByte
     ldi r16, 32
     rcall LCD_PrintChar
-    lds r18, utilsSeed
-    lds r19, utilsSeed+1
-    rcall LCD_PrintHexWord
+    lds r16, cproMode
+    rcall LCD_PrintHexByte
+    lds r16, cproAddressWaitCounter
+    rcall LCD_PrintHexByte
+    ldi r16, 32
+    rcall LCD_PrintChar
+    lds r16, cproAddrRangeBegin
+    rcall LCD_PrintHexByte
 
+    ldi r18, 0
+    ldi r19, 4
+    rcall LCD_SetCursor
+    ldi zl, LOW(textBitmap)
+    ldi zh, HIGH(textBitmap)
+    rcall LCD_PrintFromFlash
+    lds r16, cproUsedAddresses
+    rcall LCD_PrintHexByte
+    lds r16, cproUsedAddresses+1
+    rcall LCD_PrintHexByte
+    lds r16, cproUsedAddresses+2
+    rcall LCD_PrintHexByte
+    lds r16, cproUsedAddresses+3
+    rcall LCD_PrintHexByte
 
 #ifdef MODULES_COM
     ldi r18, 0
-    ldi r19, 4
+    ldi r19, 5
     rcall LCD_SetCursor
     ldi zl, LOW(textStatsPacketsIn)
     ldi zh, HIGH(textStatsPacketsIn)
@@ -204,17 +224,6 @@ printSendStats:
     lds r19, comStatsPacketsIn+1
     rcall LCD_PrintHexWord
   
-    ldi r18, 0
-    ldi r19, 5
-    rcall LCD_SetCursor
-    ldi zl, LOW(textStatsPacketsRecvErr)
-    ldi zh, HIGH(textStatsPacketsRecvErr)
-    rcall LCD_PrintFromFlash
-    lds r18, comStatsRecvErrs
-    lds r19, comStatsRecvErrs+1
-    rcall LCD_PrintHexWord
-  
-  
     ldi r18, 0
     ldi r19, 6
     rcall LCD_SetCursor
@@ -224,16 +233,6 @@ printSendStats:
     lds r18, comStatsPacketsOut
     lds r19, comStatsPacketsOut+1
     rcall LCD_PrintHexWord
-  
-    ldi r18, 0
-    ldi r19, 7
-    rcall LCD_SetCursor
-    ldi zl, LOW(textStatsCollisions)
-    ldi zh, HIGH(textStatsCollisions)
-    rcall LCD_PrintFromFlash
-    lds r18, comStatsCollisions
-    lds r19, comStatsCollisions+1
-    rcall LCD_PrintHexWord
 #endif
  
   pop r15
@@ -285,10 +284,10 @@ textSi7021Firmware:      .db "SI: ", 0, 0
 textStatsPacketsIn:      .db "In     : ", 0
 textStatsPacketsRecvErr: .db "RecvErr: ", 0
 textStatsPacketsOut:     .db "Out    : ", 0
-textStatsCollisions:     .db "Coll   : ", 0
-textStatsAborted:        .db "Aborted: ", 0
 textUid:                 .db "UID    : ", 0
 textRandom:              .db "RANDOM : ", 0
+textAddress:             .db "ADDR   :", 0, 0
+textBitmap:              .db "BITMAP :", 0, 0
 
 
 
diff --git a/avr/utils.asm b/avr/utils.asm
index 0ea7d23..c857d8d 100644
--- a/avr/utils.asm
+++ b/avr/utils.asm
@@ -419,11 +419,17 @@ Utils_SetupUid:
   in r15, SREG
   cli
   rcall Utils_ReadUid          ; R16, X
-  mov r16, r18                 ; all 0xff?
+  mov r16, r18                 ; all 0x00?
   or r16, r19
   or r16, r20
   or r16, r21
   breq Utils_SetupUid_generate ; yep, go generate one
+  mov r16, r18                 ; all 0xff?
+  and r16, r19
+  and r16, r20
+  and r16, r21
+  inc r16
+  breq Utils_SetupUid_generate ; yep, go generate one
   out SREG, r15
   clc
   ret
@@ -448,3 +454,35 @@ Utils_SetupUid_generate:
   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
+
+Utils_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(utilsModuleBitNumToMaskMap*2)
+  ldi zl, LOW(utilsModuleBitNumToMaskMap*2)
+  add zl, r2
+  brcc Utils_GetPosAndMaskInBitField_noOverflow
+  inc zh
+Utils_GetPosAndMaskInBitField_noOverflow:
+  lpm r2, z    ; r2=mask for bit in byte from bitfield
+  ret
+
+
+utilsModuleBitNumToMaskMap:
+  .db 1, 2, 4, 8, 16, 32, 64, 128
+
+