; program to pack the configuration in the TRAP chip and send using the readout tree
;
; by V.Angelov
;
; $Id$:

;#def    cpu_clocks_pk  = 0xF0F0         ; number of clocks for packing (if defined, enable to measure the time for pack & send)

#def    mask_data   =   r12             ; data bit mask
#def    ret_addr    =   r7              ; return address

    ; write and exit
cm_pk_wr:
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP
    jmp     cc_uncond, ret_addr         ; exit

cm_pack_5:
    ; address in r14
    ; number of words in r1
    ; step in r3

    ; first build the header
    lgio    0, r14                      ; request the 1st data
    shl     8, r14, r5
    add     r14, r3, r14                ; increment the read address
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     7, r5, r5
    mov     0x10, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (widthM1=4 << 3) | (step=1 << 1)
    or      r5, r3, r5
    shl     1, r5, r5
    spio    r5, NODP                    ; store the header

    mov     0x1F, mask_data             ; the mask for the data
cm_pk_rp5:
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 2nd data
    lpio    GBUSR0, r5                  ; read data into packed data reg
    and     r5, mask_data, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    add     r14, r3, r14                ; increment the read address
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 3th data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     5, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    add     r14, r3, r14                ; increment the read address
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 4th data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     10, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    add     r14, r3, r14                ; increment the read address
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 5th data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     15, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    add     r14, r3, r14                ; increment the read address
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 2nd data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     15, r4, r4
    shl     5, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    add     r14, r3, r14                ; increment the read address
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio    0, r14                      ; request the 1st data
    lpio    GBUSR0, r4                  ; read data
;    and     r4, mask_data, r4           ; last time not necessary, the upper bits are not used
    shl     15, r4, r4
    shl     10, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    ; no more bits, write the packed registers
    add     r14, r3, r14                ; increment the read address
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP                    ; store
    jmp     cc_uncond, cm_pk_rp5        ; read next group of CNF reg

cm_pack_6:
    ; address in r14
    ; number of words in r1

    ; first build the header
    shl     8, r14, r5
    lgio+   0                           ; request the 1st data
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     8, r5, r5
    mov     0x2A, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (widthM1=5 << 3) | (step=1 << 1)
    spio    r5, NODP                    ; store the header

    mov     0x3F, mask_data             ; the mask for the data
cm_pk_rp6:
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r5                  ; read data into packed data reg
    and     r5, mask_data, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     6, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     12, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     12, r4, r4
    shl      6, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
;    and     r4, mask_data, r4           ; last time not necessary, the upper bits are not used
    shl     12, r4, r4
    shl     12, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    ; no more bits, write the packed registers
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP                    ; store
    jmp     cc_uncond, cm_pk_rp6        ; read next group of CNF reg


cm_pack_7:
    ; address in r14
    ; number of words in r1

    ; first build the header
    shl     8, r14, r5
    lgio+   0                           ; request the 1st data
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     8, r5, r5
    mov     0x32, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (widthM1=6 << 3) | (step=1 << 1)
    spio    r5, NODP                    ; store the header

    mov     0x7F, mask_data             ; the mask for the data
cm_pk_rp7:
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r5                  ; read data into packed data reg
    and     r5, mask_data, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     7, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     14, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
;    and     r4, mask_data, r4           ; last time not necessary, the upper bits are not used
    shl     14, r4, r4
    shl      7, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    ; no more bits, write the packed registers
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP                    ; store
    jmp     cc_uncond, cm_pk_rp7        ; read next group of CNF reg

cm_pack_10:
    ; address in r14
    ; number of words in r1

    ; first build the header
    shl     8, r14, r5
    lgio+   0                           ; request the 1st data
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     8, r5, r5
    mov     0x4A, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (widthM1=9 << 3) | (step=1 << 1)
    spio    r5, NODP                    ; store the header

    mov     0x3FF, mask_data            ; the mask for the data
cm_pk_rp10:
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r5                  ; read data into packed data reg
    and     r5, mask_data, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
    and     r4, mask_data, r4
    shl     10, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
;    and     r4, mask_data, r4           ; last time not necessary, the upper bits are not used
    shl     10, r4, r4
    shl     10, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    ; no more bits, write the packed registers
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP                    ; store
    jmp     cc_uncond, cm_pk_rp10       ; read next group of CNF reg

cm_pack_15:
    ; address in r14
    ; number of words in r1

    ; first build the header
    shl     8, r14, r5
    lgio+   0                           ; request the 1st data
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     8, r5, r5
    mov     0x72, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (widthM1=14 << 3) | (step=1 << 1)
    spio    r5, NODP                    ; store the header
    iext    0x7FFF
    mov     0x7FFF, mask_data           ; the mask for the data
cm_pk_rp15:
    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r5                  ; read data into packed data reg
    and     r5, mask_data, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    jmpr    cc_busy, 0                  ; may be not needed!
    lgio+   0                           ; request the next data
    lpio    GBUSR0, r4                  ; read data
;    and     r4, mask_data, r4           ; last time not necessary, the upper bits are not used
    shl     15, r4, r4
    or      r5, r4, r5
    sub     r1, c1, r1                  ; remaining number of registers
    jmp     cc_zero, cm_pk_wr           ; no more registers in the block, write end exit

    ; no more bits, write the packed registers
    shl     1, r5, r5                   ; r5 = (r5 << 1) | 1
    or      r5, c1, r5
    spio    r5, NODP                    ; store
    jmp     cc_uncond, cm_pk_rp15       ; read next group of CNF reg

    ; 31 bit case
cm_pack_31:
    ; step is always 1
    ; address in r14
    ; number of words in r1

    ; first build the header
    shl     8, r14, r5
    or      r5, r1, r5                  ; r5 = (addr << 8) | nregs
    shl     8, r5, r5                   ; r5 = (addr << 16) | (nregs << 8)
    mov     0xF2, r2
    or      r5, r2, r5                  ; r5 = (addr << 16) | (nregs << 8) | (30 << 3) | (1 << 1)
    spio    r5, NODP                    ; store the header
cm_pk_31n:
    lgio+   0                           ; request the next data
    jmpr    cc_busy, 0
    lpio    GBUSR0, r4                  ; read data
    shl     1, r4, r4                   ; shift left
    or      r4, c1, r4                  ; set the LSB
    spio    r4, NODP
    sub     r1, c1, r1                  ; decrement the number of words
    jmp     cc_nzero, cm_pk_31n         ; next 32 bit word
    jmp     cc_uncond, ret_addr         ; exit

        ; single case
cm_pack_s16:
    ; here we have in r14 the address
    lgio    0, r14
    swp     r14, r5                     ; r5 = (addr << 16)
    iext    0xFFFF
    mov     0xFFFF, r2
    jmpr    cc_busy, 0
    lpio    GBUSR0, r4                  ; read data
    and     r4, r2, r4                  ; data = (data & 0xFFFF)
    or      r5, r4, r5                  ; (addr << 16) | data
    shl     2, r5, r5
    or      r5, c1, r5                  ; (addr << 18) | (data << 2) | 1
    spio    r5, NODP
    jmp     cc_uncond, ret_addr         ; exit

cm_pack_s32:
    ; here we have in r14 the address
    lgio    0, r14
    swp     r14, r5                     ; r5 = (addr << 16)
    iext    0xFFFF
    mov     0xFFFF, r2
    jmpr    cc_busy, 0
    lpio    GBUSR0, r4                  ; read data
    and     r4, r2, r3                  ; dataLo = data & 0xFFFF
    or      r5, r3, r5                  ; (addr << 16) | data
    shl     2, r5, r5
    or      r5, c3, r5                  ; (addr << 18) | (data << 2) | 3
    spio    r5, NODP
    or      r4, c1, r4
    spio    r4, NODP
    jmp     cc_uncond, ret_addr         ; exit

raw_send_cnf:

    #ifdef cpu_clocks_pk
    mov     0x640, r0
    spio    r0,  CTPCTRL
    mov     0, r0
    spio    r0,  CTPDINI
    #endif

#inc "pack_conf_inc.asm"

    #ifdef cpu_clocks_pk
    lpio    CTPDOUT, r6                 ; the number of CPU clocks
    jmpr    cc_busy, 0
    iext    cpu_clocks_pk
    mov     cpu_clocks_pk, r5
    add     r5, c5, r5
    sgio    r6, r5
    #endif

    #ifdef cpu3
    iext 0x00FE7FFF
    mov  0x00FE7FFF, r0
    swp  r0, r0
    spio r0, NODP                       ; end marker for config
    #endif
    jmp cc_uncond, raw_end_ni_tmsn