am64x/am243x: hdsl: Convert macros into functions for size optimisation

- Reduced PRU instructions

Fixes: PINDSW-7051

Signed-off-by: Rajul Bhambay <r-bhambay@ti.com>

source/position_sense/hdsl/firmware/datalink.asm

@@ -48,6 +48,9 @@
 	.ref transport_layer_recv_msg
 	.ref load_code
 	.ref datalink_reset
+	.global PUSH_FIFO_2B_8x
+	.global PUSH_FIFO_3_8x
+	.global WAIT_TX_FIFO_FREE
 	.global transport_layer_send_msg_done
 	.global transport_layer_recv_msg_done
 	.global transport_layer_done
@@ -191,14 +194,14 @@ datalink_rx0_7_vsync_continue:
 	.if !$defined("HDSL_MULTICHANNEL") ;Single channel(225m) starts here
 ;sending sync and 2 bits of sample early to buy processing time for h frame processing
 	qbeq			modified_header_early_data_push_free_run, EXTRA_SIZE, 0
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x2f
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	RESET_CYCLCNT
 	qba modified_header_early_data_push_done
 modified_header_early_data_push_free_run:
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x2f
 modified_header_early_data_push_done:
      ;READ_CYCLCNT		r19
@@ -224,18 +227,17 @@ modified_header_early_data_push_done:
 modified_header_early_data_push_done:
 ;go to H-Frame callback on transport layer (max. 120-50=70 cycles)
  	CALL			transport_on_h_frame
-
 	sub		LOOP_CNT.b2, LOOP_CNT.b2, 1
 	qbeq 	hframe_7_fifo_push,LOOP_CNT.b2,7
 	qblt 	Hframe_fifo_push, LOOP_CNT.b2, 0
 	jmp 	Vframe_fifo_push
 
 Hframe_fifo_push:
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0xff
 	PUSH_FIFO_CONST  0x00
 	LOOP push_2b_0_0 ,4
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0xff
 
 push_2b_0_0:
@@ -254,7 +256,7 @@ Vframe_fifo_push:
 	qba			datalink_rx0_7
 hframe_7_fifo_push:
 	qbbc			Hframe_fifo_push, H_FRAME.flags, FLAG_NORMAL_FLOW
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0xff
 	PUSH_FIFO_CONST  0x00
 	qba			datalink_rx0_7
@@ -404,7 +406,9 @@ send_01:
 	or			REG_FNC.b2, REG_FNC.b2, 0x15;0bPPS10101
     .if $defined("HDSL_MULTICHANNEL")
 	mov FIFO_L,REG_FNC.b2
-	PUSH_FIFO_8x	FIFO_L
+	loop dd1,4
+	CALL3 PUSH_FIFO_2B_8x
+dd1:
     .else
 	PUSH_FIFO		REG_FNC.b2
     .endif
@@ -414,20 +418,18 @@ send_header_send_01_pattern_loop:
 	;;PUSH 8 bytes for 1 byte data (0x55) in FIFO
 
     .if $defined("HDSL_MULTICHANNEL")
-	LOOP push_2B_1 ,4
+	ldi FIFO_L,0x55
-    WAIT_TX_FIFO_FREE
+	loop aaa9,4
-	PUSH_FIFO_CONST		0x00
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0xff
+aaa9:
-push_2B_1:
-
     .else
-    WAIT_TX_FIFO_FREE
+    CALL2 WAIT_TX_FIFO_FREE
     PUSH_FIFO_CONST		0x55
     .endif
 	sub			REG_TMP0.b0, REG_TMP0.b0, 1
 	qbne			send_header_send_01_pattern_loop, REG_TMP0.b0, 0
 ;send last 0101 (4 bits)
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 ;overclock(8x)
 	PUSH_FIFO_CONST		0x00
 	ldi			REG_TMP0, (9*(CLKDIV_NORMAL+1)-9)
@@ -436,20 +438,17 @@ push_2B_1:
 	TX_CLK_DIV		CLKDIV_FAST, REG_TMP0
     .endif
 	PUSH_FIFO_CONST		0xff
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x00
 	PUSH_FIFO_CONST		0xff
 ;push TRAILER
 	;PUSH 8 bytes for 1 byte data (0x03) in FIFO
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
     .if $defined("HDSL_MULTICHANNEL")
-	LOOP push_2B_0 ,3
+	ldi FIFO_L,0x03
-	PUSH_FIFO_CONST		0x00
+	loop aaa8,4
-	PUSH_FIFO_CONST		0x00
+	CALL3 PUSH_FIFO_2B_8x
-	WAIT_TX_FIFO_FREE
+aaa8:
-push_2B_0:
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
     .else
     PUSH_FIFO_CONST		0x03
     .endif
@@ -468,6 +467,7 @@ push_2B_0:
 
 ;reset cyclecount
 	RESET_CYCLCNT
+
 	RET1
 ;Reroute data link abort to avoid branching error.
 datalink_abort_jmp:
@@ -880,7 +880,7 @@ send_header:
 	;check if we have an EXTRA period
 ;if we have a EXTRA period: do TX FIFO synchronization here to gain processing time
 	qbeq			send_header_no_extra_wait, EXTRA_SIZE, 0
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	RESET_CYCLCNT
 
@@ -1009,17 +1009,17 @@ send_header_extra_no_wait:
 
 send_header_extra_loop:
 
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	sub			REG_TMP1.b0, REG_TMP1.b0, 1
 	qbne			send_header_extra_loop, REG_TMP1.b0, 0
 	ldi			REG_TMP0, (11*(CLKDIV_FAST+1)-0)
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO		EXTRA_EDGE_SELF
 
 send_header_extra_no_edge:
 ;reset clock to normal frequency
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO		REG_TMP11.b0
 ;skip synch pulse measurement if we generate pulse ourself
 	WAIT			REG_TMP0
@@ -1160,7 +1160,7 @@ send_header_encode_sec_subblock_end:
 
 transport_layer_send_msg_done:
 ;encoding end
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO		REG_FNC.b3
 	;check if we receive or send 01 pattern
 	qbeq			send_header_send_01_pattern, REG_FNC.b0, M_PAR_RESET
@@ -1172,7 +1172,7 @@ send_header_send_01_pattern:
 	qba			send_header_end
 send_header_dont_send_01:
 ;send last 2 parameter bits
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 ;overclock
 	qbbs			send_header_dont_send_01_send_1, REG_FNC.b2, 7
 	PUSH_FIFO_CONST		0x00
@@ -1234,23 +1234,11 @@ send_header_end:
  	.if $defined("HDSL_MULTICHANNEL")
 send_header_300m:
 ;push SYNC and first 2 bits of SAMPLE
-
+	ldi FIFO_L,0x2f
 	;PUSH 8 bytes for 1 byte data (0x2f) in FIFO
-	WAIT_TX_FIFO_FREE
+	loop aaa,4
-	PUSH_FIFO_CONST  0x00
+	CALL3 PUSH_FIFO_2B_8x
-	WAIT_TX_FIFO_FREE
+aaa:
-	PUSH_FIFO_CONST  0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0xff
-	PUSH_FIFO_CONST  0xff
 ;check if we have an EXTRA period
 ;if we have a EXTRA period: do TX FIFO synchronization here to gain processing time
 	qbeq			send_header_no_extra_wait, EXTRA_SIZE, 0
@@ -1305,7 +1293,7 @@ send_header_end_disp:
 ; V-frame processing is split into two parts : transport_on_v_frame and
 ; transport_on_v_frame_2.
 	qbne			datalink_transport_no_v_frame, LOOP_CNT.b2, 8
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0x00
 	PUSH_FIFO_CONST  0xff
 	jmp			transport_on_v_frame
@@ -1316,7 +1304,7 @@ datalink_transport_no_v_frame:
 ; is being sent). If yes, perform the remaining part of V-frame processing
 	qbne			datalink_transport_no_v_frame_2, LOOP_CNT.b2, 7
  	qbbc			dont_push_for_non7_hframe_1, H_FRAME.flags, FLAG_NORMAL_FLOW
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0xff
 	PUSH_FIFO_CONST  0xff
 dont_push_for_non7_hframe_1:
@@ -1385,23 +1373,25 @@ send_header_extra_no_wait:
 	sub			REG_TMP1.b0, EXTRA_SIZE_SELF, 1
 
 send_header_extra_loop:
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	sub			REG_TMP1.b0, REG_TMP1.b0, 1
 	qbne			send_header_extra_loop, REG_TMP1.b0, 0
 
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO		EXTRA_EDGE_SELF
 send_header_extra_no_edge:
 ;reset clock to normal frequency
 
 	.if $defined(EXT_SYNC_ENABLE)
 	mov FIFO_L,REG_TMP11.b0
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
 
 	.else
 	mov FIFO_L,REG_TMP11.b0
-	PUSH_FIFO_8x		FIFO_L
+	loop dd2,4
+	CALL3 PUSH_FIFO_2B_8x
+dd2:
     .endif
 
 send_header_no_wait_after_synch:
@@ -1501,7 +1491,7 @@ send_header_no_extra:
 ;push last bit of SAMPLE, 3 bits of CYCLE RESET and 4 bits EQUALIZATION
 	.if !$defined(EXT_SYNC_ENABLE)
 	mov FIFO_L,REG_TMP11.b0
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
     .endif
 send_header_encode:
 
@@ -1539,24 +1529,28 @@ send_header_encode_sec_subblock_end:
 	lsr			REG_TMP0.b0, REG_FNC.b2, 2
 	or			REG_FNC.b3, REG_FNC.b3, REG_TMP0.b0
 	lsl			REG_FNC.b2, REG_FNC.b2, 6
-	PUSH_FIFO_1_8x
+	CALL3 PUSH_FIFO_3_8x
-	PUSH_FIFO_2_8x
 	jmp			transport_layer_send_msg
 
 transport_layer_send_msg_done:
 ;encoding end
-	PUSH_FIFO_2B_8x
+
+	CALL3 PUSH_FIFO_2B_8x
 	;check if we receive or send 01 pattern
 	qbeq			send_header_send_01_pattern, REG_FNC.b0, M_PAR_RESET
 	qbeq			send_header_send_01_pattern, REG_FNC.b0, M_PAR_SYNC
 	qba			send_header_dont_send_01
 send_header_send_01_pattern:
+
 	mov FIFO_L,REG_FNC.b3
-	PUSH_FIFO_8x		FIFO_L
+	loop dd3,4
+	CALL3 PUSH_FIFO_2B_8x
+dd3:
 ;send 01 pattern
 	CALL1			send_01
 	qba			send_header_end
 send_header_dont_send_01:
+
 	.if $defined(EXT_SYNC_ENABLE_DEBUG)
 	lbco        &REG_TMP0, c25, 0, 4
 	add         REG_TMP0,REG_TMP0,4
@@ -1602,7 +1596,7 @@ Push_done:
 comp_logic_done:
 transport_layer_recv_msg_done:
 ;send last 2 parameter bits
-    WAIT_TX_FIFO_FREE
+    CALL2 WAIT_TX_FIFO_FREE
 ;overclock
 	qbbs			send_header_dont_send_01_send_1, REG_FNC.b2, 7
 	PUSH_FIFO_CONST		0x00
@@ -1699,18 +1693,10 @@ calculation_for_wait_done:
 	.endif;EXT_SYNC_ENABLE
 
 	.if $defined("HDSL_MULTICHANNEL")
-	WAIT_TX_FIFO_FREE
+	ldi FIFO_L,0x2c
-	PUSH_FIFO_CONST		0x00
+	loop aaa4,3
-	WAIT_TX_FIFO_FREE
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0x00
+aaa4:
-
-	PUSH_FIFO_CONST		0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0x00
-
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
 	.else
 	PUSH_FIFO_CONST		0x0b
 	WAIT			REG_TMP2
@@ -1747,34 +1733,20 @@ send_stuffing_first:
 
 	.if $defined("HDSL_MULTICHANNEL")
 send_stuffing_loop_8x:
-	WAIT_TX_FIFO_FREE
+	ldi FIFO_L,0x2c
-	PUSH_FIFO_CONST		0x00
+	loop aaa5,3
-	PUSH_FIFO_CONST		0x00
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0xff
+aaa5:
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0x00
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
-
 	sub			REG_FNC.b3, REG_FNC.b3, 1
 	qbne			send_stuffing_loop_8x, REG_FNC.b3, 1
-	WAIT_TX_FIFO_FREE
+	ldi FIFO_L,0x2c
-	PUSH_FIFO_CONST		0x00
+	loop aaa6,3
-	WAIT_TX_FIFO_FREE
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0x00
+aaa6:
-
-
-	PUSH_FIFO_CONST		0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0x00
-
-
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
 	.else
 
 send_stuffing_loop:
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x0b
 ;wait 4 cycles
 	ldi			REG_FNC.w0, CLKDIV_DOUBLE
@@ -1807,26 +1779,17 @@ send_trailer:
 ;send TRAILER
 	;PUSH 8 bytes for 1 byte data (0x03) in FIFO
     .if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 9
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
 	PUSH_FIFO_CONST		0x00
 	TX_CHANNEL
 	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	PUSH_FIFO_CONST		0xff
+
     .else
     PUSH_FIFO_CONST		0x03
 	TX_CHANNEL
@@ -1836,15 +1799,8 @@ send_trailer:
 	or			REG_TMP1.b0, REG_TMP1.b0, SLAVE_DELAY
 
 ;  additional delay here shortens the the first trailer byte
-	NOP_2
+    .if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 7
-	NOP_2
-    .if $defined("FREERUN_300_MHZ") | $defined("SYNC_300_MHZ")
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
     .endif
     .if !$defined("HDSL_MULTICHANNEL")
 	TX_CLK_DIV		CLKDIV_SLOW, REG_TMP0
@@ -1946,29 +1902,11 @@ qm_add_end:
 ;--------------------------------------------------------------------------------------------------
 wait_delay:
 	WAIT_TX_DONE
-    .if $defined("FREERUN_300_MHZ") | $defined("SYNC_300_MHZ")
-    NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	.endif
-    .if $defined("FREERUN_300_MHZ")
-	NOP_2
-	NOP_2
-	.endif
     .if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 12
-	NOP_2
-	NOP_2
     .endif  ;HDSL_MULTICHANNEL
 	.if $defined(EXT_SYNC_ENABLE)
-	NOP_2
+	NOP_n 1
-	NOP_2
 	.endif ;EXT_SYNC_ENABLE
 ; same code as in learn
 ; with 4 or 3 bit encoder does not respond after time, starts working with 2 set it to 1
@@ -2061,8 +1999,9 @@ comp_logic_starts:
 	;compensation logic for diff between sync signal and extra edge starts;
 	qbne        num_pulses_is_not_one2, NUM_PULSES, 1 ;not the last frame of period
 	.if $defined("HDSL_MULTICHANNEL")
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
 	.endif
+
 	mov         EXTRA_EDGE_COMP, EXTRA_EDGE
 	mov         EXTRA_SIZE_COMP, EXTRA_SIZE
 	mov         NUM_STUFFING_COMP, NUM_STUFFING
@@ -2157,7 +2096,7 @@ comp_done1:
 	mov			REG_TMP0.b1, TIME_REST_COMP
 	ldi			EXTRA_EDGE_COMP, 0
 	.if $defined("HDSL_MULTICHANNEL")
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
 	.endif
 
 	qbeq		extra_edge_bit_setting_loop_end1, REG_TMP0.b1, 0
@@ -2188,7 +2127,7 @@ send_header_extra_not_too_small1:
 num_pulses_is_not_one2:
 	loop extra_size_validation_done1,2
 	.if $defined("HDSL_MULTICHANNEL")
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
 	.endif
 extra_size_validation_done1:
 	.if !$defined("HDSL_MULTICHANNEL")
@@ -2197,14 +2136,16 @@ extra_size_validation_done1:
 	;compensation logic for diff between sync signal and extra edge ends;
 	.else ;free run mode starts
 	.if $defined("HDSL_MULTICHANNEL")
-	PUSH_FIFO_2B_8x
+	loop push_2B_c,2
-	PUSH_FIFO_2B_8x
+	CALL3 PUSH_FIFO_2B_8x
+push_2B_c:
 	.else
 	jmp comp_logic_done_1
 	.endif
 	.endif
 	.if $defined("HDSL_MULTICHANNEL")
 comp_logic_ends:
+
 	qbeq	comp2,LEARN_STATE_STARTED,2
 	qbeq	comp1,LEARN_STATE_STARTED,3
 	ldi			LEARN_STATE_STARTED , 1
@@ -2214,10 +2155,76 @@ comp1:
 	ldi			LEARN_STATE_STARTED , 1
 	jmp transport_layer_recv_msg
 comp2:
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
-	PUSH_FIFO_1_8x
+	CALL3 PUSH_FIFO_3_8x
-	PUSH_FIFO_2_8x
 	ldi			LEARN_STATE_STARTED , 1
 	jmp transport_layer_recv_msg
 	.endif
 ;***********************************************************************************************************;
+
+
+PUSH_FIFO_2B_8x:
+    ;mov FIFO_L,dat
+    qbbs L2_01, FIFO_L, 7 ; check bit 0
+    CALL2 WAIT_TX_FIFO_FREE
+    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
+    qba    L3_01
+L2_01:
+    CALL2 WAIT_TX_FIFO_FREE
+    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
+L3_01:
+    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
+
+    qbbs L2_11, FIFO_L, 7 ; check bit 0
+    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
+    qba    L3_11
+L2_11:
+    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
+L3_11:
+    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
+
+    RET3
+
+WAIT_TX_FIFO_FREE:
+l2:
+	.if $defined(CHANNEL_2)
+	qbbs		l2, r31, TX_GLOBAL_TX_GO
+	qbbs		l2, r31, TX_GLOBAL_REINIT
+	.endif
+	.if $defined(CHANNEL_1)
+	qbbs		l2, r31, 12
+	qbbs		l2, r31, 11
+	.endif
+	.if $defined(CHANNEL_0)
+	qbbs		l2, r31, 4 ;fill level 4
+	qbbs		l2, r31, 3 ;fill level 2
+	.endif
+	RET2
+
+PUSH_FIFO_3_8x:
+    ;mov FIFO_L,dat
+    qbbs L2_05, FIFO_L, 7 ; check bit 0
+
+    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
+    qba    L3_05
+L2_05:
+
+    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
+L3_05:
+    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
+
+	CALL2 WAIT_TX_FIFO_FREE
+
+	loop L4_15,3
+    qbbs L2_15, FIFO_L, 7 ; check bit 0
+
+    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
+    qba    L3_15
+L2_15:
+    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
+
+L3_15:
+    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
+
+L4_15:
+    RET3

source/position_sense/hdsl/firmware/datalink_init.asm

@@ -34,6 +34,8 @@
 	.include "memory.inc"
 	.include "defines.inc"
 	.include "macros.inc"
+	.ref PUSH_FIFO_2B_8x
+	.ref WAIT_TX_FIFO_FREE
 	.ref transport_init
 	.ref qm_add
 	.ref calc_rssi
@@ -205,12 +207,10 @@ datalink_reset2:
 	PUSH_FIFO_CONST			0x00
 push_1b_0:
     TX_CHANNEL
-   	LOOP push_2b_0,6
+	ldi FIFO_L,0x0
-    WAIT_TX_FIFO_FREE
+	loop aaa10,6
-	PUSH_FIFO_CONST		0x00
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0x00
+aaa10:
-push_2b_0:
-
     .else
 	PUSH_FIFO_CONST		0x00
 	PUSH_FIFO_CONST		0x00
@@ -227,6 +227,7 @@ RESET_LOOP:
 	.else
 	CALL			send_header
 	.endif
+
 	CALL1			send_stuffing
 	add 			LOOP_CNT_0, LOOP_CNT_0, 1
 	qbne RESET_LOOP,LOOP_CNT_0,2
@@ -251,6 +252,7 @@ datalink_sync:
 datalink_sync_end:
 	add 			LOOP_CNT_0, LOOP_CNT_0, 1
 	qbne SYNC_LOOP,LOOP_CNT_0,16
+
 ;--------------------------------------------------------------------------------------------------
 ;State LEARN
 ; DLS response window is 1 switch bit + 61 slave answer and 12 delay bits
@@ -265,7 +267,6 @@ datalink_sync_end:
 	ldi			LOOP_CNT.b1, 9			;9
 
 datalink_learn:
-	;;WAIT_TX_FIFO_FREE
 ;send m_par_reset 8b/10b: 5b/6b and 3b/4b, first=0,vsync=0,reserved=0
 	ldi			REG_FNC.w0, (0x0000 | M_PAR_START)
 	.if $defined("HDSL_MULTICHANNEL")
@@ -275,21 +276,9 @@ datalink_learn:
 	.endif
 ; indication of TX_DONE comes about 53ns after wire timing
 	WAIT_TX_DONE
-    .if $defined("FREERUN_300_MHZ") | $defined("SYNC_300_MHZ")
+	.if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 11
-	NOP_2
+   .endif
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-    .endif
- 	.if $defined("HDSL_MULTICHANNEL")
-	NOP_2
-	NOP_2
-    .endif
-
 ; measured starting point at 0 cable length
 ; first 8 bits will be all ones is delay from encoder and transceiver
 ; second 8 bits is oversampled DSL bit which is 0 on test pattern
@@ -387,7 +376,7 @@ datalink_learn_recv_loop_last_bit:
 
 	qbbc			datalink_learn_recv_loop_last_bit, r31, RX_VALID_FLAG
 
-; now finisch with last bit sample and store
+; now finish with last bit sample and store
 	POP_FIFO		REG_TMP0.b0
 	sub			LOOP_CNT.b2, LOOP_CNT.b2, 1
 	qbbc		datalink_learn_recv_loop_final, REG_TMP0.b0, SAMPLE_EDGE
@@ -419,16 +408,7 @@ datalink_learn_recv_loop_final:
 datalink_learn_skip_wait:
 	TX_EN
     .if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 9
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	nop
     .endif
 ;send TRAILER 0x03 (skipping first 2 bits of logic 0 to avoid some extra delays)
     .if $defined("HDSL_MULTICHANNEL")
@@ -436,7 +416,7 @@ datalink_learn_skip_wait:
 	TX_CHANNEL
 	LOOP push_3b_0,3
 	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 push_3b_0:
 	PUSH_FIFO_CONST		0xff
 	PUSH_FIFO_CONST		0xff
@@ -444,19 +424,10 @@ push_3b_0:
     PUSH_FIFO_CONST		0x03
 	TX_CHANNEL
     .endif
-;	2 dummy cycles
-	NOP_2
 ; test: we are in oversample mode (3 PRU clocks per bit)
 ; extra NOPs should make it shorter
-	NOP_2
+    .if $defined("HDSL_MULTICHANNEL")
-	NOP_2
+	NOP_n 8
-	NOP_2
-    .if $defined("FREERUN_300_MHZ") | $defined("SYNC_300_MHZ")
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
-	NOP_2
     .endif
     .if !$defined("HDSL_MULTICHANNEL")
     TX_CLK_DIV		CLKDIV_SLOW, REG_TMP2
@@ -464,7 +435,7 @@ push_3b_0:
 ;reset DISPARITY
 	ldi			DISPARITY, 0
 	;2 dummy cycles
-	NOP_2
+	nop
     .if !$defined("HDSL_MULTICHANNEL")
     TX_CLK_DIV		CLKDIV_NORMAL, REG_TMP2
     .endif
@@ -474,53 +445,31 @@ push_3b_0:
 datalink_learn_pattern:
 	.if $defined(EXT_SYNC_ENABLE)
 	.else
-    WAIT_TX_FIFO_FREE
+    CALL2 WAIT_TX_FIFO_FREE
     .if $defined("HDSL_MULTICHANNEL")
 ;add stuffing to gain processing time
 	;PUSH 8 bytes for 1 byte data (0x2c) in FIFO
-	PUSH_FIFO_CONST		0x00
+	ldi FIFO_L,0x2c
-	PUSH_FIFO_CONST		0x00
+	loop aaa1,4
-	WAIT_TX_FIFO_FREE
+	CALL3 PUSH_FIFO_2B_8x
-	PUSH_FIFO_CONST		0xff
+aaa1:
-	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0x00
-	PUSH_FIFO_CONST		0x00
 
 	;PUSH 8 bytes for 1 byte data (0xb2) in FIFO
-	WAIT_TX_FIFO_FREE
+	ldi FIFO_L,0xb2
-	PUSH_FIFO_CONST		0xff
+	loop aaa2,4
-	PUSH_FIFO_CONST		0x00
+	CALL3 PUSH_FIFO_2B_8x
-	WAIT_TX_FIFO_FREE
+aaa2:
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0x00
-	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0x00
 
 ;	PUSH 8 bytes for 1 byte data (0xcb) in FIFO
-	WAIT_TX_FIFO_FREE
+	ldi FIFO_L,0xcb
-	PUSH_FIFO_CONST		0xff
+	loop aaa3,4
-	PUSH_FIFO_CONST		0xff
+	CALL3 PUSH_FIFO_2B_8x
-	WAIT_TX_FIFO_FREE
+aaa3:
-	PUSH_FIFO_CONST		0x00
+
-	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0x00
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST		0xff
-	PUSH_FIFO_CONST		0xff
     .else
 ;add stuffing to gain processing time
 	PUSH_FIFO_CONST		0x2c
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xb2
 	PUSH_FIFO_CONST		0xcb
     .endif  ;HDSL_MULTICHANNEL
@@ -568,11 +517,6 @@ datalink_learn_end:
 datalink_abort2:
 	qbbs			datalink_abort2_no_wait, r30, RX_ENABLE						;changed here from 24 to 26
 	WAIT_TX_DONE
-    .if $defined("FREERUN_300_MHZ") | $defined("SYNC_300_MHZ")
-	LOOP no_operation_2cycle,9
-	NOP_2
-no_operation_2cycle:
-    .endif
 datalink_abort3:
 datalink_abort2_no_wait:
 	lbco			&REG_TMP0.b0, MASTER_REGS_CONST, NUM_RESETS, 1
@@ -586,7 +530,7 @@ datalink_learn2_before:
 	ldi			LOOP_CNT.b1, 9; 16
 datalink_learn2:
     .if !$defined("HDSL_MULTICHANNEL")
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
     .endif
 ;send m_par_reset 8b/10b: 5b/6b and 3b/4b, first=0,vsync=0,reserved=0
 	ldi			REG_FNC.w0, (0x0000 | M_PAR_LEARN)
@@ -690,6 +634,7 @@ datalink_id_compute:
 	ldi32			REG_TMP1, 0xffffffff
 	lsr			REG_TMP1, REG_TMP1, REG_TMP0.b0
 	sbco			&REG_TMP1, MASTER_REGS_CONST, MASK_POS, 4
+	ldi 		DELTA_ACC0, 0
 	;qba datalink_id_req
 	CALL1		send_stuffing
 	jmp         datalink_wait_vsynch

source/position_sense/hdsl/firmware/defines.inc

@@ -34,7 +34,7 @@
 ;  bit7..4		major number
 FIRMWARE_VERSION_MAJOR	.set			0x0
 ;  bit3..0		minor number
-FIRMWARE_VERSION_MINOR	.set			0xC
+FIRMWARE_VERSION_MINOR	.set			0xD
 
 ICSS_FIRMWARE_RELEASE	.set            ((FIRMWARE_VERSION_MAJOR << 4) | (FIRMWARE_VERSION_MINOR << 0))
 

source/position_sense/hdsl/firmware/macros.inc

@@ -46,6 +46,23 @@ RET1 .macro
     JMP     RET_ADDR1
     .endm
 
+CALL2 .macro func
+    jal     RET_ADDR2, func
+    .endm
+
+RET2 .macro
+    JMP     RET_ADDR2
+    .endm
+
+CALL3 .macro func
+    jal     RET_ADDR3, func
+    .endm
+
+RET3 .macro
+    JMP     RET_ADDR3
+    .endm
+
+
 QM_ADD .macro val
     ldi     REG_FNC.b0, (val & 0xff)
     add     QM, QM, REG_FNC.b0
@@ -158,21 +175,7 @@ l?:
 	qbbs		l?, r31, TX_GLOBAL_REINIT_ACTIVE
 	.endm
 
-WAIT_TX_FIFO_FREE .macro
+
-l?:
-	.if $defined(CHANNEL_2)
-	qbbs		l?, r31, TX_GLOBAL_TX_GO
-	qbbs		l?, r31, TX_GLOBAL_REINIT
-	.endif
-	.if $defined(CHANNEL_1)
-	qbbs		l?, r31, 12
-	qbbs		l?, r31, 11
-	.endif
-	.if $defined(CHANNEL_0)
-	qbbs		l?, r31, 4 ;fill level 4
-	qbbs		l?, r31, 3 ;fill level 2
-	.endif
-	.endm
 
 SET_TX_CH0 .macro
 	and				r30.b2, r30.b2, 0xfc ;clear the channel select bits
@@ -225,128 +228,6 @@ PUSH_FIFO_CONST .macro dat
 PUSH_FIFO .macro dat
     mov         r30.b0, dat
     .endm
-    .if $defined(HDSL_MULTICHANNEL)
-PUSH_FIFO_8x .macro dat
-    loop L1?, 4
-    qbbs L2_0?, dat, 7 ; check bit 0
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_0?
-L2_0?:
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_0?:
-    lsl   dat,dat, 1 ; Drop least significant bit
-
-    qbbs L2_1?, dat, 7 ; check bit 0
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_1?
-L2_1?:
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_1?:
-    lsl   dat,dat, 1 ; Drop least significant bit
-
-
-L1?:
-    .endm
-
-PUSH_FIFO_8x_1 .macro dat
-    loop L1?, 4
-    qbbs L2_0?, dat, 7 ; check bit 0
-
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_0?
-L2_0?:
-
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_0?:
-    lsl   dat,dat, 1 ; Drop least significant bit
-
-    qbbs L2_1?, dat, 7 ; check bit 0
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_1?
-L2_1?:
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_1?:
-    lsl   dat,dat, 1 ; Drop least significant bit
-
-L1?:
-    .endm
-
-PUSH_FIFO_2B_8x .macro
-    ;mov FIFO_L,dat
-    qbbs L2_0?, FIFO_L, 7 ; check bit 0
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_0?
-L2_0?:
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_0?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    qbbs L2_1?, FIFO_L, 7 ; check bit 0
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_1?
-L2_1?:
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_1?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    .endm
-
-PUSH_FIFO_1_8x .macro
-    ;mov FIFO_L,dat
-    qbbs L2_0?, FIFO_L, 7 ; check bit 0
-
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_0?
-L2_0?:
-
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_0?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    qbbs L2_1?, FIFO_L, 7 ; check bit 0
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_1?
-L2_1?:
-    WAIT_TX_FIFO_FREE
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-
-L3_1?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    .endm
-
-PUSH_FIFO_2_8x .macro
-   ; mov FIFO_L,dat
-    qbbs L2_0?, FIFO_L, 7 ; check bit 0
-
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_0?
-L2_0?:
-
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-L3_0?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    qbbs L2_1?, FIFO_L, 7 ; check bit 0
-
-    ldi   r30.b0, 0x0 ; Push oversampled 0s on the wire
-    qba    L3_1?
-L2_1?:
-
-    ldi   r30.b0, 0xff ; Push oversampled 1s on the wire
-
-L3_1?:
-    lsl   FIFO_L,FIFO_L, 1 ; Drop least significant bit
-
-    .endm
-    .endif
 
 POP_FIFO .macro dst
 	.if $defined(CHANNEL_2)
@@ -451,11 +332,8 @@ RESET_ECAP_INT .macro tmp0, tmp1
     sbco            &tmp1, PWMSS1_CONST, tmp0, 2
     .endm
 
-NOP .macro
+NOP_n .macro dat
+    loop lw?,dat
     and         r0, r0, r0
+lw?:
     .endm
-
-NOP_2 .macro
-    loop            l?, 2
-l?:
-	.endm

source/position_sense/hdsl/firmware/memory.inc

@@ -134,6 +134,64 @@ LUT_EE_SIZE .set						9
 ;--------------------------------------------------------------------------------------------------
 ;Register Usage
 ;--------------------------------------------------------------------------------------------------
+
+	.asg							r0, REG_TMP0
+	.asg							r1, REG_TMP1
+	.asg							r2, REG_TMP2
+	.asg							r2.b0, EXTRA_SIZE_SELF ;take care! these are not persistent registers, r2 is actually temp reigister. these names are
+	.asg							r2.b1, EXTRA_EDGE_SELF ;given to temp register for ease of reading. they are used as temp.
+
+h_frame_s .struct
+vert		.byte
+s_par		.byte
+pipe		.byte
+secondary	.byte
+acc		.short
+flags		.short
+	.endstruct
+H_FRAME .sassign r3, h_frame_s
+	.asg							r4.b0, H_FRAME_acc0
+	.asg							r4.b1, H_FRAME_acc1
+	.asg							r4.b2, H_FRAME_flags_l
+	.asg							r4.b3, H_FRAME_flags_h
+; R3, R4 are used for h_frame_s
+
+	.asg							r5, LOOP_CNT
+
+	.asg							r6.b0, SAMPLE_EDGE
+	.asg							r6.b1, LAST_BIT_SENT
+	.asg							r6.b2, SLAVE_DELAY
+	.asg							r6.b3, QM
+; Reusing DELTA_ACC0 as and in initialization during datalink_id_compute state
+	.asg							r7.b0, NUM_ACC_BITS
+	.asg							r7.b1, NUM_ST_BITS
+	.asg							r7.w0, DELTA_ACC0
+	.asg							r7.w2, DELTA_ACC1
+	.asg 							r8.w0, DELTA_ACC2
+	.asg							r8.w2, DELTA_ACC3
+	.asg							r9.w0, DELTA_ACC4
+	.asg							r9.w2, LAST_ACC
+	.asg							r10.b0, EXTRA_EDGE_COMP
+	.asg							r10.b1, EXTRA_SIZE_COMP
+	.asg							r10.b2, TIME_REST_COMP
+	.asg							r10.b3, NUM_STUFFING_COMP
+	.asg							r11, REG_TMP11
+	.asg							r12.b0, EXTRA_SIZE
+	.asg							r12.b1, NUM_STUFFING
+	.asg							r12.b2, EXTRA_EDGE
+	.asg							r12.b3, TIME_REST
+	.asg							r13, REG_FNC
+	.asg							r14.b3, FAST_POSL
+	.asg							r14, SPEED ; NOTE: r14.b3 is used as FAST_POSL
+	.asg							r15, FAST_POSH
+	.asg							r16.b0, RSSI
+	.asg							r16.b1, CUR_EDGES
+	.asg							r16.b2, SPECIAL_CHARACTER
+	.asg							r16.b3, BYTE_ERROR
+	.asg							r17.b0, DISPARITY
+	.asg							r17.b1, SEND_PARA
+	.asg							r17.w2, RET_ADDR1
+
 ;channel structure
 channel_s .struct
 ch_vertl	.int
@@ -149,20 +207,16 @@ CHANNEL .sassign r18, channel_s
 	.asg	r19, VERT_H
 ; R18, R19, R20, R21, R22 and R23 are used for channel_s
 
-h_frame_s .struct
+	.asg							r24, CRC
-vert		.byte
+	.asg							r24.w0, CRC_VERT
-s_par		.byte
+	.asg							r24.b0, CRC_VERT_L
-pipe		.byte
+	.asg							r24.b1, CRC_VERT_H
-secondary	.byte
+	.asg							r24.w2, CRC_SEC
-acc		.short
+	.asg							r24.b2, CRC_SEC_L
-flags		.short
+	.asg							r24.b3, CRC_SEC_H
-	.endstruct
+	.asg							r25.w0, RET_ADDR3
-H_FRAME .sassign r3, h_frame_s
+	.asg							r25.b2, MODIFIED_HEADER_STARTED
-	.asg							r4.b0, H_FRAME_acc0
+	.asg							r25.b3, FIFO_L ;load fifo
-	.asg							r4.b1, H_FRAME_acc1
-	.asg							r4.b2, H_FRAME_flags_l
-	.asg							r4.b3, H_FRAME_flags_h
-
 
 short_msg_s .struct
 crc		.short
@@ -175,6 +229,10 @@ SHORT_MSG .sassign r26, short_msg_s
 	.asg							r26, SHORT_MSG32
 	.asg							r26.b0, SHORT_MSG_CRC_L
 	.asg							r26.b1, SHORT_MSG_CRC_H
+; R26 and R27.w0 is used for short_msg_s
+
+	.asg							r27.b2, NUM_PULSES
+	.asg							r27.b3, LEARN_STATE_STARTED
 
 long_msg_recv_s .struct
 crc		.short
@@ -185,63 +243,14 @@ LONG_MSG_RECV .sassign r28, long_msg_recv_s
 	.asg							r28.b1, LONG_MSG_RECV_CRC_H
 	.asg							r28.b0, LONG_MSG_RECV_CRC_L
 
+; R28 is used for LONG_MSG_RECV
 
-	.asg							r6.b3, QM
-	.asg							r6.b2, SLAVE_DELAY
-	.asg							r6.b1, LAST_BIT_SENT
-	.asg							r6.b0, SAMPLE_EDGE
-	.asg							r5, LOOP_CNT
-	.asg							r0, REG_TMP0
-	.asg							r1, REG_TMP1
-	.asg							r2, REG_TMP2
-	.asg							r20, REG_SCRATCH
-	.asg							r11, REG_TMP11
-	.asg							r16.b0, RSSI
-	.asg							r16.b1, CUR_EDGES
-	.asg							r16.b2, SPECIAL_CHARACTER
-	.asg							r16.b3, BYTE_ERROR
-	.asg							r15, FAST_POSH
-	.asg							r14.b3, FAST_POSL
-	.asg							r14, SPEED ; NOTE: r14.b3 is used as FAST_POSL
-	.asg							r17.b0, DISPARITY
-	.asg							r17.b1, SEND_PARA
-	.asg							r29.w0, RET_ADDR0
-	.asg							r17.w2, RET_ADDR1
-	.asg							r29.b2, NUM_ACC_BITS
-	.asg							r29.b3, NUM_ST_BITS
-	.asg							r13, REG_FNC
-	.asg							r7.w0, DELTA_ACC0
-	.asg							r7.w2, DELTA_ACC1
-	.asg 							r8.w0, DELTA_ACC2
-	.asg							r8.w2, DELTA_ACC3
-	.asg							r9.w0, DELTA_ACC4
-	.asg							r9.w2, LAST_ACC
-	.asg							r10.w0, SYNC_DIFF0
-	.asg							r12.b0, EXTRA_SIZE
-	.asg							r12.b1, NUM_STUFFING
-	.asg							r12.b2, EXTRA_EDGE
-	.asg							r12.b3, TIME_REST
-	.asg							r27.b2, NUM_PULSES
-	.asg							r25.b2, TIME_EXTRA_WINDOW ;free for am65xx use case
-	.asg							r25.b3,FIFO_L ;load fifo
-	.asg							r25.b2, MODIFIED_HEADER_STARTED
-	.asg							r27.b3, LEARN_STATE_STARTED
-	.asg							r25.b0, ALIGN_PH
-	.asg							r24, CRC
-	.asg							r24.w0, CRC_VERT
-	.asg							r24.b1, CRC_VERT_H
-	.asg							r24.b0, CRC_VERT_L
-	.asg							r24.w2, CRC_SEC
-	.asg							r24.b3, CRC_SEC_H
-	.asg							r24.b2, CRC_SEC_L
     .asg							r28.b0, LOOP_CNT_0
-	.asg							r2.b0, EXTRA_SIZE_SELF ;take care! these are not persistent registers, r2 is actually temp reigister. these names are
-	.asg							r2.b1, EXTRA_EDGE_SELF ;given to temp register for ease of reading. they are used as temp.
 
-	.asg							r10.b0, EXTRA_EDGE_COMP
+	.asg							r29.w0, RET_ADDR0
-	.asg							r10.b1, EXTRA_SIZE_COMP
+	.asg 							r29.w2, RET_ADDR2
-	.asg							r10.b2, TIME_REST_COMP
+	;.asg							r29.b2, NUM_ACC_BITS
-	.asg							r10.b3, NUM_STUFFING_COMP
+	;.asg							r29.b3, NUM_ST_BITS
 
 ;--------------------------------------------------------------------------------------------------
 ;DSL Master Register offsets
@@ -506,3 +515,4 @@ STATUS2_TEMP .set                       0x62 ; 1 byte
 VPOS2_TEMP .set                         0x63 ; 5 bytes
 VPOSCRC2_TEMP .set                      0x68 ; 2 bytes
 SAFE_SUM_TEMP .set                      0x70 ; 1 byte
+ALIGN_PH .set                      		0x71 ; 1 byte

source/position_sense/hdsl/firmware/transport.asm

@@ -35,6 +35,9 @@
 	.include "macros.inc"
 	;.sect	".text"
 	.ref transport_init_done
+	.ref PUSH_FIFO_2B_8x
+	.ref PUSH_FIFO_3_8x
+	.ref WAIT_TX_FIFO_FREE
 	.ref datalink_transport_on_v_frame_done
 	.ref datalink_transport_on_v_frame_done_2
 	.ref transport_layer_processing_1_done
@@ -79,6 +82,8 @@ transport_init:
 	sbco		&SPEED, MASTER_REGS_CONST, REL_POS0, 4
 transport_init_abs_err_loop:
 	zero		&DELTA_ACC0, 12
+	ldi 	REG_TMP0.b0, 0
+	sbco 	&REG_TMP0.b0, MASTER_REGS_CONST, ALIGN_PH, 1
 exit_transport_init:
 ;return back to datalink
 	RET
@@ -137,12 +142,7 @@ transport_on_v_frame_dont_update_qm:
 	.endif
 	qba		transport_on_v_frame_exit
 check_for_slave_error_on_v_frame:
-    .if $defined("HDSL_MULTICHANNEL")
+
-    WAIT_TX_FIFO_FREE
-    LOOP push_1B,3
-	PUSH_FIFO_CONST  0xff
-push_1B:
-    .endif
 ;CRC was correct -> add 1 to QM
 ;Note: QM_ADD uses REG_TMP1
 	QM_ADD		1
@@ -160,9 +160,16 @@ update_events_no_int5:
 ; Set ONLINE_STATUS_1_VPOS in ONLINE_STATUS_1 register
     set         REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_1_VPOS
     sbco		&REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_1_H, 1
-	qba		no_first_push_for_exit
+	qba		transport_on_v_frame_exit
 transport_on_v_frame_check_pos:
 
+    .if $defined("HDSL_MULTICHANNEL")
+    CALL2 WAIT_TX_FIFO_FREE
+    LOOP push_1B,3
+	PUSH_FIFO_CONST  0xff
+push_1B:
+    .endif
+
 	sbco		&REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_1_H, 1
 	lsl		REG_TMP2, CHANNEL.ch_verth, 8
 	mov		REG_TMP2.b0, VERT_L.b3
@@ -191,7 +198,8 @@ transport_on_v_frame_diff_pos:
 	qbne		transport_on_v_frame_estimate, VERT_H.b2, FAST_POSL
 
 ;reset ALIGN_PH
-	ldi		ALIGN_PH, 0
+	ldi 	REG_TMP2.b0, 0
+	sbco 	&REG_TMP2.b0, MASTER_REGS_CONST, ALIGN_PH, 1
 	qba		transport_on_v_frame_no_pos_mismatch
 ; estimate/correct
 transport_on_v_frame_estimate:
@@ -227,7 +235,7 @@ transport_on_v_frame_exit:
 ;we are in RX0
 ;reset rel. pos
 	loop Wait_and_Push_2_byte,2
-   	WAIT_TX_FIFO_FREE
+   	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST  0xff
 Wait_and_Push_2_byte:
 
@@ -236,7 +244,7 @@ Wait_and_Push_2_byte:
 no_first_push_for_exit:
     .if $defined("HDSL_MULTICHANNEL")
 	qbeq			free_run_mode1, EXTRA_SIZE, 0
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
 	PUSH_FIFO_CONST		0xff
 	RESET_CYCLCNT
 free_run_mode1:
@@ -333,7 +341,7 @@ transport_on_v_frame_2_exit:
 	qbne			not_7th_hframe_0, LOOP_CNT.b2, 7
 	qbbc			not_7th_hframe_0, H_FRAME.flags, FLAG_NORMAL_FLOW
  	.if $defined("HDSL_MULTICHANNEL")
- 	WAIT_TX_FIFO_FREE
+ 	CALL2 WAIT_TX_FIFO_FREE
 	LOOP push_1B_0 ,2
 	PUSH_FIFO_CONST  0xff
 push_1B_0:
@@ -520,9 +528,8 @@ transport_layer_check_for_new_msg:
 	and		REG_TMP0.b0, REG_TMP0.b0, 0x0f
 	or		REG_TMP11.b3, REG_TMP11.b3, REG_TMP0.b0
 	.if $defined("HDSL_MULTICHANNEL")
-	WAIT_TX_FIFO_FREE
+	CALL2 WAIT_TX_FIFO_FREE
-	PUSH_FIFO_1_8x
+	CALL3 PUSH_FIFO_3_8x
-	PUSH_FIFO_2_8x
 	.endif
 	mov		REG_TMP2, CHANNEL.ch_paral
 	ldi		REG_TMP1.b0, &REG_TMP11.b0
@@ -1079,17 +1086,18 @@ calc_speed_extend_acc1:
 	add		SPEED.w0, SPEED.w0, REG_TMP0.w0
 	adc		SPEED.b2, SPEED.b2, REG_TMP0.b2
 ;updating the delta acceleration regs
+	.if $defined("HDSL_MULTICHANNEL")
+	CALL2 WAIT_TX_FIFO_FREE
+	PUSH_FIFO_CONST  0x00
+	PUSH_FIFO_CONST  0x00
+	.endif
 	mov		DELTA_ACC4, DELTA_ACC3
 	mov		DELTA_ACC3, DELTA_ACC2
 	mov		DELTA_ACC2, DELTA_ACC1
 	mov		DELTA_ACC1, DELTA_ACC0
 	sub		DELTA_ACC0, REG_TMP0.w0, LAST_ACC
 	mov		LAST_ACC, REG_TMP0.w0
-	.if $defined("HDSL_MULTICHANNEL")
+
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0x00
-	PUSH_FIFO_CONST  0x00
-	.endif
     CALL1		calc_fastpos
 ;restore return addr
 	mov		RET_ADDR0, REG_TMP11.w0
@@ -1116,17 +1124,18 @@ calc_speed_extend_acc0:
 	add		SPEED.w0, SPEED.w0, REG_TMP0.w0
 	adc		SPEED.b2, SPEED.b2, REG_TMP0.b2
 ;updating the delta acceleration regs
+	.if $defined("HDSL_MULTICHANNEL")
+	CALL2 WAIT_TX_FIFO_FREE
+	PUSH_FIFO_CONST  0x00
+	PUSH_FIFO_CONST  0x00
+	.endif
 	mov		DELTA_ACC4, DELTA_ACC3
 	mov		DELTA_ACC3, DELTA_ACC2
 	mov		DELTA_ACC2, DELTA_ACC1
 	mov		DELTA_ACC1, DELTA_ACC0
 	sub		DELTA_ACC0, REG_TMP0.w0, LAST_ACC
 	mov		LAST_ACC, REG_TMP0.w0
-	.if $defined("HDSL_MULTICHANNEL")
+
-	WAIT_TX_FIFO_FREE
-	PUSH_FIFO_CONST  0x00
-	PUSH_FIFO_CONST  0x00
-	.endif
 	CALL1		calc_fastpos
 transport_on_h_frame_exit:
 ;calculate rel. pos and store
@@ -1232,7 +1241,8 @@ calc_speed_extend_acc:
 ;
 ;--------------------------------------------------------------------------------------------------
 estimator_fpos:
-	add		ALIGN_PH, ALIGN_PH, 1
+	lbco 		&REG_TMP2.b0, MASTER_REGS_CONST, ALIGN_PH, 1
+	add			REG_TMP2.b0, REG_TMP2.b0, 1
 ;pos=abspos+relpos
 	lbco		&REG_TMP0, MASTER_REGS_CONST, REL_POS0, 4
 	add		FAST_POSL, VERT_L.b2, REG_TMP0.b0
@@ -1245,7 +1255,7 @@ estimator_fpos:
 estimator_fpos_add_relpos_positive:
 	adc		FAST_POSH.b3, VERT_H.b2, 0
 estimator_fpos_add_relpos_done:
-	qbne		estimator_fpos_align_ph_not_2, ALIGN_PH, 2
+	qbne		estimator_fpos_align_ph_not_2, REG_TMP2.b0, 2
 ;vel = vel+acc/8
     ldi     REG_TMP0.b2, 0
 	qbbc	estimator_fpos_acc_pos, LAST_ACC, 15
@@ -1265,6 +1275,7 @@ estimator_fpos_acc_sing_check_end:
 	add		SPEED.w0, SPEED.w0, REG_TMP0.w0
 	adc		SPEED.b2, SPEED.b2, REG_TMP0.b2
 estimator_fpos_align_ph_not_2:
+	sbco 	&REG_TMP2.b0, MASTER_REGS_CONST, ALIGN_PH, 1
 	RET1
 ;--------------------------------------------------------------------------------------------------
 ;Function: store_error (RET_ADDR1)