; DEBHP.a -- Debug Support 
;  BGLOOP copies this to C000 for execution
; 
; Variant of "DEBH".  This produces housekeeping packets (ApID=720)
; instead of writing to the debug serial port.

; Revision History
; 15-Dec-2002
;   First use in ISUALFF
; 15-Jan-2003
;   Modified to work with "debug.a"
;   Added entries for HEXOUT and DECOUT
;   Removed DEBUG_ASC, DEBUG_ASC_DEC, DEBUG_ASC_HL
;   Added check of GBL_SOH_WAIT
; 17-Oct-2003
;   new GBL.i

        include ioports.i
        include gbl.i
        org 0C000h      ; starts at C000
ccsds_send: jmp 0
task:       jmp 0
task_new:   jmp 0
        dw DEBUG_INIT
        dw DEBUG_PUTBYTE
        dw DEBUG_SHOW
        dw DEBUG_DUMP
        dw DEBUG_SHOWUT
        dw DEBUG_SHOWA
        dw DEBUG_REGSET
        dw HEXOUT
        dw DECOUT

debug_trap:
        di                      ; no more interrupts 
        xthl                    ; exchange top-of-stack with HL
        shld GBL_CMD            ; this is where the Trap happened
yaagh:  out 0           ; force a reset
        jmp yaagh

trap7: lxi b,7
        jmp debug_trap
trap6: lxi b,6
        jmp debug_trap
trap5: lxi b,5
        jmp debug_trap
trap4: lxi b,4
        jmp debug_trap
trap3: lxi b,3
        jmp debug_trap
trap2: lxi b,2
        jmp debug_trap
trap1: lxi b,1
        jmp debug_trap
trap05: lxi b,0a5h
        jmp debug_trap
trap15: lxi b,15h
        jmp debug_trap
trap25: lxi b,25h
        jmp debug_trap
trap35: lxi b,35h
        jmp debug_trap
trap45: lxi b,45h
        jmp debug_trap
trap55: lxi b,55h
        jmp debug_trap
trap65: lxi b,65h
        jmp debug_trap
trap0:  lxi b,0
        jmp debug_trap


; ===================
; Initialization
DEBUG_INIT:

; stuff all the interrupt vectors 
;
; this will make jumps to 0000 do a Trap
        mvi a,0CDh  ; "CALL" OPCODE
        sta RST_0               ; 0
        lxi h,TRAP0
        shld RST_0+1
; wild RST 7 is also a trap
        sta RST_7
        lxi h,TRAP7
        shld RST_7+1
;
        sta RST_0+4             ; 0.5
        lxi h,TRAP05
        shld RST_0+5
;
        sta RST_1               ; 1
        lxi h,TRAP1
        shld RST_1+1
;
        sta RST_1+4             ; 1.5
        lxi h,TRAP15
        shld RST_1+5
;
        sta RST_2               ; 2
        lxi h,TRAP2
        shld RST_2+1
;
        sta RST_2+4             ; 2.5
        lxi h,TRAP25
        shld RST_2+5
;
        sta RST_3               ; 3
        lxi h,TRAP3
        shld RST_3+1
;
        sta RST_3+4             ; 3.5
        lxi h,TRAP35
        shld RST_3+5
;
        sta RST_4               ; 4
        lxi h,TRAP4
        shld RST_4+1
;
        sta RST_4+4
        lxi h,TRAP45
        shld RST_4+5
;
        sta RST_5               ; 5
        lxi h,TRAP5
        shld RST_5+1
;
        sta RST_6
        lxi h,TRAP6
        shld RST_6+1
;
        sta RST_55
        lxi h,TRAP55
        shld RST_55+1
;
        sta RST_65
        lxi h,TRAP65
        shld RST_65+1
;
        sta RST_75
        lxi h,TRAP5
        shld RST_75+1
;
; set TRAP vector 
        mvi a,JMP_OPCODE  ; JMP opcode
        sta TRAP
        lxi h,DEBUG_TRAP  ; address of Trap handler
        shld TRAP+1       ; set as JMP address

; setup output queue for debug bytes
        lxi h,GBL_DTP_OUTQ
        shld qpr
        shld qpw

; zero packet 720 sequence counter
        lxi h,0
        shld sq20

; Start Debug task
        lxi d,debug_task
        lxi h,debug_STACK+stack_size
        call task_new
        ret

; send byte to output queue
; A - the byte
DEBUG_PUTBYTE:
        push psw
        push d
        push h

        lhld qpw        ; pointer for writing into queue
        mov M,a         ; store byte in queue
        inx h           ; step pointer
        lxi d,GBL_DTP_OUTQ_WRAP
        mov a,l
        cmp e
        jnz nowrapw
        mov a,h
        cmp d
        jnz nowrapw
        lxi h,GBL_DTP_OUTQ ; back to the beginning
nowrapw: shld qpw       ; store the pointer

        pop h
        pop d
        pop psw
        ret

; show A= as 2-digit hex
DEBUG_SHOWA:
        push psw
        push b
        mov b,a
        call send_packet_prefix
        mvi a,SHOW_HEXA
        call debug_putbyte
        mov a,b
        call debug_putbyte
        pop b
        pop psw
        ret
;
; show A as 2-digit hex
DEBUG_SHOW_HEX1:
        push psw
        push b
        mov b,a
        call send_packet_prefix
        mvi a,SHOW_HEX1
        call debug_putbyte
        mov a,b
        call debug_putbyte
        pop b
        pop psw
        ret
;
; packet codes recognized by U
        include u.i

dtp_present db 0 ; 1=DTP attached, 0=flight mode
;
; 
; serial output queue
qpr             ds 2    ; pointer for reading the queue
qpw             ds 2    ; pointer for writing the queue
;


debug_task:
; start pointer to packet data
        lxi h,packet_data
        shld pp
; zero number of data bytes
        mvi a,0
        sta nbpkt

dtloop: call task
; look for byte in output queue 
        lhld qpr
        xchg
        lhld qpw
        mov a,l
; if the queue pointers are equal, queue is empty
        cmp e
        jnz not_empty
        mov a,h
        cmp d
        jnz not_empty
        jmp queue_empty

; not empty; send out next byte
not_empty:
        lhld qpr        ; pointer for reading queue
        mov a,M         ; get byte
        lhld pp         ; pointer to packet
        mov M,a         ; store byte in packet
        inx h           ; step packet pointer
        shld pp

; step queue pointer
        lhld qpr
        inx h           ; step the pointer
; wrap queue pointer if neccessary
        lxi d,GBL_DTP_OUTQ_WRAP
        mov a,l
        cmp e
        jnz nowrapr
        mov a,h
        cmp d
        jnz nowrapr     
        lxi h,GBL_DTP_OUTQ  ; wrap to start
nowrapr: 
        shld qpr       ; store the pointer

; increment count of bytes in packet
        lda nbpkt
        inr a
        sta nbpkt
; if packet full, send packet
        cpi 127
        jnz dtloop
       
queue_empty:
        lda nbpkt
        cpi 0
        jz dtloop       ; no bytes in packet
; send the Debug Packet
; (delay to keep Astrium happy -- see SOH.a)
dast:   lda GBL_SOH_WAIT
        cpi 0
        jz happy
        call task
        jmp dast
happy:
        mvi a,20h       ; ApID 720
        lxi b,146       ; total packet length is 146
        lhld sq20       ; this ApID's source sequence number
        xchg    
        lxi h,nbpkt     ; data after 2ndary header
        call CCSDS_SEND  ; send packet
; increment the source sequence count
        lhld sq20
        inx h
        shld sq20
; zero nbytes-in-packet
        mvi a,0
        sta nbpkt
; start packet pointer again
        lxi h,packet_data
        shld pp
        jmp dtloop

sq20    ds 2


;
debug_stack ds stack_size
;
;
send_packet_prefix:
        push psw
        mvi a,0FEh
        call DEBUG_PUTBYTE
        mvi a,07Fh
        call DEBUG_PUTBYTE
        pop psw
        ret
;
; display message 
; HL - points to null-terminated message string
DEBUG_SHOW:
        push psw
        push h
l1:     mov a,M         ; get byte
        cpi 0           ; null?
        jz done         ;  yes, end of string
        call DEBUG_PUTBYTE
        inx h           ; next byte
        jmp l1
done:   pop h
        pop psw
        ret
;
;

hex:    cpi 10
        jm low
        adi 'A'-10
        ret
low:    adi '0'
        ret
;
; make "U" packet to display number in HL as 4 Hex digits
; (with newline at end)
HEXOUT:
        push psw
        call send_packet_prefix
        mvi a,SHOW_HEX   ; function code for "U" program in PC
        call debug_putbyte
        mov a,h
        call debug_putbyte
        mov a,l
        call debug_putbyte
        pop psw
        ret
;
; make "U" packet to display number in HL as 4 Hex digits
; (with no newline)
        public DEBUG_SHOW_HEXN
DEBUG_SHOW_HEXN:
        push psw
        call send_packet_prefix
        mvi a,SHOW_HEXN   ; function code for "U" program in PC
        call debug_putbyte
        mov a,h
        call debug_putbyte
        mov a,l
        call debug_putbyte
        pop psw
        ret
;
; make "U" packet to display number in HL as signed decimal
DECOUT:
        push psw
        call send_packet_prefix
        mvi a,SHOW_DECIMAL
        call debug_putbyte
        mov a,h
        call debug_putbyte
        mov a,l
        call debug_putbyte
        pop psw
        ret
;
; make "U" packet to display number in HL as signed decimal
        public DEBUG_SHOW_DECN
DEBUG_SHOW_DECN
        push psw
        call send_packet_prefix
        mvi a,SHOW_DECN
        call debug_putbyte
        mov a,h
        call debug_putbyte
        mov a,l
        call debug_putbyte
        pop psw
        ret
;
;
DEBUG_REGSET:
        push psw
        push b
        push psw
        call send_packet_prefix
        mvi a,SHOW_REGSET
        call debug_putbyte
        pop b
        mov a,b
        call debug_putbyte
        pop b
        mov a,c
        call debug_putbyte
        mov a,b
        call debug_putbyte
        mov a,c
        call debug_putbyte
        mov a,d
        call debug_putbyte
        mov a,e
        call debug_putbyte
        mov a,h
        call debug_putbyte
        mov a,l
        call debug_putbyte
        pop psw
        ret
;
DEBUG_DUMP:
; dump bytes pointed to by HL
; C gives number of bytes
        push psw
        push b
        push h
        call send_packet_prefix
        mvi a,SHOW_DUMP
        call debug_putbyte
        mov a,c         ; number of bytes to follow
        call debug_putbyte
ddl:    mov a,M
        call debug_putbyte
        inx h
        dcr c
        jnz ddl
        pop h
        pop b
        pop psw
        ret
;

; display spacecraft time as hh:mm:ss
DEBUG_SHOWMET:
; HL - points to 6 bytes of MET
        push psw
        push b
        push h
        call send_packet_prefix
        mvi a,SHOW_MET
        call debug_putbyte
        mvi c,6
sml:    mov a,M
        call debug_putbyte
        inx h
        dcr c
        jnz sml
        pop h
        pop b
        pop psw
        ret

; display ISUAL 10-byte time as hh:mm:ss.xxx
DEBUG_SHOWUT:
; HL - points to 10 bytes of UT
        push psw
        push b
        push h
        call send_packet_prefix
        mvi a,SHOW_UT
        call debug_putbyte
        mvi c,10
sul:    mov a,M
        call debug_putbyte
        inx h
        dcr c
        jnz sul
        pop h
        pop b
        pop psw
        ret

pp              ds 2
pktd            ds 128
nbpkt           equ pktd
packet_data     equ pktd+1

END_DPU ds 1
        lxi h,end_dpu