diff options
Diffstat (limited to 'arm7')
-rw-r--r-- | arm7/Makefile | 1 | ||||
-rw-r--r-- | arm7/arm7.lsf | 2 | ||||
-rw-r--r-- | arm7/asm/CARD_pullOut.s | 1 | ||||
-rw-r--r-- | arm7/asm/CTRDG_sp.s | 542 | ||||
-rw-r--r-- | arm7/asm/OS_mutex.s | 131 | ||||
-rw-r--r-- | arm7/asm/wram2.s | 494 | ||||
-rw-r--r-- | arm7/asm/wram_bss.s | 32 | ||||
-rw-r--r-- | arm7/asm/wram_data.s | 13 | ||||
-rw-r--r-- | arm7/lib/include/MI_memory.h | 10 | ||||
-rw-r--r-- | arm7/lib/include/OS_alarm.h | 9 | ||||
-rw-r--r-- | arm7/lib/include/OS_context.h | 4 | ||||
-rw-r--r-- | arm7/lib/include/OS_mutex.h | 14 | ||||
-rw-r--r-- | arm7/lib/include/OS_thread.h | 29 | ||||
-rw-r--r-- | arm7/lib/include/consts.h | 1 | ||||
-rw-r--r-- | arm7/lib/include/systemWork.h | 7 | ||||
-rw-r--r-- | arm7/lib/src/OS_mutex.c | 95 | ||||
-rw-r--r-- | arm7/lib/src/OS_thread.c | 597 |
17 files changed, 1309 insertions, 673 deletions
diff --git a/arm7/Makefile b/arm7/Makefile index 1ade0ed4..1b0cd3c3 100644 --- a/arm7/Makefile +++ b/arm7/Makefile @@ -196,7 +196,6 @@ DUMMY != mkdir -p $(ALL_DIRS) %.pal: ; # Included files -baserom.%: ; %.h: ; %.inc: ; diff --git a/arm7/arm7.lsf b/arm7/arm7.lsf index 038c37c5..cbffa45a 100644 --- a/arm7/arm7.lsf +++ b/arm7/arm7.lsf @@ -68,8 +68,8 @@ Autoload WRAM Object SPI_pm.o Object SPI_mic.o Object CTRDG.o + Object CTRDG_sp.o Object wram2.o - Object wram_data.o Object wram_bss.o } diff --git a/arm7/asm/CARD_pullOut.s b/arm7/asm/CARD_pullOut.s index ed919647..b4d52c6c 100644 --- a/arm7/asm/CARD_pullOut.s +++ b/arm7/asm/CARD_pullOut.s @@ -215,4 +215,3 @@ _038010C0: bx lr _038010CC: .word _038099A4 _038010D0: .word FUN_03801048 - .balign 2, 0 ; Don't pad with nop diff --git a/arm7/asm/CTRDG_sp.s b/arm7/asm/CTRDG_sp.s new file mode 100644 index 00000000..72c502c3 --- /dev/null +++ b/arm7/asm/CTRDG_sp.s @@ -0,0 +1,542 @@ + .include "asm/macros.inc" + .include "global.inc" + + .section .data + + .global _03807704 +_03807704: + .word 0x00000001 + + .global _03807708 +_03807708: + .word 0xFFFFFFFF + + .section .bss + + .global _03809FBC +_03809FBC: ;0x03809FBC + .space 0x03809FC0 - 0x03809FBC + + .global current_vib +current_vib: ;0x03809FC0 + .space 0x03809FC4 - 0x03809FC0 + + .global _03809FC4 +_03809FC4: ;0x03809FC4 + .space 0x03809FC8 - 0x03809FC4 + + .global _03809FC8 +_03809FC8: ;0x03809FC8 + .space 0x03809FCC - 0x03809FC8 + + .global _03809FCC +_03809FCC: ;0x03809FCC + .space 0x03809FD0 - 0x03809FCC + + .global _03809FD0 +_03809FD0: ;0x03809FD0 + .space 0x03809FD4 - 0x03809FD0 + + .global _03809FD4 +_03809FD4: ;0x03809FD4 + .space 0x03809FEC - 0x03809FD4 + + .global _03809FEC +_03809FEC: ;0x03809FEC + .space 0x0380A018 - 0x03809FEC + + .section .text + + arm_func_start FUN_03803BD4 +FUN_03803BD4: ;@ 0x03803BD4 + stmdb sp!, {r4, r5, r6, r7, lr} + sub sp, sp, #4 + and r0, r1, #63 ;@ 0x3f + cmp r0, #3 + bne _03803C40 + ldr r0, _03803C50 ;@ =0x01FFFFC0 + and r0, r1, r0 + mov r2, r0, lsr #6 + ldr r1, _03803C54 ;@ =0x04000204 + ldrh r0, [r1] + bic r0, r0, #96 ;@ 0x60 + orr r0, r0, r2, lsl #5 + strh r0, [r1] + mov r7, #1 + mov r6, #17 + mov r5, #18 + mov r4, #0 + b _03803C24 +_03803C1C: + mov r0, r7 + bl FUN_037F8CB4 +_03803C24: + mov r0, r6 + mov r1, r5 + mov r2, r4 + bl PXI_SendWordByFifo + cmp r0, #0 + bne _03803C1C + b _03803C44 +_03803C40: + bl OS_Terminate +_03803C44: + add sp, sp, #4 + ldmia sp!, {r4, r5, r6, r7, lr} + bx lr +_03803C50: .word 0x01FFFFC0 +_03803C54: .word 0x04000204 + + arm_func_start CTRDGi_TerminateARM7 +CTRDGi_TerminateARM7: ;@ 0x03803C58 + stmfd sp!, {lr} + sub sp, sp, #4 + mov r0, #0 + bl CTRDG_VibPulseEdgeUpdate + bl SND_BeginSleep + bl WVR_Shutdown + bl OS_Terminate + add sp, sp, #4 + ldmia sp!, {lr} + bx lr + + arm_func_start CTRDG_CheckPullOut_Polling +CTRDG_CheckPullOut_Polling: ;@ 0x03803C80 + stmdb sp!, {r4, r5, r6, r7, lr} + sub sp, sp, #4 + ldr r1, _03803D80 ;@ =_03807708 + ldr r3, [r1] + mvn r0, #0 + cmp r3, r0 + ldreq r0, _03803D84 ;@ =0x027FFC3C + ldreq r0, [r0] + addeq r0, r0, #10 + streq r0, [r1] + beq _03803D74 + ldr r0, _03803D88 ;@ =_03809FCC + ldr r0, [r0] + cmp r0, #0 + bne _03803D74 + ldr r0, _03803D8C ;@ =_03809FC8 + ldr r0, [r0] + cmp r0, #0 + bne _03803D74 + ldr r2, _03803D84 ;@ =0x027FFC3C + ldr r0, [r2] + cmp r0, r3 + bcc _03803D74 + ldr r0, [r2] + add r0, r0, #10 + str r0, [r1] + bl CTRDG_IsPulledOut + ldr r1, _03803D8C ;@ =_03809FC8 + str r0, [r1] + bl CTRDG_IsExisting + cmp r0, #0 + bne _03803D28 + ldr r0, _03803D90 ;@ =_03807704 + ldr r0, [r0] + cmp r0, #0 + movne r1, #1 + ldrne r0, _03803D88 ;@ =_03809FCC + strne r1, [r0] + bne _03803D74 + mov r1, #1 + ldr r0, _03803D8C ;@ =_03809FC8 + str r1, [r0] +_03803D28: + mov r7, #0 + ldr r0, _03803D90 ;@ =_03807704 + str r7, [r0] + ldr r0, _03803D8C ;@ =_03809FC8 + ldr r0, [r0] + cmp r0, #0 + beq _03803D74 + mov r6, #100 ;@ 0x64 + mov r5, #13 + mov r4, #17 + b _03803D5C +_03803D54: + mov r0, r6 + bl OS_Sleep +_03803D5C: + mov r0, r5 + mov r1, r4 + mov r2, r7 + bl PXI_SendWordByFifo + cmp r0, #0 + bne _03803D54 +_03803D74: + add sp, sp, #4 + ldmia sp!, {r4, r5, r6, r7, lr} + bx lr +_03803D80: .word _03807708 +_03803D84: .word 0x027FFC3C +_03803D88: .word _03809FCC +_03803D8C: .word _03809FC8 +_03803D90: .word _03807704 + + arm_func_start CTRDGi_IsNinLogoOfAgb +CTRDGi_IsNinLogoOfAgb: ;@ 0x03803D94 + stmdb sp!, {r4, r5, r6, r7, r8, lr} + mov r7, r0 + mov r0, #134217728 ;@ 0x8000000 + add r6, r0, #4 + mov r4, #1 + bl OS_GetLockID + mov r0, r0, lsl #16 + mov r5, r0, lsr #16 + mov r0, r5 + bl OS_LockCartridge + mov r1, #0 + ldr r0, _03803E3C ;@ =0x0000FFFF + eor r2, r0, #3 + mov r2, r2, lsl #16 + mov lr, r2, lsr #16 + eor r2, r0, #132 ;@ 0x84 + mov r2, r2, lsl #16 + mov r8, r2, lsr #16 + b _03803E18 +_03803DE0: + mov ip, r0 + cmp r1, #76 ;@ 0x4c + moveq ip, r8 + beq _03803DF8 + cmp r1, #77 ;@ 0x4d + moveq ip, lr +_03803DF8: + mov r3, r1, lsl #1 + ldrh r2, [r7, r3] + and ip, ip, r2 + ldrh r2, [r6, r3] + cmp ip, r2 + movne r4, #0 + bne _03803E20 + add r1, r1, #1 +_03803E18: + cmp r1, #78 ;@ 0x4e + blt _03803DE0 +_03803E20: + mov r0, r5 + bl OS_UnLockCartridge + mov r0, r5 + bl OS_ReleaseLockID + mov r0, r4 + ldmia sp!, {r4, r5, r6, r7, r8, lr} + bx lr +_03803E3C: .word 0x0000FFFF + + arm_func_start FUN_03803E40 +FUN_03803E40: ;@ 0x03803E40 + ldr r1, _03803E54 ;@ =current_vib + str r0, [r1] + ldr r1, _03803E58 ;@ =0x08001000 + strh r0, [r1] + bx lr +_03803E54: .word current_vib +_03803E58: .word 0x08001000 + + arm_func_start CTRDG_VibPulseEdgeUpdate +CTRDG_VibPulseEdgeUpdate: ;@ 0x03803E5C + stmdb sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr} + sub sp, sp, #4 + movs r5, r0 + beq _03803E9C + ldr r0, [r5] + cmp r0, #0 + bne _03803E9C + ldr r0, [r5, #68] ;@ 0x44 + add r0, r0, #1 + str r0, [r5, #68] ;@ 0x44 + ldr r1, [r5, #64] ;@ 0x40 + cmp r1, #0 + beq _03803E9C + ldr r0, [r5, #68] ;@ 0x44 + cmp r0, r1 + movhi r5, #0 +_03803E9C: + cmp r5, #0 + beq _03803EB0 + ldr r0, [r5, #60] ;@ 0x3c + cmp r0, #0 + bne _03803F48 +_03803EB0: + bl OS_DisableInterrupts + mov r9, r0 + ldr r0, _03804068 ;@ =current_vib + ldr r0, [r0] + cmp r0, #2 + bne _03803F34 + mov r8, #0 + ldr r7, _0380406C ;@ =0x027FFFE8 + ldr r4, _03804070 ;@ =_03809FBC + ldr fp, _03804074 ;@ =0x000080E8 + mov r6, r8 + mov r5, #1 + b _03803F2C +_03803EE4: + mov r0, r7 + bl OS_ReadOwnerOfLockWord + ands sl, r0, #128 ;@ 0x80 + bne _03803F04 + ldrh r0, [r4] + bl OS_TryLockCartridge + cmp r0, #0 + bne _03803F24 +_03803F04: + mov r0, r6 + bl FUN_03803E40 + mov r8, r5 + cmp sl, #0 + bne _03803F2C + ldrh r0, [r4] + bl OS_UnlockCartridge + b _03803F2C +_03803F24: + mov r0, fp + bl OS_SpinWait +_03803F2C: + cmp r8, #0 + beq _03803EE4 +_03803F34: + ldr r0, _03804078 ;@ =_03809FEC + bl OS_CancelAlarm + mov r0, r9 + bl OS_RestoreInterrupts + b _0380405C +_03803F48: + cmp r5, #0 + beq _0380405C + ldr r0, _0380406C ;@ =0x027FFFE8 + bl OS_ReadOwnerOfLockWord + ands r4, r0, #128 ;@ 0x80 + bne _03803F74 + ldr r0, _03804070 ;@ =_03809FBC + ldrh r0, [r0] + bl OS_TryLockCartridge + cmp r0, #0 + bne _03804044 +_03803F74: + ldr r1, [r5] + ldr r0, [r5, #4] + cmp r1, r0 + bne _03803FB0 + mov r0, #0 + bl FUN_03803E40 + str r5, [sp] + ldr r0, _03804078 ;@ =_03809FEC + ldr r1, [r5, #8] + mov r2, #0 + ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate + bl OS_SetAlarm + mov r0, #0 + str r0, [r5] + b _0380402C +_03803FB0: + ands r0, r1, #1 + beq _03803FF4 + mov r0, #0 + bl FUN_03803E40 + str r5, [sp] + ldr r0, _03804078 ;@ =_03809FEC + ldr r1, [r5] + mov r1, r1, lsr #1 + add r1, r5, r1, lsl #2 + ldr r1, [r1, #36] ;@ 0x24 + mov r2, #0 + ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate + bl OS_SetAlarm + ldr r0, [r5] + add r0, r0, #1 + str r0, [r5] + b _0380402C +_03803FF4: + mov r0, #2 + bl FUN_03803E40 + str r5, [sp] + ldr r0, _03804078 ;@ =_03809FEC + ldr r1, [r5] + mov r1, r1, lsr #1 + add r1, r5, r1, lsl #2 + ldr r1, [r1, #12] + mov r2, #0 + ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate + bl OS_SetAlarm + ldr r0, [r5] + add r0, r0, #1 + str r0, [r5] +_0380402C: + cmp r4, #0 + bne _0380405C + ldr r0, _03804070 ;@ =_03809FBC + ldrh r0, [r0] + bl OS_UnlockCartridge + b _0380405C +_03804044: + str r5, [sp] + ldr r0, _03804078 ;@ =_03809FEC + ldr r1, _03804080 ;@ =0x0000020B + mov r2, #0 + ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate + bl OS_SetAlarm +_0380405C: + add sp, sp, #4 + ldmia sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr} + bx lr +_03804068: .word current_vib +_0380406C: .word 0x027FFFE8 +_03804070: .word _03809FBC +_03804074: .word 0x000080E8 +_03804078: .word _03809FEC +_0380407C: .word CTRDG_VibPulseEdgeUpdate +_03804080: .word 0x0000020B +_03804084: + mov r0, r1 + ldr ip, _03804090 ;@ =CTRDG_VibPulseEdgeUpdate + bx ip +_03804090: .word CTRDG_VibPulseEdgeUpdate + + arm_func_start CTRDGi_CallbackForPulledOut +CTRDGi_CallbackForPulledOut: ;@ 0x03804094 + stmfd sp!, {lr} + sub sp, sp, #4 + and r0, r1, #63 ;@ 0x3ff + cmp r0, #2 + bne _038040B0 + bl CTRDGi_TerminateARM7 + b _038040B4 +_038040B0: + bl OS_Terminate +_038040B4: + add sp, sp, #4 + ldmia sp!, {lr} + bx lr + + arm_func_start CTRDGi_CallbackForInitModuleInfo +CTRDGi_CallbackForInitModuleInfo: ;@ 0x038040C0 + stmfd sp!, {lr} + sub sp, sp, #4 + and r0, r1, #63 ;@ 0x3f + cmp r0, #1 + ldreq r0, _038040F4 ;@ =_03809FD4 + streq r1, [r0] + moveq r1, #1 + streq r1, [r0, #16] + beq _038040E8 + bl OS_Terminate +_038040E8: + add sp, sp, #4 + ldmia sp!, {lr} + bx lr +_038040F4: .word _03809FD4 + + arm_func_start CTRDGi_InitModuleInfo +CTRDGi_InitModuleInfo: ;@ 0x038040F8 + stmdb sp!, {r4, r5, r6, r7, lr} + sub sp, sp, #4 + ldr r0, _038041C8 ;@ =_03809FC4 + ldr r1, [r0] + cmp r1, #0 + bne _038041BC + mov r1, #1 + str r1, [r0] + ldr r0, _038041CC ;@ =0x04000300 + ldrh r0, [r0] + ands r0, r0, #1 + beq _038041BC + mov r0, #262144 ;@ 0x40000 + bl OS_SetIrqMask + mov r5, r0 + ldr r1, _038041D0 ;@ =0x04000208 + ldrh r4, [r1] + mov r0, #1 + strh r0, [r1] + mov r7, #256 ;@ 0x100 + ldr r6, _038041D4 ;@ =_03809FD4 + b _03804158 +_03804150: + mov r0, r7 + bl FUN_037F8CB4 +_03804158: + ldr r0, [r6, #16] + cmp r0, #1 + bne _03804150 + ldr r1, [r6] + ldr r0, _038041D8 ;@ =0x01FFFFC0 + and r0, r1, r0 + mov r0, r0, lsr #6 + mov r0, r0, lsl #5 + add r0, r0, #33554432 ;@ 0x2000000 + add r0, r0, #4 + bl CTRDGi_IsNinLogoOfAgb + ldr r2, _038041DC ;@ =0x027FFC30 + ldrb r1, [r2, #5] + bic r1, r1, #1 + and r0, r0, #255 ;@ 0xff + and r0, r0, #1 + orr r0, r1, r0 + strb r0, [r2, #5] + mov r0, #1 + bl CTRDGi_SendtoPxi + ldr r1, _038041D0 ;@ =0x04000208 + ldrh r0, [r1] + strh r4, [r1] + mov r0, r5 + bl OS_SetIrqMask +_038041BC: + add sp, sp, #4 + ldmia sp!, {r4, r5, r6, r7, lr} + bx lr +_038041C8: .word _03809FC4 +_038041CC: .word 0x04000300 +_038041D0: .word 0x04000208 +_038041D4: .word _03809FD4 +_038041D8: .word 0x01FFFFC0 +_038041DC: .word 0x027FFC30 + + arm_func_start CTRDG_Init +CTRDG_Init: ;@ 0x038041E0 + stmfd sp!, {lr} + sub sp, sp, #4 + bl OS_InitTick + bl OS_InitAlarm + ldr r0, _03804270 ;@ =_03809FEC + bl OS_CreateAlarm + ldr r0, _03804274 ;@ =_03809FD0 + ldr r1, [r0] + cmp r1, #0 + bne _03804264 + mov r1, #1 + str r1, [r0] + bl CTRDGi_InitCommon + bl OS_GetLockID + mvn r1, #2 + cmp r0, r1 + beq _03804264 + ldr r1, _03804278 ;@ =_03809FBC + strh r0, [r1] + bl PXI_Init + mov r0, #13 + ldr r1, _0380427C ;@ =CTRDGi_CallbackForInitModuleInfo + bl PXI_SetFifoRecvCallback + bl CTRDGi_InitModuleInfo + mov r0, #13 + ldr r1, _03804280 ;@ =CTRDGi_CallbackForPulledOut + bl PXI_SetFifoRecvCallback + mov r0, #16 + ldr r1, _03804284 ;@ =_03804084 + bl PXI_SetFifoRecvCallback + mov r0, #17 + ldr r1, _03804288 ;@ =FUN_03803BD4 + bl PXI_SetFifoRecvCallback +_03804264: + add sp, sp, #4 + ldmia sp!, {lr} + bx lr +_03804270: .word _03809FEC +_03804274: .word _03809FD0 +_03804278: .word _03809FBC +_0380427C: .word CTRDGi_CallbackForInitModuleInfo +_03804280: .word CTRDGi_CallbackForPulledOut +_03804284: .word _03804084 +_03804288: .word FUN_03803BD4 diff --git a/arm7/asm/OS_mutex.s b/arm7/asm/OS_mutex.s deleted file mode 100644 index 981842b8..00000000 --- a/arm7/asm/OS_mutex.s +++ /dev/null @@ -1,131 +0,0 @@ - .include "asm/macros.inc" - .include "global.inc" - - .text - - arm_func_start OSi_DequeueItem -OSi_DequeueItem: ; 0x037F99E8 - ldr r2, [r1, #16] - ldr r1, [r1, #20] - cmp r2, #0 - streq r1, [r0, #112] ; 0x70 - strne r1, [r2, #20] - cmp r1, #0 - streq r2, [r0, #108] ; 0x6c - strne r2, [r1, #16] - bx lr - - arm_func_start OSi_EnqueueTail -OSi_EnqueueTail: ; 0x037F9A0C - ldr r2, [r0, #112] ; 0x70 - cmp r2, #0 - streq r1, [r0, #108] ; 0x6c - strne r1, [r2, #16] - str r2, [r1, #20] - mov r2, #0 - str r2, [r1, #16] - str r1, [r0, #112] ; 0x70 - bx lr - - arm_func_start OSi_UnlockAllMutex -OSi_UnlockAllMutex: ; 0x037F9A30 - stmdb sp!, {r4, r5, r6, lr} - mov r6, r0 - add r5, r6, #108 ; 0x6c - mov r4, #0 - b _037F9A58 -_037F9A44: - mov r0, r5 - bl OSi_RemoveMutexLinkFromQueue - str r4, [r0, #12] - str r4, [r0, #8] - bl OS_WakeupThread -_037F9A58: - ldr r0, [r6, #108] ; 0x6c - cmp r0, #0 - bne _037F9A44 - ldmia sp!, {r4, r5, r6, lr} - bx lr - - arm_func_start OS_UnlockMutex -OS_UnlockMutex: ; 0x037F9A6C - stmdb sp!, {r4, r5, lr} - sub sp, sp, #4 - mov r5, r0 - bl OS_DisableInterrupts - mov r4, r0 - ldr r0, _037F9AD8 ; =OSi_ThreadInfo - ldr r0, [r0, #4] - ldr r1, [r5, #8] - cmp r1, r0 - bne _037F9AC4 - ldr r1, [r5, #12] - sub r1, r1, #1 - str r1, [r5, #12] - ldr r1, [r5, #12] - cmp r1, #0 - bne _037F9AC4 - mov r1, r5 - bl OSi_DequeueItem - mov r0, #0 - str r0, [r5, #8] - mov r0, r5 - bl OS_WakeupThread -_037F9AC4: - mov r0, r4 - bl OS_RestoreInterrupts - add sp, sp, #4 - ldmia sp!, {r4, r5, lr} - bx lr -_037F9AD8: .word OSi_ThreadInfo - - arm_func_start OS_LockMutex -OS_LockMutex: ; 0x037F9ADC - stmdb sp!, {r4, r5, r6, r7, lr} - sub sp, sp, #4 - mov r5, r0 - bl OS_DisableInterrupts - mov r4, r0 - ldr r0, _037F9B64 ; =OSi_ThreadInfo - ldr r7, [r0, #4] - mov r6, #0 -_037F9AFC: - ldr r0, [r5, #8] - cmp r0, #0 - bne _037F9B28 - str r7, [r5, #8] - ldr r0, [r5, #12] - add r0, r0, #1 - str r0, [r5, #12] - mov r0, r7 - mov r1, r5 - bl OSi_EnqueueTail - b _037F9B50 -_037F9B28: - cmp r0, r7 - ldreq r0, [r5, #12] - addeq r0, r0, #1 - streq r0, [r5, #12] - beq _037F9B50 - str r5, [r7, #104] ; 0x68 - mov r0, r5 - bl OS_SleepThread - str r6, [r7, #104] ; 0x68 - b _037F9AFC -_037F9B50: - mov r0, r4 - bl OS_RestoreInterrupts - add sp, sp, #4 - ldmia sp!, {r4, r5, r6, r7, lr} - bx lr -_037F9B64: .word OSi_ThreadInfo - - arm_func_start OS_InitMutex -OS_InitMutex: ; 0x037F9B68 - mov r2, #0 - str r2, [r0, #4] - ldr r1, [r0, #4] - str r1, [r0] - str r2, [r0, #8] - str r2, [r0, #12] - bx lr diff --git a/arm7/asm/wram2.s b/arm7/asm/wram2.s index 4e48222d..68b31543 100644 --- a/arm7/asm/wram2.s +++ b/arm7/asm/wram2.s @@ -3,500 +3,6 @@ .section .text - arm_func_start FUN_03803BD4 -FUN_03803BD4: ;@ 0x03803BD4 - stmdb sp!, {r4, r5, r6, r7, lr} - sub sp, sp, #4 - and r0, r1, #63 ;@ 0x3f - cmp r0, #3 - bne _03803C40 - ldr r0, _03803C50 ;@ =0x01FFFFC0 - and r0, r1, r0 - mov r2, r0, lsr #6 - ldr r1, _03803C54 ;@ =0x04000204 - ldrh r0, [r1] - bic r0, r0, #96 ;@ 0x60 - orr r0, r0, r2, lsl #5 - strh r0, [r1] - mov r7, #1 - mov r6, #17 - mov r5, #18 - mov r4, #0 - b _03803C24 -_03803C1C: - mov r0, r7 - bl FUN_037F8CB4 -_03803C24: - mov r0, r6 - mov r1, r5 - mov r2, r4 - bl PXI_SendWordByFifo - cmp r0, #0 - bne _03803C1C - b _03803C44 -_03803C40: - bl OS_Terminate -_03803C44: - add sp, sp, #4 - ldmia sp!, {r4, r5, r6, r7, lr} - bx lr -_03803C50: .word 0x01FFFFC0 -_03803C54: .word 0x04000204 - - arm_func_start FUN_03803C58 -FUN_03803C58: ;@ 0x03803C58 - stmfd sp!, {lr} - sub sp, sp, #4 - mov r0, #0 - bl CTRDG_VibPulseEdgeUpdate - bl SND_BeginSleep - bl WVR_Shutdown - bl OS_Terminate - add sp, sp, #4 - ldmia sp!, {lr} - bx lr - - arm_func_start CTRDG_CheckPullOut_Polling -CTRDG_CheckPullOut_Polling: ;@ 0x03803C80 - stmdb sp!, {r4, r5, r6, r7, lr} - sub sp, sp, #4 - ldr r1, _03803D80 ;@ =_03807708 - ldr r3, [r1] - mvn r0, #0 - cmp r3, r0 - ldreq r0, _03803D84 ;@ =0x027FFC3C - ldreq r0, [r0] - addeq r0, r0, #10 - streq r0, [r1] - beq _03803D74 - ldr r0, _03803D88 ;@ =_03809FCC - ldr r0, [r0] - cmp r0, #0 - bne _03803D74 - ldr r0, _03803D8C ;@ =_03809FC8 - ldr r0, [r0] - cmp r0, #0 - bne _03803D74 - ldr r2, _03803D84 ;@ =0x027FFC3C - ldr r0, [r2] - cmp r0, r3 - bcc _03803D74 - ldr r0, [r2] - add r0, r0, #10 - str r0, [r1] - bl CTRDG_IsPulledOut - ldr r1, _03803D8C ;@ =_03809FC8 - str r0, [r1] - bl CTRDG_IsExisting - cmp r0, #0 - bne _03803D28 - ldr r0, _03803D90 ;@ =_03807704 - ldr r0, [r0] - cmp r0, #0 - movne r1, #1 - ldrne r0, _03803D88 ;@ =_03809FCC - strne r1, [r0] - bne _03803D74 - mov r1, #1 - ldr r0, _03803D8C ;@ =_03809FC8 - str r1, [r0] -_03803D28: - mov r7, #0 - ldr r0, _03803D90 ;@ =_03807704 - str r7, [r0] - ldr r0, _03803D8C ;@ =_03809FC8 - ldr r0, [r0] - cmp r0, #0 - beq _03803D74 - mov r6, #100 ;@ 0x64 - mov r5, #13 - mov r4, #17 - b _03803D5C -_03803D54: - mov r0, r6 - bl OS_Sleep -_03803D5C: - mov r0, r5 - mov r1, r4 - mov r2, r7 - bl PXI_SendWordByFifo - cmp r0, #0 - bne _03803D54 -_03803D74: - add sp, sp, #4 - ldmia sp!, {r4, r5, r6, r7, lr} - bx lr -_03803D80: .word _03807708 -_03803D84: .word 0x027FFC3C -_03803D88: .word _03809FCC -_03803D8C: .word _03809FC8 -_03803D90: .word _03807704 - - arm_func_start FUN_03803D94 -FUN_03803D94: ;@ 0x03803D94 - stmdb sp!, {r4, r5, r6, r7, r8, lr} - mov r7, r0 - mov r0, #134217728 ;@ 0x8000000 - add r6, r0, #4 - mov r4, #1 - bl OS_GetLockID - mov r0, r0, lsl #16 - mov r5, r0, lsr #16 - mov r0, r5 - bl OS_LockCartridge - mov r1, #0 - ldr r0, _03803E3C ;@ =0x0000FFFF - eor r2, r0, #3 - mov r2, r2, lsl #16 - mov lr, r2, lsr #16 - eor r2, r0, #132 ;@ 0x84 - mov r2, r2, lsl #16 - mov r8, r2, lsr #16 - b _03803E18 -_03803DE0: - mov ip, r0 - cmp r1, #76 ;@ 0x4c - moveq ip, r8 - beq _03803DF8 - cmp r1, #77 ;@ 0x4d - moveq ip, lr -_03803DF8: - mov r3, r1, lsl #1 - ldrh r2, [r7, r3] - and ip, ip, r2 - ldrh r2, [r6, r3] - cmp ip, r2 - movne r4, #0 - bne _03803E20 - add r1, r1, #1 -_03803E18: - cmp r1, #78 ;@ 0x4e - blt _03803DE0 -_03803E20: - mov r0, r5 - bl OS_UnLockCartridge - mov r0, r5 - bl OS_ReleaseLockID - mov r0, r4 - ldmia sp!, {r4, r5, r6, r7, r8, lr} - bx lr -_03803E3C: .word 0x0000FFFF - - arm_func_start FUN_03803E40 -FUN_03803E40: ;@ 0x03803E40 - ldr r1, _03803E54 ;@ =current_vib - str r0, [r1] - ldr r1, _03803E58 ;@ =0x08001000 - strh r0, [r1] - bx lr -_03803E54: .word current_vib -_03803E58: .word 0x08001000 - - arm_func_start CTRDG_VibPulseEdgeUpdate -CTRDG_VibPulseEdgeUpdate: ;@ 0x03803E5C - stmdb sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr} - sub sp, sp, #4 - movs r5, r0 - beq _03803E9C - ldr r0, [r5] - cmp r0, #0 - bne _03803E9C - ldr r0, [r5, #68] ;@ 0x44 - add r0, r0, #1 - str r0, [r5, #68] ;@ 0x44 - ldr r1, [r5, #64] ;@ 0x40 - cmp r1, #0 - beq _03803E9C - ldr r0, [r5, #68] ;@ 0x44 - cmp r0, r1 - movhi r5, #0 -_03803E9C: - cmp r5, #0 - beq _03803EB0 - ldr r0, [r5, #60] ;@ 0x3c - cmp r0, #0 - bne _03803F48 -_03803EB0: - bl OS_DisableInterrupts - mov r9, r0 - ldr r0, _03804068 ;@ =current_vib - ldr r0, [r0] - cmp r0, #2 - bne _03803F34 - mov r8, #0 - ldr r7, _0380406C ;@ =0x027FFFE8 - ldr r4, _03804070 ;@ =_03809FBC - ldr fp, _03804074 ;@ =0x000080E8 - mov r6, r8 - mov r5, #1 - b _03803F2C -_03803EE4: - mov r0, r7 - bl OS_ReadOwnerOfLockWord - ands sl, r0, #128 ;@ 0x80 - bne _03803F04 - ldrh r0, [r4] - bl OS_TryLockCartridge - cmp r0, #0 - bne _03803F24 -_03803F04: - mov r0, r6 - bl FUN_03803E40 - mov r8, r5 - cmp sl, #0 - bne _03803F2C - ldrh r0, [r4] - bl OS_UnlockCartridge - b _03803F2C -_03803F24: - mov r0, fp - bl OS_SpinWait -_03803F2C: - cmp r8, #0 - beq _03803EE4 -_03803F34: - ldr r0, _03804078 ;@ =_03809FEC - bl OS_CancelAlarm - mov r0, r9 - bl OS_RestoreInterrupts - b _0380405C -_03803F48: - cmp r5, #0 - beq _0380405C - ldr r0, _0380406C ;@ =0x027FFFE8 - bl OS_ReadOwnerOfLockWord - ands r4, r0, #128 ;@ 0x80 - bne _03803F74 - ldr r0, _03804070 ;@ =_03809FBC - ldrh r0, [r0] - bl OS_TryLockCartridge - cmp r0, #0 - bne _03804044 -_03803F74: - ldr r1, [r5] - ldr r0, [r5, #4] - cmp r1, r0 - bne _03803FB0 - mov r0, #0 - bl FUN_03803E40 - str r5, [sp] - ldr r0, _03804078 ;@ =_03809FEC - ldr r1, [r5, #8] - mov r2, #0 - ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate - bl OS_SetAlarm - mov r0, #0 - str r0, [r5] - b _0380402C -_03803FB0: - ands r0, r1, #1 - beq _03803FF4 - mov r0, #0 - bl FUN_03803E40 - str r5, [sp] - ldr r0, _03804078 ;@ =_03809FEC - ldr r1, [r5] - mov r1, r1, lsr #1 - add r1, r5, r1, lsl #2 - ldr r1, [r1, #36] ;@ 0x24 - mov r2, #0 - ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate - bl OS_SetAlarm - ldr r0, [r5] - add r0, r0, #1 - str r0, [r5] - b _0380402C -_03803FF4: - mov r0, #2 - bl FUN_03803E40 - str r5, [sp] - ldr r0, _03804078 ;@ =_03809FEC - ldr r1, [r5] - mov r1, r1, lsr #1 - add r1, r5, r1, lsl #2 - ldr r1, [r1, #12] - mov r2, #0 - ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate - bl OS_SetAlarm - ldr r0, [r5] - add r0, r0, #1 - str r0, [r5] -_0380402C: - cmp r4, #0 - bne _0380405C - ldr r0, _03804070 ;@ =_03809FBC - ldrh r0, [r0] - bl OS_UnlockCartridge - b _0380405C -_03804044: - str r5, [sp] - ldr r0, _03804078 ;@ =_03809FEC - ldr r1, _03804080 ;@ =0x0000020B - mov r2, #0 - ldr r3, _0380407C ;@ =CTRDG_VibPulseEdgeUpdate - bl OS_SetAlarm -_0380405C: - add sp, sp, #4 - ldmia sp!, {r4, r5, r6, r7, r8, r9, sl, fp, lr} - bx lr -_03804068: .word current_vib -_0380406C: .word 0x027FFFE8 -_03804070: .word _03809FBC -_03804074: .word 0x000080E8 -_03804078: .word _03809FEC -_0380407C: .word CTRDG_VibPulseEdgeUpdate -_03804080: .word 0x0000020B -_03804084: - mov r0, r1 - ldr ip, _03804090 ;@ =CTRDG_VibPulseEdgeUpdate - bx ip -_03804090: .word CTRDG_VibPulseEdgeUpdate - - arm_func_start FUN_03804094 -FUN_03804094: ;@ 0x03804094 - stmfd sp!, {lr} - sub sp, sp, #4 - and r0, r1, #63 ;@ 0x3f - cmp r0, #2 - bne _038040B0 - bl FUN_03803C58 - b _038040B4 -_038040B0: - bl OS_Terminate -_038040B4: - add sp, sp, #4 - ldmia sp!, {lr} - bx lr - - arm_func_start FUN_038040C0 -FUN_038040C0: ;@ 0x038040C0 - stmfd sp!, {lr} - sub sp, sp, #4 - and r0, r1, #63 ;@ 0x3f - cmp r0, #1 - ldreq r0, _038040F4 ;@ =_03809FD4 - streq r1, [r0] - moveq r1, #1 - streq r1, [r0, #16] - beq _038040E8 - bl OS_Terminate -_038040E8: - add sp, sp, #4 - ldmia sp!, {lr} - bx lr -_038040F4: .word _03809FD4 - - arm_func_start CTRDGi_InitModuleInfo -CTRDGi_InitModuleInfo: ;@ 0x038040F8 - stmdb sp!, {r4, r5, r6, r7, lr} - sub sp, sp, #4 - ldr r0, _038041C8 ;@ =_03809FC4 - ldr r1, [r0] - cmp r1, #0 - bne _038041BC - mov r1, #1 - str r1, [r0] - ldr r0, _038041CC ;@ =0x04000300 - ldrh r0, [r0] - ands r0, r0, #1 - beq _038041BC - mov r0, #262144 ;@ 0x40000 - bl OS_SetIrqMask - mov r5, r0 - ldr r1, _038041D0 ;@ =0x04000208 - ldrh r4, [r1] - mov r0, #1 - strh r0, [r1] - mov r7, #256 ;@ 0x100 - ldr r6, _038041D4 ;@ =_03809FD4 - b _03804158 -_03804150: - mov r0, r7 - bl FUN_037F8CB4 -_03804158: - ldr r0, [r6, #16] - cmp r0, #1 - bne _03804150 - ldr r1, [r6] - ldr r0, _038041D8 ;@ =0x01FFFFC0 - and r0, r1, r0 - mov r0, r0, lsr #6 - mov r0, r0, lsl #5 - add r0, r0, #33554432 ;@ 0x2000000 - add r0, r0, #4 - bl FUN_03803D94 - ldr r2, _038041DC ;@ =0x027FFC30 - ldrb r1, [r2, #5] - bic r1, r1, #1 - and r0, r0, #255 ;@ 0xff - and r0, r0, #1 - orr r0, r1, r0 - strb r0, [r2, #5] - mov r0, #1 - bl CTRDGi_SendtoPxi - ldr r1, _038041D0 ;@ =0x04000208 - ldrh r0, [r1] - strh r4, [r1] - mov r0, r5 - bl OS_SetIrqMask -_038041BC: - add sp, sp, #4 - ldmia sp!, {r4, r5, r6, r7, lr} - bx lr -_038041C8: .word _03809FC4 -_038041CC: .word 0x04000300 -_038041D0: .word 0x04000208 -_038041D4: .word _03809FD4 -_038041D8: .word 0x01FFFFC0 -_038041DC: .word 0x027FFC30 - - arm_func_start CTRDG_Init -CTRDG_Init: ;@ 0x038041E0 - stmfd sp!, {lr} - sub sp, sp, #4 - bl OS_InitTick - bl OS_InitAlarm - ldr r0, _03804270 ;@ =_03809FEC - bl OS_CreateAlarm - ldr r0, _03804274 ;@ =_03809FD0 - ldr r1, [r0] - cmp r1, #0 - bne _03804264 - mov r1, #1 - str r1, [r0] - bl CTRDGi_InitCommon - bl OS_GetLockID - mvn r1, #2 - cmp r0, r1 - beq _03804264 - ldr r1, _03804278 ;@ =_03809FBC - strh r0, [r1] - bl PXI_Init - mov r0, #13 - ldr r1, _0380427C ;@ =FUN_038040C0 - bl PXI_SetFifoRecvCallback - bl CTRDGi_InitModuleInfo - mov r0, #13 - ldr r1, _03804280 ;@ =FUN_03804094 - bl PXI_SetFifoRecvCallback - mov r0, #16 - ldr r1, _03804284 ;@ =_03804084 - bl PXI_SetFifoRecvCallback - mov r0, #17 - ldr r1, _03804288 ;@ =FUN_03803BD4 - bl PXI_SetFifoRecvCallback -_03804264: - add sp, sp, #4 - ldmia sp!, {lr} - bx lr -_03804270: .word _03809FEC -_03804274: .word _03809FD0 -_03804278: .word _03809FBC -_0380427C: .word FUN_038040C0 -_03804280: .word FUN_03804094 -_03804284: .word _03804084 -_03804288: .word FUN_03803BD4 - arm_func_start FUN_0380428C FUN_0380428C: ;@ 0x0380428C stmfd sp!, {lr} diff --git a/arm7/asm/wram_bss.s b/arm7/asm/wram_bss.s index 5d53542a..b8123702 100644 --- a/arm7/asm/wram_bss.s +++ b/arm7/asm/wram_bss.s @@ -2,38 +2,6 @@ .include "global.inc" .section .bss - .global _03809FBC -_03809FBC: ;0x03809FBC - .space 0x03809FC0 - 0x03809FBC - - .global current_vib -current_vib: ;0x03809FC0 - .space 0x03809FC4 - 0x03809FC0 - - .global _03809FC4 -_03809FC4: ;0x03809FC4 - .space 0x03809FC8 - 0x03809FC4 - - .global _03809FC8 -_03809FC8: ;0x03809FC8 - .space 0x03809FCC - 0x03809FC8 - - .global _03809FCC -_03809FCC: ;0x03809FCC - .space 0x03809FD0 - 0x03809FCC - - .global _03809FD0 -_03809FD0: ;0x03809FD0 - .space 0x03809FD4 - 0x03809FD0 - - .global _03809FD4 -_03809FD4: ;0x03809FD4 - .space 0x03809FEC - 0x03809FD4 - - .global _03809FEC -_03809FEC: ;0x03809FEC - .space 0x0380A018 - 0x03809FEC - .global _0380A018 _0380A018: ;0x0380A018 .space 0x0380A038 - 0x0380A018 diff --git a/arm7/asm/wram_data.s b/arm7/asm/wram_data.s deleted file mode 100644 index 6b7aa989..00000000 --- a/arm7/asm/wram_data.s +++ /dev/null @@ -1,13 +0,0 @@ - .include "asm/macros.inc" - .include "global.inc" - - .section .data - - .global _03807704 -_03807704: - .word 0x00000001 - - .global _03807708 -_03807708: - .word 0xFFFFFFFF - .balign 2, 0 ; Don't pad with nop diff --git a/arm7/lib/include/MI_memory.h b/arm7/lib/include/MI_memory.h index cb0c53fc..53398ec5 100644 --- a/arm7/lib/include/MI_memory.h +++ b/arm7/lib/include/MI_memory.h @@ -12,4 +12,14 @@ void MIi_CpuCopyFast(register u32 * src, register u32 * dst, register u32 size); void MI_CpuFill8(register u8 value, register u8 * dst, register u32 size); void MI_CpuCopy8(register u8 * src, register u8 * dst, register u32 size); +static inline void MI_CpuFill32(void *dest, u32 data, u32 size) +{ + MIi_CpuClear32(data, dest, size); +} + +static inline void MI_CpuClear32(void *dest, u32 size) +{ + MI_CpuFill32(dest, 0, size); +} + #endif diff --git a/arm7/lib/include/OS_alarm.h b/arm7/lib/include/OS_alarm.h new file mode 100644 index 00000000..8ae89e97 --- /dev/null +++ b/arm7/lib/include/OS_alarm.h @@ -0,0 +1,9 @@ +#ifndef POKEDIAMOND_ARM7_OS_ALARM_H +#define POKEDIAMOND_ARM7_OS_ALARM_H + +#include "nitro/OS_alarm_shared.h" + +void OS_CreateAlarm(OSAlarm *alarm); +void OS_SetAlarm(OSAlarm *alarm, OSTick tick, OSAlarmHandler handler, void *arg); + +#endif //POKEDIAMOND_ARM7_OS_ALARM_H diff --git a/arm7/lib/include/OS_context.h b/arm7/lib/include/OS_context.h index 7819e29c..b01ccb8b 100644 --- a/arm7/lib/include/OS_context.h +++ b/arm7/lib/include/OS_context.h @@ -13,4 +13,8 @@ typedef struct OSContext u32 sp_svc; } OSContext; +extern u32 OS_SaveContext(OSContext *context); +extern void OS_LoadContext(OSContext *context); +extern void OS_InitContext(OSContext *context, u32 func, u32 stack); + #endif //POKEDIAMOND_ARM7_OS_CONTEXT_H diff --git a/arm7/lib/include/OS_mutex.h b/arm7/lib/include/OS_mutex.h new file mode 100644 index 00000000..920d5594 --- /dev/null +++ b/arm7/lib/include/OS_mutex.h @@ -0,0 +1,14 @@ +#ifndef POKEDIAMOND_ARM7_OS_MUTEX_H +#define POKEDIAMOND_ARM7_OS_MUTEX_H + +#include "OS_context.h" +#include "nitro/OS_mutex_shared.h" + +void OSi_DequeueItem(OSThread * thread, OSMutex * mutex); +void OSi_EnqueueTail(OSThread * thread, OSMutex * mutex); +void OS_InitMutex(OSMutex* mutex); +void OS_LockMutex(OSMutex* mutex); +void OS_UnlockMutex(OSMutex* mutex); +void OSi_UnlockAllMutex(OSThread * thread); + +#endif //POKEDIAMOND_ARM7_OS_MUTEX_H diff --git a/arm7/lib/include/OS_thread.h b/arm7/lib/include/OS_thread.h index 04e4fdcb..b1d3a01b 100644 --- a/arm7/lib/include/OS_thread.h +++ b/arm7/lib/include/OS_thread.h @@ -4,4 +4,33 @@ #include "OS_context.h" #include "nitro/OS_thread_shared.h" +static s32 OSi_GetUnusedThreadId(void); +static void OSi_InsertLinkToQueue(OSThreadQueue *queue, OSThread *thread); +static OSThread *OSi_RemoveLinkFromQueue(OSThreadQueue *queue); +static OSThread *OSi_RemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread); +OSMutex *OSi_RemoveMutexLinkFromQueue(OSMutexQueue *queue); +static void OSi_InsertThreadToList(OSThread *thread); +static void OSi_RemoveThreadFromList(OSThread *thread); +static void OSi_RescheduleThread(void); +void OS_InitThread(void); +void OS_CreateThread(OSThread *thread, void (*func) (void *), void *arg, void *stack, u32 stackSize, u32 prio); +void OS_ExitThread(void); +static void OSi_ExitThread_ArgSpecified(OSThread *thread, void *arg); +static void OSi_ExitThread(void *arg); +static void OSi_ExitThread_Destroy(void); +void OS_JoinThread(OSThread *thread); +BOOL OS_IsThreadTerminated(const OSThread *thread); +void OS_SleepThread(OSThreadQueue *queue); +void OS_WakeupThread(OSThreadQueue *queue); +void OS_WakeupThreadDirect(OSThread *thread); +OSThread *OS_SelectThread(void); +void OS_RescheduleThread(void); +BOOL OS_SetThreadPriority(OSThread *thread, u32 prio); +void OS_Sleep(u32 msec); +static void OSi_SleepAlarmCallback(void *arg); +OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback); +u32 OS_DisableScheduler(void); +u32 OS_EnableScheduler(void); +void OS_SetThreadDestructor(OSThread *thread, OSThreadDestructor dtor); + #endif //POKEDIAMOND_ARM7_OS_THREAD_H diff --git a/arm7/lib/include/consts.h b/arm7/lib/include/consts.h index 966b5a4b..a6c4c393 100644 --- a/arm7/lib/include/consts.h +++ b/arm7/lib/include/consts.h @@ -4,5 +4,6 @@ #include "nitro/consts_shared.h" #include "mmap.h" #include "registers.h" +#include "systemWork.h" #endif //POKEDIAMOND_ARM7_CONSTS_H diff --git a/arm7/lib/include/systemWork.h b/arm7/lib/include/systemWork.h new file mode 100644 index 00000000..95a17845 --- /dev/null +++ b/arm7/lib/include/systemWork.h @@ -0,0 +1,7 @@ +#ifndef POKEDIAMOND_ARM7_SYSTEMWORK_H +#define POKEDIAMOND_ARM7_SYSTEMWORK_H + +#include "OS_context.h" +#include "nitro/OS_systemWork_shared.h" + +#endif //POKEDIAMOND_ARM7_SYSTEMWORK_H diff --git a/arm7/lib/src/OS_mutex.c b/arm7/lib/src/OS_mutex.c new file mode 100644 index 00000000..7e9b05ec --- /dev/null +++ b/arm7/lib/src/OS_mutex.c @@ -0,0 +1,95 @@ +#include "function_target.h" +#include "OS_mutex.h" +#include "OS_system.h" +#include "OS_thread.h" + +ARM_FUNC void OS_InitMutex(OSMutex* mutex) { + OS_InitThreadQueue(&mutex->queue); + mutex->thread = NULL; + mutex->count = 0; +} + +ARM_FUNC void OS_LockMutex(OSMutex* mutex) { + OSIntrMode mode; + OSThread* current; + + mode = OS_DisableInterrupts(); + current = OS_GetCurrentThread(); + while (1) { + OSThread* owner = mutex->thread; + if (owner == NULL) { + mutex->thread = current; + mutex->count++; + OSi_EnqueueTail(current, mutex); + break; + } + else if (owner == current) { + mutex->count++; + break; + } + else { + current->mutex = mutex; + OS_SleepThread(&mutex->queue); + current->mutex = NULL; + } + } + (void)OS_RestoreInterrupts(mode); +} + +ARM_FUNC void OS_UnlockMutex(OSMutex* mutex) { + OSIntrMode mode; + OSThread* current; + + mode = OS_DisableInterrupts(); + current = OS_GetCurrentThread(); + if (mutex->thread == current) { + if (--mutex->count == 0) { + OSi_DequeueItem(current, mutex); + mutex->thread = NULL; + OS_WakeupThread(&mutex->queue); + } + } + (void)OS_RestoreInterrupts(mode); +} + +ARM_FUNC void OSi_UnlockAllMutex(OSThread * thread) { + OSMutex * mutex; + while (thread->mutexQueue.head) { + mutex = OSi_RemoveMutexLinkFromQueue(&thread->mutexQueue); + mutex->count = 0; + mutex->thread = NULL; + OS_WakeupThread(&mutex->queue); + } +} + +ARM_FUNC void OSi_EnqueueTail(OSThread * thread, OSMutex * mutex) { + OSMutex * tail = thread->mutexQueue.tail; + if (tail == NULL) { + thread->mutexQueue.head = mutex; + } + else { + tail->link.next = mutex; + } + mutex->link.prev = tail; + mutex->link.next = NULL; + thread->mutexQueue.tail = mutex; +} + +ARM_FUNC void OSi_DequeueItem(OSThread * thread, OSMutex * mutex) { + OSMutex *next = mutex->link.next; + OSMutex *prev = mutex->link.prev; + + if (next == NULL) { + thread->mutexQueue.tail = prev; + } + else { + next->link.prev = prev; + } + + if (prev == NULL) { + thread->mutexQueue.head = next; + } + else { + prev->link.next = next; + } +} diff --git a/arm7/lib/src/OS_thread.c b/arm7/lib/src/OS_thread.c new file mode 100644 index 00000000..be3f611d --- /dev/null +++ b/arm7/lib/src/OS_thread.c @@ -0,0 +1,597 @@ +#include "OS_thread.h" +#include "function_target.h" +#include "OS_system.h" +#include "consts.h" +#include "OS_terminate_proc.h" +#include "OS_mutex.h" +#include "OS_alarm.h" +#include "OS_context.h" +#include "nitro/OS_systemWork_shared.h" +#include "MI_memory.h" + +extern void SDK_SYS_STACKSIZE(void); +extern void SDK_IRQ_STACKSIZE(void); + +u32 OSi_RescheduleCount = 0; + +void *OSi_StackForDestructor = NULL; + +OSThreadInfo OSi_ThreadInfo; + +BOOL OSi_IsThreadInitialized = FALSE; + +OSThread **OSi_CurrentThreadPtr = NULL; + +void *OSi_SystemCallbackInSwitchThread = NULL; + +static s32 OSi_ThreadIdCount = 0; + +OSThread OSi_LauncherThread; +OSThread OSi_IdleThread; + +ARM_FUNC static s32 OSi_GetUnusedThreadId(void) +{ + return ++OSi_ThreadIdCount; +} + +ARM_FUNC static void OSi_InsertLinkToQueue(OSThreadQueue *queue, OSThread *thread) +{ + OSThread *next = queue->head; + + while (next && next->priority <= thread->priority) + { + if (next == thread) + return; + next = next->link.next; + } + + if (!next) + { + OSThread *prev = queue->tail; + + if (!prev) + { + queue->head = thread; + } + else + { + prev->link.next = thread; + } + + thread->link.prev = prev; + thread->link.next = NULL; + queue->tail = thread; + } + else + { + OSThread *prev = next->link.prev; + + if (!prev) + { + queue->head = thread; + } + else + { + prev->link.next = thread; + } + + thread->link.prev = prev; + thread->link.next = next; + next->link.prev = thread; + } +} + +ARM_FUNC static OSThread *OSi_RemoveLinkFromQueue(OSThreadQueue *queue) +{ + OSThread *thread = queue->head; + + if (thread) + { + OSThread *next = thread->link.next; + + queue->head = next; + + if (next) + { + next->link.prev = NULL; + } + else + { + queue->tail = NULL; + thread->queue = NULL; + } + } + + return thread; +} + +ARM_FUNC static OSThread *OSi_RemoveSpecifiedLinkFromQueue(OSThreadQueue *queue, OSThread *thread) +{ + OSThread *queueHead = queue->head; + + while (queueHead) + { + OSThread *next = queueHead->link.next; + + if (queueHead == thread) + { + OSThread *prev = queueHead->link.prev; + + if (queue->head == queueHead) + { + queue->head = next; + } + else + { + prev->link.next = next; + } + + if (queue->tail == queueHead) + { + queue->tail = prev; + } + else + { + next->link.prev = prev; + } + + break; + } + + queueHead = next; + } + + return queueHead; +} + +ARM_FUNC OSMutex *OSi_RemoveMutexLinkFromQueue(OSMutexQueue *queue) +{ + OSMutex *mutexHead = queue->head; + + if (mutexHead) + { + OSMutex *next = mutexHead->link.next; + + queue->head = next; + + if (next) + { + next->link.prev = NULL; + } + else + { + queue->tail = NULL; + } + } + + return mutexHead; +} + +ARM_FUNC static void OSi_InsertThreadToList(OSThread *thread) +{ + OSThread *t = OSi_ThreadInfo.list; + OSThread *pre = NULL; + + while(t && t->priority < thread->priority) + { + pre = t; + t = t->next; + } + + if (!pre) + { + thread->next = OSi_ThreadInfo.list; + OSi_ThreadInfo.list = thread; + } + else + { + thread->next = pre->next; + pre->next = thread; + } +} + +ARM_FUNC static void OSi_RemoveThreadFromList(OSThread *thread) +{ + OSThread *t = OSi_ThreadInfo.list; + OSThread *pre = NULL; + + while (t && t != thread) + { + pre = t; + t = t-> next; + } + + if (!pre) + { + OSi_ThreadInfo.list = thread->next; + } + else + { + pre->next = thread->next; + } +} + +ARM_FUNC static void OSi_RescheduleThread(void) +{ + if (OSi_RescheduleCount <= 0) + { + OSThreadInfo *info = &OSi_ThreadInfo; + if (info->irqDepth > 0 || OS_GetProcMode() == OS_PROCMODE_IRQ) + { + info->isNeedRescheduling = TRUE; + } + else + { + OSThread *currentThread = OSi_GetCurrentThread(); + OSThread *nextThread = OS_SelectThread(); + + if (currentThread == nextThread || !nextThread) + return; + + if (currentThread->state != OS_THREAD_STATE_TERMINATED + && OS_SaveContext(¤tThread->context)) + return; + + if (OSi_SystemCallbackInSwitchThread) + { + ((OSSwitchThreadCallback)OSi_SystemCallbackInSwitchThread) (currentThread, nextThread); + } + + if (info->switchCallback) + { + ((OSSwitchThreadCallback)info->switchCallback) (currentThread, nextThread); + } + + OS_SetCurrentThread(nextThread); + + OS_LoadContext(&nextThread->context); + } + } +} + +ARM_FUNC void OS_InitThread(void) +{ + if (OSi_IsThreadInitialized) + return; + OSi_IsThreadInitialized = TRUE; + + OSi_CurrentThreadPtr = &(OSi_ThreadInfo.current); + + OSi_LauncherThread.priority = OS_THREAD_LAUNCHER_PRIORITY; + OSi_LauncherThread.id = 0; + OSi_LauncherThread.state = OS_THREAD_STATE_READY; + OSi_LauncherThread.next = NULL; + + OSi_LauncherThread.profiler = NULL; + + OSi_ThreadInfo.list = &OSi_LauncherThread; + + OS_SetCurrentThread(&OSi_LauncherThread); + + void *stackLo = (((s32)SDK_SYS_STACKSIZE) <= 0) ? + (void *)((u32)HW_WRAM - (s32)SDK_SYS_STACKSIZE) : + (void *)((u32)(HW_PRV_WRAM_IRQ_STACK_END - (s32)SDK_IRQ_STACKSIZE) - (s32)SDK_SYS_STACKSIZE); + + OSi_LauncherThread.stackBottom = (u32)(HW_PRV_WRAM_IRQ_STACK_END - (s32)SDK_IRQ_STACKSIZE); + OSi_LauncherThread.stackTop = (u32)stackLo; + OSi_LauncherThread.stackWarningOffset = 0; + + //checksums + *(u32 *)(OSi_LauncherThread.stackBottom - sizeof(u32)) = 0xd73bfdf7UL; + *(u32 *)OSi_LauncherThread.stackTop = 0xfbdd37bbUL; + + OS_InitThreadQueue(&OSi_LauncherThread.joinQueue); + + OSi_ThreadInfo.isNeedRescheduling = FALSE; + OSi_ThreadInfo.irqDepth = 0; + + OS_GetSystemWork()->threadinfo_subp = &OSi_ThreadInfo; + + (void)OS_SetSwitchThreadCallback(NULL); +} + +ARM_FUNC void OS_CreateThread(OSThread *thread, void (*func) (void *), void *arg, void *stack, u32 stackSize, u32 prio) +{ + OSIntrMode enable = OS_DisableInterrupts(); + + s32 index = OSi_GetUnusedThreadId(); + + thread->priority = prio; + thread->id = (u32)index; + thread->state = OS_THREAD_STATE_WAITING; + + thread->profiler = NULL; + + OSi_InsertThreadToList(thread); + + thread->stackBottom = (u32)stack; + thread->stackTop = (u32)stack - stackSize; + thread->stackWarningOffset = 0; + + *(u32 *)(thread->stackBottom - sizeof(u32)) = 0xd73bfdf7UL; + *(u32 *)thread->stackTop = 0xfbdd37bbUL; + + OS_InitThreadQueue(&thread->joinQueue); + + OS_InitContext(&thread->context, (u32)func, (u32)stack - 4); + + thread->context.r[0] = (u32)arg; + thread->context.lr = (u32)OS_ExitThread; + + MI_CpuClear32((void *)((u32)stack - stackSize + 4), stackSize - 8); + + thread->mutex = NULL; + thread->mutexQueue.head = NULL; + thread->mutexQueue.tail = NULL; + + OS_SetThreadDestructor(thread, NULL); + + thread->queue = NULL; + thread->link.prev = thread->link.next = NULL; + + MI_CpuClear32(&thread->specific[0], sizeof(void *) * OS_THREAD_SPECIFIC_MAX); + + thread->alarmForSleep = NULL; + + (void)OS_RestoreInterrupts(enable); +} + +ARM_FUNC void OS_ExitThread(void) +{ + (void)OS_DisableInterrupts(); + OSi_ExitThread_ArgSpecified(OS_GetCurrentThread(), 0); +} + +ARM_FUNC static void OSi_ExitThread_ArgSpecified(OSThread *thread, void *arg) +{ + if (OSi_StackForDestructor) + { + OS_InitContext(&thread->context, (u32)OSi_ExitThread, (u32)OSi_StackForDestructor); + thread->context.r[0] = (u32)arg; + thread->context.cpsr |= HW_PSR_DISABLE_IRQ; + thread->state = OS_THREAD_STATE_READY; + OS_LoadContext(&thread->context); + } + else + { + OSi_ExitThread(arg); + } +} + +ARM_FUNC static void OSi_ExitThread(void *arg) +{ + OSThread *currentThread = OSi_GetCurrentThread(); + OSThreadDestructor destructor = currentThread->destructor; + + if (destructor) + { + currentThread->destructor = NULL; + destructor(arg); + (void)OS_DisableInterrupts(); + } + + OSi_ExitThread_Destroy(); +} + +ARM_FUNC static void OSi_ExitThread_Destroy(void) +{ + OSThread *currentThread = OSi_GetCurrentThread(); + (void)OS_DisableScheduler(); + + OSi_UnlockAllMutex(currentThread); + + if (currentThread->queue) + { + (void)OSi_RemoveSpecifiedLinkFromQueue(currentThread->queue, currentThread); + } + + OSi_RemoveThreadFromList(currentThread); + + currentThread->state = OS_THREAD_STATE_TERMINATED; + + OS_WakeupThread(¤tThread->joinQueue); + + (void)OS_EnableScheduler(); + + OS_RescheduleThread(); + + OS_Terminate(); +} + +ARM_FUNC void OS_JoinThread(OSThread *thread) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + + if (thread->state != OS_THREAD_STATE_TERMINATED) + { + OS_SleepThread(&thread->joinQueue); + } + + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC BOOL OS_IsThreadTerminated(const OSThread *thread) +{ + return (thread->state == OS_THREAD_STATE_TERMINATED) ? TRUE : FALSE; +} + +ARM_FUNC void OS_SleepThread(OSThreadQueue *queue) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + OSThread *currentThread = OSi_GetCurrentThread(); + + if (queue) + { + currentThread->queue = queue; + OSi_InsertLinkToQueue(queue, currentThread); + } + + currentThread->state = OS_THREAD_STATE_WAITING; + OSi_RescheduleThread(); + + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC void OS_WakeupThread(OSThreadQueue *queue) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + + if (queue->head) + { + while (queue->head) + { + OSThread *thread = OSi_RemoveLinkFromQueue(queue); + + thread->state = OS_THREAD_STATE_READY; + thread->queue = NULL; + thread->link.prev = thread->link.next = NULL; + } + + OS_InitThreadQueue(queue); + OSi_RescheduleThread(); + } + + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC void OS_WakeupThreadDirect(OSThread *thread) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + + thread->state = OS_THREAD_STATE_READY; + OSi_RescheduleThread(); + + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC OSThread *OS_SelectThread(void) +{ + OSThread *thread = OSi_ThreadInfo.list; + + while (thread && !OS_IsThreadRunnable(thread)) + { + thread = thread->next; + } + + return thread; +} + +ARM_FUNC void OS_RescheduleThread(void) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + OSi_RescheduleThread(); + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC BOOL OS_SetThreadPriority(OSThread *thread, u32 prio) +{ + OSThread *t = OSi_ThreadInfo.list; + OSThread *pre = NULL; + OSIntrMode enabled = OS_DisableInterrupts(); + + while (t && t != thread) + { + pre = t; + t = t->next; + } + + if (!t || t == &OSi_IdleThread) + { + (void)OS_RestoreInterrupts(enabled); + return FALSE; + } + + if (t->priority != prio) + { + if (!pre) + { + OSi_ThreadInfo.list = thread->next; + } + else + { + pre->next = thread->next; + } + + thread->priority = prio; + OSi_InsertThreadToList(thread); + + OSi_RescheduleThread(); + } + + (void)OS_RestoreInterrupts(enabled); + + return TRUE; +} + +ARM_FUNC void OS_Sleep(u32 msec) +{ + OSAlarm alarm; + + OS_CreateAlarm(&alarm); + OSThread *volatile p_thread = OSi_GetCurrentThread(); + OSIntrMode enabled = OS_DisableInterrupts(); + + p_thread->alarmForSleep = &alarm; + + OS_SetAlarm(&alarm, OS_MilliSecondsToTicks(msec), &OSi_SleepAlarmCallback, + (void*)&p_thread); + while (p_thread != NULL) + { + OS_SleepThread(NULL); + } + (void)OS_RestoreInterrupts(enabled); +} + +ARM_FUNC static void OSi_SleepAlarmCallback(void *arg) +{ + OSThread **pp_thread = (OSThread **)arg; + OSThread *p_thread = *pp_thread; + *pp_thread = NULL; + + p_thread->alarmForSleep = NULL; + + OS_WakeupThreadDirect(p_thread); +} + +ARM_FUNC OSSwitchThreadCallback OS_SetSwitchThreadCallback(OSSwitchThreadCallback callback) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + OSSwitchThreadCallback prev = OSi_ThreadInfo.switchCallback; + OSi_ThreadInfo.switchCallback = callback; + + (void)OS_RestoreInterrupts(enabled); + return prev; +} + +ARM_FUNC u32 OS_DisableScheduler(void) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + u32 count; + + if (OSi_RescheduleCount < (u32)-1) + { + count = OSi_RescheduleCount++; + } + (void)OS_RestoreInterrupts(enabled); + + return count; +} + +ARM_FUNC u32 OS_EnableScheduler(void) +{ + OSIntrMode enabled = OS_DisableInterrupts(); + u32 count = 0; + + if (OSi_RescheduleCount > 0) + { + count = OSi_RescheduleCount--; + } + (void)OS_RestoreInterrupts(enabled); + + return count; +} + +ARM_FUNC void OS_SetThreadDestructor(OSThread *thread, OSThreadDestructor dtor) +{ + thread->destructor = dtor; +} |