avr: updated flash procedure code.

avr/modules/flash/hdl_flash_start.asm

@@ -19,34 +19,16 @@
 
 
 ; ---------------------------------------------------------------------------
+; @routine flashHandleFlashStart
 ; Handle FLASH START packet.
 ;
-; IN:
-; - X  : buffer containing the message
-; OUT:
-; - CFLAG: set if message is for us, cleared otherwise
-; REGS: r16, r18, r19, r20, r21, X, Y (R15, R17, R22)
+; @return CFLAG set if message is for us, cleared otherwise
+; @param X  buffer containing the message
+; @clobbers r16, (R15, R17, R18, R19, R20, R21, R22, X)
 
 flashHandleFlashStart:
-  ldi yh, HIGH(flashUid)
-  ldi yl, LOW(flashUid)
-  ld r18, Y+
-  ld r19, Y+
-  ld r20, Y+
-  ld r21, Y
-  adiw xh:xl, FLASH_PACKET_START_OFFS_UID
-  ld r16, X+
-  cp r16, r18
-  brne flashHandleFlashStart_notMe
-  ld r16, X+
-  cp r16, r19
-  brne flashHandleFlashStart_notMe
-  ld r16, X+
-  cp r16, r20
-  brne flashHandleFlashStart_notMe
-  ld r16, X
-  cp r16, r21
-  brne flashHandleFlashStart_notMe
+  rcall flashCheckFlashStart
+  brcc flashHandleFlashStart_notMe
   ; okay, flash start message is for us
   rcall flashWaitFor100ms
   clr r16
@@ -54,8 +36,38 @@ flashHandleFlashStart:
   sec
   ret
 flashHandleFlashStart_notMe:
+  ret
+; @end
+
+
+
+; ---------------------------------------------------------------------------
+; @routine flashCheckFlashStart
+;
+; Check whether the UID in the given msg matches our UID.
+;
+; @return CFLAG set if message is for us, cleared otherwise
+; @param  X  buffer containing the message
+; @clobbers r16, r17, r18, X, Y
+
+flashCheckFlashStart:
+  ldi yh, HIGH(flashUid)
+  ldi yl, LOW(flashUid)
+  adiw xh:xl, FLASH_PACKET_START_OFFS_UID
+  ldi r18, 4
+flashCheckFlashStart_loop:
+    ld r16, X+
+    ld r17, Y+
+    cp r16, r17
+    brne flashCheckFlashStart_notMe
+    dec r18
+  brne flashCheckFlashStart_loop
+  sec
+  ret
+flashCheckFlashStart_notMe:
   clc
   ret
+; @end