diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c6c6a5c --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*build +load.bin +load.elf diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..fa80884 --- /dev/null +++ b/Makefile @@ -0,0 +1,123 @@ +#--------------------------------------------------------------------------------- +.SUFFIXES: +#--------------------------------------------------------------------------------- +ifeq ($(strip $(DEVKITARM)),) +$(error "Please set DEVKITARM in your environment. export DEVKITARM=devkitARM) +endif + +-include $(DEVKITARM)/ds_rules + +#--------------------------------------------------------------------------------- +# BUILD is the directory where object files & intermediate files will be placed +# SOURCES is a list of directories containing source code +# INCLUDES is a list of directories containing extra header files +#--------------------------------------------------------------------------------- +TARGET := load +BUILD ?= build +SOURCES := source source/patches +INCLUDES := build +SPECS := specs + +#--------------------------------------------------------------------------------- +# options for code generation +#--------------------------------------------------------------------------------- +ARCH := -mthumb -mthumb-interwork + +CFLAGS := -g -Wall -Os\ + -mcpu=arm7tdmi -mtune=arm7tdmi -fomit-frame-pointer\ + -ffast-math \ + $(ARCH) + +CFLAGS += $(INCLUDE) $(EXTRA_CFLAGS) -DARM7 + +ASFLAGS := -g $(ARCH) $(EXTRA_CFLAGS) $(INCLUDE) +LDFLAGS = -nostartfiles -T $(TOPDIR)/load.ld -Wl,--no-warn-rwx-segments -g $(ARCH) -Wl,-Map,$(TARGET).map + +LIBS := + +#--------------------------------------------------------------------------------- +# list of directories containing libraries, this must be the top level containing +# include and lib +#--------------------------------------------------------------------------------- +LIBDIRS := $(LIBNDS) + + +#--------------------------------------------------------------------------------- +# no real need to edit anything past this point unless you need to add additional +# rules for different file extensions +#--------------------------------------------------------------------------------- +ifneq ($(BUILD),$(notdir $(CURDIR))) +#--------------------------------------------------------------------------------- + +export TOPDIR := $(CURDIR) +export LOADBIN ?= $(CURDIR)/$(TARGET).bin +export LOADELF := $(CURDIR)/$(TARGET).elf +export DEPSDIR := $(CURDIR)/$(BUILD) + +export VPATH := $(foreach dir,$(SOURCES),$(CURDIR)/$(dir)) + +export CC := $(PREFIX)gcc +export CXX := $(PREFIX)g++ +export AR := $(PREFIX)ar +export OBJCOPY := $(PREFIX)objcopy + +CFILES := $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.c))) +CPPFILES := $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.cpp))) +SFILES := $(foreach dir,$(SOURCES),$(notdir $(wildcard $(dir)/*.s))) + +export OFILES := $(CPPFILES:.cpp=.o) $(CFILES:.c=.o) $(SFILES:.s=.o) + +export INCLUDE := $(foreach dir,$(INCLUDES),-I$(CURDIR)/$(dir)) \ + $(foreach dir,$(LIBDIRS),-I$(dir)/include) \ + -I$(CURDIR)/$(BUILD) + +export LIBPATHS := $(foreach dir,$(LIBDIRS),-L$(dir)/lib) + +#--------------------------------------------------------------------------------- +# use CC for linking standard C +#--------------------------------------------------------------------------------- +export LD := $(CC) +#--------------------------------------------------------------------------------- + +.PHONY: $(BUILD) clean + +#--------------------------------------------------------------------------------- +$(BUILD): + @[ -d $@ ] || mkdir -p $@ + @$(MAKE) -C $(BUILD) -f $(CURDIR)/Makefile + +#--------------------------------------------------------------------------------- +clean: + @echo clean ... + @rm -fr $(BUILD) *.elf *.bin + + +#--------------------------------------------------------------------------------- +else + +DEPENDS := $(OFILES:.o=.d) + +#--------------------------------------------------------------------------------- +# main targets +#--------------------------------------------------------------------------------- +$(LOADBIN) : $(LOADELF) + @$(OBJCOPY) -O binary $< $@ + @echo built ... $(notdir $@) + + +$(LOADELF) : $(OFILES) + @echo linking $(notdir $@) + @$(LD) $(LDFLAGS) $(OFILES) $(LIBPATHS) $(LIBS) -o $@ + +arm9mpu_reset.o: mpu_reset.bin + +mpu_reset.bin: mpu_reset.elf + $(OBJCOPY) -O binary $< $@ + +mpu_reset.elf: $(TOPDIR)/arm9code/mpu_reset.s + $(CC) $(ASFLAGS) -Ttext=0 -x assembler-with-cpp -nostartfiles -nostdlib $< -o $@ + +-include $(DEPENDS) +#--------------------------------------------------------------------------------------- +endif +#--------------------------------------------------------------------------------------- \ No newline at end of file diff --git a/arm9code/mpu_reset.s b/arm9code/mpu_reset.s new file mode 100644 index 0000000..3d499dd --- /dev/null +++ b/arm9code/mpu_reset.s @@ -0,0 +1,110 @@ +/* + Copyright 2006 - 2015 Dave Murphy (WinterMute) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +*/ + +#include + + .text + .align 4 + + .arm + + .arch armv5te + .cpu arm946e-s + +@--------------------------------------------------------------------------------- + .global _start + .type _start STT_FUNC +@--------------------------------------------------------------------------------- +_start: +@--------------------------------------------------------------------------------- + @ Switch off MPU + mrc p15, 0, r0, c1, c0, 0 + bic r0, r0, #PROTECT_ENABLE + mcr p15, 0, r0, c1, c0, 0 + + + adr r12, mpu_initial_data + ldmia r12, {r0-r10} + + mcr p15, 0, r0, c2, c0, 0 + mcr p15, 0, r0, c2, c0, 1 + mcr p15, 0, r1, c3, c0, 0 + mcr p15, 0, r2, c5, c0, 2 + mcr p15, 0, r3, c5, c0, 3 + mcr p15, 0, r4, c6, c0, 0 + mcr p15, 0, r5, c6, c1, 0 + mcr p15, 0, r6, c6, c3, 0 + mcr p15, 0, r7, c6, c4, 0 + mcr p15, 0, r8, c6, c6, 0 + mcr p15, 0, r9, c6, c7, 0 + mcr p15, 0, r10, c9, c1, 0 + + mov r0, #0 + mcr p15, 0, r0, c6, c2, 0 @ PU Protection Unit Data/Unified Region 2 + mcr p15, 0, r0, c6, c5, 0 @ PU Protection Unit Data/Unified Region 5 + + mrc p15, 0, r0, c9, c1, 0 @ DTCM + mov r0, r0, lsr #12 @ base + mov r0, r0, lsl #12 @ size + add r0, r0, #0x4000 @ dtcm top + + sub r0, r0, #4 @ irq vector + mov r1, #0 + str r1, [r0] + sub r0, r0, #4 @ IRQ1 Check Bits + str r1, [r0] + + sub r0, r0, #128 + bic r0, r0, #7 + + msr cpsr_c, #0xd3 @ svc mode + mov sp, r0 + sub r0, r0, #128 + msr cpsr_c, #0xd2 @ irq mode + mov sp, r0 + sub r0, r0, #128 + msr cpsr_c, #0xdf @ system mode + mov sp, r0 + + @ enable cache & tcm + mrc p15, 0, r0, c1, c0, 0 + ldr r1,= ITCM_ENABLE | DTCM_ENABLE | ICACHE_ENABLE | DCACHE_ENABLE + orr r0,r0,r1 + mcr p15, 0, r0, c1, c0, 0 + + ldr r10, =0x2FFFE04 + ldr r0, =0xE59FF018 + str r0, [r10] + add r1, r10, #0x20 + str r10, [r1] + bx r10 + + .pool + +mpu_initial_data: + .word 0x00000042 @ p15,0,c2,c0,0..1,r0 ;PU Cachability Bits for Data/Unified+Instruction Protection Region + .word 0x00000002 @ p15,0,c3,c0,0,r1 ;PU Write-Bufferability Bits for Data Protection Regions + .word 0x15111011 @ p15,0,c5,c0,2,r2 ;PU Extended Access Permission Data/Unified Protection Region + .word 0x05100011 @ p15,0,c5,c0,3,r3 ;PU Extended Access Permission Instruction Protection Region + .word 0x04000033 @ p15,0,c6,c0,0,r4 ;PU Protection Unit Data/Unified Region 0 + .word 0x0200002b @ p15,0,c6,c1,0,r5 ;PU Protection Unit Data/Unified Region 1 4MB + .word 0x08000035 @ p15,0,c6,c3,0,r6 ;PU Protection Unit Data/Unified Region 3 + .word 0x0300001b @ p15,0,c6,c4,0,r7 ;PU Protection Unit Data/Unified Region 4 + .word 0xffff001d @ p15,0,c6,c6,0,r8 ;PU Protection Unit Data/Unified Region 6 + .word 0x02fff017 @ p15,0,c6,c7,0,r9 ;PU Protection Unit Data/Unified Region 7 4KB + .word 0x0300000a @ p15,0,c9,c1,0,r10 ;TCM Data TCM Base and Virtual Size diff --git a/load.ld b/load.ld new file mode 100644 index 0000000..53f3728 --- /dev/null +++ b/load.ld @@ -0,0 +1,198 @@ +OUTPUT_FORMAT("elf32-littlearm", "elf32-bigarm", "elf32-littlearm") +OUTPUT_ARCH(arm) +ENTRY(_start) + +MEMORY { + + vram : ORIGIN = 0x06000000, LENGTH = 128K +} + +__vram_start = ORIGIN(vram); +__vram_top = ORIGIN(vram)+ LENGTH(vram); +__sp_irq = __vram_top - 0x60; +__sp_svc = __sp_irq - 0x100; +__sp_usr = __sp_svc - 0x100; + +__irq_flags = __vram_top - 8; +__irq_vector = __vram_top - 4; + +SECTIONS +{ + .init : + { + __text_start = . ; + KEEP (*(.init)) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .plt : + { + *(.plt) + } >vram = 0xff + + .text : /* ALIGN (4): */ + { + + *(.text*) + *(.stub) + /* .gnu.warning sections are handled specially by elf32.em. */ + *(.gnu.warning) + *(.gnu.linkonce.t*) + *(.glue_7) + *(.glue_7t) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .fini : + { + KEEP (*(.fini)) + } >vram =0xff + + __text_end = . ; + + .rodata : + { + *(.rodata) + *all.rodata*(*) + *(.roda) + *(.rodata.*) + *(.gnu.linkonce.r*) + SORT(CONSTRUCTORS) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >vram + __exidx_start = .; + .ARM.exidx : { *(.ARM.exidx* .gnu.linkonce.armexidx.*) } >vram + __exidx_end = .; + +/* Ensure the __preinit_array_start label is properly aligned. We + could instead move the label definition inside the section, but + the linker would then create the section even if it turns out to + be empty, which isn't pretty. */ + . = ALIGN(32 / 8); + PROVIDE (__preinit_array_start = .); + .preinit_array : { KEEP (*(.preinit_array)) } >vram = 0xff + PROVIDE (__preinit_array_end = .); + PROVIDE (__init_array_start = .); + .init_array : { KEEP (*(.init_array)) } >vram = 0xff + PROVIDE (__init_array_end = .); + PROVIDE (__fini_array_start = .); + .fini_array : { KEEP (*(.fini_array)) } >vram = 0xff + PROVIDE (__fini_array_end = .); + + .ctors : + { + /* gcc uses crtbegin.o to find the start of the constructors, so + we make sure it is first. Because this is a wildcard, it + doesn't matter if the user does not actually link against + crtbegin.o; the linker won't look for a file to match a + wildcard. The wildcard also means that it doesn't matter which + directory crtbegin.o is in. */ + KEEP (*crtbegin.o(.ctors)) + KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors)) + KEEP (*(SORT(.ctors.*))) + KEEP (*(.ctors)) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .dtors : + { + KEEP (*crtbegin.o(.dtors)) + KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors)) + KEEP (*(SORT(.dtors.*))) + KEEP (*(.dtors)) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .eh_frame : + { + KEEP (*(.eh_frame)) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + + .gcc_except_table : + { + *(.gcc_except_table) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram = 0xff + .jcr : { KEEP (*(.jcr)) } >vram = 0 + .got : { *(.got.plt) *(.got) } >vram = 0 + + + .vram ALIGN(4) : + { + __vram_start = ABSOLUTE(.) ; + *(.vram) + *vram.*(.text) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + __vram_end = ABSOLUTE(.) ; + } >vram = 0xff + + + .data ALIGN(4) : { + __data_start = ABSOLUTE(.); + *(.data) + *(.data.*) + *(.gnu.linkonce.d*) + CONSTRUCTORS + . = ALIGN(4); + __data_end = ABSOLUTE(.) ; + } >vram = 0xff + + + + .bss ALIGN(4) : + { + __bss_start = ABSOLUTE(.); + __bss_start__ = ABSOLUTE(.); + *(.dynbss) + *(.gnu.linkonce.b*) + *(.bss*) + *(COMMON) + . = ALIGN(4); /* REQUIRED. LD is flaky without it. */ + } >vram + + __bss_end = . ; + __bss_end__ = . ; + + _end = . ; + __end__ = . ; + PROVIDE (end = _end); + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + /* DWARF debug sections. + Symbols in the DWARF debugging sections are relative to the beginning + of the section so we begin them at 0. */ + /* DWARF 1 */ + .debug 0 : { *(.debug) } + .line 0 : { *(.line) } + /* GNU DWARF 1 extensions */ + .debug_srcinfo 0 : { *(.debug_srcinfo) } + .debug_sfnames 0 : { *(.debug_sfnames) } + /* DWARF 1.1 and DWARF 2 */ + .debug_aranges 0 : { *(.debug_aranges) } + .debug_pubnames 0 : { *(.debug_pubnames) } + /* DWARF 2 */ + .debug_info 0 : { *(.debug_info) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_line 0 : { *(.debug_line) } + .debug_frame 0 : { *(.debug_frame) } + .debug_str 0 : { *(.debug_str) } + .debug_loc 0 : { *(.debug_loc) } + .debug_macinfo 0 : { *(.debug_macinfo) } + /* SGI/MIPS DWARF 2 extensions */ + .debug_weaknames 0 : { *(.debug_weaknames) } + .debug_funcnames 0 : { *(.debug_funcnames) } + .debug_typenames 0 : { *(.debug_typenames) } + .debug_varnames 0 : { *(.debug_varnames) } + .stack 0x80000 : { _stack = .; *(.stack) } + /* These must appear regardless of . */ +} diff --git a/source/arm7clear.s b/source/arm7clear.s new file mode 100644 index 0000000..52343e3 --- /dev/null +++ b/source/arm7clear.s @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + + .arm + .global arm7clearRAM + .type arm7clearRAM STT_FUNC +arm7clearRAM: + + push {r0-r9} + // clear exclusive IWRAM + // 0380:0000 to 0380:FFFF, total 64KiB + mov r0, #0 + mov r1, #0 + mov r2, #0 + mov r3, #0 + mov r4, #0 + mov r5, #0 + mov r6, #0 + mov r7, #0 + mov r8, #0x03800000 + sub r8, #0x00008000 + mov r9, #0x03800000 + orr r9, r9, #0x10000 +clear_IWRAM_loop: + stmia r8!, {r0, r1, r2, r3, r4, r5, r6, r7} + cmp r8, r9 + blt clear_IWRAM_loop + + // clear most of EWRAM - except after RAM end - 0xc000, which has the bootstub + mov r8, #0x02000000 + + ldr r9,=0x4004008 + ldr r9,[r9] + ands r9,r9,#0x8000 + bne dsi_mode + + mov r9, #0x02400000 + b ds_mode +dsi_mode: + mov r9, #0x03000000 +ds_mode: + sub r9, #0x0000c000 +clear_EWRAM_loop: + stmia r8!, {r0, r1, r2, r3, r4, r5, r6, r7} + cmp r8, r9 + blt clear_EWRAM_loop + + pop {r0-r9} + + bx lr + diff --git a/source/arm9clear.arm.c b/source/arm9clear.arm.c new file mode 100644 index 0000000..9a076f9 --- /dev/null +++ b/source/arm9clear.arm.c @@ -0,0 +1,94 @@ +#define ARM9 +#undef ARM7 + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "boot.h" + +/*------------------------------------------------------------------------- +resetMemory2_ARM9 +Clears the ARM9's DMA channels and resets video memory +Written by Darkain. +Modified by Chishm: + * Changed MultiNDS specific stuff +--------------------------------------------------------------------------*/ +void __attribute__ ((long_call)) __attribute__((naked)) __attribute__((noreturn)) resetMemory2_ARM9 (void) +{ + register int i; + + //clear out ARM9 DMA channels + for (i=0; i<4; i++) { + DMA_CR(i) = 0; + DMA_SRC(i) = 0; + DMA_DEST(i) = 0; + TIMER_CR(i) = 0; + TIMER_DATA(i) = 0; + } + + VRAM_CR = (VRAM_CR & 0xffff0000) | 0x00008080 ; + + vu16 *mainregs = (vu16*)0x04000000; + vu16 *subregs = (vu16*)0x04001000; + + for (i=0; i<43; i++) { + mainregs[i] = 0; + subregs[i] = 0; + } + + REG_DISPSTAT = 0; + + VRAM_A_CR = 0; + VRAM_B_CR = 0; +// Don't mess with the ARM7's VRAM +// VRAM_C_CR = 0; + VRAM_D_CR = 0; + VRAM_E_CR = 0; + VRAM_F_CR = 0; + VRAM_G_CR = 0; + VRAM_H_CR = 0; + VRAM_I_CR = 0; + REG_POWERCNT = 0x820F; + + //set shared ram to ARM7 + WRAM_CR = 0x03; + + // Return to passme loop + *((vu32*)0x02FFFE04) = (u32)0xE59FF018; // ldr pc, 0x02FFFE24 + *((vu32*)0x02FFFE24) = (u32)0x02FFFE04; // Set ARM9 Loop address + + asm volatile( + "\tbx %0\n" + : : "r" (0x02FFFE04) + ); + while(1); +} + +/*------------------------------------------------------------------------- +startBinary_ARM9 +Jumps to the ARM9 NDS binary in sync with the display and ARM7 +Written by Darkain. +Modified by Chishm: + * Removed MultiNDS specific stuff +--------------------------------------------------------------------------*/ +void __attribute__ ((long_call)) __attribute__((noreturn)) __attribute__((naked)) startBinary_ARM9 (void) +{ + REG_IME=0; + REG_EXMEMCNT = 0xE880; + // set ARM9 load address to 0 and wait for it to change again + ARM9_START_FLAG = 0; + while(REG_VCOUNT!=191); + while(REG_VCOUNT==191); + while ( ARM9_START_FLAG != 1 ); + VoidFn arm9code = *(VoidFn*)(0x2FFFE24); + arm9code(); + while(1); +} + diff --git a/source/arm9mpu_reset.s b/source/arm9mpu_reset.s new file mode 100644 index 0000000..bbd10f0 --- /dev/null +++ b/source/arm9mpu_reset.s @@ -0,0 +1,6 @@ + .arm + .global mpu_reset, mpu_reset_end + +mpu_reset: + .incbin "mpu_reset.bin" +mpu_reset_end: diff --git a/source/bios.s b/source/bios.s new file mode 100644 index 0000000..e98f57c --- /dev/null +++ b/source/bios.s @@ -0,0 +1,13 @@ + .text + .align 4 + + .thumb + +@--------------------------------------------------------------------------------- + .global swiDelay + .thumb_func +@--------------------------------------------------------------------------------- +swiDelay: +@--------------------------------------------------------------------------------- + swi 0x03 + bx lr diff --git a/source/boot.c b/source/boot.c new file mode 100644 index 0000000..0e431e3 --- /dev/null +++ b/source/boot.c @@ -0,0 +1,387 @@ +/*----------------------------------------------------------------- + boot.c + + BootLoader + Loads a file into memory and runs it + + All resetMemory and startBinary functions are based + on the MultiNDS loader by Darkain. + Original source available at: + http://cvs.sourceforge.net/viewcvs.py/ndslib/ndslib/examples/loader/boot/main.cpp + +License: + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com + +Helpful information: + This code runs from VRAM bank C on ARM7 +------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include "fat.h" +#include "dldi_patcher.h" +#include "card.h" +#include "boot.h" +#include "sdmmc.h" + +void arm7clearRAM(); + +//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Important things +#define TEMP_MEM 0x02FFD000 +#define TWL_HEAD 0x02FFE000 +#define NDS_HEAD 0x02FFFE00 +#define TEMP_ARM9_START_ADDRESS (*(vu32*)0x02FFFFF4) + + +const char* bootName = "BOOT.NDS"; + +extern unsigned long _start; +extern unsigned long storedFileCluster; +extern unsigned long initDisc; +extern unsigned long wantToPatchDLDI; +extern unsigned long argStart; +extern unsigned long argSize; +extern unsigned long dsiSD; +extern unsigned long dsiMode; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Firmware stuff + +#define FW_READ 0x03 + +void boot_readFirmware (uint32 address, uint8 * buffer, uint32 size) { + uint32 index; + + // Read command + while (REG_SPICNT & SPI_BUSY); + REG_SPICNT = SPI_ENABLE | SPI_CONTINUOUS | SPI_DEVICE_NVRAM; + REG_SPIDATA = FW_READ; + while (REG_SPICNT & SPI_BUSY); + + // Set the address + REG_SPIDATA = (address>>16) & 0xFF; + while (REG_SPICNT & SPI_BUSY); + REG_SPIDATA = (address>>8) & 0xFF; + while (REG_SPICNT & SPI_BUSY); + REG_SPIDATA = (address) & 0xFF; + while (REG_SPICNT & SPI_BUSY); + + for (index = 0; index < size; index++) { + REG_SPIDATA = 0; + while (REG_SPICNT & SPI_BUSY); + buffer[index] = REG_SPIDATA & 0xFF; + } + REG_SPICNT = 0; +} + + +static inline void copyLoop (u32* dest, const u32* src, u32 size) { + size = (size +3) & ~3; + do { + *dest++ = *src++; + } while (size -= 4); +} + +//#define resetCpu() __asm volatile("\tswi 0x000000\n"); + +static char boot_nds[] = "fat:/boot.nds"; +static unsigned long argbuf[4]; +/*------------------------------------------------------------------------- +passArgs_ARM7 +Copies the command line arguments to the end of the ARM9 binary, +then sets a flag in memory for the loaded NDS to use +--------------------------------------------------------------------------*/ +void passArgs_ARM7 (void) { + u32 ARM9_DST = *((u32*)(NDS_HEAD + 0x028)); + u32 ARM9_LEN = *((u32*)(NDS_HEAD + 0x02C)); + u32* argSrc; + u32* argDst; + + if (!argStart || !argSize) { + + char *arg = boot_nds; + argSize = __builtin_strlen(boot_nds); + + if (dsiSD) { + arg++; + arg[0] = 's'; + arg[1] = 'd'; + } + __builtin_memcpy(argbuf,arg,argSize+1); + argSrc = argbuf; + } else { + argSrc = (u32*)(argStart + (int)&_start); + } + + if ( ARM9_DST == 0 && ARM9_LEN == 0) { + ARM9_DST = *((u32*)(NDS_HEAD + 0x038)); + ARM9_LEN = *((u32*)(NDS_HEAD + 0x03C)); + } + + + argDst = (u32*)((ARM9_DST + ARM9_LEN + 3) & ~3); // Word aligned + + if (dsiMode && (*(u8*)(NDS_HEAD + 0x012) & BIT(1))) + { + u32 ARM9i_DST = *((u32*)(TWL_HEAD + 0x1C8)); + u32 ARM9i_LEN = *((u32*)(TWL_HEAD + 0x1CC)); + if (ARM9i_LEN) + { + u32* argDst2 = (u32*)((ARM9i_DST + ARM9i_LEN + 3) & ~3); // Word aligned + if (argDst2 > argDst) + argDst = argDst2; + } + } + + copyLoop(argDst, argSrc, argSize); + + __system_argv->argvMagic = ARGV_MAGIC; + __system_argv->commandLine = (char*)argDst; + __system_argv->length = argSize; +} + + + + +/*------------------------------------------------------------------------- +resetMemory_ARM7 +Clears all of the NDS's RAM that is visible to the ARM7 +Written by Darkain. +Modified by Chishm: + * Added STMIA clear mem loop +--------------------------------------------------------------------------*/ +void resetMemory_ARM7 (void) +{ + int i; + u8 settings1, settings2; + u32 settingsOffset = 0; + + REG_IME = 0; + + for (i=0; i<16; i++) { + SCHANNEL_CR(i) = 0; + SCHANNEL_TIMER(i) = 0; + SCHANNEL_SOURCE(i) = 0; + SCHANNEL_LENGTH(i) = 0; + } + + REG_SOUNDCNT = 0; + + //clear out ARM7 DMA channels and timers + for (i=0; i<4; i++) { + DMA_CR(i) = 0; + DMA_SRC(i) = 0; + DMA_DEST(i) = 0; + TIMER_CR(i) = 0; + TIMER_DATA(i) = 0; + } + + arm7clearRAM(); + + REG_IE = 0; + REG_IF = ~0; + (*(vu32*)(0x04000000-4)) = 0; //IRQ_HANDLER ARM7 version + (*(vu32*)(0x04000000-8)) = ~0; //VBLANK_INTR_WAIT_FLAGS, ARM7 version + REG_POWERCNT = 1; //turn off power to stuff + + // Get settings location + boot_readFirmware((u32)0x00020, (u8*)&settingsOffset, 0x2); + settingsOffset *= 8; + + // Reload DS Firmware settings + boot_readFirmware(settingsOffset + 0x070, &settings1, 0x1); + boot_readFirmware(settingsOffset + 0x170, &settings2, 0x1); + + if ((settings1 & 0x7F) == ((settings2+1) & 0x7F)) { + boot_readFirmware(settingsOffset + 0x000, (u8*)0x02FFFC80, 0x70); + } else { + boot_readFirmware(settingsOffset + 0x100, (u8*)0x02FFFC80, 0x70); + } + + ((vu32*)0x040044f0)[2] = 0x202DDD1D; + ((vu32*)0x040044f0)[3] = 0xE1A00005; + while((*(vu32*)0x04004400) & 0x2000000); + +} + + +void loadBinary_ARM7 (u32 fileCluster) +{ + u32 ndsHeader[0x170>>2]; + + // read NDS header + fileRead ((char*)ndsHeader, fileCluster, 0, 0x170); + // read ARM9 info from NDS header + u32 ARM9_SRC = ndsHeader[0x020>>2]; + char* ARM9_DST = (char*)ndsHeader[0x028>>2]; + u32 ARM9_LEN = ndsHeader[0x02C>>2]; + // read ARM7 info from NDS header + u32 ARM7_SRC = ndsHeader[0x030>>2]; + char* ARM7_DST = (char*)ndsHeader[0x038>>2]; + u32 ARM7_LEN = ndsHeader[0x03C>>2]; + + // Load binaries into memory + fileRead(ARM9_DST, fileCluster, ARM9_SRC, ARM9_LEN); + fileRead(ARM7_DST, fileCluster, ARM7_SRC, ARM7_LEN); + + // first copy the header to its proper location, excluding + // the ARM9 start address, so as not to start it + TEMP_ARM9_START_ADDRESS = ndsHeader[0x024>>2]; // Store for later + ndsHeader[0x024>>2] = 0; + dmaCopyWords(3, (void*)ndsHeader, (void*)NDS_HEAD, 0x170); + + if (dsiMode && (ndsHeader[0x10>>2]&BIT(16+1))) + { + // Read full TWL header + fileRead((char*)TWL_HEAD, fileCluster, 0, 0x1000); + + u32 ARM9i_SRC = *(u32*)(TWL_HEAD+0x1C0); + char* ARM9i_DST = (char*)*(u32*)(TWL_HEAD+0x1C8); + u32 ARM9i_LEN = *(u32*)(TWL_HEAD+0x1CC); + u32 ARM7i_SRC = *(u32*)(TWL_HEAD+0x1D0); + char* ARM7i_DST = (char*)*(u32*)(TWL_HEAD+0x1D8); + u32 ARM7i_LEN = *(u32*)(TWL_HEAD+0x1DC); + + if (ARM9i_LEN) + fileRead(ARM9i_DST, fileCluster, ARM9i_SRC, ARM9i_LEN); + if (ARM7i_LEN) + fileRead(ARM7i_DST, fileCluster, ARM7i_SRC, ARM7i_LEN); + } +} + +/*------------------------------------------------------------------------- +startBinary_ARM7 +Jumps to the ARM7 NDS binary in sync with the display and ARM9 +Written by Darkain. +Modified by Chishm: + * Removed MultiNDS specific stuff +--------------------------------------------------------------------------*/ +void startBinary_ARM7 (void) { + REG_IME=0; + while(REG_VCOUNT!=191); + while(REG_VCOUNT==191); + // copy NDS ARM9 start address into the header, starting ARM9 + *((vu32*)0x02FFFE24) = TEMP_ARM9_START_ADDRESS; + ARM9_START_FLAG = 1; + // Start ARM7 + VoidFn arm7code = *(VoidFn*)(0x2FFFE34); + arm7code(); +} +#ifndef NO_SDMMC +int sdmmc_sd_readsectors(u32 sector_no, u32 numsectors, void *out); +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Main function +bool sdmmc_inserted() { + return true; +} + +bool sdmmc_startup() { + sdmmc_controller_init(true); + return sdmmc_sdcard_init() == 0; +} + +bool sdmmc_readsectors(u32 sector_no, u32 numsectors, void *out) { + return sdmmc_sdcard_readsectors(sector_no, numsectors, out) == 0; +} +#endif +void mpu_reset(); +void mpu_reset_end(); + +int main (void) { +#ifdef NO_DLDI + dsiSD = true; + dsiMode = true; +#endif +#ifndef NO_SDMMC + if (dsiSD && dsiMode) { + _io_dldi.fn_readSectors = sdmmc_readsectors; + _io_dldi.fn_isInserted = sdmmc_inserted; + _io_dldi.fn_startup = sdmmc_startup; + } +#endif + u32 fileCluster = storedFileCluster; + // Init card + if(!FAT_InitFiles(initDisc)) + { + return -1; + } + if ((fileCluster < CLUSTER_FIRST) || (fileCluster >= CLUSTER_EOF)) /* Invalid file cluster specified */ + { + fileCluster = getBootFileCluster(bootName); + } + if (fileCluster == CLUSTER_FREE) + { + return -1; + } + + // ARM9 clears its memory part 2 + // copy ARM9 function to RAM, and make the ARM9 jump to it + copyLoop((void*)TEMP_MEM, (void*)resetMemory2_ARM9, resetMemory2_ARM9_size); + (*(vu32*)0x02FFFE24) = (u32)TEMP_MEM; // Make ARM9 jump to the function + // Wait until the ARM9 has completed its task + while ((*(vu32*)0x02FFFE24) == (u32)TEMP_MEM); + + // ARM9 sets up mpu + // copy ARM9 function to RAM, and make the ARM9 jump to it + copyLoop((void*)TEMP_MEM, (void*)mpu_reset, mpu_reset_end - mpu_reset); + (*(vu32*)0x02FFFE24) = (u32)TEMP_MEM; // Make ARM9 jump to the function + // Wait until the ARM9 has completed its task + while ((*(vu32*)0x02FFFE24) == (u32)TEMP_MEM); + + // Get ARM7 to clear RAM + resetMemory_ARM7(); + + // ARM9 enters a wait loop + // copy ARM9 function to RAM, and make the ARM9 jump to it + copyLoop((void*)TEMP_MEM, (void*)startBinary_ARM9, startBinary_ARM9_size); + (*(vu32*)0x02FFFE24) = (u32)TEMP_MEM; // Make ARM9 jump to the function + + // Load the NDS file + loadBinary_ARM7(fileCluster); + +#ifndef NO_DLDI + // Patch with DLDI if desired + if (wantToPatchDLDI) { + dldiPatchBinary ((u8*)((u32*)NDS_HEAD)[0x0A], ((u32*)NDS_HEAD)[0x0B]); + } +#endif + +#ifndef NO_SDMMC + if (dsiSD && dsiMode) { + sdmmc_controller_init(true); + *(vu16*)(SDMMC_BASE + REG_SDDATACTL32) &= 0xFFFDu; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL) &= 0xFFDDu; + *(vu16*)(SDMMC_BASE + REG_SDBLKLEN32) = 0; + } +#endif + // Pass command line arguments to loaded program + passArgs_ARM7(); + + startBinary_ARM7(); + + return 0; +} + diff --git a/source/boot.h b/source/boot.h new file mode 100644 index 0000000..e71d9ff --- /dev/null +++ b/source/boot.h @@ -0,0 +1,10 @@ +#ifndef _BOOT_H_ +#define _BOOT_H_ + +#define resetMemory2_ARM9_size 0x400 +void __attribute__ ((long_call)) __attribute__((naked)) __attribute__((noreturn)) resetMemory2_ARM9(); +#define startBinary_ARM9_size 0x100 +void __attribute__ ((long_call)) __attribute__((noreturn)) __attribute__((naked)) startBinary_ARM9 (); +#define ARM9_START_FLAG (*(vu8*)0x02FFFDFB) + +#endif // _BOOT_H_ diff --git a/source/card.h b/source/card.h new file mode 100644 index 0000000..cd6f6fb --- /dev/null +++ b/source/card.h @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + +#ifndef CARD_H +#define CARD_H + +#include "disc_io.h" +#include "io_dldi.h" + +static inline bool CARD_StartUp (void) { + return _io_dldi.fn_startup(); +} + +static inline bool CARD_IsInserted (void) { + return _io_dldi.fn_isInserted(); +} + +static inline bool CARD_ReadSector (u32 sector, void *buffer) { + return _io_dldi.fn_readSectors(sector, 1, buffer); +} + +static inline bool CARD_ReadSectors (u32 sector, int count, void *buffer) { + return _io_dldi.fn_readSectors(sector, count, buffer); +} + +#endif // CARD_H diff --git a/source/disc_io.h b/source/disc_io.h new file mode 100644 index 0000000..29033b7 --- /dev/null +++ b/source/disc_io.h @@ -0,0 +1,82 @@ +/* + disc_io.h + Interface template for low level disc functions. + + Copyright (c) 2006 Michael "Chishm" Chisholm + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + 3. The name of the author may not be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY + AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + 2006-07-11 - Chishm + * Original release + + 2006-07-16 - Chishm + * Renamed _CF_USE_DMA to _IO_USE_DMA + * Renamed _CF_ALLOW_UNALIGNED to _IO_ALLOW_UNALIGNED +*/ + +#ifndef _DISC_IO_H +#define _DISC_IO_H + +#include +#define BYTES_PER_SECTOR 512 + +//---------------------------------------------------------------------- +// Customisable features + +// Use DMA to read the card, remove this line to use normal reads/writes +// #define _IO_USE_DMA + +// Allow buffers not alligned to 16 bits when reading files. +// Note that this will slow down access speed, so only use if you have to. +// It is also incompatible with DMA +#define _IO_ALLOW_UNALIGNED + +#if defined _IO_USE_DMA && defined _IO_ALLOW_UNALIGNED + #error "You can't use both DMA and unaligned memory" +#endif + +#define FEATURE_MEDIUM_CANREAD 0x00000001 +#define FEATURE_MEDIUM_CANWRITE 0x00000002 +#define FEATURE_SLOT_GBA 0x00000010 +#define FEATURE_SLOT_NDS 0x00000020 + +typedef bool (* FN_MEDIUM_STARTUP)(void) ; +typedef bool (* FN_MEDIUM_ISINSERTED)(void) ; +typedef bool (* FN_MEDIUM_READSECTORS)(u32 sector, u32 numSectors, void* buffer) ; +typedef bool (* FN_MEDIUM_WRITESECTORS)(u32 sector, u32 numSectors, const void* buffer) ; +typedef bool (* FN_MEDIUM_CLEARSTATUS)(void) ; +typedef bool (* FN_MEDIUM_SHUTDOWN)(void) ; + +struct IO_INTERFACE_STRUCT { + unsigned long ioType ; + unsigned long features ; + FN_MEDIUM_STARTUP fn_startup ; + FN_MEDIUM_ISINSERTED fn_isInserted ; + FN_MEDIUM_READSECTORS fn_readSectors ; + FN_MEDIUM_WRITESECTORS fn_writeSectors ; + FN_MEDIUM_CLEARSTATUS fn_clearStatus ; + FN_MEDIUM_SHUTDOWN fn_shutdown ; +} ; + +typedef struct IO_INTERFACE_STRUCT IO_INTERFACE ; + +#endif // define _DISC_IO_H diff --git a/source/dldi_patcher.c b/source/dldi_patcher.c new file mode 100644 index 0000000..061b955 --- /dev/null +++ b/source/dldi_patcher.c @@ -0,0 +1,223 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ +#ifndef NO_DLDI +#include +#include +#include "dldi_patcher.h" + +#define FIX_ALL 0x01 +#define FIX_GLUE 0x02 +#define FIX_GOT 0x04 +#define FIX_BSS 0x08 + +enum DldiOffsets { + DO_magicString = 0x00, // "\xED\xA5\x8D\xBF Chishm" + DO_magicToken = 0x00, // 0xBF8DA5ED + DO_magicShortString = 0x04, // " Chishm" + DO_version = 0x0C, + DO_driverSize = 0x0D, + DO_fixSections = 0x0E, + DO_allocatedSpace = 0x0F, + + DO_friendlyName = 0x10, + + DO_text_start = 0x40, // Data start + DO_data_end = 0x44, // Data end + DO_glue_start = 0x48, // Interworking glue start -- Needs address fixing + DO_glue_end = 0x4C, // Interworking glue end + DO_got_start = 0x50, // GOT start -- Needs address fixing + DO_got_end = 0x54, // GOT end + DO_bss_start = 0x58, // bss start -- Needs setting to zero + DO_bss_end = 0x5C, // bss end + + // IO_INTERFACE data + DO_ioType = 0x60, + DO_features = 0x64, + DO_startup = 0x68, + DO_isInserted = 0x6C, + DO_readSectors = 0x70, + DO_writeSectors = 0x74, + DO_clearStatus = 0x78, + DO_shutdown = 0x7C, + DO_code = 0x80 +}; + +static addr_t readAddr (data_t *mem, addr_t offset) { + return ((addr_t*)mem)[offset/sizeof(addr_t)]; +} + +static void writeAddr (data_t *mem, addr_t offset, addr_t value) { + ((addr_t*)mem)[offset/sizeof(addr_t)] = value; +} + + +static addr_t quickFind (const data_t* data, const data_t* search, size_t dataLen, size_t searchLen) { + const int* dataChunk = (const int*) data; + int searchChunk = ((const int*)search)[0]; + addr_t i; + addr_t dataChunkEnd = (addr_t)(dataLen / sizeof(int)); + + for ( i = 0; i < dataChunkEnd; i++) { + if (dataChunk[i] == searchChunk) { + if ((i*sizeof(int) + searchLen) > dataLen) { + return -1; + } + if (memcmp (&data[i*sizeof(int)], search, searchLen) == 0) { + return i*sizeof(int); + } + } + } + + return -1; +} + +// Strings are stored with bit 0x20 flipped (case inverted) to prevent accidental DLDI patching of them +#define DLDI_MAGIC_LEN 12 +#define DLDI_MAGIC_MANGLE_VALUE 0x20 +static const data_t dldiMagicStringMangled[DLDI_MAGIC_LEN] = "\xCD\x85\xAD\x9F\0cHISHM"; // Normal DLDI file + +// Demangle the magic string by XORing every byte with 0x20, except the NULL terminator +static void demangleMagicString(data_t *dest, const data_t *src) { + int i; + + memcpy(dest, src, DLDI_MAGIC_LEN); + for (i = 0; i < DLDI_MAGIC_LEN - 1; ++i) { + dest[i] ^= DLDI_MAGIC_MANGLE_VALUE; + } +} + +#define DEVICE_TYPE_DLDI 0x49444C44 + +extern data_t _dldi_start[]; + +bool dldiPatchBinary (data_t *binData, u32 binSize) { + + addr_t memOffset; // Offset of DLDI after the file is loaded into memory + addr_t patchOffset; // Position of patch destination in the file + addr_t relocationOffset; // Value added to all offsets within the patch to fix it properly + addr_t ddmemOffset; // Original offset used in the DLDI file + addr_t ddmemStart; // Start of range that offsets can be in the DLDI file + addr_t ddmemEnd; // End of range that offsets can be in the DLDI file + addr_t ddmemSize; // Size of range that offsets can be in the DLDI file + addr_t addrIter; + + data_t *pDH; + data_t *pAH; + + data_t dldiMagicString[DLDI_MAGIC_LEN]; + size_t dldiFileSize = 0; + + // Find the DLDI reserved space in the file + demangleMagicString(dldiMagicString, dldiMagicStringMangled); + patchOffset = quickFind (binData, dldiMagicString, binSize, sizeof(dldiMagicString)); + + if (patchOffset < 0) { + // does not have a DLDI section + return false; + } + + pDH = _dldi_start; + pAH = &(binData[patchOffset]); + + if (*((u32*)(pDH + DO_ioType)) == DEVICE_TYPE_DLDI) { + // No DLDI patch + return false; + } + + if (pDH[DO_driverSize] > pAH[DO_allocatedSpace]) { + // Not enough space for patch + return false; + } + + dldiFileSize = 1 << pDH[DO_driverSize]; + + memOffset = readAddr (pAH, DO_text_start); + if (memOffset == 0) { + memOffset = readAddr (pAH, DO_startup) - DO_code; + } + ddmemOffset = readAddr (pDH, DO_text_start); + relocationOffset = memOffset - ddmemOffset; + + ddmemStart = readAddr (pDH, DO_text_start); + ddmemSize = (1 << pDH[DO_driverSize]); + ddmemEnd = ddmemStart + ddmemSize; + + // Remember how much space is actually reserved + pDH[DO_allocatedSpace] = pAH[DO_allocatedSpace]; + // Copy the DLDI patch into the application + memcpy (pAH, pDH, dldiFileSize); + + // Fix the section pointers in the header + writeAddr (pAH, DO_text_start, readAddr (pAH, DO_text_start) + relocationOffset); + writeAddr (pAH, DO_data_end, readAddr (pAH, DO_data_end) + relocationOffset); + writeAddr (pAH, DO_glue_start, readAddr (pAH, DO_glue_start) + relocationOffset); + writeAddr (pAH, DO_glue_end, readAddr (pAH, DO_glue_end) + relocationOffset); + writeAddr (pAH, DO_got_start, readAddr (pAH, DO_got_start) + relocationOffset); + writeAddr (pAH, DO_got_end, readAddr (pAH, DO_got_end) + relocationOffset); + writeAddr (pAH, DO_bss_start, readAddr (pAH, DO_bss_start) + relocationOffset); + writeAddr (pAH, DO_bss_end, readAddr (pAH, DO_bss_end) + relocationOffset); + // Fix the function pointers in the header + writeAddr (pAH, DO_startup, readAddr (pAH, DO_startup) + relocationOffset); + writeAddr (pAH, DO_isInserted, readAddr (pAH, DO_isInserted) + relocationOffset); + writeAddr (pAH, DO_readSectors, readAddr (pAH, DO_readSectors) + relocationOffset); + writeAddr (pAH, DO_writeSectors, readAddr (pAH, DO_writeSectors) + relocationOffset); + writeAddr (pAH, DO_clearStatus, readAddr (pAH, DO_clearStatus) + relocationOffset); + writeAddr (pAH, DO_shutdown, readAddr (pAH, DO_shutdown) + relocationOffset); + + // Put the correct DLDI magic string back into the DLDI header + memcpy (pAH, dldiMagicString, sizeof (dldiMagicString)); + + if (pDH[DO_fixSections] & FIX_ALL) { + // Search through and fix pointers within the data section of the file + for (addrIter = (readAddr(pDH, DO_text_start) - ddmemStart); addrIter < (readAddr(pDH, DO_data_end) - ddmemStart); addrIter++) { + if ((ddmemStart <= readAddr(pAH, addrIter)) && (readAddr(pAH, addrIter) < ddmemEnd)) { + writeAddr (pAH, addrIter, readAddr(pAH, addrIter) + relocationOffset); + } + } + } + + if (pDH[DO_fixSections] & FIX_GLUE) { + // Search through and fix pointers within the glue section of the file + for (addrIter = (readAddr(pDH, DO_glue_start) - ddmemStart); addrIter < (readAddr(pDH, DO_glue_end) - ddmemStart); addrIter++) { + if ((ddmemStart <= readAddr(pAH, addrIter)) && (readAddr(pAH, addrIter) < ddmemEnd)) { + writeAddr (pAH, addrIter, readAddr(pAH, addrIter) + relocationOffset); + } + } + } + + if (pDH[DO_fixSections] & FIX_GOT) { + // Search through and fix pointers within the Global Offset Table section of the file + for (addrIter = (readAddr(pDH, DO_got_start) - ddmemStart); addrIter < (readAddr(pDH, DO_got_end) - ddmemStart); addrIter++) { + if ((ddmemStart <= readAddr(pAH, addrIter)) && (readAddr(pAH, addrIter) < ddmemEnd)) { + writeAddr (pAH, addrIter, readAddr(pAH, addrIter) + relocationOffset); + } + } + } + + if (pDH[DO_fixSections] & FIX_BSS) { + // Initialise the BSS to 0 + memset (&pAH[readAddr(pDH, DO_bss_start) - ddmemStart] , 0, readAddr(pDH, DO_bss_end) - readAddr(pDH, DO_bss_start)); + } + + return true; +} +#endif \ No newline at end of file diff --git a/source/dldi_patcher.h b/source/dldi_patcher.h new file mode 100644 index 0000000..f54c531 --- /dev/null +++ b/source/dldi_patcher.h @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + +#ifndef DLDI_PATCHER_H +#define DLDI_PATCHER_H + +#include + +typedef signed int addr_t; +typedef unsigned char data_t; +bool dldiPatchBinary (data_t *binData, u32 binSize); + +#endif // DLDI_PATCHER_H diff --git a/source/fat.c b/source/fat.c new file mode 100644 index 0000000..f1c2e33 --- /dev/null +++ b/source/fat.c @@ -0,0 +1,592 @@ +/*----------------------------------------------------------------- + fat.c + + NDS MP + GBAMP NDS Firmware Hack Version 2.12 + An NDS aware firmware patch for the GBA Movie Player. + By Michael Chisholm (Chishm) + + Filesystem code based on GBAMP_CF.c by Chishm (me). + +License: + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + +#include "fat.h" +#include "card.h" + + +//--------------------------------------------------------------- +// FAT constants + +#define FILE_LAST 0x00 +#define FILE_FREE 0xE5 + +#define ATTRIB_ARCH 0x20 +#define ATTRIB_DIR 0x10 +#define ATTRIB_LFN 0x0F +#define ATTRIB_VOL 0x08 +#define ATTRIB_HID 0x02 +#define ATTRIB_SYS 0x04 +#define ATTRIB_RO 0x01 + +#define FAT16_ROOT_DIR_CLUSTER 0x00 + +// File Constants +#ifndef EOF +#define EOF -1 +#define SEEK_SET 0 +#define SEEK_CUR 1 +#define SEEK_END 2 +#endif + + +//----------------------------------------------------------------- +// FAT constants +#define CLUSTER_EOF_16 0xFFFF + +#define ATTRIB_ARCH 0x20 +#define ATTRIB_DIR 0x10 +#define ATTRIB_LFN 0x0F +#define ATTRIB_VOL 0x08 +#define ATTRIB_HID 0x02 +#define ATTRIB_SYS 0x04 +#define ATTRIB_RO 0x01 + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Data Structures + +#define __PACKED __attribute__ ((__packed__)) + +// BIOS Parameter Block +typedef struct { + + u16 bytesPerSector; + u8 sectorsPerCluster; + u16 reservedSectors; + u8 numFATs; + u16 rootEntries; + u16 numSectorsSmall; + u8 mediaDesc; + u16 sectorsPerFAT; + u16 sectorsPerTrk; + u16 numHeads; + u32 numHiddenSectors; + u32 numSectors; +} __PACKED BIOS_BPB; + +// Boot Sector - must be packed +typedef struct +{ + u8 jmpBoot[3]; + u8 OEMName[8]; + BIOS_BPB bpb; + union // Different types of extended BIOS Parameter Block for FAT16 and FAT32 + { + struct + { + // Ext BIOS Parameter Block for FAT16 + u8 driveNumber; + u8 reserved1; + u8 extBootSig; + u32 volumeID; + u8 volumeLabel[11]; + u8 fileSysType[8]; + // Bootcode + u8 bootCode[448]; + } __PACKED fat16; + struct + { + // FAT32 extended block + u32 sectorsPerFAT32; + u16 extFlags; + u16 fsVer; + u32 rootClus; + u16 fsInfo; + u16 bkBootSec; + u8 reserved[12]; + // Ext BIOS Parameter Block for FAT16 + u8 driveNumber; + u8 reserved1; + u8 extBootSig; + u32 volumeID; + u8 volumeLabel[11]; + u8 fileSysType[8]; + // Bootcode + u8 bootCode[420]; + } __PACKED fat32; + } __PACKED extBlock; + + __PACKED u16 bootSig; + +} __PACKED BOOT_SEC; + +_Static_assert(sizeof(BOOT_SEC) == 512); + +// Directory entry - must be packed +typedef struct +{ + u8 name[8]; + u8 ext[3]; + u8 attrib; + u8 reserved; + u8 cTime_ms; + u16 cTime; + u16 cDate; + u16 aDate; + u16 startClusterHigh; + u16 mTime; + u16 mDate; + u16 startCluster; + u32 fileSize; +} __PACKED DIR_ENT; + +// File information - no need to pack +typedef struct +{ + u32 firstCluster; + u32 length; + u32 curPos; + u32 curClus; // Current cluster to read from + int curSect; // Current sector within cluster + int curByte; // Current byte within sector + char readBuffer[512]; // Buffer used for unaligned reads + u32 appClus; // Cluster to append to + int appSect; // Sector within cluster for appending + int appByte; // Byte within sector for appending + bool read; // Can read from file + bool write; // Can write to file + bool append;// Can append to file + bool inUse; // This file is open + u32 dirEntSector; // The sector where the directory entry is stored + int dirEntOffset; // The offset within the directory sector +} FAT_FILE; + + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// Global Variables + +// _VARS_IN_RAM variables are stored in the largest section of WRAM +// available: IWRAM on NDS ARM7, EWRAM on NDS ARM9 and GBA + +// Locations on card +int discRootDir; +int discRootDirClus; +int discFAT; +int discSecPerFAT; +int discNumSec; +int discData; +int discBytePerSec; +int discSecPerClus; +int discBytePerClus; + +enum {FS_UNKNOWN, FS_FAT12, FS_FAT16, FS_FAT32} discFileSystem; + +// Global sector buffer to save on stack space +unsigned char globalBuffer[BYTES_PER_SECTOR]; + + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +//FAT routines + +u32 FAT_ClustToSect (u32 cluster) { + return (((cluster-2) * discSecPerClus) + discData); +} + +/*----------------------------------------------------------------- +FAT_NextCluster +Internal function - gets the cluster linked from input cluster +-----------------------------------------------------------------*/ +u32 FAT_NextCluster(u32 cluster) +{ + u32 nextCluster = CLUSTER_FREE; + u32 sector; + int offset; + + + switch (discFileSystem) + { + case FS_UNKNOWN: + nextCluster = CLUSTER_FREE; + break; + + case FS_FAT12: + sector = discFAT + (((cluster * 3) / 2) / BYTES_PER_SECTOR); + offset = ((cluster * 3) / 2) % BYTES_PER_SECTOR; + CARD_ReadSector(sector, globalBuffer); + nextCluster = ((u8*) globalBuffer)[offset]; + offset++; + + if (offset >= BYTES_PER_SECTOR) { + offset = 0; + sector++; + } + + CARD_ReadSector(sector, globalBuffer); + nextCluster |= (((u8*) globalBuffer)[offset]) << 8; + + if (cluster & 0x01) { + nextCluster = nextCluster >> 4; + } else { + nextCluster &= 0x0FFF; + } + + break; + + case FS_FAT16: + sector = discFAT + ((cluster << 1) / BYTES_PER_SECTOR); + offset = cluster % (BYTES_PER_SECTOR >> 1); + + CARD_ReadSector(sector, globalBuffer); + // read the nextCluster value + nextCluster = ((u16*)globalBuffer)[offset]; + + if (nextCluster >= 0xFFF7) + { + nextCluster = CLUSTER_EOF; + } + break; + + case FS_FAT32: + sector = discFAT + ((cluster << 2) / BYTES_PER_SECTOR); + offset = cluster % (BYTES_PER_SECTOR >> 2); + + CARD_ReadSector(sector, globalBuffer); + // read the nextCluster value + nextCluster = (((u32*)globalBuffer)[offset]) & 0x0FFFFFFF; + + if (nextCluster >= 0x0FFFFFF7) + { + nextCluster = CLUSTER_EOF; + } + break; + + default: + nextCluster = CLUSTER_FREE; + break; + } + + return nextCluster; +} + +/*----------------------------------------------------------------- +ucase +Returns the uppercase version of the given char +char IN: a character +char return OUT: uppercase version of character +-----------------------------------------------------------------*/ +char ucase (char character) +{ + if ((character > 0x60) && (character < 0x7B)) + character = character - 0x20; + return (character); +} + +/*----------------------------------------------------------------- +FAT_InitFiles +Reads the FAT information from the CF card. +You need to call this before reading any files. +bool return OUT: true if successful. +-----------------------------------------------------------------*/ +bool FAT_InitFiles (bool initCard) +{ + int i; + int bootSector; + BOOT_SEC* bootSec; + + if (initCard && !CARD_StartUp()) + { + return (false); + } + + // Read first sector of card + if (!CARD_ReadSector (0, globalBuffer)) + { + return false; + } + // Check if there is a FAT string, which indicates this is a boot sector + if ((globalBuffer[0x36] == 'F') && (globalBuffer[0x37] == 'A') && (globalBuffer[0x38] == 'T')) + { + bootSector = 0; + } + // Check for FAT32 + else if ((globalBuffer[0x52] == 'F') && (globalBuffer[0x53] == 'A') && (globalBuffer[0x54] == 'T')) + { + bootSector = 0; + } + else // This is an MBR + { + // Find first valid partition from MBR + // First check for an active partition + for (i=0x1BE; (i < 0x1FE) && (globalBuffer[i] != 0x80); i+= 0x10); + // If it didn't find an active partition, search for any valid partition + if (i == 0x1FE) + for (i=0x1BE; (i < 0x1FE) && (globalBuffer[i+0x04] == 0x00); i+= 0x10); + + // Go to first valid partition + if ( i != 0x1FE) // Make sure it found a partition + { + bootSector = globalBuffer[0x8 + i] + (globalBuffer[0x9 + i] << 8) + (globalBuffer[0xA + i] << 16) + ((globalBuffer[0xB + i] << 24) & 0x0F); + } else { + bootSector = 0; // No partition found, assume this is a MBR free disk + } + } + + // Read in boot sector + bootSec = (BOOT_SEC*) globalBuffer; + CARD_ReadSector (bootSector, bootSec); + + // Store required information about the file system + if (bootSec->bpb.sectorsPerFAT != 0) + { + discSecPerFAT = bootSec->bpb.sectorsPerFAT; + } + else + { + discSecPerFAT = bootSec->extBlock.fat32.sectorsPerFAT32; + } + + if (bootSec->bpb.numSectorsSmall != 0) + { + discNumSec = bootSec->bpb.numSectorsSmall; + } + else + { + discNumSec = bootSec->bpb.numSectors; + } + + discBytePerSec = BYTES_PER_SECTOR; // Sector size is redefined to be 512 bytes + discSecPerClus = bootSec->bpb.sectorsPerCluster * bootSec->bpb.bytesPerSector / BYTES_PER_SECTOR; + discBytePerClus = discBytePerSec * discSecPerClus; + discFAT = bootSector + bootSec->bpb.reservedSectors; + + discRootDir = discFAT + (bootSec->bpb.numFATs * discSecPerFAT); + discData = discRootDir + ((bootSec->bpb.rootEntries * sizeof(DIR_ENT)) / BYTES_PER_SECTOR); + + if ((discNumSec - discData) / bootSec->bpb.sectorsPerCluster < 4085) + { + discFileSystem = FS_FAT12; + } + else if ((discNumSec - discData) / bootSec->bpb.sectorsPerCluster < 65525) + { + discFileSystem = FS_FAT16; + } + else + { + discFileSystem = FS_FAT32; + } + + if (discFileSystem != FS_FAT32) + { + discRootDirClus = FAT16_ROOT_DIR_CLUSTER; + } + else // Set up for the FAT32 way + { + discRootDirClus = bootSec->extBlock.fat32.rootClus; + // Check if FAT mirroring is enabled + if (!(bootSec->extBlock.fat32.extFlags & 0x80)) + { + // Use the active FAT + discFAT = discFAT + ( discSecPerFAT * (bootSec->extBlock.fat32.extFlags & 0x0F)); + } + } + + return (true); +} + + +/*----------------------------------------------------------------- +getBootFileCluster +-----------------------------------------------------------------*/ +u32 getBootFileCluster (const char* bootName) +{ + DIR_ENT dir; + int firstSector = 0; + bool notFound = false; + bool found = false; +// int maxSectors; + u32 wrkDirCluster = discRootDirClus; + u32 wrkDirSector = 0; + int wrkDirOffset = 0; + int nameOffset; + + dir.startCluster = CLUSTER_FREE; // default to no file found + dir.startClusterHigh = CLUSTER_FREE; + + + // Check if fat has been initialised + if (discBytePerSec == 0) + { + return (CLUSTER_FREE); + } + + char *ptr = (char*)bootName; + while (*ptr != '.') ptr++; + int namelen = ptr - bootName; + +// maxSectors = (wrkDirCluster == FAT16_ROOT_DIR_CLUSTER ? (discData - discRootDir) : discSecPerClus); + // Scan Dir for correct entry + firstSector = discRootDir; + CARD_ReadSector (firstSector + wrkDirSector, globalBuffer); + found = false; + notFound = false; + wrkDirOffset = -1; // Start at entry zero, Compensating for increment + while (!found && !notFound) { + wrkDirOffset++; + if (wrkDirOffset == BYTES_PER_SECTOR / sizeof (DIR_ENT)) + { + wrkDirOffset = 0; + wrkDirSector++; + if ((wrkDirSector == discSecPerClus) && (wrkDirCluster != FAT16_ROOT_DIR_CLUSTER)) + { + wrkDirSector = 0; + wrkDirCluster = FAT_NextCluster(wrkDirCluster); + if (wrkDirCluster == CLUSTER_EOF) + { + notFound = true; + } + firstSector = FAT_ClustToSect(wrkDirCluster); + } + else if ((wrkDirCluster == FAT16_ROOT_DIR_CLUSTER) && (wrkDirSector == (discData - discRootDir))) + { + notFound = true; // Got to end of root dir + } + CARD_ReadSector (firstSector + wrkDirSector, globalBuffer); + } + dir = ((DIR_ENT*) globalBuffer)[wrkDirOffset]; + found = true; + if ((dir.attrib & ATTRIB_DIR) || (dir.attrib & ATTRIB_VOL)) + { + found = false; + } + if(namelen<8 && dir.name[namelen]!=0x20) found = false; + for (nameOffset = 0; nameOffset < namelen && found; nameOffset++) + { + if (ucase(dir.name[nameOffset]) != bootName[nameOffset]) + found = false; + } + for (nameOffset = 0; nameOffset < 3 && found; nameOffset++) + { + if (ucase(dir.ext[nameOffset]) != bootName[nameOffset+namelen+1]) + found = false; + } + if (dir.name[0] == FILE_LAST) + { + notFound = true; + } + } + + // If no file is found, return CLUSTER_FREE + if (notFound) + { + return CLUSTER_FREE; + } + + return (dir.startCluster | (dir.startClusterHigh << 16)); +} + +/*----------------------------------------------------------------- +fileRead(buffer, cluster, startOffset, length) +-----------------------------------------------------------------*/ +u32 fileRead (char* buffer, u32 cluster, u32 startOffset, u32 length) +{ + int curByte; + int curSect; + + int dataPos = 0; + int chunks; + int beginBytes; + + if (cluster == CLUSTER_FREE || cluster == CLUSTER_EOF) + { + return 0; + } + + // Follow cluster list until desired one is found + for (chunks = startOffset / discBytePerClus; chunks > 0; chunks--) + { + cluster = FAT_NextCluster (cluster); + } + + // Calculate the sector and byte of the current position, + // and store them + curSect = (startOffset % discBytePerClus) / BYTES_PER_SECTOR; + curByte = startOffset % BYTES_PER_SECTOR; + + // Load sector buffer for new position in file + CARD_ReadSector( curSect + FAT_ClustToSect(cluster), globalBuffer); + curSect++; + + // Number of bytes needed to read to align with a sector + beginBytes = (BYTES_PER_SECTOR < length + curByte ? (BYTES_PER_SECTOR - curByte) : length); + + // Read first part from buffer, to align with sector boundary + for (dataPos = 0 ; dataPos < beginBytes; dataPos++) + { + buffer[dataPos] = globalBuffer[curByte++]; + } + + // Read in all the 512 byte chunks of the file directly, saving time + for ( chunks = ((int)length - beginBytes) / BYTES_PER_SECTOR; chunks > 0;) + { + int sectorsToRead; + + // Move to the next cluster if necessary + if (curSect >= discSecPerClus) + { + curSect = 0; + cluster = FAT_NextCluster (cluster); + } + + // Calculate how many sectors to read (read a maximum of discSecPerClus at a time) + sectorsToRead = discSecPerClus - curSect; + if(chunks < sectorsToRead) + sectorsToRead = chunks; + + // Read the sectors + CARD_ReadSectors(curSect + FAT_ClustToSect(cluster), sectorsToRead, buffer + dataPos); + chunks -= sectorsToRead; + curSect += sectorsToRead; + dataPos += BYTES_PER_SECTOR * sectorsToRead; + } + + // Take care of any bytes left over before end of read + if (dataPos < length) + { + + // Update the read buffer + curByte = 0; + if (curSect >= discSecPerClus) + { + curSect = 0; + cluster = FAT_NextCluster (cluster); + } + CARD_ReadSector( curSect + FAT_ClustToSect( cluster), globalBuffer); + + // Read in last partial chunk + for (; dataPos < length; dataPos++) + { + buffer[dataPos] = globalBuffer[curByte]; + curByte++; + } + } + + return dataPos; +} diff --git a/source/fat.h b/source/fat.h new file mode 100644 index 0000000..d39076b --- /dev/null +++ b/source/fat.h @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------- + fat.h + + NDS MP + GBAMP NDS Firmware Hack Version 2.12 + An NDS aware firmware patch for the GBA Movie Player. + By Michael Chisholm (Chishm) + + Filesystem code based on GBAMP_CF.c by Chishm (me). + +License: + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + +#ifndef FAT_H +#define FAT_H + +#include + +#define CLUSTER_FREE 0x00000000 +#define CLUSTER_EOF 0x0FFFFFFF +#define CLUSTER_FIRST 0x00000002 + +bool FAT_InitFiles (bool initCard); +u32 getBootFileCluster (const char* bootName); +u32 fileRead (char* buffer, u32 cluster, u32 startOffset, u32 length); +u32 FAT_ClustToSect (u32 cluster); + +#endif // FAT_H diff --git a/source/io_dldi.h b/source/io_dldi.h new file mode 100644 index 0000000..e0f4efa --- /dev/null +++ b/source/io_dldi.h @@ -0,0 +1,44 @@ +/* + io_dldi.h + + Reserved space for post-compilation adding of an extra driver + + Copyright (c) 2006 Michael "Chishm" Chisholm + + Redistribution and use in source and binary forms, with or without modification, + are permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + 3. The name of the author may not be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED + WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY + AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + 2006-12-22 - Chishm + * Original release +*/ + +#ifndef IO_DLDI_H +#define IO_DLDI_H + +// 'DLDD' +#define DEVICE_TYPE_DLDD 0x49444C44 + +#include "disc_io.h" + +// export interface +extern IO_INTERFACE _io_dldi ; + +#endif // define IO_DLDI_H diff --git a/source/io_dldi.s b/source/io_dldi.s new file mode 100644 index 0000000..fdfbde1 --- /dev/null +++ b/source/io_dldi.s @@ -0,0 +1,124 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ +@--------------------------------------------------------------------------------- + .align 4 + .arm + .global _dldi_start + .global _io_dldi +@--------------------------------------------------------------------------------- +.equ FEATURE_MEDIUM_CANREAD, 0x00000001 +.equ FEATURE_MEDIUM_CANWRITE, 0x00000002 +.equ FEATURE_SLOT_GBA, 0x00000010 +.equ FEATURE_SLOT_NDS, 0x00000020 + + +_dldi_start: +#ifndef NO_DLDI + +@--------------------------------------------------------------------------------- +@ Driver patch file standard header -- 16 bytes +#ifdef STANDARD_DLDI + .word 0xBF8DA5ED @ Magic number to identify this region +#else + .word 0xBF8DA5EE @ Magic number to identify this region +#endif + .asciz " Chishm" @ Identifying Magic string (8 bytes with null terminator) + .byte 0x01 @ Version number + .byte 0x0e @ 16KiB @ Log [base-2] of the size of this driver in bytes. + .byte 0x00 @ Sections to fix + .byte 0x0e @ 16KiB @ Log [base-2] of the allocated space in bytes. + +@--------------------------------------------------------------------------------- +@ Text identifier - can be anything up to 47 chars + terminating null -- 16 bytes + .align 4 + .asciz "Loader (No interface)" + +@--------------------------------------------------------------------------------- +@ Offsets to important sections within the data -- 32 bytes + .align 6 + .word _dldi_start @ data start + .word _dldi_end @ data end + .word 0x00000000 @ Interworking glue start -- Needs address fixing + .word 0x00000000 @ Interworking glue end + .word 0x00000000 @ GOT start -- Needs address fixing + .word 0x00000000 @ GOT end + .word 0x00000000 @ bss start -- Needs setting to zero + .word 0x00000000 @ bss end +@--------------------------------------------------------------------------------- +@ IO_INTERFACE data -- 32 bytes +_io_dldi: + .ascii "DLDI" @ ioType + .word 0x00000000 @ Features + .word _DLDI_startup @ + .word _DLDI_isInserted @ + .word _DLDI_readSectors @ Function pointers to standard device driver functions + .word _DLDI_writeSectors @ + .word _DLDI_clearStatus @ + .word _DLDI_shutdown @ + + +@--------------------------------------------------------------------------------- + +_DLDI_startup: +_DLDI_isInserted: +_DLDI_readSectors: +_DLDI_writeSectors: +_DLDI_clearStatus: +_DLDI_shutdown: + mov r0, #0x00 @ Return false for every function + bx lr + + + +@--------------------------------------------------------------------------------- + .align + .pool + + .space (_dldi_start + 16384) - . @ Fill to 16KiB + +_dldi_end: + .end +@--------------------------------------------------------------------------------- +#else +@--------------------------------------------------------------------------------- +@ IO_INTERFACE data -- 32 bytes +_io_dldi: + .ascii "DLDI" @ ioType + .word 0x00000000 @ Features + .word _DLDI_startup @ + .word _DLDI_isInserted @ + .word _DLDI_readSectors @ Function pointers to standard device driver functions + .word _DLDI_writeSectors @ + .word _DLDI_clearStatus @ + .word _DLDI_shutdown @ + + _DLDI_startup: +_DLDI_isInserted: +_DLDI_readSectors: +_DLDI_writeSectors: +_DLDI_clearStatus: +_DLDI_shutdown: + mov r0, #0x00 @ Return false for every function + bx lr + + +#endif diff --git a/source/load_crt0.s b/source/load_crt0.s new file mode 100644 index 0000000..6c57e6d --- /dev/null +++ b/source/load_crt0.s @@ -0,0 +1,147 @@ +/*----------------------------------------------------------------- + + Copyright (C) 2005 Michael "Chishm" Chisholm + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + + If you use this code, please give due credit and email me about your + project at chishm@hotmail.com +------------------------------------------------------------------*/ + +@--------------------------------------------------------------------------------- + .section ".init" + .global _start + .global storedFileCluster + .global initDisc + .global wantToPatchDLDI + .global argStart + .global argSize + .global dsiSD + .global dsiMode +@--------------------------------------------------------------------------------- + .align 4 + .arm +@--------------------------------------------------------------------------------- +_start: +@--------------------------------------------------------------------------------- + b startUp + +storedFileCluster: + .word 0x0FFFFFFF @ default BOOT.NDS +initDisc: + .word 0x00000001 @ init the disc by default +wantToPatchDLDI: + .word 0x00000001 @ by default patch the DLDI section of the loaded NDS +@ Used for passing arguments to the loaded app +argStart: + .word _end - _start +argSize: + .word 0x00000000 +dldiOffset: + .word _dldi_start - _start +dsiSD: + .word 0 +dsiMode: + .word 0 + +startUp: + mov r0, #0x04000000 + mov r1, #0 + str r1, [r0,#0x208] @ REG_IME + str r1, [r0,#0x210] @ REG_IE + str r1, [r0,#0x218] @ REG_AUXIE + + mov r0, #0x12 @ Switch to IRQ Mode + msr cpsr, r0 + ldr sp, =__sp_irq @ Set IRQ stack + + mov r0, #0x13 @ Switch to SVC Mode + msr cpsr, r0 + ldr sp, =__sp_svc @ Set SVC stack + + mov r0, #0x1F @ Switch to System Mode + msr cpsr, r0 + ldr sp, =__sp_usr @ Set user stack + + ldr r0, =__bss_start @ Clear BSS section to 0x00 + ldr r1, =__bss_end + sub r1, r1, r0 + bl ClearMem + + mov r0, #0 @ int argc + mov r1, #0 @ char *argv[] + ldr r3, =main + bl _blx_r3_stub @ jump to user code + + @ If the user ever returns, restart + b _start + +@--------------------------------------------------------------------------------- +_blx_r3_stub: +@--------------------------------------------------------------------------------- + bx r3 + +@--------------------------------------------------------------------------------- +@ Clear memory to 0x00 if length != 0 +@ r0 = Start Address +@ r1 = Length +@--------------------------------------------------------------------------------- +ClearMem: +@--------------------------------------------------------------------------------- + mov r2, #3 @ Round down to nearest word boundary + add r1, r1, r2 @ Shouldn't be needed + bics r1, r1, r2 @ Clear 2 LSB (and set Z) + bxeq lr @ Quit if copy size is 0 + + mov r2, #0 +ClrLoop: + stmia r0!, {r2} + subs r1, r1, #4 + bne ClrLoop + bx lr + +@--------------------------------------------------------------------------------- +@ Copy memory if length != 0 +@ r1 = Source Address +@ r2 = Dest Address +@ r4 = Dest Address + Length +@--------------------------------------------------------------------------------- +CopyMemCheck: +@--------------------------------------------------------------------------------- + sub r3, r4, r2 @ Is there any data to copy? +@--------------------------------------------------------------------------------- +@ Copy memory +@ r1 = Source Address +@ r2 = Dest Address +@ r3 = Length +@--------------------------------------------------------------------------------- +CopyMem: +@--------------------------------------------------------------------------------- + mov r0, #3 @ These commands are used in cases where + add r3, r3, r0 @ the length is not a multiple of 4, + bics r3, r3, r0 @ even though it should be. + bxeq lr @ Length is zero, so exit +CIDLoop: + ldmia r1!, {r0} + stmia r2!, {r0} + subs r3, r3, #4 + bne CIDLoop + bx lr + +@--------------------------------------------------------------------------------- + .align + .pool + .end +@--------------------------------------------------------------------------------- diff --git a/source/sdmmc.c b/source/sdmmc.c new file mode 100644 index 0000000..1da63ee --- /dev/null +++ b/source/sdmmc.c @@ -0,0 +1,242 @@ +#ifndef NO_SDMMC +#include +#include +#include "sdmmc.h" + +static struct mmcdevice deviceSD; + +//--------------------------------------------------------------------------------- +int geterror(struct mmcdevice *ctx) { +//--------------------------------------------------------------------------------- + //if(ctx->error == 0x4) return -1; + //else return 0; + return (ctx->error << 29) >> 31; +} + + +//--------------------------------------------------------------------------------- +void setTarget(struct mmcdevice *ctx) { +//--------------------------------------------------------------------------------- + sdmmc_mask16(REG_SDPORTSEL,0x3,(u16)ctx->devicenumber); + setckl(ctx->clk); + if (ctx->SDOPT == 0) { + sdmmc_mask16(REG_SDOPT, 0, 0x8000); + } else { + sdmmc_mask16(REG_SDOPT, 0x8000, 0); + } + +} + + +//--------------------------------------------------------------------------------- +void sdmmc_send_command(struct mmcdevice *ctx, uint32_t cmd, uint32_t args) { +//--------------------------------------------------------------------------------- + int i; + bool getSDRESP = (cmd << 15) >> 31; + uint16_t flags = (cmd << 15) >> 31; + const bool readdata = cmd & 0x20000; + const bool writedata = cmd & 0x40000; + + if(readdata || writedata) + { + flags |= TMIO_STAT0_DATAEND; + } + + ctx->error = 0; + while((sdmmc_read16(REG_SDSTATUS1) & TMIO_STAT1_CMD_BUSY)); //mmc working? + sdmmc_write16(REG_SDIRMASK0,0); + sdmmc_write16(REG_SDIRMASK1,0); + sdmmc_write16(REG_SDSTATUS0,0); + sdmmc_write16(REG_SDSTATUS1,0); + sdmmc_write16(REG_SDCMDARG0,args &0xFFFF); + sdmmc_write16(REG_SDCMDARG1,args >> 16); + sdmmc_write16(REG_SDCMD,cmd &0xFFFF); + + uint32_t size = ctx->size; + uint16_t *dataPtr = (uint16_t*)ctx->data; + uint32_t *dataPtr32 = (uint32_t*)ctx->data; + + bool useBuf = ( NULL != dataPtr ); + bool useBuf32 = (useBuf && (0 == (3 & ((uint32_t)dataPtr)))); + + uint16_t status0 = 0; + + while(1) { + volatile uint16_t status1 = sdmmc_read16(REG_SDSTATUS1); + volatile uint16_t ctl32 = sdmmc_read16(REG_SDDATACTL32); + if((ctl32 & 0x100)) + { + if(readdata) { + if(useBuf) { + sdmmc_mask16(REG_SDSTATUS1, TMIO_STAT1_RXRDY, 0); + if(size > 0x1FF) { + if(useBuf32) { + for(i = 0; i<0x200; i+=4) { + *dataPtr32++ = sdmmc_read32(REG_SDFIFO32); + } + } else { + for(i = 0; i<0x200; i+=2) { + *dataPtr++ = sdmmc_read16(REG_SDFIFO); + } + } + size -= 0x200; + } + } + + sdmmc_mask16(REG_SDDATACTL32, 0x800, 0); + } + } + + if(status1 & TMIO_MASK_GW) { + ctx->error |= 4; + break; + } + + if(!(status1 & TMIO_STAT1_CMD_BUSY)) { + status0 = sdmmc_read16(REG_SDSTATUS0); + if(sdmmc_read16(REG_SDSTATUS0) & TMIO_STAT0_CMDRESPEND) { + ctx->error |= 0x1; + } + if(status0 & TMIO_STAT0_DATAEND) { + ctx->error |= 0x2; + } + + if((status0 & flags) == flags) + break; + } + } + ctx->stat0 = sdmmc_read16(REG_SDSTATUS0); + ctx->stat1 = sdmmc_read16(REG_SDSTATUS1); + sdmmc_write16(REG_SDSTATUS0,0); + sdmmc_write16(REG_SDSTATUS1,0); + + if(getSDRESP != 0) { + ctx->ret[0] = sdmmc_read16(REG_SDRESP0) | (sdmmc_read16(REG_SDRESP1) << 16); + ctx->ret[1] = sdmmc_read16(REG_SDRESP2) | (sdmmc_read16(REG_SDRESP3) << 16); + ctx->ret[2] = sdmmc_read16(REG_SDRESP4) | (sdmmc_read16(REG_SDRESP5) << 16); + ctx->ret[3] = sdmmc_read16(REG_SDRESP6) | (sdmmc_read16(REG_SDRESP7) << 16); + } +} + + +//--------------------------------------------------------------------------------- +int sdmmc_cardinserted() { +//--------------------------------------------------------------------------------- + return 1; //sdmmc_cardready; +} + +//--------------------------------------------------------------------------------- +void sdmmc_controller_init(bool force) { +//--------------------------------------------------------------------------------- + deviceSD.isSDHC = 0; + deviceSD.SDOPT = 0; + deviceSD.res = 0; + deviceSD.initarg = 0; + deviceSD.clk = 0x80; + deviceSD.devicenumber = 0; + + *(vu16*)(SDMMC_BASE + REG_SDDATACTL32) &= 0xF7FFu; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL32) &= 0xEFFFu; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL32) |= 0x402u; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL) = (*(vu16*)(SDMMC_BASE + REG_SDDATACTL) & 0xFFDD) | 2; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL32) &= 0xFFFFu; + *(vu16*)(SDMMC_BASE + REG_SDDATACTL) &= 0xFFDFu; + *(vu16*)(SDMMC_BASE + REG_SDBLKLEN32) = 512; + *(vu16*)(SDMMC_BASE + REG_SDBLKCOUNT32) = 1; + *(vu16*)(SDMMC_BASE + REG_SDRESET) &= 0xFFFEu; + *(vu16*)(SDMMC_BASE + REG_SDRESET) |= 1u; + *(vu16*)(SDMMC_BASE + REG_SDIRMASK0) |= TMIO_MASK_ALL; + *(vu16*)(SDMMC_BASE + REG_SDIRMASK1) |= TMIO_MASK_ALL>>16; + *(vu16*)(SDMMC_BASE + 0x0fc) |= 0xDBu; //SDCTL_RESERVED7 + *(vu16*)(SDMMC_BASE + 0x0fe) |= 0xDBu; //SDCTL_RESERVED8 + *(vu16*)(SDMMC_BASE + REG_SDPORTSEL) &= 0xFFFCu; + *(vu16*)(SDMMC_BASE + REG_SDCLKCTL) = 0x20; + *(vu16*)(SDMMC_BASE + REG_SDOPT) = 0x40EE; + *(vu16*)(SDMMC_BASE + REG_SDPORTSEL) &= 0xFFFCu; + *(vu16*)(SDMMC_BASE + REG_SDBLKLEN) = 512; + *(vu16*)(SDMMC_BASE + REG_SDSTOP) = 0; + +} + +//--------------------------------------------------------------------------------- +int sdmmc_sdcard_init() { +//--------------------------------------------------------------------------------- + setTarget(&deviceSD); + swiDelay(0xF000); + sdmmc_send_command(&deviceSD,0,0); + sdmmc_send_command(&deviceSD,0x10408,0x1AA); + u32 temp = (deviceSD.error & 0x1) << 0x1E; + + u32 temp2 = 0; + do { + do { + sdmmc_send_command(&deviceSD,0x10437,deviceSD.initarg << 0x10); + sdmmc_send_command(&deviceSD,0x10769,0x00FF8000 | temp); + temp2 = 1; + } while ( !(deviceSD.error & 1) ); + + } while((deviceSD.ret[0] & 0x80000000) == 0); + + if(!((deviceSD.ret[0] >> 30) & 1) || !temp) + temp2 = 0; + + deviceSD.isSDHC = temp2; + + sdmmc_send_command(&deviceSD,0x10602,0); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x10403,0); + if (deviceSD.error & 0x4) return -1; + deviceSD.initarg = deviceSD.ret[0] >> 0x10; + + sdmmc_send_command(&deviceSD,0x10609,deviceSD.initarg << 0x10); + if (deviceSD.error & 0x4) return -1; + + deviceSD.clk = 1; + setckl(1); + + sdmmc_send_command(&deviceSD,0x10507,deviceSD.initarg << 0x10); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x10437,deviceSD.initarg << 0x10); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x1076A,0x0); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x10437,deviceSD.initarg << 0x10); + if (deviceSD.error & 0x4) return -1; + + deviceSD.SDOPT = 1; + sdmmc_send_command(&deviceSD,0x10446,0x2); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x1040D,deviceSD.initarg << 0x10); + if (deviceSD.error & 0x4) return -1; + + sdmmc_send_command(&deviceSD,0x10410,0x200); + if (deviceSD.error & 0x4) return -1; + deviceSD.clk |= 0x200; + + return 0; + +} + +//--------------------------------------------------------------------------------- +int sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, void *out) { +//--------------------------------------------------------------------------------- + if (deviceSD.isSDHC == 0) + sector_no <<= 9; + setTarget(&deviceSD); + sdmmc_write16(REG_SDSTOP,0x100); + + sdmmc_write16(REG_SDBLKCOUNT32,numsectors); + sdmmc_write16(REG_SDBLKLEN32,0x200); + + sdmmc_write16(REG_SDBLKCOUNT,numsectors); + deviceSD.data = out; + deviceSD.size = numsectors << 9; + sdmmc_send_command(&deviceSD,0x33C12,sector_no); + return geterror(&deviceSD); +} +#endif \ No newline at end of file diff --git a/source/sdmmc.h b/source/sdmmc.h new file mode 100644 index 0000000..3313be1 --- /dev/null +++ b/source/sdmmc.h @@ -0,0 +1,194 @@ +#ifndef __SDMMC_H__ +#define __SDMMC_H__ + +#include + +#define DATA32_SUPPORT + +#define SDMMC_BASE 0x04004800 + + +#define REG_SDCMD 0x00 +#define REG_SDPORTSEL 0x02 +#define REG_SDCMDARG 0x04 +#define REG_SDCMDARG0 0x04 +#define REG_SDCMDARG1 0x06 +#define REG_SDSTOP 0x08 +#define REG_SDRESP 0x0c +#define REG_SDBLKCOUNT 0x0a + +#define REG_SDRESP0 0x0c +#define REG_SDRESP1 0x0e +#define REG_SDRESP2 0x10 +#define REG_SDRESP3 0x12 +#define REG_SDRESP4 0x14 +#define REG_SDRESP5 0x16 +#define REG_SDRESP6 0x18 +#define REG_SDRESP7 0x1a + +#define REG_SDSTATUS0 0x1c +#define REG_SDSTATUS1 0x1e + +#define REG_SDIRMASK0 0x20 +#define REG_SDIRMASK1 0x22 +#define REG_SDCLKCTL 0x24 + +#define REG_SDBLKLEN 0x26 +#define REG_SDOPT 0x28 +#define REG_SDFIFO 0x30 + +#define REG_SDDATACTL 0xd8 +#define REG_SDRESET 0xe0 +#define REG_SDPROTECTED 0xf6 //bit 0 determines if sd is protected or not? + +#define REG_SDDATACTL32 0x100 +#define REG_SDBLKLEN32 0x104 +#define REG_SDBLKCOUNT32 0x108 +#define REG_SDFIFO32 0x10C + +#define REG_CLK_AND_WAIT_CTL 0x138 +#define REG_RESET_SDIO 0x1e0 +//The below defines are from linux kernel drivers/mmc tmio_mmc.h. +/* Definitions for values the CTRL_STATUS register can take. */ +#define TMIO_STAT0_CMDRESPEND 0x0001 +#define TMIO_STAT0_DATAEND 0x0004 +#define TMIO_STAT0_CARD_REMOVE 0x0008 +#define TMIO_STAT0_CARD_INSERT 0x0010 +#define TMIO_STAT0_SIGSTATE 0x0020 +#define TMIO_STAT0_WRPROTECT 0x0080 +#define TMIO_STAT0_CARD_REMOVE_A 0x0100 +#define TMIO_STAT0_CARD_INSERT_A 0x0200 +#define TMIO_STAT0_SIGSTATE_A 0x0400 + +#define TMIO_STAT1_CMD_IDX_ERR 0x0001 +#define TMIO_STAT1_CRCFAIL 0x0002 +#define TMIO_STAT1_STOPBIT_ERR 0x0004 +#define TMIO_STAT1_DATATIMEOUT 0x0008 +#define TMIO_STAT1_RXOVERFLOW 0x0010 +#define TMIO_STAT1_TXUNDERRUN 0x0020 +#define TMIO_STAT1_CMDTIMEOUT 0x0040 +#define TMIO_STAT1_RXRDY 0x0100 +#define TMIO_STAT1_TXRQ 0x0200 +#define TMIO_STAT1_ILL_FUNC 0x2000 +#define TMIO_STAT1_CMD_BUSY 0x4000 +#define TMIO_STAT1_ILL_ACCESS 0x8000 + +#define SDMC_NORMAL 0x00000000 +#define SDMC_ERR_COMMAND 0x00000001 +#define SDMC_ERR_CRC 0x00000002 +#define SDMC_ERR_END 0x00000004 +#define SDMC_ERR_TIMEOUT 0x00000008 +#define SDMC_ERR_FIFO_OVF 0x00000010 +#define SDMC_ERR_FIFO_UDF 0x00000020 +#define SDMC_ERR_WP 0x00000040 +#define SDMC_ERR_ABORT 0x00000080 +#define SDMC_ERR_FPGA_TIMEOUT 0x00000100 +#define SDMC_ERR_PARAM 0x00000200 +#define SDMC_ERR_R1_STATUS 0x00000800 +#define SDMC_ERR_NUM_WR_SECTORS 0x00001000 +#define SDMC_ERR_RESET 0x00002000 +#define SDMC_ERR_ILA 0x00004000 +#define SDMC_ERR_INFO_DETECT 0x00008000 + +#define SDMC_STAT_ERR_UNKNOWN 0x00080000 +#define SDMC_STAT_ERR_CC 0x00100000 +#define SDMC_STAT_ERR_ECC_FAILED 0x00200000 +#define SDMC_STAT_ERR_CRC 0x00800000 +#define SDMC_STAT_ERR_OTHER 0xf9c70008 + +#define TMIO_MASK_ALL 0x837f031d + +#define TMIO_MASK_GW (TMIO_STAT1_ILL_ACCESS | TMIO_STAT1_CMDTIMEOUT | TMIO_STAT1_TXUNDERRUN | TMIO_STAT1_RXOVERFLOW | \ + TMIO_STAT1_DATATIMEOUT | TMIO_STAT1_STOPBIT_ERR | TMIO_STAT1_CRCFAIL | TMIO_STAT1_CMD_IDX_ERR) + +#define TMIO_MASK_READOP (TMIO_STAT1_RXRDY | TMIO_STAT1_DATAEND) +#define TMIO_MASK_WRITEOP (TMIO_STAT1_TXRQ | TMIO_STAT1_DATAEND) + +typedef struct mmcdevice { + u8* data; + u32 size; + u32 error; + u16 stat0; + u16 stat1; + u32 ret[4]; + u32 initarg; + u32 isSDHC; + u32 clk; + u32 SDOPT; + u32 devicenumber; + u32 total_size; //size in sectors of the device + u32 res; +} mmcdevice; + +enum { + MMC_DEVICE_SDCARD, + MMC_DEVICE_NAND, +}; + +void sdmmc_controller_init(bool force_init); +void sdmmc_initirq(); +int sdmmc_cardinserted(); + +int sdmmc_sdcard_init(); +int sdmmc_nand_init(); +void sdmmc_get_cid(int devicenumber, u32 *cid); + +static inline void sdmmc_nand_cid( u32 *cid) { + sdmmc_get_cid(MMC_DEVICE_NAND,cid); +} + +static inline void sdmmc_sdcard_cid( u32 *cid) { + sdmmc_get_cid(MMC_DEVICE_SDCARD,cid); +} + +int sdmmc_sdcard_readsectors(u32 sector_no, u32 numsectors, void *out); +int sdmmc_sdcard_writesectors(u32 sector_no, u32 numsectors, void *in); +int sdmmc_nand_readsectors(u32 sector_no, u32 numsectors, void *out); +int sdmmc_nand_writesectors(u32 sector_no, u32 numsectors, void *in); + +extern u32 sdmmc_cid[]; +extern int sdmmc_curdevice; + +//--------------------------------------------------------------------------------- +static inline u16 sdmmc_read16(u16 reg) { +//--------------------------------------------------------------------------------- + return *(vu16*)(SDMMC_BASE + reg); +} + +//--------------------------------------------------------------------------------- +static inline void sdmmc_write16(u16 reg, u16 val) { +//--------------------------------------------------------------------------------- + *(vu16*)(SDMMC_BASE + reg) = val; +} + +//--------------------------------------------------------------------------------- +static inline u32 sdmmc_read32(u16 reg) { +//--------------------------------------------------------------------------------- + return *(vu32*)(SDMMC_BASE + reg); +} + +//--------------------------------------------------------------------------------- +static inline void sdmmc_write32(u16 reg, u32 val) { +//--------------------------------------------------------------------------------- + *(vu32*)(SDMMC_BASE + reg) = val; +} + +//--------------------------------------------------------------------------------- +static inline void sdmmc_mask16(u16 reg, u16 clear, u16 set) { +//--------------------------------------------------------------------------------- + u16 val = sdmmc_read16(reg); + val &= ~clear; + val |= set; + sdmmc_write16(reg, val); +} + + +//--------------------------------------------------------------------------------- +static inline void setckl(u32 data) { +//--------------------------------------------------------------------------------- + sdmmc_mask16(REG_SDCLKCTL, 0x100, 0); + sdmmc_mask16(REG_SDCLKCTL, 0x2FF, data & 0x2FF); + sdmmc_mask16(REG_SDCLKCTL, 0x0, 0x100); +} + +#endif