suppasm.s

AT91所有开发板的资料 AT91所有开发板的资料

        ORR     v1, v1, tmp1, $SLA #24   ; word 1
        STMDB   dst!, {v1, v2, tmp3, lr}
        SUBS    n, n, #16
        BGE     %B0
        ; Reload registers

        LDMFD   sp!, {v1, v2, lr}

        ADDS    n, n, #16-4              ; check for at least 4
        BLT     %F2                      ; < 4 bytes

1       MOV     tmp3, tmp1, $SHA #8      ; last three bytes
        LDR     tmp1, [src, #-4]!        ; previous four bytes
        ORR     tmp3, tmp3, tmp1, $SLA #24
        STR     tmp3, [dst, #-4]!
        SUBS    n, n, #4
        BGE     %B1                      ; tmp1 contains three bytes

        ; Less than four bytes to go - readjust the src
        ; address.

2       ADD     src, src, #3
        B       Down_TrailingBytes

; The last two source bytes are in tmp1, two bytes must
; come from the previous source word.  At least four bytes
; more must be stored.

Down_TwoBytes
        CMP     n, #16-4                ; at least 16 bytes?
        BLT     %F1                     ; no
        SUB     n, n, #16-4             ; (n+16) bytes to go

        ; form a stack frame and save registers

        STMFD   sp!, {v1, v2, lr}

        ; loop doing 32 bytes at a time.  There are currently
        ; two useful bytes in tmp1 (a4).

0       MOV     lr, tmp1, $SHA #16       ; last two bytes
        LDMDB   src!, {tmp1, v1, v2, tmp3}
        ORR     lr, lr, tmp3, $SLA #16   ; word 4
        MOV     tmp3, tmp3, $SHA #16
        ORR     tmp3, tmp3, v2, $SLA #16 ; word 3
        MOV     v2, v2, $SHA #16
        ORR     v2, v2, v1, $SLA #16     ; word 2
        MOV     v1, v1, $SHA #16
        ORR     v1, v1, tmp1, $SLA #16   ; word 1
        STMDB   dst!, {v1, v2, tmp3, lr}
        SUBS    n, n, #16
        BGE     %B0
        ; Reload registers

        LDMFD   sp!, {v1, v2, lr}

        ADDS    n, n, #16-4              ; check for at least 4
        BLT     %F2                      ; < 4 bytes

1       MOV     tmp3, tmp1, $SHA #16     ; last two bytes
        LDR     tmp1, [src, #-4]!        ; previous four bytes
        ORR     tmp3, tmp3, tmp1, $SLA #16
        STR     tmp3, [dst, #-4]!
        SUBS    n, n, #4
        BGE     %B1                      ; tmp1 contains two bytes

        ; Less than four bytes to go - readjust the src
        ; address.

2       ADD     src, src, #2
        B       Down_TrailingBytes

; The last source byte is in tmp1, three bytes must
; come from the previous source word.  At least four bytes
; more must be stored.

Down_OneByte
        CMP     n, #16-4                ; at least 16 bytes?
        BLT     %F1                     ; no
        SUB     n, n, #16-4             ; (n+16) bytes to go

        ; form a stack frame and save registers

        STMFD   sp!, {v1, v2, lr}

        ; loop doing 32 bytes at a time.  There is currently
        ; one useful byte in tmp1 (a4).

0       MOV     lr, tmp1, $SHA #24       ; last byte
        LDMDB   src!, {tmp1, v1, v2, tmp3}
        ORR     lr, lr, tmp3, $SLA #8    ; word 4
        MOV     tmp3, tmp3, $SHA #24
        ORR     tmp3, tmp3, v2, $SLA #8  ; word 3
        MOV     v2, v2, $SHA #24
        ORR     v2, v2, v1, $SLA #8      ; word 2
        MOV     v1, v1, $SHA #24
        ORR     v1, v1, tmp1, $SLA #8    ; word 1
        STMDB   dst!, {v1, v2, tmp3, lr}
        SUBS    n, n, #16
        BGE     %B0
        ; Reload registers

        LDMFD   sp!, {v1, v2, lr}

        ADDS    n, n, #16-4               ; check for at least 4
        BLT     %F2                       ; < 4 bytes

1       MOV     tmp3, tmp1, $SHA #24      ; last byte
        LDR     tmp1, [src, #-4]!         ; previous four bytes
        ORR     tmp3, tmp3, tmp1, $SLA #8
        STR     tmp3, [dst, #-4]!
        SUBS    n, n, #4
        BGE     %B1                       ; tmp1 contains one byte

        ; Less than four bytes to go - one already in tmp3.

2       ADD     src, src, #1
        B       Down_TrailingBytes

 |      ; old, slow memcpy

__rt_memcpy
        ; a1 = dst address
        ; a2 = src address
        ; a3 = byte count.
        ;
        ; A very slow version of memcpy. Its only merit is that it is quite
        ; small.
        ;
        CMP     a3,#0
        MOVEQ   pc,lr
        CMP     a2,a1
        BLO     CopyDown
        MOVEQ   pc,lr

        MOV     ip,a1
CopyUpLoop
        LDRB    a4,[a2],#1
        STRB    a4,[ip],#1
        SUBS    a3,a3,#1
        BNE     CopyUpLoop

memcpyExit
        MOV     pc,lr

CopyDown
        ADD     a2,a2,a3
        ADD     a1,a1,a3
CopyDownLoop
        LDRB    a4,[a2,#-1]!
        STRB    a4,[a1,#-1]!
        SUBS    a3,a3,#1
        BNE     CopyDownLoop
        BAL     memcpyExit

 ]

        EXPORT __rt_udiv
__rt_udiv
        ; Signed divide of a2 by a1, returns quotient in a1, remainder in a2
        ; Unsigned divide of a2 by a1: returns quotient in a1, remainder in a2
        ; Destroys a3, a4 and ip
        MOVS    a3,a1
        BEQ     __division_by_zero
        MOV     a4,#0
        MOV     ip,#0x80000000
        CMP     a2,# ip
        MOVLO   ip,a2
01
        CMP     ip,a3,ASL #0
        BLS     u_shifted0mod8
        CMP     ip,a3,ASL #1
        BLS     u_shifted1mod8
        CMP     ip,a3,ASL #2
        BLS     u_shifted2mod8
        CMP     ip,a3,ASL #3
        BLS     u_shifted3mod8
        CMP     ip,a3,ASL #4
        BLS     u_shifted4mod8
        CMP     ip,a3,ASL #5
        BLS     u_shifted5mod8
        CMP     ip,a3,ASL #6
        BLS     u_shifted6mod8
        CMP     ip,a3,ASL #7
        MOVHI   a3,a3,ASL #8
        BHI     %BT01
02
u_shifted7mod8
        CMP     a2,a3,ASL #7
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #7
u_shifted6mod8
        CMP     a2,a3,ASL #6
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #6
u_shifted5mod8
        CMP     a2,a3,ASL #5
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #5
u_shifted4mod8
        CMP     a2,a3,ASL #4
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #4
u_shifted3mod8
        CMP     a2,a3,ASL #3
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #3
u_shifted2mod8
        CMP     a2,a3,ASL #2
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #2
u_shifted1mod8
        CMP     a2,a3,ASL #1
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #1
u_shifted0mod8
        CMP     a2,a3,ASL #0
        ADC     a4,a4,a4
        SUBHS   a2,a2,a3,ASL #0
        CMP     a1,a3,LSR #1
        MOVLS   a3,a3,LSR #8
        BLS     %BT02
        MOV     a1,a4
        MOV     pc,lr

__division_by_zero
        ; Currently we do not deal with division by zero in a nice way
        MOV     a1,#0
        MOV     a2,#0
        MOV     pc,lr

        EXPORT  __rt_udiv10
__rt_udiv10     ROUT
        SUB     a2, a1, #10
        SUB     a1, a1, a1, lsr #2
        ADD     a1, a1, a1, lsr #4
        ADD     a1, a1, a1, lsr #8
        ADD     a1, a1, a1, lsr #16
        MOV     a1, a1, lsr #3
        ADD     a3, a1, a1, asl #2
        SUBS    a2, a2, a3, asl #1
        ADDPL   a1, a1, #1
        ADDMI   a2, a2, #10
        MOV     pc, lr

        EXPORT  __rt_memcmp
__rt_memcmp     ROUT
        MOV     a4, a1
        MOVS    a1, a3
        MOVEQ   pc, lr

0       LDRB    a1, [a2], #1
        LDRB    ip, [a4], #1
        SUBS    a1, a1, ip
        MOVNE   pc, lr

        SUBS    a3, a3, #1
        BNE     %0
        MOV     pc, lr

        EXPORT  __rt_strlen
__rt_strlen     ROUT
        MOV     a2, a1
        MOV     a1, #0

0       LDRB    a3, [a2], #1
        CMP     a3, #0
        MOVEQ   pc, lr

        ADD     a1, a1, #1
        B       %0
        
;;      The following routines are currently only needed for the Fusion
;;      TCP/IP stack and Ethernet drivers, so don't include them if we
;;      don't have to

  IF    ETHERNET_SUPPORTED /= 0

        EXPORT  __rt_divtest
__rt_divtest    ROUT
        CMPS    a1, #0
        MOVNE   pc, lr

;;
;;      Fall through to...
;;

__rt_div0       ROUT
        MOV     a1, #angel_SWIreason_ReportException
        LDR     a2, =ADP_Stopped_DivisionByZero
        SWI     angel_SWI_ARM

        ; if we ever continue from this SWI, then report this and die
        FatalError "Divide by zero\n"

  ENDIF ; ETHERNET_SUPPORTED /= 0
  
;;
;; __rt_memXXX routines - small but inefficient
;;
        EXPORT  __rt_memset
__rt_memset     ROUT
        ADD     a3, a3, #1
        MOV     a4, a1

0       SUBS    a3, a3, #1
        MOVEQ   pc, lr
        STRB    a2, [a4], #1
        B       %0

        EXPORT  __rt_strcmp
__rt_strcmp     ROUT
        MOV     ip, a1

0       LDRB    a3, [ip], #1
        LDRB    a4, [a2], #1

        SUBS    a1, a3, a4
        MOVNE   pc, lr

        CMP     a3, #0
        CMPNE   a4, #0
        BNE     %0
        MOV     pc, lr

        EXPORT  __rt_strncmp
__rt_strncmp    ROUT
        MOV     ip, a1
        MOVS    a1, a3
        MOVEQ   pc, lr

        STR     lr, [sp, #-4]!

0       LDRB    lr, [ip], #1
        LDRB    a4, [a2], #1

        SUBS    a1, lr, a4
        LDRNE   pc, [sp], #4

        CMP     lr, #0
        CMPNE   a4, #0
        SUBNES  a3, a3, #1
        BNE     %0
        LDR     pc, [sp], #4

        EXPORT  __rt_strcpy
__rt_strcpy     ROUT
        MOV     ip, a1

0       LDRB    a3, [a2], #1
        STRB    a3, [ip], #1
        CMP     a3, #0
        BNE     %0

        MOV     pc, lr

        EXPORT  __rt_strcat
__rt_strcat     ROUT
        MOV     ip, a1          ; so we can return a1
0
        LDRB    a3, [ip], #1
        CMP     a3, #0
        BNE     %0
        SUB     ip, ip, #1      ; want to overwrite NUL
        
1       LDRB    a3, [a2], #1
        STRB    a3, [ip], #1
        CMP     a3, #0
        BNE     %1
        
        MOV     pc, lr


      IF CACHE_SUPPORTED <> 0
        EXPORT Cache_IBR
        ; Added for EBSARM, Instruction barrier range function. r0
        ; contains start of range, r1 end.
Cache_IBR
        ; Simply call the board dependent macro
        CACHE_IBR r0,r1,r2,r3
        ; And return
        MOV   pc,lr
      ENDIF

        IMPORT  Angel_EnterSVC
        IMPORT  Angel_GlobalRegBlock 
        EXPORT  __rt_uninterruptable_loop
__rt_uninterruptable_loop
      IF DEBUG <> 0 :LAND: LOGTERM_DEBUGGING <> 0
        IMPORT  logterm_fatalhandler

        STMFD   sp!, {r0-r3}
        LDR     r0, =angel_SWIreason_EnterSVC
        SWI     angel_SWI_ARM          ; Get into SVC with IRQ and FIQ disabled
        LDMIA   sp, {r0-r3}
        SAVETASK Angel_GlobalRegBlock + (RB_Fatal * Angel_RegBlockSize), 0
        LDMFD   sp!, {r0-r3}

        SetStack Angel_SVCStackOffset   ; should be: Angel_FatalStackOffset

        ; Rather than sit in an infinite loop, allow memory to be scanned
        B       logterm_fatalhandler
      ENDIF
        
        MRS     a1, CPSR
        AND     a2, a1, #ModeMaskUFIS
        CMP     a2, #USRmode :AND: ModeMaskUFIS
        BLEQ    Angel_EnterSVC
        ORRNE   a1, a1, #InterruptMask
        MSRNE   CPSR_cf, a1
DeadLoop
        B       DeadLoop

        ; This code simulates the LogFatalError macro used in the C parts of
        ; Angel. Enter it with a1 pointing at the error message.
        EXPORT  __rt_asm_fatalerror

__rt_asm_fatalerror
      IF DEBUG = 1

        IMPORT  log_logerror
        IMPORT  log_set_log_id
        IMPORT  Log_logmsginfo

        STMFD   sp!, {r0-r3}

        LDR     r0, =angel_SWIreason_EnterSVC
        SWI     angel_SWI_ARM          ; Get into SVC with IRQ and FIQ disabled

        LDMIA   sp, {r0-r3}

        SAVETASK Angel_GlobalRegBlock + (RB_Fatal * Angel_RegBlockSize), 0
        
        LDMFD   sp!, {r0-r3}

        SetStack Angel_SVCStackOffset   ; should be: Angel_FatalStackOffset

        ; save the initial parameters
        STMFD   sp!, {r0-r3,r12,lr}

        MOV     r0, #0  ; id = 0 -> matches id used in msg info
        MOV     r1, #1  ; onoff -> on
        BL      log_set_log_id

        ;; these variables are used in C code to say where the call came from;
        ;;  we don't have that info to hand for asm.
        MOV     r0, #0  ; file
        MOV     r1, #0  ; line
        MOV     r2, #0  ; id
        BL      Log_logmsginfo
        LDMFD   sp, {r0-r3,r12,lr}

        ; restore initial params -- r0 - ptr to msg; r1/2/3 msg args
        BL      log_logerror
        LDMFD   sp!, {r0-r3,r12,lr}

        ; debug build jumps into DeadLoop, release build restarts Angel.
        B       __rt_uninterruptable_loop

      ELSE

        B       __rt_angel_restart

      ENDIF

        ;; This routine exists as the action used when Angel needs to be
        ;; restarted. There are two occasions; when the debugger asks for
        ;; it, and when Angel detects a fatal error. The routine should
        ;; reload Angel from ROM, if possible, and restart as if a
        ;; power-on-reset had just occurred, as NOTHING can be assumed about
        ;; the current state of anything in RAM.
        ;; 
        ;; THIS ROUTINE NEVER RETURNS!
        
__rt_angel_restart

        IMPORT __rom
        EXPORT __rt_angel_restart

        ;; for the moment, just start at the beginning again...
        B __rom
        
                                
        END     ; EOF suppasm.s