comproto: started simplifying code to prepare and enqueue packets.
This commit is contained in:
Martin Preuss
202
avr/comproto.asm
202
avr/comproto.asm
@@ -16,6 +16,18 @@
|
||||
.equ CPRO_CMD_HAVE_ADDRESS = 61
|
||||
|
||||
|
||||
; flags for variable payload enqueue function
|
||||
.equ CPRO_PAYLOAD_FLAGS_SECONDS = 0x01
|
||||
.equ CPRO_PAYLOAD_FLAGS_UID = 0x02
|
||||
.equ CPRO_PAYLOAD_FLAGS_RESERVED1 = 0x04
|
||||
.equ CPRO_PAYLOAD_FLAGS_RESERVED2 = 0x08
|
||||
.equ CPRO_PAYLOAD_FLAGS_RESERVED3 = 0x10
|
||||
.equ CPRO_PAYLOAD_FLAGS_NUM0 = 0x20
|
||||
.equ CPRO_PAYLOAD_FLAGS_NUM1 = 0x40
|
||||
.equ CPRO_PAYLOAD_FLAGS_NUM3 = 0x80
|
||||
|
||||
|
||||
|
||||
|
||||
; ***************************************************************************
|
||||
; data
|
||||
@@ -330,44 +342,22 @@ CPRO_EnqueueValue_error:
|
||||
; - R16: destination address
|
||||
; OUT:
|
||||
; - CFLAG: set if okay, clear otherwise
|
||||
; MODIFIED REGS: R16, R18, R19, R20, R21, X, Y (R15, R17)
|
||||
; 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)
|
||||
rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
|
||||
pop r16
|
||||
brcc CPRO_EnqueueNeedAddress_error
|
||||
clr r1 ; r1: XOR byte
|
||||
; write header (dest address, msg length)
|
||||
st X+, r16 ; destination address
|
||||
eor r1, r16
|
||||
ldi r16, 6 ; 6 bytes payload
|
||||
st X+, r16 ; msglen
|
||||
eor r1, r16
|
||||
; write payload
|
||||
ldi r16, CPRO_CMD_NEED_ADDRESS ; 0: msg type
|
||||
st X+, r16
|
||||
eor r1, r16
|
||||
clr r16 ; dummy src address
|
||||
st X+, r16
|
||||
eor r1, r16
|
||||
|
||||
push xh ; 2-5: UID
|
||||
ldi r17, CPRO_PAYLOAD_FLAGS_UID
|
||||
ldi r18, CPRO_CMD_NEED_ADDRESS
|
||||
push xh
|
||||
push xl
|
||||
rcall Utils_ReadUid ; (R16, X)
|
||||
rcall cproBeginMsgWithVariablePayload ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
|
||||
pop xl
|
||||
pop xh
|
||||
st X+, r18
|
||||
eor r1, r18
|
||||
st X+, r19
|
||||
eor r1, r19
|
||||
st X+, r20
|
||||
eor r1, r20
|
||||
st X+, r21
|
||||
eor r1, r21
|
||||
; store XOR byte
|
||||
mov r16, r1
|
||||
st X+, r16
|
||||
rcall cproCalcAndAddChecksumByte
|
||||
|
||||
; mark buffer as enqueued with PRIO "important" (higher retry count)
|
||||
ldi r20, COM_BUFFER_PRIO_IMPORTANT
|
||||
rcall COM_EnqueuePacket ; (R15, R16)
|
||||
@@ -389,44 +379,25 @@ CPRO_EnqueueNeedAddress_error:
|
||||
; - R20: priority of the message
|
||||
; OUT:
|
||||
; - CFLAG: set if okay, clear otherwise
|
||||
; MODIFIED REGS: R16, R17, R20, X (R15, Y)
|
||||
; MODIFIED REGS: R16, R17, X, Y (R3, R4, R15, R16, R17, R18, R19, R20, R21)
|
||||
|
||||
cproEnqueueMsgWithCmdAndSrcAddr:
|
||||
push r16
|
||||
rcall COM_AllocBufferAndGetXY ; (r16, r17, r21)
|
||||
pop r16
|
||||
brcc cproEnqueueMsgWithCmdAndSrcAddr_error
|
||||
clr r17 ; r17: XOR byte
|
||||
; write header (dest address, msg length)
|
||||
st X+, r16 ; destination address
|
||||
eor r17, r16
|
||||
ldi r16, 6 ; 6 bytes payload
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
; write payload
|
||||
mov r16, r18 ; 0: CMD
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
lds r16, comAddress ; 1: source address
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
|
||||
lds r16, timerModuleCounterSecs ; 2-5: current seconds counter
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
lds r16, timerModuleCounterSecs+1
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
lds r16, timerModuleCounterSecs+2
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
lds r16, timerModuleCounterSecs+3
|
||||
st X+, r16
|
||||
eor r17, r16
|
||||
ldi r17, CPRO_PAYLOAD_FLAGS_SECONDS
|
||||
push xh
|
||||
push xl
|
||||
push r20
|
||||
rcall cproBeginMsgWithVariablePayload ; (R3, R4, R16, R17, R18, R19, R20, R21, X)
|
||||
pop r20
|
||||
pop xl
|
||||
pop xh
|
||||
rcall cproCalcAndAddChecksumByte
|
||||
|
||||
; store XOR byte
|
||||
st X+, r17
|
||||
; mark buffer as enqueued with PRIO "info" (limited amount of retries)
|
||||
; mark buffer as enqueued with PRIO given in R20
|
||||
rcall COM_EnqueuePacket ; (R15, R16)
|
||||
brcc cproEnqueueMsgWithCmdAndSrcAddr_error
|
||||
sec
|
||||
@@ -437,3 +408,114 @@ cproEnqueueMsgWithCmdAndSrcAddr_error:
|
||||
|
||||
|
||||
|
||||
; ---------------------------------------------------------------------------
|
||||
; begin packet with variable payload.
|
||||
;
|
||||
; IN:
|
||||
; - R16: destination address
|
||||
; - R17: flags
|
||||
; - R18: command (e.g. CPRO_CMD_PING)
|
||||
; - X : pointer to packet buffer
|
||||
; OUT:
|
||||
; - X : points to end of packet as it was written so far
|
||||
; MODIFIED REGS: R3, R16, R17, R18, R19, R20, R21, X (R4)
|
||||
|
||||
cproBeginMsgWithVariablePayload:
|
||||
; write header (dest address, msg length)
|
||||
st X+, r16 ; destination address
|
||||
mov r16, r17 ; calculate payload size
|
||||
mov r3, r17
|
||||
rcall cproCalcPayloadSize ; (R4, R16, R17)
|
||||
inc r16 ; add CMD byte
|
||||
inc r16 ; add source address byte
|
||||
st X+, r16
|
||||
; write payload
|
||||
st X+, r18 ; 0: CMD
|
||||
lds r16, comAddress ; 1: source address
|
||||
st X+, r16
|
||||
lsr r3 ; shift out CPRO_PAYLOAD_FLAGS_SECONDS
|
||||
brcc cproBeginMsgWithVariablePayload_l1
|
||||
lds r16, timerModuleCounterSecs ; adding of current seconds counter requested
|
||||
st X+, r16
|
||||
lds r16, timerModuleCounterSecs+1
|
||||
st X+, r16
|
||||
lds r16, timerModuleCounterSecs+2
|
||||
st X+, r16
|
||||
lds r16, timerModuleCounterSecs+3
|
||||
st X+, r16
|
||||
cproBeginMsgWithVariablePayload_l1:
|
||||
lsr r3 ; shift out shift out CPRO_PAYLOAD_FLAGS_UID
|
||||
brcc cproBeginMsgWithVariablePayload_l2
|
||||
push xh
|
||||
push xl
|
||||
rcall Utils_ReadUid ; (R16, X)
|
||||
pop xl
|
||||
pop xh
|
||||
st X+, r18
|
||||
st X+, r19
|
||||
st X+, r20
|
||||
st X+, r21
|
||||
cproBeginMsgWithVariablePayload_l2:
|
||||
ret
|
||||
|
||||
|
||||
|
||||
; ---------------------------------------------------------------------------
|
||||
; add checksum byte to buffer
|
||||
;
|
||||
; IN:
|
||||
; - X : pointer to packet buffer
|
||||
; OUT:
|
||||
; - CFLAG: set if okay, clear otherwise
|
||||
; MODIFIED REGS: R16, R17, R18, R19, R20, R21, X (R3, R4, R6)
|
||||
|
||||
cproCalcAndAddChecksumByte:
|
||||
clr r17
|
||||
ld r16, X+ ; dest addr
|
||||
eor r17, r16
|
||||
ld r18, X+ ; msg len
|
||||
eor r17, r18
|
||||
cproCalcAndAddChecksumByte_loop:
|
||||
ld r16, X+
|
||||
eor r17, r16
|
||||
dec r18
|
||||
brne cproCalcAndAddChecksumByte_loop
|
||||
st X+, r17
|
||||
ret
|
||||
|
||||
|
||||
|
||||
; ---------------------------------------------------------------------------
|
||||
; cproCalcPayloadSize
|
||||
;
|
||||
; Calculate payload size from given flags
|
||||
;
|
||||
; IN:
|
||||
; - R16: flags
|
||||
; OUT:
|
||||
; - R16: payload size
|
||||
; MODIFIED REGS: R4, R16, R17
|
||||
|
||||
cproCalcPayloadSize:
|
||||
clr r4
|
||||
ldi r17, 4
|
||||
lsr r16 ; shift out CPRO_PAYLOAD_FLAGS_SECONDS
|
||||
brcc cproCalcPayloadSize_l1
|
||||
add r4, r17 ; add 4 bytes
|
||||
cproCalcPayloadSize_l1:
|
||||
lsr r16 ; shift out CPRO_PAYLOAD_FLAGS_UID
|
||||
brcc cproCalcPayloadSize_l2
|
||||
add r4, r17 ; add 4 bytes
|
||||
cproCalcPayloadSize_l2:
|
||||
lsr r16
|
||||
lsr r16
|
||||
lsr r16 ; after this R16 contains CPRO_PAYLOAD_FLAGS_NUM0-3
|
||||
add r16, r4 ; add previous bytes to R16
|
||||
ret
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user