mirror of
https://github.com/rvtr/ntr_bootrom.git
synced 2025-10-31 07:11:11 -04:00
567 lines
15 KiB
ArmAsm
567 lines
15 KiB
ArmAsm
;********************************************************************
|
||
; ＩＲＩＳモニタプログラム サブルーチン群
|
||
;********************************************************************
|
||
|
||
AREA IrisCrt0Sub, CODE, READONLY
|
||
INCLUDE IrisDefineArm.s
|
||
INCLUDE IrisMemoryMapArm.s
|
||
INCLUDE IrisSystemCallDefineArm.s
|
||
|
||
INCLUDE IrisMonDefineArm.s
|
||
INCLUDE IrisMonMempryMapArm.s
|
||
|
||
MON_ONLY
|
||
NDEBUG2
|
||
|
||
|
||
;-----------------------------------------------------------------------
|
||
; ＨＡＬＴ
|
||
;-----------------------------------------------------------------------
|
||
GLOBAL Halt
|
||
CODE32
|
||
Halt
|
||
mov r0, #0
|
||
mcr p15, 0, r0, c7, c0, 4
|
||
bx lr
|
||
|
||
|
||
;-----------------------------------------------------------------------
|
||
; ループ待ち（r0=loopCount）
|
||
;-----------------------------------------------------------------------
|
||
GLOBAL WaitByLoop
|
||
ALIGN
|
||
CODE16
|
||
WaitByLoop
|
||
sub r0, #1
|
||
bgt WaitByLoop
|
||
|
||
bx lr
|
||
|
||
;--------------------------------------------------------------------
|
||
; Ｖブランク割り込みウェイト
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL WaitVBlankIntr
|
||
ALIGN
|
||
CODE32
|
||
WaitVBlankIntr mov r0, #1
|
||
mov r1, #V_BLANK_INTR_FLAG
|
||
;--------------------------------------------------------------------
|
||
; 割り込みウェイト（r0=InitCheckClear, r1=IntrFlag）
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL WaitIntr
|
||
CODE32
|
||
WaitIntr stmfd sp!, {r4, lr} ; r4 はアライメント調整用
|
||
|
||
cmp r0, #0
|
||
blne WaitIntr_sub ; IfCheck ｸﾘｱ
|
||
|
||
0
|
||
mov lr, #0
|
||
mcr p15, 0, lr, c7, c0, 4 ; OS_Halt()
|
||
bl WaitIntr_sub
|
||
beq %b0 ; while (!(IfCheck & IfFlags))
|
||
|
||
ldmfd sp!, {r4, lr}
|
||
bx lr
|
||
|
||
WaitIntr_sub mov r12,#REG_BASE ; r12: #REG_BASE（stopサンプル依存のため書換えないこと！）
|
||
str r12,[r12,#REG_IME - REG_BASE] ; IME = 0
|
||
|
||
mrc p15, 0, r3, c9, c1, 0 ; DTCMｱﾄﾞﾚｽ獲得
|
||
; bic r3, r3, lsr #0xff
|
||
mov r3, r3, lsr #C9_TCMR_BASE_SHIFT
|
||
mov r3, r3, lsl #C9_TCMR_BASE_SHIFT
|
||
add r3, r3, #DTCM_SIZE
|
||
|
||
ldr r2, [r3, #INTR_CHECK_BUF - DTCM_END]
|
||
ands r0, r1, r2 ; IfCheck 判定
|
||
eorne r2, r2, r0 ; IfCheck ｸﾘｱ
|
||
strne r2, [r3, #INTR_CHECK_BUF - DTCM_END]
|
||
|
||
mov r0, #1
|
||
str r0, [r12,#REG_IME - REG_BASE] ; IME = 1
|
||
|
||
bx lr
|
||
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; 汎用符号付き割り算（r0/r1→r0',r0%r1→r1',|r0'|→r3'）
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL DivS32
|
||
ALIGN
|
||
CODE32
|
||
DivS32 ands r3, r1, #0x80000000 ; r1: 除数の絶対値
|
||
rsbmi r1, r1, #0
|
||
eors r12,r3, r0, asr #32 ; r12: d31=結果の符合,d30=被除数の符号
|
||
rsbcs r0, r0, #0 ; r0: 被除数の絶対値
|
||
movs r2, r1 ; r2: コントロール・ビット
|
||
; beq devide_by_zero
|
||
|
||
divide_just_l cmp r2, r0, lsr #1
|
||
movls r2, r2, lsl #1
|
||
blo divide_just_l
|
||
|
||
divide_div_l cmp r0, r2 ; 減算できるかチェック
|
||
adc r3, r3, r3 ; r3*2 + キャリー
|
||
subcs r0, r0, r2 ; 減算可能なら減算
|
||
teq r2, r1 ; 終了チェック
|
||
movne r2, r2, lsr #1 ; 終了でなければ２で割り再チェック
|
||
bne divide_div_l
|
||
mov r1, r0
|
||
mov r0, r3
|
||
|
||
movs r12,r12,lsl #1
|
||
rsbcs r0, r0, #0
|
||
rsbmi r1, r1, #0
|
||
|
||
bx lr
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; 平方根算出（r0=param）
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL SqrtU32
|
||
ALIGN
|
||
CODE32
|
||
SqrtU32 stmfd sp!, {r4, r5} ; r5 はアライメント調整用
|
||
|
||
mov r12,r0 ; r12: ParamBak
|
||
mov r1, #1
|
||
|
||
1 cmp r0, r1
|
||
movhi r0, r0, lsr #1
|
||
movhi r1, r1, lsl #1
|
||
bhi %b1
|
||
|
||
2 mov r0, r12 ; r0: 被除数
|
||
mov r4, r1 ; r1: 除数
|
||
mov r3, #0 ; r3: 商
|
||
mov r2, r1 ; r2: コントロール・ビット
|
||
|
||
3 cmp r2, r0, lsr #1
|
||
movls r2, r2, lsl #1
|
||
blo %b3
|
||
|
||
4 cmp r0, r2 ; 減算できるかチェック
|
||
adc r3, r3, r3 ; r3*2 + キャリー
|
||
subcs r0, r0, r2 ; 減算可能なら減算
|
||
teq r2, r1 ; 終了チェック
|
||
movne r2, r2, lsr #1 ; 終了でなければ２で割り再チェック
|
||
bne %b4
|
||
|
||
add r1, r1, r3
|
||
movs r1, r1, lsr #1
|
||
cmp r1, r4
|
||
bcc %b2
|
||
|
||
mov r0, r4
|
||
ldmfd sp!, {r4, r5}
|
||
bx lr
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; ＣＰＵ セット (r0=srcp, r1=destp, r2=dmaCntData)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL CpuSet16_32
|
||
CODE16
|
||
CpuSet16_32 push {r4-r6, lr} ; r6 はアライメント調整用
|
||
|
||
lsl r4, r2, #11 ; r4: size = (dmaCntData & 0x1fffff)*4
|
||
lsr r4, #11-2
|
||
|
||
; bl CheckSrcp16_32 ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f6
|
||
|
||
0 mov r5, #0 ; r5: i = 0
|
||
|
||
lsr r3, r2, #27 ; if ((dmaCntData) & DMA_32BIT_BUS)
|
||
bcc %f3
|
||
|
||
add r5, r1, r4 ; r5: destEndp = destp + size
|
||
|
||
lsr r3, r2, #25 ; if ((dmaCntData) & DMA_SRC_FIX)
|
||
bcc %f2
|
||
|
||
ldmia r0!, {r3}
|
||
1 cmp r1, r5 ; while (destp < destEndp)
|
||
bge %f6
|
||
stmia r1!, {r3} ; *((vu32 *)(destp))++ = *((vu32 *)(srcp))
|
||
b %b1
|
||
|
||
2 cmp r1, r5 ; while (destp < destEndp)
|
||
bge %f6
|
||
ldmia r0!, {r3} ; *((vu32 *)(destp))++ = *((vu32 *)(srcp))++
|
||
stmia r1!, {r3}
|
||
b %b2
|
||
|
||
3 lsr r4, #1
|
||
|
||
lsr r3, r2, #25 ; if ((dmaCntData) & DMA_SRC_FIX)
|
||
bcc %f5
|
||
|
||
ldrh r3, [r0, #0]
|
||
4 cmp r5, r4 ; for ( ; i<count; i++)
|
||
bge %f6
|
||
strh r3, [r1, r5] ; ((vu16 *)(destp))[i] = ((vu16 *)(srcp))[0]
|
||
add r5, #2
|
||
b %b4
|
||
|
||
5 cmp r5, r4 ; for ( ; i<count; i++)
|
||
bge %f6
|
||
ldrh r3, [r0, r5] ; ((vu16 *)(destp))[i] = ((vu16 *)(srcp))[i]
|
||
strh r3, [r1, r5]
|
||
add r5, #2
|
||
b %b5
|
||
|
||
6 pop {r4, r5}
|
||
pop {r2, r3}
|
||
bx r3
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; ＣＰＵ 高速セット(32Byte) (r0=srcp, r1=destp, r2=dmaCntData)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL CpuSetFast32_T
|
||
GLOBAL CpuSetFast32
|
||
CODE16
|
||
CpuSetFast32_T mov r3, pc
|
||
bx r3
|
||
ALIGN
|
||
CODE32
|
||
CpuSetFast32 stmfd sp!, {r4-r10, lr}
|
||
|
||
mov r10, r2, lsl #11 ; r10: count = (dmaCntData & 0x1fffff)*4
|
||
|
||
; movs r12, r10, lsr #11-2 ; r12: size = count * 4
|
||
; bl CheckSrcp ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f5
|
||
|
||
add lr, r1, r10, lsr #11-2 ; lr: destEndp = destp + size
|
||
mov r10, r10, lsr #11+3 ; r10: blockEndp = srcp + size/32*32
|
||
add r10, r1, r10,lsl #3
|
||
|
||
0 movs r2, r2, lsr #25 ; if ((dmaCntData) & DMA_SRC_FIX)
|
||
bcc %f3
|
||
|
||
ldr r2, [r0, #0]
|
||
mov r3, r2
|
||
mov r4, r2
|
||
mov r5, r2
|
||
mov r6, r2
|
||
mov r7, r2
|
||
mov r8, r2
|
||
mov r9, r2
|
||
1 cmp r1, r10 ; while (destp < destEndp)
|
||
stmltia r1!, {r2-r9} ; *((vu32 *)(destp++)) = *((vu32 *)(srcp))
|
||
blt %b1
|
||
2 cmp r1, lr
|
||
stmltia r1!, {r2}
|
||
blt %b2
|
||
b %f5
|
||
|
||
3 cmp r1, r10 ; while (destp < destEndp)
|
||
ldmltia r0!, {r2-r9} ; *((vu32 *)(destp)++) = *((vu32 *)(srcp)++)
|
||
stmltia r1!, {r2-r9}
|
||
blt %b3
|
||
4 cmp r1, lr
|
||
ldmltia r0!, {r2}
|
||
stmltia r1!, {r2}
|
||
blt %b4
|
||
|
||
5 ldmfd sp!, {r4-r10, lr}
|
||
bx lr
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; Bit圧縮データ展開 (r0=srcp, r1=destp, r2=unPackBitsParamp)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL UnPackBits32
|
||
ALIGN
|
||
CODE32
|
||
UnPackBits32
|
||
stmfd sp!, {r4-r12, lr} ; r12 はアライメント調整用
|
||
sub sp, sp, #8
|
||
|
||
ldrh r7, [r2, #0] ; r7: srcNum = unPackBitsParamp->srcNum
|
||
|
||
; movs r12, r7 ; r12: srcCount
|
||
; bl CheckSrcp ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f6
|
||
|
||
0 ldrb r6, [r2, #2] ; r6: srcBitNum = unPackBitsParamp->srcBitNum
|
||
rsb r10, r6, #8 ; r10: srcBitNumInv = 8 - srcBitNum
|
||
mov r14, #0 ; r14: destBak = 0
|
||
ldr r11, [r2, #4] ; r8: destOffset0_On
|
||
mov r8, r11, lsr #31 ; = unPackBitsParamp->destOffset0_On
|
||
ldr r11,[r2, #4] ; destOffset = unPackBitsParamp->destOffset
|
||
mov r11,r11, lsl #1
|
||
mov r11,r11, lsr #1
|
||
str r11,[sp, #0]
|
||
ldrb r2, [r2, #3] ; r2: destBitNum = unPackBitsParamp->destBitNum
|
||
mov r3, #0 ; r3: destBitCount = 0
|
||
|
||
1 subs r7, r7, #1 ; while (--srcNum >= 0)
|
||
blt %f6
|
||
|
||
mov r11, #0xff ; r5: srcMask = 0xff >> srcBitNumInv
|
||
mov r5, r11, asr r10
|
||
ldrb r9, [r0], #1 ; r9: srcTmp = *srcp++
|
||
mov r4, #0 ; r4: srcBitCount = 0
|
||
|
||
2 cmp r4, #8 ; while (srcBitCount < 8)
|
||
bge %b1
|
||
|
||
and r11, r9, r5 ; r12: destTmp = ((srcTmp&srcMask) >>srcBitCount)
|
||
movs r12, r11, lsr r4
|
||
cmpeq r8, #0
|
||
beq %f4
|
||
|
||
3 ldr r11, [sp, #0] ; destTmp += destOffset
|
||
add r12, r12, r11
|
||
4 orr r14, r14, r12, lsl r3 ; destBak |= destTmp << destBitCount
|
||
add r3, r3, r2 ; destBitCount += destBitNum
|
||
|
||
cmp r3, #0x20 ; if (destBitCount >= 32)
|
||
blt %f5
|
||
|
||
str r14, [r1], #4 ; *destp++ = destBak
|
||
mov r14, #0 ; destBak = 0
|
||
mov r3, #0 ; destBitCount = 0
|
||
|
||
5 mov r5, r5, lsl r6 ; srcMask <<= srcBitNum
|
||
add r4, r4, r6 ; srcBitCount += srcBitNum
|
||
b %b2
|
||
|
||
6 add sp, sp, #8
|
||
ldmfd sp!, {r4-r12, lr}
|
||
bx lr
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; ＬＺ７７データ展開（8Bit→8Bit）(r0=srcp, r1=destp)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL UnCompLZ77Byte
|
||
ALIGN
|
||
CODE32
|
||
UnCompLZ77Byte
|
||
stmfd sp!, {r4-r6, lr}
|
||
|
||
ldr r5, [r0], #4 ; r2: destCount = *(u32 *)srcp >> 8
|
||
mov r2, r5, lsr #8 ; r0: srcp += 4
|
||
|
||
; movs r12, r2 ; r12: srcCount
|
||
; bl CheckSrcp ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f6
|
||
|
||
1 cmp r2, #0 ; while (destCount > 0)
|
||
ble %f6
|
||
|
||
ldrb r14, [r0], #1 ; r14: flags = *srcp++
|
||
mov r4, #8 ; for (i=8; --i>=0; )
|
||
2 subs r4, r4, #1
|
||
blt %b1
|
||
|
||
tst r14, #0x80 ; if (!(flags & 0x80))
|
||
bne %f3
|
||
|
||
ldrb r6, [r0], #1 ; *srcp++
|
||
swpb r6, r6, [r1] ; r1: *destp++（バイト書き込み対策）
|
||
add r1, r1, #1
|
||
sub r2, r2, #1 ; destCount--
|
||
b %f5
|
||
|
||
3 ldrb r5, [r0, #0] ; r3: length = (*srcp >> 4) + 3
|
||
mov r6, #3
|
||
add r3, r6, r5, asr #4
|
||
ldrb r6, [r0], #1 ; r12: offset = (*srcp++ & 0x0f) << 8
|
||
and r5, r6, #0xf
|
||
mov r12,r5, lsl #8
|
||
ldrb r6, [r0], #1 ; offset = (offset | *srcp++) + 1
|
||
orr r5, r6, r12
|
||
add r12, r5, #1
|
||
sub r2, r2, r3 ; destCount -= length
|
||
|
||
4 ldrb r5, [r1, -r12] ; *destp++ = destp[-offset]
|
||
swpb r5, r5, [r1] ; （バイト書き込み対策）
|
||
add r1, r1, #1
|
||
subs r3, r3, #1 ; while (--length > 0)
|
||
bgt %b4
|
||
|
||
5 cmp r2, #0 ; if (destCount <= 0) break;
|
||
movgt r14, r14, lsl #1 ; flags <<= 1
|
||
bgt %b2
|
||
b %b1
|
||
|
||
6 ldmfd sp!, {r4-r6, lr}
|
||
bx lr
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; ランレングスデータ展開（8Bit→8Bit）(r0=srcp, r1=destp)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL UnCompRLByte
|
||
CODE16
|
||
UnCompRLByte
|
||
push {r7, lr} ; r7 はアライメント調整用
|
||
push {r4-r7}
|
||
|
||
ldmia r0!, {r3} ; r7: destCount = *(u32 *)srcp >> 8
|
||
lsr r7, r3, #8 ; r0: srcp += 4
|
||
|
||
; mov r4, r7 ; r4: count
|
||
; bl CheckSrcp16_32 ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f5
|
||
|
||
1 cmp r7, #0 ; while (destCount > 0)
|
||
ble %f5
|
||
|
||
ldrb r4, [r0, #0] ; r4: flags = *srcp++
|
||
add r0, #1
|
||
lsl r2, r4, #25 ; r2: length = flags & 0x7f
|
||
lsr r2, #25
|
||
lsr r3, r4, #8 ; if (!(flags & 0x80))
|
||
bcs %f3
|
||
|
||
add r2, #1 ; length++
|
||
sub r7, r2 ; destCount -= length
|
||
2 ldrb r3, [r0, #0] ; *destp++ = *srcp++
|
||
strb r3, [r1, #0]
|
||
add r0, #1
|
||
add r1, #1
|
||
sub r2, r2, #1 ; while (--length > 0)
|
||
bgt %b2
|
||
b %b1
|
||
|
||
3 add r2, #3 ; length += 3
|
||
sub r7, r2 ; destCount -= length
|
||
ldrb r5, [r0, #0] ; srcTmp = *srcp++
|
||
add r0, #1
|
||
4 strb r5, [r1, #0] ; *destp++ = srcTmp
|
||
add r1, #1
|
||
sub r2, r2, #1 ; while (--length > 0)
|
||
bgt %b4
|
||
b %b1
|
||
|
||
5 pop {r4-r7}
|
||
pop {r2, r3}
|
||
bx r3
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; 差分フィルタ展開（8Bit→8Bit） (r0=srcp, r1=destp)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL UnDiffByte2Byte
|
||
CODE16
|
||
UnDiffByte2Byte
|
||
push {r4, lr}
|
||
push {r4, r5} ; アライメント調整用
|
||
|
||
ldmia r0!, {r4} ; r4: destCount = *(u32 *)srcp >> 8
|
||
lsr r4, r4, #8 ; r0: srcp += 4
|
||
|
||
; bl CheckSrcp16_32 ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f2
|
||
|
||
ldrb r2, [r0, #0] ; r2: destTmp = *srcp++
|
||
add r0, #1
|
||
strb r2, [r1, #0] ; r1: *destp++ = destTmp
|
||
add r1, #1
|
||
|
||
1 sub r4, r4, #1 ; while (--destCount > 0)
|
||
ble %f2
|
||
ldrb r3, [r0, #0] ; destTmp += *srcp++
|
||
add r2, r3, r2
|
||
add r0, #1
|
||
strb r2, [r1, #0] ; *destp++ = destTmp
|
||
add r1, #1
|
||
b %b1
|
||
|
||
2 pop {r4, r5}
|
||
pop {r2, r3}
|
||
bx r3
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; 差分フィルタ展開（16Bit→16Bit） (r0=srcp, r1=destp)
|
||
;--------------------------------------------------------------------
|
||
; IF :DEF: SYSCALL_ASM
|
||
GLOBAL UnDiffShort2Short
|
||
CODE16
|
||
UnDiffShort2Short
|
||
push {r4, lr}
|
||
push {r4, r5} ; アライメント調整用
|
||
|
||
ldmia r0!, {r4} ; r4: destCount = *(u32 *)srcp >> 8
|
||
lsr r4, r4, #8 ; r0: srcp += 2
|
||
|
||
; bl CheckSrcp16_32 ; ｿｰｽｱﾄﾞﾚｽ ﾁｪｯｸ
|
||
; beq %f2
|
||
|
||
ldrh r2, [r0, #0] ; r2: destTmp = *srcp++
|
||
add r0, #2
|
||
strh r2, [r1, #0] ; r1: *destp++ = destTmp
|
||
add r1, #2
|
||
|
||
1 sub r4, r4, #2 ; while ((destCount-=2) > 0)
|
||
ble %f2
|
||
|
||
ldrh r3, [r0, #0] ; destTmp += *srcp++
|
||
add r2, r3, r2
|
||
add r0, #2
|
||
strh r2, [r1, #0] ; *destp++ = destTmp
|
||
add r1, #2
|
||
b %b1
|
||
|
||
2 pop {r4, r5}
|
||
pop {r2, r3}
|
||
bx r3
|
||
; ENDIF
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; r3レジスタによるルーチン呼び出し
|
||
;--------------------------------------------------------------------
|
||
GLOBAL __call_via_r3
|
||
CODE16
|
||
__call_via_r3 bx r3
|
||
|
||
|
||
;--------------------------------------------------------------------
|
||
; r2レジスタによるルーチン呼び出し
|
||
;--------------------------------------------------------------------
|
||
GLOBAL __call_via_r2
|
||
CODE16
|
||
__call_via_r2 bx r2
|
||
|
||
;--------------------------------------------------------------------
|
||
; r1レジスタによるルーチン呼び出し
|
||
;--------------------------------------------------------------------
|
||
GLOBAL __call_via_r1
|
||
CODE16
|
||
__call_via_r1 bx r1
|
||
|
||
|
||
|
||
END
|
||
|