
; Modified by Blue Hell on 2011-10-24 to serve as pic4th
; startup file instead of C startup. This is meant to be
; for the boot loader only, apps will just use crt0.
;
; C Run-time startup module for dsPIC30 C compiler.
; (c) Copyright 2002,2004,2007 Microchip Technology, All rights reserved
;
; Entry __reset takes control at device reset and
; performs the following:
;
;  1. initialize stack and stack limit register
;  2. jump into the pic4th _main entry point
;

; // -------------------------------------------------------------------------

  .equ __24fj256da206, 1
  .include "p24fj256da206.inc"

 .text

  .global   __resetPRI
  .global   __resetALT

  .ifdef    __C30ELF
  .type     __resetPRI, @function
  .type     __resetALT, @function
  .endif

; // -------------------------------------------------------------------------

__resetPRI:
__resetALT:

  .weak     __reset
  .ifdef    __C30ELF
  .type     __reset, @function
  .endif

__reset:
;;
;; Initialize stack, PSV, and data
;;
;; registers used:  w0
;;
;; Inputs (defined by user or linker):
;;
;;  __SP_init
;;  __SPLIM_init
;;
;; Outputs:
;;
;;  does not return - jumps into main()
;;
;; Calls:
;;  __data_init
;;  _main
;;

;;  .weak    __user_init, __has_user_init

    mov      #__SP_init, w15                ; // initialize w15
    mov      #__SPLIM_init, w0
    mov      w0, _SPLIM                     ; // initialize SPLIM

    rcall    __data_init                    ; // initialize data

    bra       _main                         ; // call user's main()


__data_init:

;; Process data init template

;; The template is null-terminated, with records
;; in the following format:

;; struct data_record {
;;  char * dst;               /* destination address  */
;;  int    len;               /* length in bytes      */
;;  int    format;            /* format code          */
;;  char   data[ 0];          /* variable length data */
;; };

;; Registers used:  w0 w1 w2 w3 w4 w5

;; Inputs (defined by linker):
;;  __dinit_tbloffset
;;  __dinit_tblpage

;; Outputs:
;;  (none)

;; Calls:
;;  __memcpypd3


    .equiv   FMT_CLEAR, 0                   ; // format codes
    .equiv   FMT_COPY2, 2
    .equiv   FMT_COPY3, 3

    mov      #__dinit_tbloffset, w0         ; // w0, w1 = template
    mov      #__dinit_tblpage, w1
    bra      4f                             ; // br to continue

1:

    add      w0, #2, w0                     ; // template += 2
    addc     w1, #0, w1
    mov      w1, _TBLPAG                    ; // TBLPAG = tblpage( template)

    tblrdl.w [w0], w3                       ; // w3 = len
    add      w0, #2, w0                     ; // template += 2
    addc     w1, #0, w1
    mov      w1, _TBLPAG                    ; // TBLPAG = tblpage( template)

    tblrdl.w [w0], w5                       ; // w5 = format
    mov      #0x7f, w4
    and      w5, w4, w5

    add      w0, #2, w0                     ; // template += 2
    addc     w1, #0, w1
    clr      w4                             ; // upper = FALSE (default)

    cp       w5, #FMT_CLEAR                 ; // test format
    bra      nz, 2f                         ; // br if not FMT_CLEAR


    ;; FMT_CLEAR - clear destination memory

9:

    clr.b    [w2++]                         ; // clear memory
    dec      w3, w3                         ; // done?
    bra      gtu, 9b                        ; // loop if not
    bra      4f                             ; // br to continue


    ;; FMT_COPY2, FMT_COPY3 - copy bytes

2:

    cp       w5, #FMT_COPY2                 ; // test format
    bra      z, 3f                          ; // br if FMT_COPY2

    setm     w4                             ; // upper = TRUE

3:

    rcall    __memcpypd3                    ; // copy 2 or 3 bytes

4:

    mov      w1, _TBLPAG                    ; // TBLPAG = tblpage( template)
    tblrdl.w [w0], w2                       ; // w2 = next dst
    cp0      w2
    bra      nz, 1b                         ; // loop on non-zero dst

    retlw    #0, w0                         ; // exit ( clears ARGC also)


__memcpypd3:

;; Copy data from program memory to data memory
;;
;; Registers used:  w0 w1 w2 w3 w4 w5
;;
;; Inputs:
;;  w0, w1 = source address   (24 bits)
;;  w2 = destination address (16 bits)
;;  w3 = number of bytes (even or odd)
;;  w4 = upper byte flag   (0 = false)

;; Outputs:
;;  w0, w1 = next source address (24 bits)


1:

    mov      w1, _TBLPAG                    ; // TBLPAG = tblpage  ( src)
    mov      w0, w5                         ; // w5     = tbloffset( src)
    add      w0, #2, w0                     ; // src+=2
    addc     w1, #0, w1

    tblrdl.b [w5++], [w2++]                 ; // dst++ = lo byte
    dec      w3, w3                         ; // num--
    bra      z, 2f                          ; // br if done

    tblrdl.b [w5--], [w2++]                 ; // dst++ = hi byte
    dec      w3, w3                         ; // num--
    bra      z, 2f                          ; // br if done

    cp0      w4                             ; // test upper flag
    bra      z, 1b                          ; // br if false

    tblrdh.b [w5], [w2++]                   ; // dst++ = upper byte
    dec      w3, w3                         ; // num--
    bra      nz, 1b                         ; // br if not done

2:
    return                                  ; // exit



  .end

