am64x/am243x: hdsl: Fix the fast position and velocity calculation

- Fix sign extension in estimator for relative position and acceleration
  addition
- Fix sign estimation for relative position calculation
- Remove an unnecessary RET instruction
- Fix register corruption for ALIGN_PH
- Fix register usage for DTE error signaling

Fixes: PINDSW-5689

Signed-off-by: Dhaval Khandla <dhavaljk@ti.com>

source/position_sense/hdsl/firmware/datalink_init.asm

@@ -376,12 +376,12 @@ datalink_learn_recv_loop_final:
 	sbco        &REG_TMP2, c25, 0, 4
 	.endif
 
-	READ_CYCLCNT	r25
+	READ_CYCLCNT	REG_TMP2
 ; avoid wrap around, need to skip on equal as wait does not work for 0.
-;	qble	    datalink_learn_skip_wait, r25, r3
+;	qble	    datalink_learn_skip_wait, REG_TMP2, r3
-	qble	    datalink_abort2, r25, r3
+	qble	    datalink_abort2, REG_TMP2, r3
-	sub			REG_TMP11, r3, r25
+	sub			REG_TMP11, r3, REG_TMP2
-	MOV			r25.b0, REG_TMP11.b0
+	MOV			REG_TMP2.b0, REG_TMP11.b0
 ; WAIT subracts -1 from parameter before compare. On 0 it wraps around!!!
 	WAIT		REG_TMP11
 datalink_learn_skip_wait:

source/position_sense/hdsl/firmware/hdsl_master_icssg_300_mhz_bin.h

@@ -55,7 +55,7 @@
  */
 
 const unsigned int Hiperface_DSL2_0[]= {
-0x21066e00,
+0x21067800,
 0x24000125,
 0x2eff818f,
 0x24001d8d,
@@ -530,7 +530,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x511f0d03,
 0x51190d02,
 0x79000003,
-0x230635d1,
+0x23063fd1,
 0x7900001a,
 0xd104ff00,
 0xd703ffff,
@@ -637,7 +637,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x1f00c0c0,
 0x1f008181,
 0x81505880,
-0x2104eb00,
+0x2104f500,
 0x91042580,
 0xd708e0ff,
 0x91042580,
@@ -746,7 +746,6 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x10404000,
 0x10606040,
 0x10000060,
-0x91501801,
 0x70c0e204,
 0x10222200,
 0x10020220,
@@ -755,22 +754,23 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x10404000,
 0x10606040,
 0x10000060,
-0x91501801,
+0x70c0e208,
-0x70c0e207,
+0x91501802,
-0x1f010101,
+0x1f010202,
 0x91043880,
 0x1f018080,
 0x81041880,
 0xc901c002,
 0x2400229f,
-0x1d010101,
+0x91501802,
-0x81501801,
+0x1d010202,
+0x81501802,
 0x6900e105,
 0x686e5304,
 0x813a188d,
 0x24000019,
 0x79000002,
-0x2304d1d1,
+0x2304d4d1,
 0x10535300,
 0x10333320,
 0x10131340,
@@ -931,7 +931,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x24003001,
 0xd1066b0e,
 0x2400010d,
-0x230660d1,
+0x23066ad1,
 0x68ab8d45,
 0x13803b3b,
 0x913d1880,
@@ -944,7 +944,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x81531800,
 0x7900003b,
 0x2400020d,
-0x230660d1,
+0x23066ad1,
 0x688b8d38,
 0x8137184b,
 0x13803b3b,
@@ -987,7 +987,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x1d09c4c4,
 0x2400040d,
 0x24003001,
-0x230660d1,
+0x23066ad1,
 0x15ff8d9c,
 0x69005c34,
 0x51009c33,
@@ -1196,7 +1196,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0xf020020d,
 0x69000d03,
 0x24fdfd80,
-0x68808427,
+0x68808426,
 0x91381800,
 0x01010000,
 0x81381800,
@@ -1228,8 +1228,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x108787c7,
 0x04c98087,
 0x108080c9,
-0x2304b8d1,
+0x2304bbd1,
-0x209d0000,
 0x108b8b9d,
 0x91aa1800,
 0x1f018000,
@@ -1249,10 +1248,14 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x108787c7,
 0x04c98087,
 0x108080c9,
-0x2304b8d1,
+0x2304bbd1,
 0x91983880,
-0x008e8080,
+0x10eeeee1,
-0x024ec0c0,
+0x24000061,
+0xc917ee02,
+0x2400ff61,
+0x00818080,
+0x02c1c0c0,
 0x81983880,
 0x10efefe0,
 0x10eeeee1,
@@ -1293,18 +1296,25 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x0000526e,
 0x0220720f,
 0x02c093af,
+0xc91fe003,
+0x03ff536f,
+0x79000002,
 0x0300536f,
-0x6902190b,
+0x6902190f,
-0xc90fc907,
+0x24000040,
+0xc90fc909,
 0x1600c980,
 0x01018080,
 0x0b038080,
 0x16008080,
 0x01018080,
-0x79000002,
+0x51008005,
+0x13ff4040,
+0x79000003,
+0x10c9c980,
 0x0b038080,
 0x00808e8e,
-0x03006e6e,
+0x02404e4e,
 0x20d10000,
 0x151f8484,
 0x240478e2,
@@ -1358,7 +1368,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x9101185b,
 0x51005b0e,
 0x7900000d,
-0x23061dd1,
+0x230627d1,
 0x91dc388c,
 0x240000e2,
 0x91e21882,
@@ -1366,7 +1376,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x100c0c2a,
 0x106c6c4a,
 0x102c2c6a,
-0x23061dd1,
+0x230627d1,
 0x0b01e2e2,
 0x0501e2e2,
 0x4f00e2ff,
@@ -1444,10 +1454,10 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x1e452121,
 0x10000020,
 0x2d802101,
-0x910c3c99,
+0x910c3c82,
-0x58e3f933,
+0x58e3e233,
-0x04f9e3eb,
+0x04e2e3eb,
-0x100b0b19,
+0x100b0b02,
 0x0b01ebeb,
 0x0501ebeb,
 0x4f00ebff,
@@ -1489,7 +1499,7 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x0b077200,
 0x12001313,
 0x0901f2f2,
-0x230629d1,
+0x230633d1,
 0x510e6506,
 0x6f010df6,
 0x10656546,
@@ -1528,16 +1538,16 @@ const unsigned int Hiperface_DSL2_0[]= {
 0xd703ffff,
 0x24001b8d,
 0x23016a9d,
-0x2305ef9d,
+0x2305f99d,
-0x230629d1,
+0x230633d1,
 0x6f010dea,
 0x05012525,
 0x4f0025f8,
 0x24001025,
 0x24001c8d,
 0x23016a9d,
-0x2305ef9d,
+0x2305f99d,
-0x230629d1,
+0x230633d1,
 0x6f010dd6,
 0x05012525,
 0x4f0025fa,
@@ -1704,4 +1714,4 @@ const unsigned int Hiperface_DSL2_0[]= {
 0x91003c82,
 0x1308e2e2,
 0x81003c82,
-0x2104ea00 };
+0x2104f400 };

source/position_sense/hdsl/firmware/memory.inc

@@ -202,7 +202,7 @@ LONG_MSG_RECV .sassign r28, long_msg_recv_s
 	.asg							r16.b3, BYTE_ERROR
 	.asg							r15, FAST_POSH
 	.asg							r14.b3, FAST_POSL
-	.asg							r14, SPEED
+	.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

source/position_sense/hdsl/firmware/transport.asm

@@ -177,7 +177,6 @@ transport_on_v_frame_diff_pos:
 	mov		REG_TMP0.b2, REG_TMP0.b3
 	mov		REG_TMP0.b3, REG_TMP0.b0
 ;check if it is larger
-	lbco		&REG_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
 	qbge		transport_on_v_frame_dont_update_maxdev, REG_TMP2, REG_TMP0.w2
 	mov		REG_TMP0.b0, REG_TMP2.b1
 	mov		REG_TMP0.b1, REG_TMP2.b0
@@ -193,10 +192,10 @@ transport_on_v_frame_dont_update_maxdev:
 	mov		REG_TMP0.b2, REG_TMP0.b3
 	mov		REG_TMP0.b3, REG_TMP0.b0
 ;check if it is larger
-	lbco		&REG_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
 	qbge		transport_on_v_frame_dont_update_dte, REG_TMP2, REG_TMP0.w2
 ; Set EVENT_DTE in ONLINE_STATUS_D register
-    set         REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_DTE
+	lbco		&REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
+    set         REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE
 ; Set EVENT_DTE in EVENT register
 	lbco		&REG_TMP0, MASTER_REGS_CONST, EVENT_H, 4
 	set		REG_TMP0.w0, REG_TMP0.w0, EVENT_DTE
@@ -208,8 +207,9 @@ transport_on_v_frame_dont_update_maxdev:
 update_events_no_int6:
 transport_on_v_frame_dont_update_dte:
 ; Clear EVENT_DTE in ONLINE_STATUS_D register
-    clr         REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_DTE
+	lbco		&REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
-	sbco		&REG_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
+    clr         REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE
+	sbco		&REG_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1
 ;check for diff. is 0 -> estimate if not
 
 	qbne		transport_on_v_frame_estimate, REG_TMP1, 0
@@ -1073,7 +1073,6 @@ calc_speed_extend_acc1:
 	sub		DELTA_ACC0, REG_TMP0.w0, LAST_ACC
 	mov		LAST_ACC, REG_TMP0.w0
     CALL1		calc_fastpos
-	RET
 ;restore return addr
 	mov		RET_ADDR0, REG_TMP11.w0
 ; Moving the event and online register update during stuffing
@@ -1108,8 +1107,14 @@ calc_speed_extend_acc0:
 transport_on_h_frame_exit:
 ;calculate rel. pos and store
 	lbco		&REG_TMP0, MASTER_REGS_CONST, REL_POS0, 4
-	add		REG_TMP0.w0, REG_TMP0.w0, SPEED.w0
+;sign extend speed to 32 bits and add it to REL_POS
-	adc		REG_TMP0.w2, REG_TMP0.w2, SPEED.b2
+	mov		REG_TMP1, SPEED
+    ldi     REG_TMP1.b3, 0
+	qbbc	calc_relpos_extend_vel, SPEED, 23
+	ldi		REG_TMP1.b3, 0xff
+calc_relpos_extend_vel:
+	add		REG_TMP0.w0, REG_TMP0.w0, REG_TMP1.w0
+	adc		REG_TMP0.w2, REG_TMP0.w2, REG_TMP1.w2
 	sbco		&REG_TMP0, MASTER_REGS_CONST, REL_POS0, 4
 ;store fast pos. and velocity
     mov     REG_TMP0, FAST_POSH
@@ -1205,21 +1210,32 @@ estimator_fpos:
 	add		FAST_POSL, VERT_L.b2, REG_TMP0.b0
 	adc		FAST_POSH.b0, VERT_L.b3, REG_TMP0.b1
 	adc		FAST_POSH.w1, VERT_H.w0, REG_TMP0.w2
+;sign extend relative position to 40 bits
+	qbbc	estimator_fpos_add_relpos_positive, REG_TMP0, 31
+	adc		FAST_POSH.b3, VERT_H.b2, 0xFF
+    qba     estimator_fpos_add_relpos_done
+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
 ;vel = vel+acc/8
-	qbbc		estimator_fpos_acc_pos, LAST_ACC, 15
+    ldi     REG_TMP0.b2, 0
+	qbbc	estimator_fpos_acc_pos, LAST_ACC, 15
 	not		REG_TMP0.w0, LAST_ACC
 	add		REG_TMP0.w0, REG_TMP0.w0, 1
 	lsr		REG_TMP0.w0, REG_TMP0.w0, 3
 	not		REG_TMP0.w0, REG_TMP0.w0
 	add		REG_TMP0.w0, REG_TMP0.w0, 1
+;sign extend acceleration to  24 bit -> speed size
+	qbeq    estimator_fpos_acc_sing_check_end, REG_TMP0.w0, 0
+    or      REG_TMP0.b2, REG_TMP0.b2, 0xFF
 	qba		estimator_fpos_acc_sing_check_end
 estimator_fpos_acc_pos:
+	mov		REG_TMP0.w0, LAST_ACC
 	lsr		REG_TMP0.w0, REG_TMP0.w0, 3
 estimator_fpos_acc_sing_check_end:
 	add		SPEED.w0, SPEED.w0, REG_TMP0.w0
-	adc		SPEED.b3, SPEED.b3, 0
+	adc		SPEED.b2, SPEED.b2, REG_TMP0.b2
 estimator_fpos_align_ph_not_2:
 	RET1
 ;--------------------------------------------------------------------------------------------------