External/motor-control-sdk
8
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pull request #86: HDSL: Code reduction

Browse Source 
Merge in PINDSW/motor_control_sdk from PINDSW-7051_add_hdsl_3rd_channel to next

* commit '40b71b3474c3c6b075f07bf940cfe1a5bf407a08':
  am64x/am243x: hdsl: Convert macros into functions for size optimisation
This commit is contained in:
Dhaval Khandla 2023-12-27 06:20:17 -06:00 committed by Pratheesh Gangadhar TK
commit 764b58a1cb
10 changed files with 1398 additions and 2635 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

295
source/position_sense/hdsl/firmware/datalink.asm
View File

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

123
source/position_sense/hdsl/firmware/datalink_init.asm
View File

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

2
source/position_sense/hdsl/firmware/defines.inc
View File

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

832
source/position_sense/hdsl/firmware/hdsl_master_icssg_multichannel_ch0_bin.h
View File

File diff suppressed because it is too large Load Diff

788
source/position_sense/hdsl/firmware/hdsl_master_icssg_multichannel_ch0_sync_mode_bin.h
View File

File diff suppressed because it is too large Load Diff

832
source/position_sense/hdsl/firmware/hdsl_master_icssg_multichannel_ch1_bin.h
View File

File diff suppressed because it is too large Load Diff

788
source/position_sense/hdsl/firmware/hdsl_master_icssg_multichannel_ch1_sync_mode_bin.h
View File

File diff suppressed because it is too large Load Diff

164
source/position_sense/hdsl/firmware/macros.inc
View File

@ -46,6 +46,23 @@ RET1 .macro
JMP RET_ADDR1 JMP RET_ADDR1
.endm .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 QM_ADD .macro val
ldi REG_FNC.b0, (val & 0xff) ldi REG_FNC.b0, (val & 0xff)
add QM, QM, REG_FNC.b0 add QM, QM, REG_FNC.b0
@ -158,21 +175,7 @@ l?:
qbbs l?, r31, TX_GLOBAL_REINIT_ACTIVE qbbs l?, r31, TX_GLOBAL_REINIT_ACTIVE
.endm .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 SET_TX_CH0 .macro
and r30.b2, r30.b2, 0xfc ;clear the channel select bits and r30.b2, r30.b2, 0xfc ;clear the channel select bits
@ -225,128 +228,6 @@ PUSH_FIFO_CONST .macro dat
PUSH_FIFO .macro dat PUSH_FIFO .macro dat
mov r30.b0, dat mov r30.b0, dat
.endm .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 POP_FIFO .macro dst
.if $defined(CHANNEL_2) .if $defined(CHANNEL_2)
@ -451,11 +332,8 @@ RESET_ECAP_INT .macro tmp0, tmp1
sbco &tmp1, PWMSS1_CONST, tmp0, 2 sbco &tmp1, PWMSS1_CONST, tmp0, 2
.endm .endm
NOP .macro NOP_n .macro dat
loop lw?,dat
and r0, r0, r0 and r0, r0, r0
lw?:
.endm .endm
NOP_2 .macro
loop l?, 2
l?:
.endm

146
source/position_sense/hdsl/firmware/memory.inc
View File

@ -134,6 +134,64 @@ LUT_EE_SIZE .set 9
;-------------------------------------------------------------------------------------------------- ;--------------------------------------------------------------------------------------------------
;Register Usage ;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 structure
channel_s .struct channel_s .struct
ch_vertl .int ch_vertl .int
@ -149,20 +207,16 @@ CHANNEL .sassign r18, channel_s
.asg r19, VERT_H .asg r19, VERT_H
; R18, R19, R20, R21, R22 and R23 are used for channel_s ; 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 short_msg_s .struct
crc .short crc .short
@ -175,6 +229,10 @@ SHORT_MSG .sassign r26, short_msg_s
.asg r26, SHORT_MSG32 .asg r26, SHORT_MSG32
.asg r26.b0, SHORT_MSG_CRC_L .asg r26.b0, SHORT_MSG_CRC_L
.asg r26.b1, SHORT_MSG_CRC_H .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 long_msg_recv_s .struct
crc .short 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.b1, LONG_MSG_RECV_CRC_H
.asg r28.b0, LONG_MSG_RECV_CRC_L .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 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 ;DSL Master Register offsets
@ -506,3 +515,4 @@ STATUS2_TEMP .set 0x62 ; 1 byte
VPOS2_TEMP .set 0x63 ; 5 bytes VPOS2_TEMP .set 0x63 ; 5 bytes
VPOSCRC2_TEMP .set 0x68 ; 2 bytes VPOSCRC2_TEMP .set 0x68 ; 2 bytes
SAFE_SUM_TEMP .set 0x70 ; 1 byte SAFE_SUM_TEMP .set 0x70 ; 1 byte
ALIGN_PH .set 0x71 ; 1 byte

63
source/position_sense/hdsl/firmware/transport.asm
View File

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