From b2a7afbfb86c67aafa7ce7f9fe54047175a1d50a Mon Sep 17 00:00:00 2001
From: Mathias Rasmussen <mathiasvr@gmail.com>
Date: Sun, 6 Sep 2020 21:58:33 +0200
Subject: [PATCH] perf: Reduce DShot decode code

---
 BLHeli_S.asm | 67 ++++++++++++++++++++++------------------------------
 Common.inc   | 43 ++++++---------------------------
 2 files changed, 35 insertions(+), 75 deletions(-)

diff --git a/BLHeli_S.asm b/BLHeli_S.asm
index e288b86..8fafb62 100644
--- a/BLHeli_S.asm
+++ b/BLHeli_S.asm
@@ -511,16 +511,16 @@ t1_int:
 
 t1_int_frame_time_scaled:
 	mov	A, Temp2
-	jnz	t1_int_msb_fail		; Frame too long
+	jnz	t1_int_frame_fail		; Frame too long
 	mov	A, Temp1
 	subb	A, DShot_Frame_Length_Thr
-	jc	t1_int_msb_fail		; Frame too short
+	jc	t1_int_frame_fail		; Frame too short
 	subb	A, DShot_Frame_Length_Thr
-	jnc	t1_int_msb_fail		; Frame too long
+	jnc	t1_int_frame_fail		; Frame too long
 
 	; Check that correct number of pulses is received
 	mov	A, DPL				; Read current pointer
-	cjne	A, #16, t1_int_msb_fail
+	cjne	A, #16, t1_int_frame_fail
 
 	; Decode transmitted data
 	mov	Temp5, #0				; Reset timestamp
@@ -539,7 +539,7 @@ t1_int_frame_time_scaled:
 t1_int_decode:
 	ajmp	t1_int_decode_msb
 
-t1_int_msb_fail:
+t1_int_frame_fail:
 	mov	DPTR, #0			; Set pointer to start
 	setb	IE_EX0			; Enable int0 interrupts
 	setb	IE_EX1			; Enable int1 interrupts
@@ -547,34 +547,37 @@ t1_int_msb_fail:
 
 t1_int_decode_msb:
 	; Decode DShot data Msb. Use more code space to save time (by not using loop)
-	Decode_DShot_2Msb
-	Decode_DShot_2Msb
-	Decode_DShot_2Msb
-	Decode_DShot_2Msb
+	Decode_DShot_2Bit	Temp4
+	Decode_DShot_2Bit	Temp4
+	; Decode DShot data Lsb high nibble
+	Decode_DShot_2Bit	Temp3
+	Decode_DShot_2Bit	Temp3
 	ajmp	t1_int_decode_lsb
 
-t1_int_lsb_fail:
+t1_int_decode_fail:
 	mov	DPTR, #0			; Set pointer to start
 	setb	IE_EX0			; Enable int0 interrupts
 	setb	IE_EX1			; Enable int1 interrupts
 	ajmp	int0_int_outside_range
 
 t1_int_decode_lsb:
-	; Decode DShot data Lsb
-	Decode_DShot_2Lsb
-	Decode_DShot_2Lsb
-	Decode_DShot_2Lsb
-	Decode_DShot_2Lsb
+	; Decode DShot data Lsb low nibble
+	Decode_DShot_2Bit	Temp3
+	Decode_DShot_2Bit	Temp3
+	; Decode DShot data checksum
+	Decode_DShot_2Bit	Temp7
+	Decode_DShot_2Bit	Temp7
+
 	; XOR check (in inverted data, which is ok)
-	mov	A, Temp4
+	mov	A, Temp3
 	swap	A
-	xrl	A, Temp4
 	xrl	A, Temp3
-	anl	A, #0F0h
+	xrl	A, Temp4
+	anl	A, #0Fh
 	mov	Temp2, A
-	mov	A, Temp3
-	swap	A
-	anl	A, #0F0h
+
+	mov	A, Temp7
+	anl	A, #0Fh
 	clr	C
 	subb	A, Temp2
 	jz	t1_int_xor_ok		; XOR check
@@ -585,23 +588,15 @@ t1_int_decode_lsb:
 	ajmp	int0_int_outside_range
 
 t1_int_xor_ok:
-	; Swap to be LSB aligned to 12 bits (and invert)
+	; Invert DShot data
 	mov	A, Temp4
 	cpl	A
-	swap	A
-	anl	A, #0F0h			; Low nibble of high byte
-	mov	Temp2, A
+	anl	A, #0Fh
+	mov	Temp4, A
 	mov	A, Temp3
 	cpl	A
-	swap	A
-	anl	A, #0Fh			; High nibble of low byte
-	orl	A, Temp2
 	mov	Temp3, A
-	mov	A, Temp4			; High nibble of high byte
-	cpl	A
-	swap	A
-	anl	A, #0Fh
-	mov	Temp4, A
+
 	; Subtract 96 (still 12 bits)
 	clr	C
 	mov	A, Temp3
@@ -780,12 +775,6 @@ ENDIF
 
 	ajmp	int0_int_pulse_ready
 
-t1_int_frame_fail:
-	mov	DPTR, #0					; Set pointer to start
-	setb	IE_EX0					; Enable int0 interrupts
-	setb	IE_EX1					; Enable int1 interrupts
-	ajmp	int0_int_outside_range
-
 
 ;**** **** **** **** **** **** **** **** **** **** **** **** ****
 ;
diff --git a/Common.inc b/Common.inc
index 80dc2a1..e6dc477 100644
--- a/Common.inc
+++ b/Common.inc
@@ -230,19 +230,19 @@ Get_Rcp_End:
 ENDIF
 ENDM
 
-Decode_Dshot_2Msb MACRO
+Decode_DShot_2Bit MACRO dest
 	movx	A, @DPTR
 	mov	Temp6, A
 	clr	C
 	subb	A, Temp5					; Subtract previous timestamp
 	clr	C
 	subb	A, Temp1
-	jc	t1_int_msb_fail			; Check that bit is longer than minimum
+	jc	t1_int_decode_fail			; Check that bit is longer than minimum
 
 	subb	A, Temp1					; Check if bit is zero or one
-	mov	A, Temp4					; Shift bit into data byte
+	mov	A, dest					; Shift bit into data byte
 	rlc	A
-	mov	Temp4, A
+	mov	dest, A
 	inc	DPL						; Next bit
 	movx	A, @DPTR
 	mov	Temp5, A
@@ -250,41 +250,12 @@ Decode_Dshot_2Msb MACRO
 	subb	A, Temp6
 	clr	C
 	subb	A, Temp1
-	jc	t1_int_msb_fail
+	jc	t1_int_decode_fail
 
 	subb	A, Temp1
-	mov	A, Temp4
+	mov	A, dest
 	rlc	A
-	mov	Temp4, A
-	inc	DPL
-ENDM
-
-Decode_Dshot_2Lsb MACRO
-	movx	A, @DPTR
-	mov	Temp6, A
-	clr	C
-	subb	A, Temp5					; Subtract previous timestamp
-	clr	C
-	subb	A, Temp1
-	jc	t1_int_lsb_fail			; Check that bit is longer than minimum
-
-	subb	A, Temp1					; Check if bit is zero or one
-	mov	A, Temp3					; Shift bit into data byte
-	rlc	A
-	mov	Temp3, A
-	inc	DPL						; Next bit
-	movx	A, @DPTR
-	mov	Temp5, A
-	clr	C
-	subb	A, Temp6
-	clr	C
-	subb	A, Temp1
-	jc	t1_int_lsb_fail
-
-	subb	A, Temp1
-	mov	A, Temp3
-	rlc	A
-	mov	Temp3, A
+	mov	dest, A
 	inc	DPL
 ENDM