avr: refactor SEND_VALUE message code.

avr/modules/comproto/msg_value.asm

@@ -17,42 +17,80 @@
 
 
 ; ---------------------------------------------------------------------------
-; Write a VALUE packet.
+; @routine CPRO_WriteReportValue
+; Write a REPORT_VALUE packet.
 ;
-; IN:
-; - R16: destination address
-; - R17: value id
-; - R19:R18: value
-; - R21:R20: denom (e.g. 100, meaning value must be divided by 100)
-; - R22: value type
-; - X  : buffer to write to
-; OUT:
-; - CFLAG: set if okay, clear otherwise
-; MODIFIED REGS: R6, R7, R8, R9, R10, R11, R12, R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)
+; @return nothing
+; @param  R16     destination address
+; @param  R17     value id
+; @param  R19:R18 value
+; @param  R21:R20 denom (e.g. 100, meaning value must be divided by 100)
+; @param  R22     value type
+; @param  X       buffer to write to
+; @clobbers       R6, R7, R8, R9, R10, R11, R12, R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)
 
-CPRO_WriteValue:
-  mov r7, r17
-  mov r8, r18
-  mov r9, r19
-  mov r10, r20
-  mov r11, r21
-  mov r12, r22
-
-  ldi r17, COM2_PAYLOAD_FLAGS_UID | (6<<COM2_PAYLOAD_FLAGS_SHIFT_NUM)
-  ldi r18, CPRO_CMD_VALUE
-  push xh
-    push xl
-      rcall COM2_BeginMsgWithVariablePayload  ; R3, R4, R16, R17, R18, R19, R20, R21, X
-      st X+, r7                               ; 6: value id
-      st X+, r12                              ; 7: value type
-      st X+, r8                               ; 8: low value
-      st X+, r9                               ; 9: high value
-      st X+, r10                              ; 10: low denom
-      st X+, r11                              ; 11: high denom
-    pop xl
-  pop xh                                        
+CPRO_WriteReportValue:
+  st X+, r16  ; dest address
+  ldi r16, 10 ; msg code+src address+8 payload bytes
+  st X+, r16  ; msg len
+  ldi r16, CPRO_CMD_VALUE_REPORT
+  st X+, r16  ; msg code
+  ldi r16, com2Address
+  st X+, r16  ; src address
+  clr r16
+  st X+, r16  ; msg id/ref id, low
+  st X+, r16  ; msg id/ref id, high
+  st X+, r17  ; value id
+  st X+, r22  ; value type
+  st X+, r18  ; value (low)
+  st X+, r19  ; value (high)
+  st X+, r20  ; denom (low)
+  st X+, r21  ; denom (high)
+  
+  sbiw xh:xl, 12 ; go back to beginning of message
   rcall com2CalcAndAddChecksumByte            ; (R16, R17, R18, R19, X)
+  sbiw xh:xl, 13 ; go back to beginning of message
   ret
+; @end
+
+
+
+; ---------------------------------------------------------------------------
+; @routine CPRO_WriteSetValueResponse
+; Write response to a SET_VALUE request.
+;
+; @return nothing
+; @param R16  message code (CPRO_CMD_VALUE_SET_ACK or CPRO_CMD_VALUE_SET_NACK)
+; @param X    buffer to write to
+; @param Y    buffer with received setValueRequest message
+; @clobbers   R16, R17, R18, R19, (Y)
+
+CPRO_WriteSetValueResponse:
+  ; copy received message into new message
+  mov r19, r16
+  ldi r18, 12            ; message size without checksum byte
+  rcall Utils_Copy_SDRAM ; (R17, R18, X, Y)
+  sbiw xh:xl, 12         ; go back to beginning of message
+
+  ; exchange src and dest address
+  adiw xh:xl, 3          ; src address
+  ld r16, X
+  lds r17, com2Address
+  st X, r16              ; set our src address
+  sbiw xh:xl, 3
+  st X, r16              ; set old src address as destination address
+  
+  ; replace msg code
+  adiw xh:xl, 2          ; msg code
+  st X, r19
+  sbiw xh:xl, 2
+
+  ; add checksum byte
+  rcall com2CalcAndAddChecksumByte            ; (R16, R17, R18, R19, X)
+  sbiw xh:xl, 13         ; go back to beginning of message
+  ret
+; @end
+