; Copyright (C) 2021-2023 Texas Instruments Incorporated ; ; Redistribution and use in source and binary forms, with or without ; modification, are permitted provided that the following conditions ; are met: ; ; Redistributions of source code must retain the above copyright ; notice, this list of conditions and the following disclaimer. ; ; Redistributions in binary form must reproduce the above copyright ; notice, this list of conditions and the following disclaimer in the ; documentation and/or other materials provided with the ; distribution. ; ; Neither the name of Texas Instruments Incorporated nor the names of ; its contributors may be used to endorse or promote products derived ; from this software without specific prior written permission. ; ; THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ; "AS IS" AND ANY EXPgResS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT ; LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR ; A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT ; OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, ; SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT ; LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, ; DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY ; THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT ; (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE ; OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ; .include "memory.inc" .include "defines.inc" .include "macros.inc" ;.sect ".text" .ref transport_init_done .ref datalink_transport_on_v_frame_done .ref datalink_transport_on_v_frame_done_2 .ref transport_layer_processing_1_done .ref transport_layer_processing_2_done .ref datalink_abort .ref datalink_abort2 .ref qm_add .ref transport_layer_done .ref transport_layer_send_msg_done .ref transport_layer_recv_msg_done ;.ref transport_layer_assemble_msg_done .global transport_layer_send_msg ;.global transport_layer_assemble_msg .global transport_layer_recv_msg .global transport_init .global transport_on_h_frame .global transport_on_v_frame .global transport_on_v_frame_2 .global transport_layer_processing_1 .global transport_layer_processing_2 .global transport_layer .global calc_speed .global calc_16bit_crc .global calc_fastpos .sect ".text" ;Initialize transport layer here transport_init: ;initialize velocity to 0 ;initializing fasp pos as (need to be replaced with first vertical position) zero &SPEED, (4*2) ;resert short msg ctrl ldi REG_TMP0.b0, 0x3f sbco ®_TMP0.b0, MASTER_REGS_CONST, SLAVE_REG_CTRL, 1 ;initialize NUM_VERT_FRAMES, MEAN_MISMTACH, MISMATCH, NUM_VERT_ERR ;sbco &SPEED, MASTER_REGS_CONST, MISMATCH0, 4 ;initialize acc_err_cnt to 0 sbco &SPEED.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1 sbco &SPEED.b0, MASTER_REGS_CONST, POS4, 8 ;reset rel. pos sbco &SPEED, MASTER_REGS_CONST, REL_POS0, 4 transport_init_abs_err_loop: zero &DELTA_ACC0, 12 exit_transport_init: ;return back to datalink RET ;-------------------------------------------------- ;v_frame calculations ;102+20=122 cycles ;extracts safe position ;verifies fast position ;NOT allowed to use REG_TMP11 ;-------------------------------------------------- transport_on_v_frame: ;save REG_FNC.w0 content mov REG_TMP11.w1, REG_FNC.w0 ;inc NUM_VERT_FRAMES -> could be replaced with PA STATS on ICSS_G ;Not required as it's not part of SICK IP requirement. .if 0 lbco ®_TMP1, MASTER_REGS_CONST, NUM_VERT_FRAMES0, 4 add REG_TMP1, REG_TMP1, 1 sbco ®_TMP1, MASTER_REGS_CONST, NUM_VERT_FRAMES0, 4 .endif ; clear VPOS_VALID zero ®_TMP0.b0, 1 sbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS_VALID, 1 ;store CRC in Master Registers mov REG_TMP1.b0, VERT_L.b1 mov REG_TMP1.b1, VERT_L.b0 ; Store the required data for vertical channel in temporary memory. ; It will be stored to DMEM in transport_on_v_frame_2 sbco ®_TMP1, MASTER_REGS_CONST, VPOSCRC_TEMP, 2 ;transmission error? qbbs transport_on_v_frame_dont_update_qm, H_FRAME.flags, FLAG_ERR_VERT lbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_1_H, 1 and REG_TMP2.b0, REG_TMP2.b0, (~((1< add 1 to QM ;Note: QM_ADD uses REG_TMP1 QM_ADD 1 ;check for special character: K29.7 is sent in first byte of vertical channel if slave error occured qbne transport_on_v_frame_check_pos, VERT_H.b3, K29_7 ; Set EVENT_S_VPOS in EVENT register, due to encoder internal error lbco ®_TMP0, MASTER_REGS_CONST, EVENT_S, 2 set REG_TMP0.b0, REG_TMP0.b0, EVENT_S_VPOS ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_S, 1 qbbc update_events_no_int5, REG_TMP0.b1, EVENT_S_VPOS ; generate interrupt_s ldi r31.w0, PRU0_ARM_IRQ4 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 ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_1_H, 1 qba transport_on_v_frame_exit transport_on_v_frame_check_pos: sbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_1_H, 1 lsl REG_TMP2, CHANNEL.ch_verth, 8 mov REG_TMP2.b0, VERT_L.b3 ;first V-Frame? -> update FAST POS with SAFE POS transport_on_v_frame_not_first: ;check for LAST FAST POS and SAFE POS mismatch lbco ®_TMP0.b0, MASTER_REGS_CONST, LAST_FAST_POS0, SIZE_FAST_POS sub REG_TMP1.w0, VERT_L.w2, REG_TMP0.w0 sub REG_TMP1.w2, VERT_H.w0, REG_TMP0.w2 qble no_sub_carry, VERT_L.w2, REG_TMP0.w0 ; if carry is needed, subtract 1 separately sub REG_TMP1.w2, REG_TMP1.w2, 1 no_sub_carry: mov REG_TMP2, REG_TMP1 ;check if diff is neg, qbbc transport_on_v_frame_diff_pos, REG_TMP2, 31 not REG_TMP2, REG_TMP2 add REG_TMP2, REG_TMP2, 1 transport_on_v_frame_diff_pos: ;load MAXDEV_H/L lbco ®_TMP0.w2, MASTER_REGS_CONST, MAXDEV_H, 2 mov REG_TMP0.b0, REG_TMP0.b2 mov REG_TMP0.b2, REG_TMP0.b3 mov REG_TMP0.b3, REG_TMP0.b0 ;check if it is larger 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 sbco ®_TMP0, MASTER_REGS_CONST, MAXDEV_H, 2 transport_on_v_frame_dont_update_maxdev: ;load MAXDEV_TRESH_H/L .if $defined("HDSL_MULTICHANNEL") PUSH_FIFO_CONST 0xff PUSH_FIFO_CONST 0xff .endif lbco ®_TMP0.w2, MASTER_REGS_CONST, MAXDEV_TRESH_H, 2 mov REG_TMP0.b0, REG_TMP0.b2 mov REG_TMP0.b2, REG_TMP0.b3 mov REG_TMP0.b3, REG_TMP0.b0 ;check if it is larger qbge transport_on_v_frame_dont_update_dte, REG_TMP2, REG_TMP0.w2 ; Set EVENT_DTE in ONLINE_STATUS_D register lbco ®_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 ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_DTE ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int6, REG_TMP0.w2, EVENT_DTE ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int6: transport_on_v_frame_dont_update_dte: ; Clear EVENT_DTE in ONLINE_STATUS_D register lbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 clr REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_DTE sbco ®_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 qbne transport_on_v_frame_estimate, VERT_H.b2, FAST_POSL ;reset MAXDEV_H/L sbco ®_FNC.w0, MASTER_REGS_CONST, MAXDEV_H, 2 ;reset ALIGN_PH ldi ALIGN_PH, 0 qba transport_on_v_frame_no_pos_mismatch ; estimate/correct transport_on_v_frame_estimate: CALL1 estimator_fpos transport_on_v_frame_no_pos_mismatch: ;store SAFE POS mov REG_TMP0.b0, VERT_H.b2 mov REG_TMP0.b1, VERT_H.b1 mov REG_TMP0.b2, VERT_H.b0 mov REG_TMP0.b3, VERT_L.b3 mov REG_TMP1.b0, VERT_L.b2 ; Store the required data for vertical channel in temporary memory. ; It will be stored to DMEM in transport_on_v_frame_2 sbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS_TEMP, 5 ; set VPOS_VALID ldi REG_TMP0.b0, 0x1 sbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS_VALID, 1 ; Store the required data for secondary channel in temporary memory. ; It will be processed in transport_on_v_frame_2 ; store H_FRAME.flags sbco &H_FRAME.flags, MASTER_REGS_CONST, H_FRAME_FLAGS_TEMP, 2 ; store CRC_SEC sbco &CRC_SEC, MASTER_REGS_CONST, CRC_SEC_TEMP, 2 ; store the 8 bytes from secondary channel sbco &R20, MASTER_REGS_CONST, VPOS2_TEMP, 8 transport_on_v_frame_exit: ;we are in RX0 ;reset rel. pos .if $defined("HDSL_MULTICHANNEL") PUSH_FIFO_CONST 0xff .endif ldi REG_TMP0, 0 sbco ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 ;store last FAST_POS sbco &FAST_POSL, MASTER_REGS_CONST, LAST_FAST_POS0, SIZE_FAST_POS ; Store summary sbco &VERT_H.b3, MASTER_REGS_CONST, SAFE_SUM_TEMP, 1 ;restore REG_FNC.w0 content mov REG_FNC.w0, REG_TMP11.w1 ;reset vertical/secondary channel crc ldi CRC, 0 ;reset flags and H_FRAME_flags_l, H_FRAME_flags_l, FLAG_ERRORS jmp datalink_transport_on_v_frame_done ;---------------------------------------------------- ;transport_on_v_frame_2 ; Cycle Budget : 140 cycles ; extracts safe position from secondary channel ; verifies the summary from encoder ; NOT allowed to use REG_TMP11 ;---------------------------------------------------- transport_on_v_frame_2: ;save REG_FNC.w0 content mov REG_TMP11.w1, REG_FNC.w0 ; retrieve the 8 bytes for secondary channel from VPOS2_TEMP lbco ®_TMP1, MASTER_REGS_CONST, VPOS2_TEMP, 8 ; error checks for secondary channel lbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 ; retrieve H_FRAME.flags from H_FRAME_FLAGS_TEMP lbco ®_TMP0.w2, MASTER_REGS_CONST, H_FRAME_FLAGS_TEMP, 2 ;channel 2 transmission error? qbbs transport_on_v_frame_dont_update_qm_secondary_channel, REG_TMP0.w2, FLAG_ERR_SEC ;checking for crc error in channel 2 ; retrieve CRC_SEC from CRC_SEC_TEMP lbco ®_TMP0.w2, MASTER_REGS_CONST, CRC_SEC_TEMP, 2 qbeq check_for_slave_error_on_secondary_channel, REG_TMP0.w2, 0 ; set SCE2 bit in ONLINE_STATUS_2 set REG_TMP0.b0, REG_TMP0.w0, ONLINE_STATUS_2_SCE2 sbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 QM_SUB 6 transport_on_v_frame_dont_update_qm_secondary_channel: qba transport_on_v_frame_2_exit check_for_slave_error_on_secondary_channel: ; clear SCE2 bit in ONLINE_STATUS_2 clr REG_TMP0.b0, REG_TMP0.w0, ONLINE_STATUS_2_SCE2 sbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 ;CRC was correct -> add 1 to QM QM_ADD 1 ; NOTE: QM_ADD uses REG_TMP0. Loading REG_TMP0 again here. It can be optimized. lbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 ;check for special character: K29.7 is sent in first byte of secondary vertical channel if slave error occured ; assumption: r21.b3 contains the first byte of secondary vertical channel qbne transport_on_v_frame_no_vpos2_error, REG_TMP2.b3, K29_7 ; set VPOS2 bit in ONLINE_STATUS_2 set REG_TMP0.b0, REG_TMP0.w0, ONLINE_STATUS_2_VPOS2 sbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 qba transport_on_v_frame_vpos2_error_exit transport_on_v_frame_no_vpos2_error: clr REG_TMP0.b0, REG_TMP0.w0, ONLINE_STATUS_2_VPOS2 transport_on_v_frame_vpos2_error_exit: ; store the data from secondary channel ; invert the bits for STATUS2 xor REG_TMP2.b3, REG_TMP2.b3, 0xe0 ; swap bytes in each 32 bit register REG_TMP1 and REG_TMP2 xin 160, ®_TMP1, 8 ; update ONLINE_STATUS_2_SUM2 in ONLINE_STATUS_2 qbbs online_status_2_sum2_set, REG_TMP2.b0, STATUS2_TEST2 qbbs online_status_2_sum2_set, REG_TMP2.b0, STATUS2_ERR2 clr REG_TMP0.b0, REG_TMP0.b0, ONLINE_STATUS_2_SUM2 qba online_status_2_sum2_not_set online_status_2_sum2_set: set REG_TMP0.b0, REG_TMP0.b0, ONLINE_STATUS_2_SUM2 online_status_2_sum2_not_set: sbco ®_TMP0.b0, MASTER_REGS_CONST, ONLINE_STATUS_2_H, 1 ; Store STATUS2, VPOS24, VPOS23 and VPOS22 sbco ®_TMP2.b0, MASTER_REGS_CONST, STATUS2, 4 ; Store VPOS21, VPOS20, VPOSCRC2_H and VPOSCRC2_L sbco ®_TMP1.b0, MASTER_REGS_CONST, VPOS21, 4 ; generate interrupt PRU0_ARM_IRQ2 ldi r31.w0, PRU0_ARM_IRQ2 transport_on_v_frame_2_exit: ; Check for VPOS_VALID, If it is valid, update the position and crc to DMEM ; and raise interrupt lbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS_VALID, 1 qbne transport_skip_vpos_update, REG_TMP0.b0, 1 lbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS_TEMP, 7 sbco ®_TMP0.b0, MASTER_REGS_CONST, VPOS4, 7 ; generate interrupt PRU0_ARM_IRQ1 ldi r31.w0, PRU0_ARM_IRQ1 transport_skip_vpos_update: ; Set POSTX to 3 ldi REG_TMP0.b0, 0x3 sbco ®_TMP0.b0, MASTER_REGS_CONST, POSTX, 1 ; Load summary lbco ®_TMP0.b0, MASTER_REGS_CONST, SAFE_SUM_TEMP, 1 sbco ®_TMP0.b0, MASTER_REGS_CONST, SAFE_SUM, 1 ;check SUMMARY and MASK_SUM lbco ®_TMP1.b1, MASTER_REGS_CONST, MASK_SUM, 1 and REG_TMP1.b0, REG_TMP0.b0, REG_TMP1.b1 qbeq summary_no_int, REG_TMP1.b0, 0x00 ;set event and generate interrupt lbco ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 ;set REG_TMP0.w0, REG_TMP0.w0, REG_FNC.b0 set REG_TMP0.w0, REG_TMP0.w0, EVENT_SUM ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int7, REG_TMP0.w2, REG_TMP0.w2 ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int7: ;set event_s and generate interrupt_s lbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_S, 2 set REG_TMP0.b0, REG_TMP0.b0, EVENT_S_SSUM ;save events sbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_S, 2 qbbc update_events_no_int17, REG_TMP0.b1, EVENT_S_SSUM ; generate interrupt_s ldi r31.w0, PRU0_ARM_IRQ4 update_events_no_int17: summary_no_int: ; Update SUM and SSUM bits in ONLINE_STATUS registers lbco ®_TMP0.b0, MASTER_REGS_CONST, SAFE_SUM, 1 lbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 3 clr REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_SUM clr REG_TMP2.b2, REG_TMP2.b0, ONLINE_STATUS_1_SSUM qbeq online_status_sum_clear, REG_TMP0.b0, 0x00 set REG_TMP2.b0, REG_TMP2.b0, ONLINE_STATUS_D_SUM set REG_TMP2.b2, REG_TMP2.b0, ONLINE_STATUS_1_SSUM online_status_sum_clear: sbco ®_TMP2.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 3 ;restore REG_FNC.w0 content mov REG_FNC.w0, REG_TMP11.w1 jmp datalink_transport_on_v_frame_done_2 ;---------------------------------------------------- ;transport_layer_recv_msg ;Handles Hiperface DSL messages receiving ;?? cycles ;input: ;output: ;modifies: ;---------------------------------------------------- transport_layer_recv_msg: ;check if we reset protocol lbco ®_TMP0.b0, MASTER_REGS_CONST, SYS_CTRL, 1 ;ERROR: qbbc datalink_abort2 is not working, though no compiler error qbbc transport_layer_no_prst, REG_TMP0.b0, SYS_CTRL_PRST jmp datalink_abort transport_layer_no_prst: ;check if we reset protocol by reading SAFE_CTRL register lbco ®_TMP0.b0, MASTER_REGS_CONST, SAFE_CTRL, 1 qbbc transport_layer_no_safe_prst, REG_TMP0.b0, SAFE_CTRL_PRST jmp datalink_abort transport_layer_no_safe_prst: ;are we already receiving a long message? qbeq transport_layer_check_for_new_msg, LONG_MSG_RECV.bits_left, 0 and REG_TMP2.b0, CHANNEL.ch_paral, 0x0f ;last byte (crc low)? -> then flip it qblt transport_layer_recving_long_msg_dont_flip_nibble, LONG_MSG_RECV.bits_left, 8 xor REG_TMP2.b0, REG_TMP2.b0, 0x0f transport_layer_recving_long_msg_dont_flip_nibble: ;calculate running crc ldi REG_TMP0, (LUT_CRC16+PDMEM00) lsr REG_TMP1.b0, LONG_MSG_RECV_CRC_H, 4 xor REG_TMP1.b0, REG_TMP1.b0, REG_TMP2.b0 lsl REG_TMP1.w0, REG_TMP1.b0, 1 lbbo ®_TMP1.w0, REG_TMP0, REG_TMP1.w0, 2 lsl LONG_MSG_RECV.crc, LONG_MSG_RECV.crc, 4 xor LONG_MSG_RECV.crc, LONG_MSG_RECV.crc, REG_TMP1.w0 ;are we receiving crc? qbge transport_layer_recving_long_msg_crc, LONG_MSG_RECV.bits_left, 16 ;still receiving data qbbs transport_layer_recving_long_msg_data_low_nibble, LONG_MSG_RECV.bits_left, 2 transport_layer_recving_long_msg_data_high_nibble: lsl REG_TMP0.b0, REG_TMP2.b0, 4 sbco ®_TMP0.b0, MASTER_REGS_CONST, LONG_MSG_RECV.ptr, 1 ;160 qba transport_layer_recving_long_msg_data_nibble_end transport_layer_recving_long_msg_data_low_nibble: lbco ®_TMP0.b0, MASTER_REGS_CONST, LONG_MSG_RECV.ptr, 1 or REG_TMP0.b0, REG_TMP0.b0, REG_TMP2.b0 sbco ®_TMP0.b0, MASTER_REGS_CONST, LONG_MSG_RECV.ptr, 1 add LONG_MSG_RECV.ptr, LONG_MSG_RECV.ptr, 1 transport_layer_recving_long_msg_data_nibble_end: qba transport_layer_recving_long_msg_end transport_layer_recving_long_msg_crc: ;we are receiving crc qbne transport_layer_recving_long_msg_end, LONG_MSG_RECV.bits_left, 4 ;set long msg channel to unbusy ; Set EVENT_FREL in EVENT register lbco ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_FREL ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int8, REG_TMP0.w2, EVENT_FREL ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int8: ; Set ONLINE_STATUS_D_FREL in ONLINE_STATUS_D register lbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 set REG_TMP0.b0, REG_TMP0.b0, (ONLINE_STATUS_D_FREL-8) sbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 ;check for crc error qbeq transport_layer_recving_long_msg_end, LONG_MSG_RECV.crc, 0 ; Set EVENT_ANS in EVENT register lbco ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_ANS ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int9, REG_TMP0.w2, EVENT_ANS ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int9: ; Set ANS in ONLINE_STATUS_D register lbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 set REG_TMP0.b0, REG_TMP0.b0, (ONLINE_STATUS_D_MIN-8) sbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 transport_layer_recving_long_msg_end: ; Clear ANS in ONLINE_STATUS_D register lbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 clr REG_TMP0.b0, REG_TMP0.b0, (ONLINE_STATUS_D_MIN-8) sbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 sub LONG_MSG_RECV.bits_left, LONG_MSG_RECV.bits_left, 4 qba transport_layer_recv_msg_end transport_layer_check_for_new_msg: ;check only position ch_para[35:39], if found at this position, we can use subsequent nibbles and have full short message available lsr REG_TMP0.b0, CHANNEL.ch_parah, 3 and REG_TMP0.b0, REG_TMP0.b0, 0x1f qbeq transport_layer_recv_no_msg, REG_TMP0.b0, S_PAR_IDLE qbbs transport_layer_recv_msg_end, H_FRAME.flags, FLAG_WAIT_IDLE ;check for special character qbbs transport_layer_recv_msg_check_for_nak, REG_TMP0.b0, 4 ;set flag to signalize that we need to wait for next S_PAR_IDLE again, so we do not parse data multiple times set H_FRAME.flags, H_FRAME.flags, FLAG_WAIT_IDLE ;reassemble 32 bits of msg, 5bits->4bits and REG_TMP11.b3, REG_TMP0.b0, 0x1f lsl REG_TMP11.b3, REG_TMP11.b3, 4 lsl REG_TMP0.b0, CHANNEL.ch_parah, 2 lsr REG_TMP0.b1, CHANNEL.ch_paral, 30 or REG_TMP0.b0, REG_TMP0.b0, REG_TMP0.b1 and REG_TMP0.b0, REG_TMP0.b0, 0x0f or REG_TMP11.b3, REG_TMP11.b3, REG_TMP0.b0 mov REG_TMP2, CHANNEL.ch_paral ldi REG_TMP1.b0, ®_TMP11.b0 loop transport_layer_reassemble_msg_loop, 3 and REG_TMP0.b1, REG_TMP2.b0, 0x0f lsr REG_TMP0.b2, REG_TMP2, 1 and REG_TMP0.b2, REG_TMP0.b2, 0xf0 or REG_TMP0.b0, REG_TMP0.b1, REG_TMP0.b2 mvib *REG_TMP1.b0++, REG_TMP0.b0 lsr REG_TMP2, REG_TMP2, 10 transport_layer_reassemble_msg_loop: ;identify message type ;jmp transport_layer_received_long_msg qbbs transport_layer_received_long_msg, REG_TMP11.b3, 7 transport_layer_received_short_msg: ;check crc ldi REG_TMP1.b0, &r12.b0 ;read or write? qbbs transport_layer_short_msg_recv_read, REG_TMP11.b3, 6 ;received write ack ldi REG_FNC.b0, 1 CALL1 (calc_16bit_crc);-DYNAMIC_CODE_OFFSET-CODE_TRANSPORT_LAYER*CODE_SIZE) qbne transport_layer_recv_msg_check_for_nak, REG_FNC.w0, REG_TMP11.w1 ;we received a valid msg -> set ACK flag or SHORT_MSG.timeout, SHORT_MSG.timeout, (1< set ACK flag or SHORT_MSG.timeout, SHORT_MSG.timeout, (1< we send lower nibble qbbc transport_layer_sned_long_msg_lower_nibble, SHORT_MSG.bits_left, 2 lsr SEND_PARA, SEND_PARA, 4 qba transport_layer_sned_long_msg_nibble_end transport_layer_sned_long_msg_lower_nibble: and SEND_PARA, SEND_PARA, 0x0f add SHORT_MSG.data, SHORT_MSG.data, 1 transport_layer_sned_long_msg_nibble_end: qba transport_layer_send_sending_calc_crc transport_layer_send_sending_short_msg: ;we are still sending short message lsr SEND_PARA, SHORT_MSG32, SHORT_MSG.bits_left ;no special char -> only 4 bits and SEND_PARA, SEND_PARA, 0x0f transport_layer_send_sending_calc_crc: ;calculate running CRC ldi REG_TMP2, (LUT_CRC16+PDMEM00) lsr REG_TMP0.b0, SHORT_MSG_CRC_H, 4 xor REG_TMP0.b0, REG_TMP0.b0, SEND_PARA lsl REG_TMP0.w0, REG_TMP0.b0, 1 lbbo ®_TMP0.w0, REG_TMP2, REG_TMP0.w0, 2 lsl SHORT_MSG.crc, SHORT_MSG.crc, 4 xor SHORT_MSG.crc, SHORT_MSG.crc, REG_TMP0.w0 ;flip lower crc byte (only once) qbne transport_layer_send_sending_msg_crc_dont_flip, SHORT_MSG.bits_left, 16 xor SHORT_MSG_CRC_L, SHORT_MSG_CRC_L, 0xff transport_layer_send_sending_msg_crc_dont_flip: qba transport_layer_send_msg_end transport_layer_check_for_new_short_msg: ;check for SLAVE_REG_CTRL if we read/write data lbco ®_TMP0.b0, MASTER_REGS_CONST, SLAVE_REG_CTRL, 1 qbeq transport_layer_no_short_msg, REG_TMP0.b0, 0x3f ; Clear ONLINE_STATUS_1_FRES in ONLINE_STATUS_1 register lbco ®_TMP0.b2, MASTER_REGS_CONST, (ONLINE_STATUS_1_L), 1 clr REG_TMP0.b2, REG_TMP0.b2, (ONLINE_STATUS_1_FRES-8) sbco ®_TMP0.b2, MASTER_REGS_CONST, (ONLINE_STATUS_1_L), 1 ;reset ldi REG_TMP0.b1, 0x3f sbco ®_TMP0.b1, MASTER_REGS_CONST, SLAVE_REG_CTRL, 1 ;only 6 bit address and SHORT_MSG.addr, REG_TMP0.b0, 0x3f ;reset CRC ldi SHORT_MSG.crc, 0 ;MSB in Master Register identifies mode (r/w) qbbc transport_layer_short_msg_write, REG_TMP0.b0, 7 ;we read slave register or SHORT_MSG.addr, SHORT_MSG.addr, (MSG_READ<<6) ;read short messages contain less content -> move addr part to data mov SHORT_MSG.data, SHORT_MSG.addr ldi SHORT_MSG.bits_left, (8*3) qba transport_layer_short_msg_dir_end transport_layer_short_msg_write: ;we write slave register -> DIR=0 ;load data from S_PC_DATA for writing lbco ®_TMP0.b1, MASTER_REGS_CONST, S_PC_DATA, 1 mov SHORT_MSG.data, REG_TMP0.b1 ;we write slave register -> DIR=0 ldi SHORT_MSG.bits_left, (8*4) transport_layer_short_msg_dir_end: ldi SHORT_MSG.timeout, 64 ;set para channel to busy set H_FRAME.flags, H_FRAME.flags, FLAG_PARA_BUSY qba transport_layer_send_msg_end transport_layer_no_short_msg: ;check for new long msg lbco ®_TMP0.b0, MASTER_REGS_CONST, PC_CTRL, 1 qbbc transport_layer_send_msg_end, REG_TMP0.b0, 0 clr REG_TMP0.b0, REG_TMP0.b0, 0 sbco ®_TMP0.b0, MASTER_REGS_CONST, PC_CTRL, 1 ;set para channel to busy set H_FRAME.flags, H_FRAME.flags, FLAG_PARA_BUSY ; Clear ONLINE_STATUS_D_FREL in ONLINE_STATUS_D register lbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 clr REG_TMP0.b0, REG_TMP0.b0, (ONLINE_STATUS_D_FREL-8) sbco ®_TMP0.b0, MASTER_REGS_CONST, (ONLINE_STATUS_D_L), 1 lbco ®_TMP1, MASTER_REGS_CONST, PC_ADD_H, 4 mov SHORT_MSG.addr, REG_TMP1.b0 ldi SHORT_MSG.bits_left, 16 ;using PC_OFF? qbbc transport_layer_assemble_long_msg_no_pc_off, REG_TMP1.b0, LOFF add SHORT_MSG.bits_left, SHORT_MSG.bits_left, 16 transport_layer_assemble_long_msg_no_pc_off: ;save ADDR(+OFF) to memory buffer lsr REG_TMP0.b0, SHORT_MSG.bits_left, 3 sbco ®_TMP1, MASTER_REGS_CONST, LONG_MSG_BUFFER, b0 ;skip LLEN on read operation qbbs transport_layer_assemble_long_msg_no_llen, SHORT_MSG.addr, 6 ;get LLEN lsr REG_TMP1.b0, REG_TMP1.b0, 2 and REG_TMP1.b0, REG_TMP1.b0, 0x03 qbeq transport_layer_assemble_long_msg_no_llen, REG_TMP1.b0, 0 ldi REG_TMP1.b1, 8 lsl REG_TMP1.b0, REG_TMP1.b1, REG_TMP1.b0;counting no. of bits length we need to send add SHORT_MSG.bits_left, SHORT_MSG.bits_left, REG_TMP1.b0 mov REG_TMP0.b2, REG_TMP0.b0 lsr REG_TMP0.b0, REG_TMP1.b0, 3 lbco ®_TMP1, MASTER_REGS_CONST, PC_BUFFER0, b0 add REG_TMP0.b2, REG_TMP0.b2, LONG_MSG_BUFFER ;Store the data at LONG_MSG_BUFFER+4 sbco ®_TMP1, MASTER_REGS_CONST, REG_TMP0.b2, b0 transport_layer_assemble_long_msg_no_llen: ;crc add SHORT_MSG.bits_left, SHORT_MSG.bits_left, 16 ;load ptr for memory buffer ldi SHORT_MSG.data, LONG_MSG_BUFFER ldi SHORT_MSG.crc, 0 transport_layer_send_msg_end: ; Check for Low QM value and update event registers if required ; Check EVENT_UPDATE_PENDING_QMLW to process a low QM value event update lbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 qbbc transport_layer_no_qmlw_event, REG_TMP0.b0, EVENT_UPDATE_PENDING_QMLW ; Set EVENT_QMLW in EVENT register lbco ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_QMLW ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int3, REG_TMP0.w2, EVENT_QMLW ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int3: ; Set EVENT_S_QMLW in EVENT_S register lbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_S, 2 set REG_TMP0.b0, REG_TMP0.b0, EVENT_S_QMLW ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_S, 1 qbbc update_events_no_int16, REG_TMP0.b1, EVENT_S_QMLW ; generate interrupt_s ldi r31.w0, PRU0_ARM_IRQ4 update_events_no_int16: ;Clear EVENT_UPDATE_PENDING_QMLW clr REG_TMP0.w0, REG_TMP0.w0, EVENT_UPDATE_PENDING_QMLW sbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 transport_layer_no_qmlw_event: ; Update QMLW bits in ONLINE_STATUS registers lbco ®_TMP0.b0, MASTER_REGS_CONST, MASTER_QM, 1 and REG_TMP0.b0, REG_TMP0.b0, 0x7f ; Set QMLW if value is < 14 qble transport_layer_online_status_qm_not_low, REG_TMP0.b0, 14 ; Set QMLW bits lbco ®_TMP0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 6 set REG_TMP0.w0, REG_TMP0.w0, ONLINE_STATUS_D_QMLW set REG_TMP0.w2, REG_TMP0.w2, ONLINE_STATUS_1_QMLW set REG_TMP1.w0, REG_TMP1.w0, ONLINE_STATUS_2_QMLW sbco ®_TMP0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 6 qba transport_layer_online_status_qm_update_done transport_layer_online_status_qm_not_low: ; Clear QMLW bits lbco ®_TMP0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 6 clr REG_TMP0.w0, REG_TMP0.w0, ONLINE_STATUS_D_QMLW clr REG_TMP0.w2, REG_TMP0.w2, ONLINE_STATUS_1_QMLW clr REG_TMP1.w0, REG_TMP1.w0, ONLINE_STATUS_2_QMLW sbco ®_TMP0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 6 transport_layer_online_status_qm_update_done: ; update POS bits in ONLINE_STATUS_D and EVENT lbco ®_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 ; Check EVENT_UPDATE_PENDING_POS to process a pending POS set lbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 qbbc transport_layer_no_pos_event, REG_TMP0.b0, EVENT_UPDATE_PENDING_POS ; Clear EVENT_UPDATE_PENDING_POS clr REG_TMP0.w0, REG_TMP0.w0, EVENT_UPDATE_PENDING_POS sbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 ; Set EVENT_POS in EVENT register lbco ®_TMP0, MASTER_REGS_CONST, EVENT_H, 4 set REG_TMP0.w0, REG_TMP0.w0, EVENT_POS ;save events sbco ®_TMP0.w0, MASTER_REGS_CONST, EVENT_H, 2 qbbc update_events_no_int14, REG_TMP0.w2, EVENT_POS ; generate interrupt ldi r31.w0, PRU0_ARM_IRQ update_events_no_int14: set REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_POS qba transport_layer_pos_update_done transport_layer_no_pos_event: clr REG_TMP1.b0, REG_TMP1.b0, ONLINE_STATUS_D_POS transport_layer_pos_update_done: sbco ®_TMP1.b0, MASTER_REGS_CONST, ONLINE_STATUS_D_H, 1 jmp transport_layer_send_msg_done ;---------------------------------------------------- ;Function: transport_on_h_frame (RET_ADDR) ;calculates the acceleration, velocity and fast position ; 38+9+52 = 99 cycles ;input: ;output: ;modifies: ;---------------------------------------------------- ;TODO: reduce processing time by 24 cycles transport_on_h_frame: ; Set POSTX to 0 ldi REG_TMP0.b0, 0x0 sbco ®_TMP0.b0, MASTER_REGS_CONST, POSTX, 1 ;check for byte error in acceleration channel qbbs transport_acc_err_inc, H_FRAME.flags, FLAG_ERR_ACC ;crc error verification ;CALL1 calc_acc_crc ;5 bits CRC of acceleration channel are flipped -> flip back xor H_FRAME.acc, H_FRAME.acc, 0x1f ldi REG_TMP2, (LUT_CRC5+PDMEM00) lbbo ®_TMP0.b1, REG_TMP2, H_FRAME_acc1, 1 lsl REG_TMP0.b1, REG_TMP0.b1, 3 xor REG_TMP0.b1, REG_TMP0.b1, H_FRAME_acc0 lbbo ®_FNC.b0, REG_TMP2, REG_TMP0.b1, 1 qbne transport_acc_err_inc, REG_FNC.b0, 0 ;check for special character: K29.7 is sent two times if slave error occured ldi REG_TMP0.w0, DOUBLE_K29_7 qbne delta_delta_position, H_FRAME.acc, REG_TMP0.w0 transport_acc_err_inc: ;update the acc_err_cnt register lbco ®_TMP0.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1 add REG_TMP0.b0, REG_TMP0.b0, 1 sbco ®_TMP0.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1 ;reset if it is too large lbco ®_TMP0.b1, MASTER_REGS_CONST, ACC_ERR_CNT_TRESH, 1 qbgt transport_on_h_frame_no_reset, REG_TMP0.b0, REG_TMP0.b1 jmp datalink_abort transport_on_h_frame_no_reset: ;save return addr mov REG_TMP11.w0, RET_ADDR0 ;CALL estimator_acc; Instead of calling the API, copy the code here to save PRU cycles. ;---------------------------------------------------- ;Function: estimator_acc (RET_ADDR) ;This function estimates the FPOS and VEL ;15+15+22=52 cycles ;input: ; ;output: ; FAST_POSH, FAST_POSL, SPEED ;modifies: ; FAST_POSH, FAST_POSL, SPEED, REG_TMP1, REG_TMP0 ;---------------------------------------------------- ;Estimating the acceleration ;ith acc = (delta_a4* 1 + delta_a3 * 2 + delta_a2 * 4 + delta_a1 * 8 + delta_a0 * 16) / 32 lsl REG_TMP0.w0, DELTA_ACC3, 1 lsl REG_TMP0.w2, DELTA_ACC2, 2 lsl REG_TMP1.w0, DELTA_ACC1, 3 lsl REG_TMP1.w2, DELTA_ACC0, 4 add REG_TMP0.w0, REG_TMP0.w0, REG_TMP0.w2 add REG_TMP1.w0, REG_TMP1.w2, REG_TMP1.w0 add REG_TMP0.w0, REG_TMP0.w0, REG_TMP1.w0 add REG_TMP0.w0, REG_TMP0.w0, DELTA_ACC4 ;divide with 32 lsr REG_TMP0.w0, REG_TMP0.w0, 5 ;sign extend delta acceleration to 16 bit -> acc size qbbc estimator_acc_sign_extend_dacc1, REG_TMP0.w0, 10 or REG_TMP0.b1, REG_TMP0.b1, 0xf8 estimator_acc_sign_extend_dacc1: ; calcuate MAXACC, cap acc ;add estimated delta acc to LAST_ACC add REG_FNC.w0, LAST_ACC, REG_TMP0.w0 ;check if estimated acc is neg. or pos. ;CALL1 calc_speed; Instead of calling the API, copy the code here to save PRU cycles. ;sign extend acceleration to 24 bit -> speed size xor REG_TMP0, REG_TMP0, REG_TMP0 qbbc calc_speed_extend_acc1, REG_FNC.w0, 10 ldi REG_TMP0.w1, 0xfff8 calc_speed_extend_acc1: or REG_TMP0.w0, REG_TMP0.w0, REG_FNC.w0 add SPEED.w0, SPEED.w0, REG_TMP0.w0 adc SPEED.b2, SPEED.b2, REG_TMP0.b2 ;updating the delta acceleration regs 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 CALL1 calc_fastpos ;restore return addr mov RET_ADDR0, REG_TMP11.w0 ; Moving the event and online register update during stuffing ; Set EVENT_UPDATE_PENDING_POS to indicate a fast position data consistency error lbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 set REG_TMP0.b0, REG_TMP0.w0, EVENT_UPDATE_PENDING_POS sbco ®_TMP0.b0, MASTER_REGS_CONST, EVENT_UPDATE_PENDING, 1 qba transport_on_h_frame_exit delta_delta_position: ;reset ACC_ERR_CNT sbco ®_FNC.b0, MASTER_REGS_CONST, ACC_ERR_CNT, 1 ;shift out crc bits lsr REG_FNC.w0, H_FRAME.acc, 5 ; learn highest abs. acc ;CALL1 calc_speed; Instead of calling the API, copy the code here to save PRU cycles. ;sign extend acceleration to 24 bit -> speed size xor REG_TMP0, REG_TMP0, REG_TMP0 qbbc calc_speed_extend_acc0, REG_FNC.w0, 10 ldi REG_TMP0.w1, 0xfff8 calc_speed_extend_acc0: or REG_TMP0.w0, REG_TMP0.w0, REG_FNC.w0 add SPEED.w0, SPEED.w0, REG_TMP0.w0 adc SPEED.b2, SPEED.b2, REG_TMP0.b2 ;updating the delta acceleration regs 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 CALL1 calc_fastpos transport_on_h_frame_exit: ;calculate rel. pos and store lbco ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 ;sign extend speed to 32 bits and add it to REL_POS 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 ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 ;store fast pos. and velocity mov REG_TMP0, FAST_POSH mov REG_TMP1, SPEED xin 160, ®_TMP0, 8 sbco ®_TMP0, MASTER_REGS_CONST, POS4, SIZE_FAST_POS+3 ; Set POSTX to 2 ldi REG_TMP0.b0, 0x2 sbco ®_TMP0.b0, MASTER_REGS_CONST, POSTX, 1 ; signal event mst_intr[0] and PRU0_ARM_IRQ3 ldi r31.w0, 32+0 ldi r31.w0, PRU0_ARM_IRQ3 RET ;-------------------------------------------------------------------------------------------------- ;Function: calc_fastpos (RET_ADDR1) ;This function sign extends speed and adds it to fast position ;22 cycles ;input:SPEED, FAST_POSH, FAST_POSL ; ;output: FAST_POSH, FAST_POSL ; ;modifies:FAST_POSH, FAST_POSL ; ;-------------------------------------------------------------------------------------------------- calc_fastpos: ;sign extend speed to NUM_POS_BITS=NUM_MT_BITS+NUM_ST_BITS and add it to FAST_POS ldi REG_TMP1.b0, 0 qbbc calc_fastpos_extend_vel, SPEED, 23 ldi REG_TMP1.b0, 0xff calc_fastpos_extend_vel: mov REG_TMP0, SPEED mov REG_TMP0.b3, REG_TMP1.b0 ;calculating fast position add FAST_POSL, FAST_POSL, REG_TMP0.b0 lsr REG_TMP0, REG_TMP0, 8 mov REG_TMP0.b3, REG_TMP1.b0 adc FAST_POSH, FAST_POSH, REG_TMP0 ;load mask for fast position lbco ®_TMP1, MASTER_REGS_CONST, MASK_POS, 4 and FAST_POSH, FAST_POSH, REG_TMP1 RET1 ;-------------------------------------------------------------------------------------------------- ;Function: calc_speed (RET_ADDR1) ;This function sign extends acceleration and adds it to speed ;15 cycles ;input: ; SPEED ; REG_FNC.w0: acceleration ; ;output: SPEED ; ;modifies: SPEED ; ;-------------------------------------------------------------------------------------------------- calc_speed: ;sign extend acceleration to 24 bit -> speed size xor REG_TMP0, REG_TMP0, REG_TMP0 qbbc calc_speed_extend_acc, REG_FNC.w0, 10 ldi REG_TMP0.w1, 0xfff8 calc_speed_extend_acc: or REG_TMP0.w0, REG_TMP0.w0, REG_FNC.w0 add SPEED.w0, SPEED.w0, REG_TMP0.w0 adc SPEED.b2, SPEED.b2, REG_TMP0.b2 ;updating the delta acceleration regs 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 RET1 ;-------------------------------------------------------------------------------------------------- ;Function: estimator_fpos (RET_ADDR1) ;This function estimates fpos when vpos and fpos mismatch ;The FAST_POS will oscillate with a small error around the SAFE_POS ; ;19 cycles ; ;input: ; REG_TMP1: vpos - last fpos ; ;output: FAST_POSH, FAST_POSL, SPEED ; ;modifies:FAST_POSH, FAST_POSL, SPEED ;NOT allowed to use REG_TMP11 ; ;-------------------------------------------------------------------------------------------------- estimator_fpos: add ALIGN_PH, ALIGN_PH, 1 ;pos=abspos+relpos lbco ®_TMP0, MASTER_REGS_CONST, REL_POS0, 4 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 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.b2, SPEED.b2, REG_TMP0.b2 estimator_fpos_align_ph_not_2: RET1 ;-------------------------------------------------------------------------------------------------- ;Function: store_error (RET_ADDR1) ;Stores error in ring buffer ;15 cycles ;input: ; REG_FNC: error ;output: ;modifies: ;-------------------------------------------------------------------------------------------------- store_error: ;load abs error ring buffer ptr. ;ldi REG_TMP1, (PDMEM00+ABS_ERR_PTR) ;xor REG_TMP0, REG_TMP0, REG_TMP0 ;lbbo ®_TMP0, REG_TMP1, 0x00, ABS_ERR_PTR_SIZE ;store error ;sbbo ®_FNC, REG_TMP0, 0x00, 4 ;update and store ptr ;add REG_TMP0.w0, REG_TMP0.w0, 4 ;ldi REG_TMP0.w2, (PDMEM00+ABS_ERR_BUF+ABS_ERR_BUF_SIZE) ;qbgt store_error_dont_reset_abs_err_ptr, REG_TMP0.w0, REG_TMP0.w2 ;ldi REG_TMP0.w0, (PDMEM00+ABS_ERR_BUF) store_error_dont_reset_abs_err_ptr: ;sbbo ®_TMP0.w0, REG_TMP1, 0x00, ABS_ERR_PTR_SIZE ;RET1 ;-------------------------------------------------------------------------------------------------- ;Function: calc_acc_crc (RET_ADD1) ;This function checks the crc for the acceleration channel ;11 cycles ;input: ; H_frame.acc ;output: ; REG_FNC.b0: 0 indicates that crc check was successfully ;modifies: ; REG_TMP0, REG_FNC ;-------------------------------------------------------------------------------------------------- calc_acc_crc: ;5 bits CRC of acceleration channel are flipped -> flip back xor H_FRAME.acc, H_FRAME.acc, 0x1f ldi REG_TMP2, (LUT_CRC5+PDMEM00) lbbo ®_TMP0.b1, REG_TMP2, H_FRAME_acc1, 1 lsl REG_TMP0.b1, REG_TMP0.b1, 3 xor REG_TMP0.b1, REG_TMP0.b1, H_FRAME_acc0 lbbo ®_FNC.b0, REG_TMP2, REG_TMP0.b1, 1 RET1 ;-------------------------------------------------------------------------------------------------- ;Function: demp_data_symbols (RET_ADDR1) ;Demaps data symbols (5bits) and converts them to data nibbles (4bits) ;input: ; REG_FNC.b0: Number of data symbols ; r1.b1: source ; r1.b0: destination ;output: ; *r1.b0[0:REG_FNC.b0]: demapped data ;modifies: ;-------------------------------------------------------------------------------------------------- demap_data_symbols: