Implemented address assignment protocol.

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