Working SDHOST/FatFS, boot partition mounts, some other minor fixes too.
authorkristina <kb@kbs-iMac.local>
Thu, 9 Jun 2016 09:58:25 +0000 (10:58 +0100)
committerkristina <kb@kbs-iMac.local>
Thu, 9 Jun 2016 09:58:56 +0000 (10:58 +0100)
 * Unified panic code for ARM/VC4
 * VC4 is back to using xprintf
 * Added minimal C++ runtime to stage 1.5
 * Use TLSF for heap (malloc) in stage 1.5
 * SDHOST driver kind of works, only tested with SDv1, probably works
   with SDv2 too but didn't bother testing.
 * Can read MBR and initialize FatFS for the first (boot) partition to
   later read stage 2 from it (Linux or u-boot or whatever).
 * Made driver code a bit more sane.

176 files changed:
LICENSE [changed mode: 0755->0644]
Makefile [changed mode: 0755->0644]
arm_chainloader/Makefile [changed mode: 0755->0644]
arm_chainloader/chainloader.h [changed mode: 0755->0644]
arm_chainloader/drivers/block_device.hpp [new file with mode: 0644]
arm_chainloader/drivers/cprman.cc [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/00history.txt [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/00readme.txt [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/diskio.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/diskio.h [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/ff.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/ff.h [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/ffconf.h [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/integer.h [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/cc932.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/cc936.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/cc949.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/cc950.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/ccsbcs.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/syscall.c [changed mode: 0755->0644]
arm_chainloader/drivers/fatfs/option/unicode.c [changed mode: 0755->0644]
arm_chainloader/drivers/mbr_disk.cc [new file with mode: 0644]
arm_chainloader/drivers/sd2.hpp [deleted file]
arm_chainloader/drivers/sd_proto.hpp [new file with mode: 0644]
arm_chainloader/drivers/sdhost.cc [deleted file]
arm_chainloader/drivers/sdhost_impl.cc [new file with mode: 0644]
arm_chainloader/drivers/uart.c [changed mode: 0755->0644]
arm_chainloader/firmware_rendezvous.c [changed mode: 0755->0644]
arm_chainloader/lib/arm_bcopy.s [new file with mode: 0644]
arm_chainloader/lib/arm_bzero.s [new file with mode: 0644]
arm_chainloader/lib/arm_locore.s [new file with mode: 0644]
arm_chainloader/linker.lds [changed mode: 0755->0644]
arm_chainloader/loader.cc [new file with mode: 0644]
arm_chainloader/main.c [changed mode: 0755->0644]
arm_chainloader/minicxx.cc [new file with mode: 0644]
arm_chainloader/start.s [changed mode: 0755->0644]
arm_loader.c [changed mode: 0755->0644]
arm_monitor.c [changed mode: 0755->0644]
bcm2708_chip/README.txt [changed mode: 0755->0644]
bcm2708_chip/apb_arbiter_control.h [changed mode: 0755->0644]
bcm2708_chip/apb_async_bridge_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/arm_control.h [changed mode: 0755->0644]
bcm2708_chip/aux_io.h [changed mode: 0755->0644]
bcm2708_chip/ave_in.h [changed mode: 0755->0644]
bcm2708_chip/ave_out.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma0.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma1.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma15.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma2.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma3.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma4.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma5.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma6.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma7.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma8.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite10.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite11.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite12.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite13.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite14.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite7.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite8.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_lite9.h [changed mode: 0755->0644]
bcm2708_chip/axi_dma_top.h [changed mode: 0755->0644]
bcm2708_chip/axi_performance0.h [changed mode: 0755->0644]
bcm2708_chip/axi_performance1.h [changed mode: 0755->0644]
bcm2708_chip/cam0.h [changed mode: 0755->0644]
bcm2708_chip/cam0_a0.h [changed mode: 0755->0644]
bcm2708_chip/cam1.h [changed mode: 0755->0644]
bcm2708_chip/cam1_a0.h [changed mode: 0755->0644]
bcm2708_chip/camccp.h [changed mode: 0755->0644]
bcm2708_chip/ccp2tx.h [changed mode: 0755->0644]
bcm2708_chip/ccp2tx_a0.h [changed mode: 0755->0644]
bcm2708_chip/cdp.h [changed mode: 0755->0644]
bcm2708_chip/clkman_image.h [changed mode: 0755->0644]
bcm2708_chip/cpg.h [changed mode: 0755->0644]
bcm2708_chip/cpi.h [changed mode: 0755->0644]
bcm2708_chip/cpr_apb2wtap.h [changed mode: 0755->0644]
bcm2708_chip/cpr_apb2wtap_a0.h [changed mode: 0755->0644]
bcm2708_chip/cpr_clkman.h [changed mode: 0755->0644]
bcm2708_chip/cpr_clkman_a0.h [changed mode: 0755->0644]
bcm2708_chip/cpr_powman.h [changed mode: 0755->0644]
bcm2708_chip/cpr_powman_a0.h [changed mode: 0755->0644]
bcm2708_chip/cryptohw.h [changed mode: 0755->0644]
bcm2708_chip/csi2.h [changed mode: 0755->0644]
bcm2708_chip/dpi.h [changed mode: 0755->0644]
bcm2708_chip/dsi.h [changed mode: 0755->0644]
bcm2708_chip/dsi4.h [changed mode: 0755->0644]
bcm2708_chip/emmc.h [changed mode: 0755->0644]
bcm2708_chip/flow_config.tcl [changed mode: 0755->0644]
bcm2708_chip/fpga_microblaze.h [changed mode: 0755->0644]
bcm2708_chip/fpga_peripheral.h [changed mode: 0755->0644]
bcm2708_chip/gpio.h [changed mode: 0755->0644]
bcm2708_chip/h264.h [changed mode: 0755->0644]
bcm2708_chip/hardware.h [changed mode: 0755->0644]
bcm2708_chip/hdcp.h [changed mode: 0755->0644]
bcm2708_chip/hdmi.h [changed mode: 0755->0644]
bcm2708_chip/hdmicore.h [changed mode: 0755->0644]
bcm2708_chip/hvs.h [changed mode: 0755->0644]
bcm2708_chip/i2c0.h [changed mode: 0755->0644]
bcm2708_chip/i2c1.h [changed mode: 0755->0644]
bcm2708_chip/i2c2.h [changed mode: 0755->0644]
bcm2708_chip/i2c_spi_slv.h [changed mode: 0755->0644]
bcm2708_chip/intctrl0.h [changed mode: 0755->0644]
bcm2708_chip/intctrl1.h [changed mode: 0755->0644]
bcm2708_chip/isp.h [changed mode: 0755->0644]
bcm2708_chip/israel_bg_dctram.h [changed mode: 0755->0644]
bcm2708_chip/israel_bg_instr.h [changed mode: 0755->0644]
bcm2708_chip/israel_dsp_registers.h [changed mode: 0755->0644]
bcm2708_chip/jpeg_top.h [changed mode: 0755->0644]
bcm2708_chip/l2_cache_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/mphi.h [changed mode: 0755->0644]
bcm2708_chip/multicore_sync.h [changed mode: 0755->0644]
bcm2708_chip/nexus_uba.h [changed mode: 0755->0644]
bcm2708_chip/otp.h [changed mode: 0755->0644]
bcm2708_chip/pcm.h [changed mode: 0755->0644]
bcm2708_chip/perfmon.h [changed mode: 0755->0644]
bcm2708_chip/peri_image_arb_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/pixel_valve0.h [changed mode: 0755->0644]
bcm2708_chip/pixel_valve1.h [changed mode: 0755->0644]
bcm2708_chip/pixel_valve2.h [changed mode: 0755->0644]
bcm2708_chip/pwm.h [changed mode: 0755->0644]
bcm2708_chip/register_map.h [changed mode: 0755->0644]
bcm2708_chip/register_map_macros.h [changed mode: 0755->0644]
bcm2708_chip/rng.h [changed mode: 0755->0644]
bcm2708_chip/rng_a0.h [changed mode: 0755->0644]
bcm2708_chip/rnghw.h [changed mode: 0755->0644]
bcm2708_chip/sdc_addr_front.h [changed mode: 0755->0644]
bcm2708_chip/sdc_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/sdc_dq_front.h [changed mode: 0755->0644]
bcm2708_chip/sdhost.h [changed mode: 0755->0644]
bcm2708_chip/slimbus.h [changed mode: 0755->0644]
bcm2708_chip/slimbus_a0.h [changed mode: 0755->0644]
bcm2708_chip/smi.h [changed mode: 0755->0644]
bcm2708_chip/spi_master.h [changed mode: 0755->0644]
bcm2708_chip/sv_chip_regmap.h [changed mode: 0755->0644]
bcm2708_chip/system_arbiter_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/tectl.h [changed mode: 0755->0644]
bcm2708_chip/tectl_a0.h [changed mode: 0755->0644]
bcm2708_chip/tempsens.h [changed mode: 0755->0644]
bcm2708_chip/testbus.h [changed mode: 0755->0644]
bcm2708_chip/thread_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/timer.h [changed mode: 0755->0644]
bcm2708_chip/txp.h [changed mode: 0755->0644]
bcm2708_chip/uart.h [changed mode: 0755->0644]
bcm2708_chip/usb.h [changed mode: 0755->0644]
bcm2708_chip/v3d.h [changed mode: 0755->0644]
bcm2708_chip/vcodec.h [changed mode: 0755->0644]
bcm2708_chip/vec.h [changed mode: 0755->0644]
bcm2708_chip/vpu_arb_ctrl.h [changed mode: 0755->0644]
bcm2708_chip/vpu_l1_cache_ctrl.h [changed mode: 0755->0644]
chainloader_inc.s [changed mode: 0755->0644]
ghetto.s [changed mode: 0755->0644]
hardware.h [changed mode: 0755->0644]
hardware_vc4.h [changed mode: 0755->0644]
lib/common.h [deleted file]
lib/memcpy.c [changed mode: 0755->0644]
lib/panic.c [new file with mode: 0644]
lib/panic.h [new file with mode: 0644]
lib/stdarg.h [changed mode: 0755->0644]
lib/tlsf/target.h [new file with mode: 0644]
lib/tlsf/tlsf.c [new file with mode: 0644]
lib/tlsf/tlsf.h [new file with mode: 0644]
lib/udelay.c [changed mode: 0755->0644]
lib/xprintf.c [changed mode: 0755->0644]
lib/xprintf.h [changed mode: 0755->0644]
panic.c [deleted file]
romstage.c [changed mode: 0755->0644]
sdram.c [changed mode: 0755->0644]
start.s [changed mode: 0755->0644]
trap.c [changed mode: 0755->0644]
vc4_include/common.h [changed mode: 0755->0644]
vc4_include/cpu.h [changed mode: 0755->0644]
vc4_include/exception.h [changed mode: 0755->0644]
vc4_include/hardware.h [changed mode: 0755->0644]
vc4_include/pcb.h [changed mode: 0755->0644]

diff --git a/LICENSE b/LICENSE
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
index b781bfb..e0d284a
--- a/Makefile
+++ b/Makefile
@@ -12,8 +12,8 @@ SRCS = \
        arm_loader.c \
        arm_monitor.c \
        trap.c \
-       lib/ee_printf.c \
-       panic.c \
+       lib/xprintf.c \
+       lib/panic.c \
        lib/udelay.c \
        lib/memcpy.c \
        chainloader_inc.s
old mode 100755 (executable)
new mode 100644 (file)
index 398d5cc..7da2213
@@ -3,13 +3,21 @@ TARGET_ARM_CHAINLOADER = arm_chainloader.bin
 
 SRCS = \
        start.s \
+       lib/arm_bcopy.s \
+       lib/arm_bzero.s \
+       lib/arm_locore.s \
        drivers/uart.c \
        drivers/cprman.cc \
        drivers/fatfs/ff.c \
-       drivers/sdhost.cc \
+       drivers/sdhost_impl.cc \
+       drivers/mbr_disk.cc \
        ../lib/xprintf.c \
+       ../lib/panic.c \
        ../lib/udelay.c \
+       ../lib/tlsf/tlsf.c \
        firmware_rendezvous.c \
+       minicxx.cc \
+       loader.cc \
        main.c
 
 ARCH = armv6zk
old mode 100755 (executable)
new mode 100644 (file)
index 853c739..14d3c27
@@ -2,9 +2,9 @@
 \r
 #include <stdint.h>\r
 #include <stddef.h>\r
+#include <lib/panic.h>\r
 #include <lib/xprintf.h>\r
-\r
-#define printf xprintf\r
+#include <lib/tlsf/tlsf.h>\r
 \r
 #ifdef __cplusplus\r
 extern "C" {\r
@@ -12,10 +12,22 @@ extern "C" {
 \r
 extern void udelay(uint32_t time);\r
 \r
+static inline void __attribute__((noreturn)) hang_cpu() {\r
+       __asm__ __volatile__ (\r
+               "wfi\n"\r
+       );\r
+\r
+       /* in case the above fails */\r
+       for (;;) {\r
+               __asm__ __volatile__ ("nop\nnop\nnop\nnop\nnop\nnop");\r
+       }\r
+}\r
+\r
 #define STATIC_INIT_PRIORITY(x) __attribute__((init_priority(x)))\r
 \r
 #define STATIC_CPRMAN_DRIVER STATIC_INIT_PRIORITY(101)\r
 #define STATIC_DRIVER STATIC_INIT_PRIORITY(200)\r
+#define STATIC_FILESYSTEM STATIC_INIT_PRIORITY(300)\r
 #define STATIC_APP STATIC_INIT_PRIORITY(600)\r
 \r
 #define mfence() __sync_synchronize()\r
diff --git a/arm_chainloader/drivers/block_device.hpp b/arm_chainloader/drivers/block_device.hpp
new file mode 100644 (file)
index 0000000..eb504e5
--- /dev/null
@@ -0,0 +1,35 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+Block device.\r
+\r
+=============================================================================*/\r
+\r
+struct BlockDevice {\r
+       unsigned int block_size;\r
+\r
+       template <typename T>\r
+       inline bool read_block(uint32_t sector, T* dest_buffer) {\r
+               read_block(sector, reinterpret_cast<uint32_t*>(dest_buffer));\r
+       }\r
+\r
+       inline unsigned int get_block_size() {\r
+               return block_size;\r
+       }\r
+\r
+       virtual bool read_block(uint32_t sector, uint32_t* buf) = 0;\r
+};\r
+\r
+extern BlockDevice* get_sdhost_device();
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/arm_chainloader/drivers/mbr_disk.cc b/arm_chainloader/drivers/mbr_disk.cc
new file mode 100644 (file)
index 0000000..bfc468e
--- /dev/null
@@ -0,0 +1,198 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+MBR based disk interface modelling primary partitions as separate disks.\r
+\r
+=============================================================================*/\r
+\r
+#include <stdint.h>\r
+#include <chainloader.h>\r
+\r
+#include "block_device.hpp"\r
+\r
+#define logf(fmt, ...) printf("[MBRDISK:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+\r
+/*****************************************************************************\r
+ * MBR\r
+ *****************************************************************************/\r
+\r
+struct MbrPartition {\r
+       uint8_t         part_flag;              /* bootstrap flags */\r
+       uint8_t         part_shd;               /* starting head */\r
+       uint8_t         part_ssect;             /* starting sector */\r
+       uint8_t         part_scyl;              /* starting cylinder */\r
+       uint8_t         part_typ;               /* partition type */\r
+       uint8_t         part_ehd;               /* end head */\r
+       uint8_t         part_esect;             /* end sector */\r
+       uint8_t         part_ecyl;              /* end cylinder */\r
+#if 0\r
+       uint16_t        part_start_lo;          /* absolute starting ... */\r
+       uint16_t        part_start_hi;          /* ... sector number */\r
+       uint16_t        part_size_lo;           /* partition size ... */\r
+       uint16_t        part_size_hi;           /* ... in sectors */\r
+#endif\r
+       uint32_t part_start;\r
+       uint32_t part_size;\r
+} __attribute__((packed));\r
+\r
+#define        MBR_SIG         0xAA55\r
+\r
+struct Mbr {\r
+       uint16_t         mbr_code[223];\r
+       MbrPartition mbr_part[4];\r
+       uint16_t         mbr_sig;\r
+} __attribute__((packed));\r
+\r
+static_assert(sizeof(Mbr) >= 512, "What the fuck");\r
+\r
+#define MBR_FAT16 0x04 \r
+#define MBR_FAT32 0x0B \r
+#define MBR_FAT32_INT13 0x0C\r
+#define MBR_FAT16_INT13 0x0E\r
+#define MBR_LINUX 0x83\r
+#define MBR_NTFS 0x07\r
+\r
+static const char* mbr_fs_to_string(int fs) {\r
+       switch (fs) {\r
+               case MBR_FAT32: return "FAT32"; break;\r
+               case MBR_FAT32_INT13: return "FAT32-INT13"; break;\r
+               case MBR_FAT16_INT13: return "FAT16-INT13"; break;\r
+               case MBR_FAT16: return "FAT16"; break;\r
+               case MBR_LINUX: return "Linux (ext2/ext3)"; break;\r
+               case MBR_NTFS: return "NTFS"; break;\r
+               default: return "<Unknown>";\r
+       }\r
+}\r
+\r
+struct MbrImpl {\r
+       Mbr* mbr;\r
+       BlockDevice* mmc;\r
+\r
+       inline bool validate_signature() {\r
+               return reinterpret_cast<uint16_t>(mbr->mbr_sig) == MBR_SIG;\r
+       }\r
+\r
+       template <typename T>\r
+       inline bool read_block(uint8_t volume, uint32_t sector, T* dest_buffer) {\r
+               read_block(volume, sector, reinterpret_cast<uint32_t*>(dest_buffer));\r
+       }\r
+\r
+       inline unsigned int get_block_size() {\r
+               return mmc->block_size;\r
+       }\r
+\r
+       inline int get_partition_type(uint8_t volume) {\r
+               if (volume > 3)\r
+                       return 0;\r
+               return mbr->mbr_part[volume].part_typ;  \r
+       }\r
+\r
+       bool read_block(uint8_t volume, uint32_t sector, uint32_t* buf) {\r
+               if (volume > 3)\r
+                       return false;\r
+\r
+               MbrPartition& p = mbr->mbr_part[volume];\r
+\r
+               if (p.part_typ == 0)\r
+                       return false;\r
+\r
+               return mmc->read_block(p.part_start + sector, buf);\r
+       }\r
+\r
+       void read_mbr() {\r
+               logf("Reading master boot record ...\n");\r
+\r
+               if (!mmc->read_block(0, mbr)) {\r
+                       panic("unable to read master boot record from the SD card");\r
+               }\r
+\r
+               if (!validate_signature()) {\r
+                       panic("invalid master boot record signature (got 0x%x)", mbr->mbr_sig);\r
+               }\r
+\r
+               logf("MBR contents:\n");\r
+\r
+               for (int i = 0; i < 4; i++) {\r
+                       MbrPartition& p = mbr->mbr_part[i];\r
+                       printf("    %d: %s at:%d size:%d\n", i, mbr_fs_to_string(p.part_typ), p.part_start, p.part_size);\r
+               }\r
+       }\r
+\r
+       MbrImpl() {\r
+               mbr = new Mbr;\r
+               mmc = get_sdhost_device();\r
+               if (!mmc) {\r
+                       panic("parent block device not initilalized!");\r
+               }\r
+               read_mbr();\r
+               logf("Disk ready!\n");\r
+       }\r
+};\r
+\r
+MbrImpl STATIC_FILESYSTEM g_MbrDisk {};\r
+\r
+/*****************************************************************************\r
+ * Wrappers for FatFS.\r
+ *****************************************************************************/\r
+\r
+#include "fatfs/diskio.h"\r
+\r
+DSTATUS disk_initialize (BYTE pdrv) {\r
+       BYTE pt = g_MbrDisk.get_partition_type(pdrv);\r
+       switch (pt) {\r
+               case MBR_FAT32_INT13:\r
+               case MBR_FAT16_INT13:\r
+               case MBR_FAT32:\r
+               case MBR_FAT16:\r
+                       logf("Mounting FAT partition %d of type 0x%x\n", pdrv, pt);\r
+                       return (DRESULT)0;\r
+       }\r
+       logf("Disk %d isn't a FAT volume (partition type is 0x%x)!\n", pdrv, pt);\r
+       return STA_NOINIT;\r
+}\r
+\r
+DSTATUS disk_status (BYTE pdrv) {\r
+       return disk_initialize(pdrv);\r
+}\r
+\r
+DRESULT disk_read (BYTE pdrv, BYTE* buff, DWORD sector, UINT count) {\r
+       while (count--)\r
+       {\r
+               g_MbrDisk.read_block(pdrv, sector, buff);\r
+               sector++;\r
+               buff += g_MbrDisk.get_block_size();\r
+       }\r
+       return (DRESULT)0;\r
+}\r
+\r
+DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void* buff) {\r
+       switch (cmd)\r
+       {\r
+               case CTRL_SYNC:\r
+                       return (DRESULT)0;\r
+               case GET_SECTOR_SIZE:\r
+                       *(WORD*)buff = g_MbrDisk.get_block_size();\r
+                       return (DRESULT)0;\r
+\r
+               case GET_SECTOR_COUNT:\r
+                       *(WORD*)buff = 0;\r
+                       return (DRESULT)0;\r
+\r
+               case GET_BLOCK_SIZE:\r
+                       *(WORD*)buff = 1;\r
+                       return (DRESULT)0;\r
+       }\r
+       return RES_PARERR;\r
+}
\ No newline at end of file
diff --git a/arm_chainloader/drivers/sd2.hpp b/arm_chainloader/drivers/sd2.hpp
deleted file mode 100755 (executable)
index 01bd117..0000000
+++ /dev/null
@@ -1,355 +0,0 @@
-/*     $NetBSD: sdmmcreg.h,v 1.21 2015/10/29 22:37:15 jmcneill Exp $   */\r
-/*     $OpenBSD: sdmmcreg.h,v 1.4 2009/01/09 10:55:22 jsg Exp $        */\r
-\r
-/*\r
- * Copyright (c) 2006 Uwe Stuehler <uwe@openbsd.org>\r
- *\r
- * Permission to use, copy, modify, and distribute this software for any\r
- * purpose with or without fee is hereby granted, provided that the above\r
- * copyright notice and this permission notice appear in all copies.\r
- *\r
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES\r
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF\r
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR\r
- * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\r
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN\r
- * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF\r
- * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\r
- */\r
-\r
-#pragma once\r
-\r
-/* MMC commands */                             /* response type */\r
-#define MMC_GO_IDLE_STATE              0       /* R0 */\r
-#define MMC_SEND_OP_COND               1       /* R3 */\r
-#define MMC_ALL_SEND_CID               2       /* R2 */\r
-#define MMC_SET_RELATIVE_ADDR          3       /* R1 */\r
-#define MMC_SWITCH                     6       /* R1b */\r
-#define MMC_SELECT_CARD                        7       /* R1 */\r
-#define MMC_SEND_EXT_CSD               8       /* R1 */\r
-#define MMC_SEND_CSD                   9       /* R2 */\r
-#define MMC_SEND_CID                   10      /* R2 */\r
-#define MMC_STOP_TRANSMISSION          12      /* R1b */\r
-#define MMC_SEND_STATUS                        13      /* R1 */\r
-#define MMC_INACTIVE_STATE             15      /* R0 */\r
-#define MMC_SET_BLOCKLEN               16      /* R1 */\r
-#define MMC_READ_BLOCK_SINGLE          17      /* R1 */\r
-#define MMC_READ_BLOCK_MULTIPLE                18      /* R1 */\r
-#define MMC_SEND_TUNING_BLOCK          19      /* R1 */\r
-#define MMC_SEND_TUNING_BLOCK_HS200    21      /* R1 */\r
-#define MMC_SET_BLOCK_COUNT            23      /* R1 */\r
-#define MMC_WRITE_BLOCK_SINGLE         24      /* R1 */\r
-#define MMC_WRITE_BLOCK_MULTIPLE       25      /* R1 */\r
-#define MMC_PROGRAM_CSD                        27      /* R1 */\r
-#define MMC_SET_WRITE_PROT             28      /* R1b */\r
-#define MMC_SET_CLR_WRITE_PROT         29      /* R1b */\r
-#define MMC_SET_SEND_WRITE_PROT                30      /* R1 */\r
-#define MMC_TAG_SECTOR_START           32      /* R1 */\r
-#define MMC_TAG_SECTOR_END             33      /* R1 */\r
-#define MMC_UNTAG_SECTOR               34      /* R1 */\r
-#define MMC_TAG_ERASE_GROUP_START      35      /* R1 */\r
-#define MMC_TAG_ERASE_GROUP_END                36      /* R1 */\r
-#define MMC_UNTAG_ERASE_GROUP          37      /* R1 */\r
-#define MMC_ERASE                      38      /* R1b */\r
-#define MMC_LOCK_UNLOCK                        42      /* R1b */\r
-#define MMC_APP_CMD                    55      /* R1 */\r
-#define MMC_READ_OCR                   58      /* R3 */\r
-\r
-/* SD commands */                      /* response type */\r
-#define SD_SEND_RELATIVE_ADDR          3       /* R6 */\r
-#define SD_SEND_SWITCH_FUNC            6       /* R1 */\r
-#define SD_SEND_IF_COND                        8       /* R7 */\r
-#define SD_VOLTAGE_SWITCH              11      /* R1 */\r
-\r
-/* SD application commands */                  /* response type */\r
-#define SD_APP_SET_BUS_WIDTH           6       /* R1 */\r
-#define SD_APP_SD_STATUS               13      /* R1 */\r
-#define SD_APP_OP_COND                 41      /* R3 */\r
-#define SD_APP_SEND_SCR                        51      /* R1 */\r
-\r
-/* OCR bits */\r
-#define MMC_OCR_MEM_READY              (1U<<31)/* memory power-up status bit */\r
-#define MMC_OCR_HCS                    (1<<30) /* SD only */\r
-#define MMC_OCR_ACCESS_MODE_MASK       (3<<29) /* MMC only */\r
-#define MMC_OCR_ACCESS_MODE_BYTE       (0<<29) /* MMC only */\r
-#define MMC_OCR_ACCESS_MODE_SECTOR     (2<<29) /* MMC only */\r
-#define MMC_OCR_S18A                   (1<<24)\r
-#define MMC_OCR_3_5V_3_6V              (1<<23)\r
-#define MMC_OCR_3_4V_3_5V              (1<<22)\r
-#define MMC_OCR_3_3V_3_4V              (1<<21)\r
-#define MMC_OCR_3_2V_3_3V              (1<<20)\r
-#define MMC_OCR_3_1V_3_2V              (1<<19)\r
-#define MMC_OCR_3_0V_3_1V              (1<<18)\r
-#define MMC_OCR_2_9V_3_0V              (1<<17)\r
-#define MMC_OCR_2_8V_2_9V              (1<<16)\r
-#define MMC_OCR_2_7V_2_8V              (1<<15)\r
-#define MMC_OCR_2_6V_2_7V              (1<<14)\r
-#define MMC_OCR_2_5V_2_6V              (1<<13)\r
-#define MMC_OCR_2_4V_2_5V              (1<<12)\r
-#define MMC_OCR_2_3V_2_4V              (1<<11)\r
-#define MMC_OCR_2_2V_2_3V              (1<<10)\r
-#define MMC_OCR_2_1V_2_2V              (1<<9)\r
-#define MMC_OCR_2_0V_2_1V              (1<<8)\r
-#define MMC_OCR_1_9V_2_0V              (1<<7)\r
-#define MMC_OCR_1_8V_1_9V              (1<<6)\r
-#define MMC_OCR_1_7V_1_8V              (1<<5)\r
-#define MMC_OCR_1_6V_1_7V              (1<<4)\r
-\r
-/* R1 response type bits */\r
-#define MMC_R1_READY_FOR_DATA          (1<<8)  /* ready for next transfer */\r
-#define MMC_R1_SWITCH_ERROR            (1<<7)  /* switch command failed */\r
-#define MMC_R1_APP_CMD                 (1<<5)  /* app. commands supported */\r
-\r
-/* 48-bit response decoding (32 bits w/o CRC) */\r
-#define MMC_R1(resp)                   ((resp)[0])\r
-#define MMC_R3(resp)                   ((resp)[0])\r
-#define SD_R6(resp)                    ((resp)[0])\r
-#define MMC_R7(resp)                   ((resp)[0])\r
-#define MMC_SPI_R1(resp)               ((resp)[0])\r
-#define MMC_SPI_R7(resp)               ((resp)[1])\r
-\r
-/* RCA argument and response */\r
-#define MMC_ARG_RCA(rca)               ((rca) << 16)\r
-#define SD_R6_RCA(resp)                        (SD_R6((resp)) >> 16)\r
-\r
-/* bus width argument */\r
-#define SD_ARG_BUS_WIDTH_1             0\r
-#define SD_ARG_BUS_WIDTH_4             2\r
-\r
-/* EXT_CSD fields */\r
-#define EXT_CSD_BUS_WIDTH              183     /* WO */\r
-#define EXT_CSD_HS_TIMING              185     /* R/W */\r
-#define EXT_CSD_REV                    192     /* RO */\r
-#define EXT_CSD_STRUCTURE              194     /* RO */\r
-#define EXT_CSD_CARD_TYPE              196     /* RO */\r
-#define EXT_CSD_SEC_COUNT              212     /* RO */\r
-\r
-/* EXT_CSD field definitions */\r
-#define EXT_CSD_CMD_SET_NORMAL         (1U << 0)\r
-#define EXT_CSD_CMD_SET_SECURE         (1U << 1)\r
-#define EXT_CSD_CMD_SET_CPSECURE       (1U << 2)\r
-\r
-/* EXT_CSD_BUS_WIDTH */\r
-#define EXT_CSD_BUS_WIDTH_1            0       /* 1 bit mode */\r
-#define EXT_CSD_BUS_WIDTH_4            1       /* 4 bit mode */\r
-#define EXT_CSD_BUS_WIDTH_8            2       /* 8 bit mode */\r
-\r
-/* EXT_CSD_STRUCTURE */\r
-#define EXT_CSD_STRUCTURE_VER_1_0      0       /* CSD Version No.1.0 */\r
-#define EXT_CSD_STRUCTURE_VER_1_1      1       /* CSD Version No.1.1 */\r
-#define EXT_CSD_STRUCTURE_VER_1_2      2       /* Version 4.1-4.2-4.3 */\r
-\r
-/* EXT_CSD_CARD_TYPE */\r
-#define EXT_CSD_CARD_TYPE_F_26M                (1 << 0)\r
-#define EXT_CSD_CARD_TYPE_F_52M                (1 << 1)\r
-#define EXT_CSD_CARD_TYPE_F_52M_1_8V   (1 << 2)\r
-#define EXT_CSD_CARD_TYPE_F_52M_1_2V   (1 << 3)\r
-#define EXT_CSD_CARD_TYPE_F_HS200_1_8V (1 << 4)\r
-#define EXT_CSD_CARD_TYPE_F_HS200_1_2V (1 << 5)\r
-#define EXT_CSD_CARD_TYPE_F_HS400_1_8V (1 << 6)\r
-#define EXT_CSD_CARD_TYPE_F_HS400_1_2V (1 << 7)\r
-#define EXT_CSD_CARD_TYPE_26M          0x01\r
-#define EXT_CSD_CARD_TYPE_52M          0x03\r
-#define EXT_CSD_CARD_TYPE_52M_V18      0x07\r
-#define EXT_CSD_CARD_TYPE_52M_V12      0x0b\r
-#define EXT_CSD_CARD_TYPE_52M_V12_18   0x0f\r
-\r
-/* MMC_SWITCH access mode */\r
-#define MMC_SWITCH_MODE_CMD_SET                0x00    /* Change the command set */\r
-#define MMC_SWITCH_MODE_SET_BITS       0x01    /* Set bits in value */\r
-#define MMC_SWITCH_MODE_CLEAR_BITS     0x02    /* Clear bits in value */\r
-#define MMC_SWITCH_MODE_WRITE_BYTE     0x03    /* Set target to value */\r
-\r
-/* SPI mode reports R1/R2(SEND_STATUS) status. */\r
-#define R1_SPI_IDLE            (1 << 0)\r
-#define R1_SPI_ERASE_RESET     (1 << 1)\r
-#define R1_SPI_ILLEGAL_COMMAND (1 << 2)\r
-#define R1_SPI_COM_CRC         (1 << 3)\r
-#define R1_SPI_ERASE_SEQ       (1 << 4)\r
-#define R1_SPI_ADDRESS         (1 << 5)\r
-#define R1_SPI_PARAMETER       (1 << 6)\r
-/* R1 bit 7 is always zero */\r
-#define R2_SPI_CARD_LOCKED     (1 << 8)\r
-#define R2_SPI_WP_ERASE_SKIP   (1 << 9)        /* or lock/unlock fail */\r
-#define R2_SPI_LOCK_UNLOCK_FAIL        R2_SPI_WP_ERASE_SKIP\r
-#define R2_SPI_ERROR           (1 << 10)\r
-#define R2_SPI_CC_ERROR                (1 << 11)\r
-#define R2_SPI_CARD_ECC_ERROR  (1 << 12)\r
-#define R2_SPI_WP_VIOLATION    (1 << 13)\r
-#define R2_SPI_ERASE_PARAM     (1 << 14)\r
-#define R2_SPI_OUT_OF_RANGE    (1 << 15)       /* or CSD overwrite */\r
-#define R2_SPI_CSD_OVERWRITE   R2_SPI_OUT_OF_RANGE\r
-\r
-/* MMC R2 response (CSD) */\r
-#define MMC_CSD_CSDVER(resp)           MMC_RSP_BITS((resp), 126, 2)\r
-#define  MMC_CSD_CSDVER_1_0            0\r
-#define  MMC_CSD_CSDVER_1_1            1\r
-#define  MMC_CSD_CSDVER_1_2            2 /* MMC 4.1 - 4.2 - 4.3 */\r
-#define  MMC_CSD_CSDVER_EXT_CSD                3 /* Version is coded in CSD_STRUCTURE in EXT_CSD */\r
-#define MMC_CSD_MMCVER(resp)           MMC_RSP_BITS((resp), 122, 4)\r
-#define  MMC_CSD_MMCVER_1_0            0 /* MMC 1.0 - 1.2 */\r
-#define  MMC_CSD_MMCVER_1_4            1 /* MMC 1.4 */\r
-#define  MMC_CSD_MMCVER_2_0            2 /* MMC 2.0 - 2.2 */\r
-#define  MMC_CSD_MMCVER_3_1            3 /* MMC 3.1 - 3.3 */\r
-#define  MMC_CSD_MMCVER_4_0            4 /* MMC 4.1 - 4.2 - 4.3 */\r
-#define MMC_CSD_TAAC(resp)             MMC_RSP_BITS((resp), 112, 8)\r
-#define MMC_CSD_TAAC_MANT(resp)                MMC_RSP_BITS((resp), 115, 4)\r
-#define MMC_CSD_TAAC_EXP(resp)         MMC_RSP_BITS((resp), 112, 3)\r
-#define MMC_CSD_NSAC(resp)             MMC_RSP_BITS((resp), 104, 8)\r
-#define MMC_CSD_TRAN_SPEED(resp)       MMC_RSP_BITS((resp), 96, 8)\r
-#define MMC_CSD_TRAN_SPEED_MANT(resp)  MMC_RSP_BITS((resp), 99, 4)\r
-#define MMC_CSD_TRAN_SPEED_EXP(resp)   MMC_RSP_BITS((resp), 96, 3)\r
-#define MMC_CSD_READ_BL_LEN(resp)      MMC_RSP_BITS((resp), 80, 4)\r
-#define MMC_CSD_C_SIZE(resp)           MMC_RSP_BITS((resp), 62, 12)\r
-#define MMC_CSD_CAPACITY(resp)         ((MMC_CSD_C_SIZE((resp))+1) << \\r
-                                        (MMC_CSD_C_SIZE_MULT((resp))+2))\r
-#define MMC_CSD_C_SIZE_MULT(resp)      MMC_RSP_BITS((resp), 47, 3)\r
-#define MMC_CSD_R2W_FACTOR(resp)       MMC_RSP_BITS((resp), 26, 3)\r
-#define MMC_CSD_WRITE_BL_LEN(resp)     MMC_RSP_BITS((resp), 22, 4)\r
-\r
-/* MMC v1 R2 response (CID) */\r
-#define MMC_CID_MID_V1(resp)           MMC_RSP_BITS((resp), 104, 24)\r
-#define MMC_CID_PNM_V1_CPY(resp, pnm)                                  \\r
-       do {                                                            \\r
-               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
-               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
-               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
-               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
-               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
-               (pnm)[5] = MMC_RSP_BITS((resp), 56, 8);                 \\r
-               (pnm)[6] = MMC_RSP_BITS((resp), 48, 8);                 \\r
-               (pnm)[7] = '\0';                                        \\r
-       } while (/*CONSTCOND*/0)\r
-#define MMC_CID_REV_V1(resp)           MMC_RSP_BITS((resp), 40, 8)\r
-#define MMC_CID_PSN_V1(resp)           MMC_RSP_BITS((resp), 16, 24)\r
-#define MMC_CID_MDT_V1(resp)           MMC_RSP_BITS((resp), 8, 8)\r
-\r
-/* MMC v2 R2 response (CID) */\r
-#define MMC_CID_MID_V2(resp)           MMC_RSP_BITS((resp), 120, 8)\r
-#define MMC_CID_OID_V2(resp)           MMC_RSP_BITS((resp), 104, 16)\r
-#define MMC_CID_PNM_V2_CPY(resp, pnm)                                  \\r
-       do {                                                            \\r
-               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
-               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
-               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
-               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
-               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
-               (pnm)[5] = MMC_RSP_BITS((resp), 56, 8);                 \\r
-               (pnm)[6] = '\0';                                        \\r
-       } while (/*CONSTCOND*/0)\r
-#define MMC_CID_PSN_V2(resp)           MMC_RSP_BITS((resp), 16, 32)\r
-\r
-/* SD R2 response (CSD) */\r
-#define SD_CSD_CSDVER(resp)            MMC_RSP_BITS((resp), 126, 2)\r
-#define  SD_CSD_CSDVER_1_0             0\r
-#define  SD_CSD_CSDVER_2_0             1\r
-#define SD_CSD_MMCVER(resp)            MMC_RSP_BITS((resp), 122, 4)\r
-#define SD_CSD_TAAC(resp)              MMC_RSP_BITS((resp), 112, 8)\r
-#define SD_CSD_TAAC_EXP(resp)          MMC_RSP_BITS((resp), 115, 4)\r
-#define SD_CSD_TAAC_MANT(resp)         MMC_RSP_BITS((resp), 112, 3)\r
-#define  SD_CSD_TAAC_1_5_MSEC          0x26\r
-#define SD_CSD_NSAC(resp)              MMC_RSP_BITS((resp), 104, 8)\r
-#define SD_CSD_SPEED(resp)             MMC_RSP_BITS((resp), 96, 8)\r
-#define SD_CSD_SPEED_MANT(resp)                MMC_RSP_BITS((resp), 99, 4)\r
-#define SD_CSD_SPEED_EXP(resp)         MMC_RSP_BITS((resp), 96, 3)\r
-#define  SD_CSD_SPEED_25_MHZ           0x32\r
-#define  SD_CSD_SPEED_50_MHZ           0x5a\r
-#define SD_CSD_CCC(resp)               MMC_RSP_BITS((resp), 84, 12)\r
-#define  SD_CSD_CCC_BASIC              (1 << 0)        /* basic */\r
-#define  SD_CSD_CCC_BR                 (1 << 2)        /* block read */\r
-#define  SD_CSD_CCC_BW                 (1 << 4)        /* block write */\r
-#define  SD_CSD_CCC_ERACE              (1 << 5)        /* erase */\r
-#define  SD_CSD_CCC_WP                 (1 << 6)        /* write protection */\r
-#define  SD_CSD_CCC_LC                 (1 << 7)        /* lock card */\r
-#define  SD_CSD_CCC_AS                 (1 << 8)        /*application specific*/\r
-#define  SD_CSD_CCC_IOM                        (1 << 9)        /* I/O mode */\r
-#define  SD_CSD_CCC_SWITCH             (1 << 10)       /* switch */\r
-#define SD_CSD_READ_BL_LEN(resp)       MMC_RSP_BITS((resp), 80, 4)\r
-#define SD_CSD_READ_BL_PARTIAL(resp)   MMC_RSP_BITS((resp), 79, 1)\r
-#define SD_CSD_WRITE_BLK_MISALIGN(resp)        MMC_RSP_BITS((resp), 78, 1)\r
-#define SD_CSD_READ_BLK_MISALIGN(resp) MMC_RSP_BITS((resp), 77, 1)\r
-#define SD_CSD_DSR_IMP(resp)           MMC_RSP_BITS((resp), 76, 1)\r
-#define SD_CSD_C_SIZE(resp)            MMC_RSP_BITS((resp), 62, 12)\r
-#define SD_CSD_CAPACITY(resp)          ((SD_CSD_C_SIZE((resp))+1) << \\r
-                                        (SD_CSD_C_SIZE_MULT((resp))+2))\r
-#define SD_CSD_VDD_R_CURR_MIN(resp)    MMC_RSP_BITS((resp), 59, 3)\r
-#define SD_CSD_VDD_R_CURR_MAX(resp)    MMC_RSP_BITS((resp), 56, 3)\r
-#define SD_CSD_VDD_W_CURR_MIN(resp)    MMC_RSP_BITS((resp), 53, 3)\r
-#define SD_CSD_VDD_W_CURR_MAX(resp)    MMC_RSP_BITS((resp), 50, 3)\r
-#define  SD_CSD_VDD_RW_CURR_100mA      0x7\r
-#define  SD_CSD_VDD_RW_CURR_80mA       0x6\r
-#define SD_CSD_V2_C_SIZE(resp)         MMC_RSP_BITS((resp), 48, 22)\r
-#define SD_CSD_V2_CAPACITY(resp)       ((SD_CSD_V2_C_SIZE((resp))+1) << 10)\r
-#define SD_CSD_V2_BL_LEN               0x9 /* 512 */\r
-#define SD_CSD_C_SIZE_MULT(resp)       MMC_RSP_BITS((resp), 47, 3)\r
-#define SD_CSD_ERASE_BLK_EN(resp)      MMC_RSP_BITS((resp), 46, 1)\r
-#define SD_CSD_SECTOR_SIZE(resp)       MMC_RSP_BITS((resp), 39, 7) /* +1 */\r
-#define SD_CSD_WP_GRP_SIZE(resp)       MMC_RSP_BITS((resp), 32, 7) /* +1 */\r
-#define SD_CSD_WP_GRP_ENABLE(resp)     MMC_RSP_BITS((resp), 31, 1)\r
-#define SD_CSD_R2W_FACTOR(resp)                MMC_RSP_BITS((resp), 26, 3)\r
-#define SD_CSD_WRITE_BL_LEN(resp)      MMC_RSP_BITS((resp), 22, 4)\r
-#define  SD_CSD_RW_BL_LEN_2G           0xa\r
-#define  SD_CSD_RW_BL_LEN_1G           0x9\r
-#define SD_CSD_WRITE_BL_PARTIAL(resp)  MMC_RSP_BITS((resp), 21, 1)\r
-#define SD_CSD_FILE_FORMAT_GRP(resp)   MMC_RSP_BITS((resp), 15, 1)\r
-#define SD_CSD_COPY(resp)              MMC_RSP_BITS((resp), 14, 1)\r
-#define SD_CSD_PERM_WRITE_PROTECT(resp)        MMC_RSP_BITS((resp), 13, 1)\r
-#define SD_CSD_TMP_WRITE_PROTECT(resp) MMC_RSP_BITS((resp), 12, 1)\r
-#define SD_CSD_FILE_FORMAT(resp)       MMC_RSP_BITS((resp), 10, 2)\r
-\r
-/* SD R2 response (CID) */\r
-#define SD_CID_MID(resp)               MMC_RSP_BITS((resp), 120, 8)\r
-#define SD_CID_OID(resp)               MMC_RSP_BITS((resp), 104, 16)\r
-#define SD_CID_PNM_CPY(resp, pnm)                                      \\r
-       do {                                                            \\r
-               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
-               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
-               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
-               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
-               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
-               (pnm)[5] = '\0';                                        \\r
-       } while (/*CONSTCOND*/0)\r
-#define SD_CID_REV(resp)               MMC_RSP_BITS((resp), 56, 8)\r
-#define SD_CID_PSN(resp)               MMC_RSP_BITS((resp), 24, 32)\r
-#define SD_CID_MDT(resp)               MMC_RSP_BITS((resp), 8, 12)\r
-\r
-/* SCR (SD Configuration Register) */\r
-#define SCR_STRUCTURE(scr)             MMC_RSP_BITS((scr), 60, 4)\r
-#define  SCR_STRUCTURE_VER_1_0         0 /* Version 1.0 */\r
-#define SCR_SD_SPEC(scr)               MMC_RSP_BITS((scr), 56, 4)\r
-#define  SCR_SD_SPEC_VER_1_0           0 /* Version 1.0 and 1.01 */\r
-#define  SCR_SD_SPEC_VER_1_10          1 /* Version 1.10 */\r
-#define  SCR_SD_SPEC_VER_2             2 /* Version 2.00 or Version 3.0X */\r
-#define SCR_DATA_STAT_AFTER_ERASE(scr) MMC_RSP_BITS((scr), 55, 1)\r
-#define SCR_SD_SECURITY(scr)           MMC_RSP_BITS((scr), 52, 3)\r
-#define  SCR_SD_SECURITY_NONE          0 /* no security */\r
-#define  SCR_SD_SECURITY_1_0           1 /* security protocol 1.0 */\r
-#define  SCR_SD_SECURITY_1_0_2         2 /* security protocol 1.0 */\r
-#define SCR_SD_BUS_WIDTHS(scr)         MMC_RSP_BITS((scr), 48, 4)\r
-#define  SCR_SD_BUS_WIDTHS_1BIT                (1 << 0) /* 1bit (DAT0) */\r
-#define  SCR_SD_BUS_WIDTHS_4BIT                (1 << 2) /* 4bit (DAT0-3) */\r
-#define SCR_SD_SPEC3(scr)              MMC_RSP_BITS((scr), 47, 1)\r
-#define SCR_EX_SECURITY(scr)           MMC_RSP_BITS((scr), 43, 4)\r
-#define SCR_RESERVED(scr)              MMC_RSP_BITS((scr), 34, 9)\r
-#define SCR_CMD_SUPPORT_CMD23(scr)     MMC_RSP_BITS((scr), 33, 1)\r
-#define SCR_CMD_SUPPORT_CMD20(scr)     MMC_RSP_BITS((scr), 32, 1)\r
-#define SCR_RESERVED2(scr)             MMC_RSP_BITS((scr), 0, 32)\r
-\r
-#define MMC_RSP_BITS(resp, start, len) __bitfield((resp), (start)-8, (len))\r
-static inline uint32_t\r
-__bitfield(const uint32_t *src, size_t start, size_t len)\r
-{\r
-       if (start + len > 512 || len == 0 || len > 32)\r
-               return 0;\r
-\r
-       src += start / 32;\r
-       start %= 32;\r
-\r
-       uint32_t dst = src[0] >> start;\r
-\r
-       if (((start + len - 1) / 32 != start / 32)) {\r
-               dst |= src[1] << (32 - start);\r
-       }\r
-\r
-       return dst & (__BIT(len) - 1);\r
-}
\ No newline at end of file
diff --git a/arm_chainloader/drivers/sd_proto.hpp b/arm_chainloader/drivers/sd_proto.hpp
new file mode 100644 (file)
index 0000000..4aaf5bf
--- /dev/null
@@ -0,0 +1,369 @@
+/*     $NetBSD: sdmmcreg.h,v 1.21 2015/10/29 22:37:15 jmcneill Exp $   */\r
+/*     $OpenBSD: sdmmcreg.h,v 1.4 2009/01/09 10:55:22 jsg Exp $        */\r
+\r
+/*\r
+ * Copyright (c) 2006 Uwe Stuehler <uwe@openbsd.org>\r
+ *\r
+ * Permission to use, copy, modify, and distribute this software for any\r
+ * purpose with or without fee is hereby granted, provided that the above\r
+ * copyright notice and this permission notice appear in all copies.\r
+ *\r
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES\r
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF\r
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR\r
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES\r
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN\r
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF\r
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.\r
+ */\r
+\r
+#include <limits.h>\r
+\r
+#pragma once\r
+\r
+/* MMC commands */                             /* response type */\r
+#define MMC_GO_IDLE_STATE              0       /* R0 */\r
+#define MMC_SEND_OP_COND               1       /* R3 */\r
+#define MMC_ALL_SEND_CID               2       /* R2 */\r
+#define MMC_SET_RELATIVE_ADDR          3       /* R1 */\r
+#define MMC_SWITCH                     6       /* R1b */\r
+#define MMC_SELECT_CARD                        7       /* R1 */\r
+#define MMC_SEND_EXT_CSD               8       /* R1 */\r
+#define MMC_SEND_CSD                   9       /* R2 */\r
+#define MMC_SEND_CID                   10      /* R2 */\r
+#define MMC_STOP_TRANSMISSION          12      /* R1b */\r
+#define MMC_SEND_STATUS                        13      /* R1 */\r
+#define MMC_INACTIVE_STATE             15      /* R0 */\r
+#define MMC_SET_BLOCKLEN               16      /* R1 */\r
+#define MMC_READ_BLOCK_SINGLE          17      /* R1 */\r
+#define MMC_READ_BLOCK_MULTIPLE                18      /* R1 */\r
+#define MMC_SEND_TUNING_BLOCK          19      /* R1 */\r
+#define MMC_SEND_TUNING_BLOCK_HS200    21      /* R1 */\r
+#define MMC_SET_BLOCK_COUNT            23      /* R1 */\r
+#define MMC_WRITE_BLOCK_SINGLE         24      /* R1 */\r
+#define MMC_WRITE_BLOCK_MULTIPLE       25      /* R1 */\r
+#define MMC_PROGRAM_CSD                        27      /* R1 */\r
+#define MMC_SET_WRITE_PROT             28      /* R1b */\r
+#define MMC_SET_CLR_WRITE_PROT         29      /* R1b */\r
+#define MMC_SET_SEND_WRITE_PROT                30      /* R1 */\r
+#define MMC_TAG_SECTOR_START           32      /* R1 */\r
+#define MMC_TAG_SECTOR_END             33      /* R1 */\r
+#define MMC_UNTAG_SECTOR               34      /* R1 */\r
+#define MMC_TAG_ERASE_GROUP_START      35      /* R1 */\r
+#define MMC_TAG_ERASE_GROUP_END                36      /* R1 */\r
+#define MMC_UNTAG_ERASE_GROUP          37      /* R1 */\r
+#define MMC_ERASE                      38      /* R1b */\r
+#define MMC_LOCK_UNLOCK                        42      /* R1b */\r
+#define MMC_APP_CMD                    55      /* R1 */\r
+#define MMC_READ_OCR                   58      /* R3 */\r
+\r
+/* SD commands */                      /* response type */\r
+#define SD_SEND_RELATIVE_ADDR          3       /* R6 */\r
+#define SD_SEND_SWITCH_FUNC            6       /* R1 */\r
+#define SD_SEND_IF_COND                        8       /* R7 */\r
+#define SD_VOLTAGE_SWITCH              11      /* R1 */\r
+\r
+/* SD application commands */                  /* response type */\r
+#define SD_APP_SET_BUS_WIDTH           6       /* R1 */\r
+#define SD_APP_SD_STATUS               13      /* R1 */\r
+#define SD_APP_OP_COND                 41      /* R3 */\r
+#define SD_APP_SEND_SCR                        51      /* R1 */\r
+\r
+/* OCR bits */\r
+#define MMC_OCR_MEM_READY              (1U<<31)/* memory power-up status bit */\r
+#define MMC_OCR_HCS                    (1<<30) /* SD only */\r
+#define MMC_OCR_ACCESS_MODE_MASK       (3<<29) /* MMC only */\r
+#define MMC_OCR_ACCESS_MODE_BYTE       (0<<29) /* MMC only */\r
+#define MMC_OCR_ACCESS_MODE_SECTOR     (2<<29) /* MMC only */\r
+#define MMC_OCR_S18A                   (1<<24)\r
+#define MMC_OCR_3_5V_3_6V              (1<<23)\r
+#define MMC_OCR_3_4V_3_5V              (1<<22)\r
+#define MMC_OCR_3_3V_3_4V              (1<<21)\r
+#define MMC_OCR_3_2V_3_3V              (1<<20)\r
+#define MMC_OCR_3_1V_3_2V              (1<<19)\r
+#define MMC_OCR_3_0V_3_1V              (1<<18)\r
+#define MMC_OCR_2_9V_3_0V              (1<<17)\r
+#define MMC_OCR_2_8V_2_9V              (1<<16)\r
+#define MMC_OCR_2_7V_2_8V              (1<<15)\r
+#define MMC_OCR_2_6V_2_7V              (1<<14)\r
+#define MMC_OCR_2_5V_2_6V              (1<<13)\r
+#define MMC_OCR_2_4V_2_5V              (1<<12)\r
+#define MMC_OCR_2_3V_2_4V              (1<<11)\r
+#define MMC_OCR_2_2V_2_3V              (1<<10)\r
+#define MMC_OCR_2_1V_2_2V              (1<<9)\r
+#define MMC_OCR_2_0V_2_1V              (1<<8)\r
+#define MMC_OCR_1_9V_2_0V              (1<<7)\r
+#define MMC_OCR_1_8V_1_9V              (1<<6)\r
+#define MMC_OCR_1_7V_1_8V              (1<<5)\r
+#define MMC_OCR_1_6V_1_7V              (1<<4)\r
+\r
+/* R1 response type bits */\r
+#define MMC_R1_READY_FOR_DATA          (1<<8)  /* ready for next transfer */\r
+#define MMC_R1_SWITCH_ERROR            (1<<7)  /* switch command failed */\r
+#define MMC_R1_APP_CMD                 (1<<5)  /* app. commands supported */\r
+\r
+/* 48-bit response decoding (32 bits w/o CRC) */\r
+#define MMC_R1(resp)                   ((resp)[0])\r
+#define MMC_R3(resp)                   ((resp)[0])\r
+#define SD_R6(resp)                    ((resp)[0])\r
+#define MMC_R7(resp)                   ((resp)[0])\r
+#define MMC_SPI_R1(resp)               ((resp)[0])\r
+#define MMC_SPI_R7(resp)               ((resp)[1])\r
+\r
+/* RCA argument and response */\r
+#define MMC_ARG_RCA(rca)               ((rca) << 16)\r
+#define SD_R6_RCA(resp)                        (SD_R6((resp)) >> 16)\r
+\r
+/* bus width argument */\r
+#define SD_ARG_BUS_WIDTH_1             0\r
+#define SD_ARG_BUS_WIDTH_4             2\r
+\r
+/* EXT_CSD fields */\r
+#define EXT_CSD_BUS_WIDTH              183     /* WO */\r
+#define EXT_CSD_HS_TIMING              185     /* R/W */\r
+#define EXT_CSD_REV                    192     /* RO */\r
+#define EXT_CSD_STRUCTURE              194     /* RO */\r
+#define EXT_CSD_CARD_TYPE              196     /* RO */\r
+#define EXT_CSD_SEC_COUNT              212     /* RO */\r
+\r
+/* EXT_CSD field definitions */\r
+#define EXT_CSD_CMD_SET_NORMAL         (1U << 0)\r
+#define EXT_CSD_CMD_SET_SECURE         (1U << 1)\r
+#define EXT_CSD_CMD_SET_CPSECURE       (1U << 2)\r
+\r
+/* EXT_CSD_BUS_WIDTH */\r
+#define EXT_CSD_BUS_WIDTH_1            0       /* 1 bit mode */\r
+#define EXT_CSD_BUS_WIDTH_4            1       /* 4 bit mode */\r
+#define EXT_CSD_BUS_WIDTH_8            2       /* 8 bit mode */\r
+\r
+/* EXT_CSD_STRUCTURE */\r
+#define EXT_CSD_STRUCTURE_VER_1_0      0       /* CSD Version No.1.0 */\r
+#define EXT_CSD_STRUCTURE_VER_1_1      1       /* CSD Version No.1.1 */\r
+#define EXT_CSD_STRUCTURE_VER_1_2      2       /* Version 4.1-4.2-4.3 */\r
+\r
+/* EXT_CSD_CARD_TYPE */\r
+#define EXT_CSD_CARD_TYPE_F_26M                (1 << 0)\r
+#define EXT_CSD_CARD_TYPE_F_52M                (1 << 1)\r
+#define EXT_CSD_CARD_TYPE_F_52M_1_8V   (1 << 2)\r
+#define EXT_CSD_CARD_TYPE_F_52M_1_2V   (1 << 3)\r
+#define EXT_CSD_CARD_TYPE_F_HS200_1_8V (1 << 4)\r
+#define EXT_CSD_CARD_TYPE_F_HS200_1_2V (1 << 5)\r
+#define EXT_CSD_CARD_TYPE_F_HS400_1_8V (1 << 6)\r
+#define EXT_CSD_CARD_TYPE_F_HS400_1_2V (1 << 7)\r
+#define EXT_CSD_CARD_TYPE_26M          0x01\r
+#define EXT_CSD_CARD_TYPE_52M          0x03\r
+#define EXT_CSD_CARD_TYPE_52M_V18      0x07\r
+#define EXT_CSD_CARD_TYPE_52M_V12      0x0b\r
+#define EXT_CSD_CARD_TYPE_52M_V12_18   0x0f\r
+\r
+/* MMC_SWITCH access mode */\r
+#define MMC_SWITCH_MODE_CMD_SET                0x00    /* Change the command set */\r
+#define MMC_SWITCH_MODE_SET_BITS       0x01    /* Set bits in value */\r
+#define MMC_SWITCH_MODE_CLEAR_BITS     0x02    /* Clear bits in value */\r
+#define MMC_SWITCH_MODE_WRITE_BYTE     0x03    /* Set target to value */\r
+\r
+/* SPI mode reports R1/R2(SEND_STATUS) status. */\r
+#define R1_SPI_IDLE            (1 << 0)\r
+#define R1_SPI_ERASE_RESET     (1 << 1)\r
+#define R1_SPI_ILLEGAL_COMMAND (1 << 2)\r
+#define R1_SPI_COM_CRC         (1 << 3)\r
+#define R1_SPI_ERASE_SEQ       (1 << 4)\r
+#define R1_SPI_ADDRESS         (1 << 5)\r
+#define R1_SPI_PARAMETER       (1 << 6)\r
+/* R1 bit 7 is always zero */\r
+#define R2_SPI_CARD_LOCKED     (1 << 8)\r
+#define R2_SPI_WP_ERASE_SKIP   (1 << 9)        /* or lock/unlock fail */\r
+#define R2_SPI_LOCK_UNLOCK_FAIL        R2_SPI_WP_ERASE_SKIP\r
+#define R2_SPI_ERROR           (1 << 10)\r
+#define R2_SPI_CC_ERROR                (1 << 11)\r
+#define R2_SPI_CARD_ECC_ERROR  (1 << 12)\r
+#define R2_SPI_WP_VIOLATION    (1 << 13)\r
+#define R2_SPI_ERASE_PARAM     (1 << 14)\r
+#define R2_SPI_OUT_OF_RANGE    (1 << 15)       /* or CSD overwrite */\r
+#define R2_SPI_CSD_OVERWRITE   R2_SPI_OUT_OF_RANGE\r
+\r
+/* MMC R2 response (CSD) */\r
+#define MMC_CSD_CSDVER(resp)           MMC_RSP_BITS((resp), 126, 2)\r
+#define  MMC_CSD_CSDVER_1_0            0\r
+#define  MMC_CSD_CSDVER_1_1            1\r
+#define  MMC_CSD_CSDVER_1_2            2 /* MMC 4.1 - 4.2 - 4.3 */\r
+#define  MMC_CSD_CSDVER_EXT_CSD                3 /* Version is coded in CSD_STRUCTURE in EXT_CSD */\r
+#define MMC_CSD_MMCVER(resp)           MMC_RSP_BITS((resp), 122, 4)\r
+#define  MMC_CSD_MMCVER_1_0            0 /* MMC 1.0 - 1.2 */\r
+#define  MMC_CSD_MMCVER_1_4            1 /* MMC 1.4 */\r
+#define  MMC_CSD_MMCVER_2_0            2 /* MMC 2.0 - 2.2 */\r
+#define  MMC_CSD_MMCVER_3_1            3 /* MMC 3.1 - 3.3 */\r
+#define  MMC_CSD_MMCVER_4_0            4 /* MMC 4.1 - 4.2 - 4.3 */\r
+#define MMC_CSD_TAAC(resp)             MMC_RSP_BITS((resp), 112, 8)\r
+#define MMC_CSD_TAAC_MANT(resp)                MMC_RSP_BITS((resp), 115, 4)\r
+#define MMC_CSD_TAAC_EXP(resp)         MMC_RSP_BITS((resp), 112, 3)\r
+#define MMC_CSD_NSAC(resp)             MMC_RSP_BITS((resp), 104, 8)\r
+#define MMC_CSD_TRAN_SPEED(resp)       MMC_RSP_BITS((resp), 96, 8)\r
+#define MMC_CSD_TRAN_SPEED_MANT(resp)  MMC_RSP_BITS((resp), 99, 4)\r
+#define MMC_CSD_TRAN_SPEED_EXP(resp)   MMC_RSP_BITS((resp), 96, 3)\r
+#define MMC_CSD_READ_BL_LEN(resp)      MMC_RSP_BITS((resp), 80, 4)\r
+#define MMC_CSD_C_SIZE(resp)           MMC_RSP_BITS((resp), 62, 12)\r
+#define MMC_CSD_CAPACITY(resp)         ((MMC_CSD_C_SIZE((resp))+1) << \\r
+                                        (MMC_CSD_C_SIZE_MULT((resp))+2))\r
+#define MMC_CSD_C_SIZE_MULT(resp)      MMC_RSP_BITS((resp), 47, 3)\r
+#define MMC_CSD_R2W_FACTOR(resp)       MMC_RSP_BITS((resp), 26, 3)\r
+#define MMC_CSD_WRITE_BL_LEN(resp)     MMC_RSP_BITS((resp), 22, 4)\r
+\r
+/* MMC v1 R2 response (CID) */\r
+#define MMC_CID_MID_V1(resp)           MMC_RSP_BITS((resp), 104, 24)\r
+#define MMC_CID_PNM_V1_CPY(resp, pnm)                                  \\r
+       do {                                                            \\r
+               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
+               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
+               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
+               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
+               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
+               (pnm)[5] = MMC_RSP_BITS((resp), 56, 8);                 \\r
+               (pnm)[6] = MMC_RSP_BITS((resp), 48, 8);                 \\r
+               (pnm)[7] = '\0';                                        \\r
+       } while (/*CONSTCOND*/0)\r
+#define MMC_CID_REV_V1(resp)           MMC_RSP_BITS((resp), 40, 8)\r
+#define MMC_CID_PSN_V1(resp)           MMC_RSP_BITS((resp), 16, 24)\r
+#define MMC_CID_MDT_V1(resp)           MMC_RSP_BITS((resp), 8, 8)\r
+\r
+/* MMC v2 R2 response (CID) */\r
+#define MMC_CID_MID_V2(resp)           MMC_RSP_BITS((resp), 120, 8)\r
+#define MMC_CID_OID_V2(resp)           MMC_RSP_BITS((resp), 104, 16)\r
+#define MMC_CID_PNM_V2_CPY(resp, pnm)                                  \\r
+       do {                                                            \\r
+               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
+               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
+               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
+               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
+               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
+               (pnm)[5] = MMC_RSP_BITS((resp), 56, 8);                 \\r
+               (pnm)[6] = '\0';                                        \\r
+       } while (/*CONSTCOND*/0)\r
+#define MMC_CID_PSN_V2(resp)           MMC_RSP_BITS((resp), 16, 32)\r
+\r
+/* SD R2 response (CSD) */\r
+#define SD_CSD_CSDVER(resp)            MMC_RSP_BITS((resp), 126, 2)\r
+#define  SD_CSD_CSDVER_1_0             0\r
+#define  SD_CSD_CSDVER_2_0             1\r
+#define SD_CSD_MMCVER(resp)            MMC_RSP_BITS((resp), 122, 4)\r
+#define SD_CSD_TAAC(resp)              MMC_RSP_BITS((resp), 112, 8)\r
+#define SD_CSD_TAAC_EXP(resp)          MMC_RSP_BITS((resp), 115, 4)\r
+#define SD_CSD_TAAC_MANT(resp)         MMC_RSP_BITS((resp), 112, 3)\r
+#define  SD_CSD_TAAC_1_5_MSEC          0x26\r
+#define SD_CSD_NSAC(resp)              MMC_RSP_BITS((resp), 104, 8)\r
+#define SD_CSD_SPEED(resp)             MMC_RSP_BITS((resp), 96, 8)\r
+#define SD_CSD_SPEED_MANT(resp)                MMC_RSP_BITS((resp), 99, 4)\r
+#define SD_CSD_SPEED_EXP(resp)         MMC_RSP_BITS((resp), 96, 3)\r
+#define  SD_CSD_SPEED_25_MHZ           0x32\r
+#define  SD_CSD_SPEED_50_MHZ           0x5a\r
+#define SD_CSD_CCC(resp)               MMC_RSP_BITS((resp), 84, 12)\r
+#define  SD_CSD_CCC_BASIC              (1 << 0)        /* basic */\r
+#define  SD_CSD_CCC_BR                 (1 << 2)        /* block read */\r
+#define  SD_CSD_CCC_BW                 (1 << 4)        /* block write */\r
+#define  SD_CSD_CCC_ERACE              (1 << 5)        /* erase */\r
+#define  SD_CSD_CCC_WP                 (1 << 6)        /* write protection */\r
+#define  SD_CSD_CCC_LC                 (1 << 7)        /* lock card */\r
+#define  SD_CSD_CCC_AS                 (1 << 8)        /*application specific*/\r
+#define  SD_CSD_CCC_IOM                        (1 << 9)        /* I/O mode */\r
+#define  SD_CSD_CCC_SWITCH             (1 << 10)       /* switch */\r
+#define SD_CSD_READ_BL_LEN(resp)       MMC_RSP_BITS((resp), 80, 4)\r
+#define SD_CSD_READ_BL_PARTIAL(resp)   MMC_RSP_BITS((resp), 79, 1)\r
+#define SD_CSD_WRITE_BLK_MISALIGN(resp)        MMC_RSP_BITS((resp), 78, 1)\r
+#define SD_CSD_READ_BLK_MISALIGN(resp) MMC_RSP_BITS((resp), 77, 1)\r
+#define SD_CSD_DSR_IMP(resp)           MMC_RSP_BITS((resp), 76, 1)\r
+#define SD_CSD_C_SIZE(resp)            MMC_RSP_BITS((resp), 62, 12)\r
+#define SD_CSD_CAPACITY(resp)          ((SD_CSD_C_SIZE((resp))+1) << \\r
+                                        (SD_CSD_C_SIZE_MULT((resp))+2))\r
+#define SD_CSD_VDD_R_CURR_MIN(resp)    MMC_RSP_BITS((resp), 59, 3)\r
+#define SD_CSD_VDD_R_CURR_MAX(resp)    MMC_RSP_BITS((resp), 56, 3)\r
+#define SD_CSD_VDD_W_CURR_MIN(resp)    MMC_RSP_BITS((resp), 53, 3)\r
+#define SD_CSD_VDD_W_CURR_MAX(resp)    MMC_RSP_BITS((resp), 50, 3)\r
+#define  SD_CSD_VDD_RW_CURR_100mA      0x7\r
+#define  SD_CSD_VDD_RW_CURR_80mA       0x6\r
+#define SD_CSD_V2_C_SIZE(resp)         MMC_RSP_BITS((resp), 48, 22)\r
+#define SD_CSD_V2_CAPACITY(resp)       ((SD_CSD_V2_C_SIZE((resp))+1) << 10)\r
+#define SD_CSD_V2_BL_LEN               0x9 /* 512 */\r
+#define SD_CSD_C_SIZE_MULT(resp)       MMC_RSP_BITS((resp), 47, 3)\r
+#define SD_CSD_ERASE_BLK_EN(resp)      MMC_RSP_BITS((resp), 46, 1)\r
+#define SD_CSD_SECTOR_SIZE(resp)       MMC_RSP_BITS((resp), 39, 7) /* +1 */\r
+#define SD_CSD_WP_GRP_SIZE(resp)       MMC_RSP_BITS((resp), 32, 7) /* +1 */\r
+#define SD_CSD_WP_GRP_ENABLE(resp)     MMC_RSP_BITS((resp), 31, 1)\r
+#define SD_CSD_R2W_FACTOR(resp)                MMC_RSP_BITS((resp), 26, 3)\r
+#define SD_CSD_WRITE_BL_LEN(resp)      MMC_RSP_BITS((resp), 22, 4)\r
+#define  SD_CSD_RW_BL_LEN_2G           0xa\r
+#define  SD_CSD_RW_BL_LEN_1G           0x9\r
+#define SD_CSD_WRITE_BL_PARTIAL(resp)  MMC_RSP_BITS((resp), 21, 1)\r
+#define SD_CSD_FILE_FORMAT_GRP(resp)   MMC_RSP_BITS((resp), 15, 1)\r
+#define SD_CSD_COPY(resp)              MMC_RSP_BITS((resp), 14, 1)\r
+#define SD_CSD_PERM_WRITE_PROTECT(resp)        MMC_RSP_BITS((resp), 13, 1)\r
+#define SD_CSD_TMP_WRITE_PROTECT(resp) MMC_RSP_BITS((resp), 12, 1)\r
+#define SD_CSD_FILE_FORMAT(resp)       MMC_RSP_BITS((resp), 10, 2)\r
+\r
+/* SD R2 response (CID) */\r
+#define SD_CID_MID(resp)               MMC_RSP_BITS((resp), 120, 8)\r
+#define SD_CID_OID(resp)               MMC_RSP_BITS((resp), 104, 16)\r
+#define SD_CID_PNM_CPY(resp, pnm)                                      \\r
+       do {                                                            \\r
+               (pnm)[0] = MMC_RSP_BITS((resp), 96, 8);                 \\r
+               (pnm)[1] = MMC_RSP_BITS((resp), 88, 8);                 \\r
+               (pnm)[2] = MMC_RSP_BITS((resp), 80, 8);                 \\r
+               (pnm)[3] = MMC_RSP_BITS((resp), 72, 8);                 \\r
+               (pnm)[4] = MMC_RSP_BITS((resp), 64, 8);                 \\r
+               (pnm)[5] = '\0';                                        \\r
+       } while (/*CONSTCOND*/0)\r
+#define SD_CID_REV(resp)               MMC_RSP_BITS((resp), 56, 8)\r
+#define SD_CID_PSN(resp)               MMC_RSP_BITS((resp), 24, 32)\r
+#define SD_CID_MDT(resp)               MMC_RSP_BITS((resp), 8, 12)\r
+\r
+/* SCR (SD Configuration Register) */\r
+#define SCR_STRUCTURE(scr)             MMC_RSP_BITS((scr), 60, 4)\r
+#define  SCR_STRUCTURE_VER_1_0         0 /* Version 1.0 */\r
+#define SCR_SD_SPEC(scr)               MMC_RSP_BITS((scr), 56, 4)\r
+#define  SCR_SD_SPEC_VER_1_0           0 /* Version 1.0 and 1.01 */\r
+#define  SCR_SD_SPEC_VER_1_10          1 /* Version 1.10 */\r
+#define  SCR_SD_SPEC_VER_2             2 /* Version 2.00 or Version 3.0X */\r
+#define SCR_DATA_STAT_AFTER_ERASE(scr) MMC_RSP_BITS((scr), 55, 1)\r
+#define SCR_SD_SECURITY(scr)           MMC_RSP_BITS((scr), 52, 3)\r
+#define  SCR_SD_SECURITY_NONE          0 /* no security */\r
+#define  SCR_SD_SECURITY_1_0           1 /* security protocol 1.0 */\r
+#define  SCR_SD_SECURITY_1_0_2         2 /* security protocol 1.0 */\r
+#define SCR_SD_BUS_WIDTHS(scr)         MMC_RSP_BITS((scr), 48, 4)\r
+#define  SCR_SD_BUS_WIDTHS_1BIT                (1 << 0) /* 1bit (DAT0) */\r
+#define  SCR_SD_BUS_WIDTHS_4BIT                (1 << 2) /* 4bit (DAT0-3) */\r
+#define SCR_SD_SPEC3(scr)              MMC_RSP_BITS((scr), 47, 1)\r
+#define SCR_EX_SECURITY(scr)           MMC_RSP_BITS((scr), 43, 4)\r
+#define SCR_RESERVED(scr)              MMC_RSP_BITS((scr), 34, 9)\r
+#define SCR_CMD_SUPPORT_CMD23(scr)     MMC_RSP_BITS((scr), 33, 1)\r
+#define SCR_CMD_SUPPORT_CMD20(scr)     MMC_RSP_BITS((scr), 32, 1)\r
+#define SCR_RESERVED2(scr)             MMC_RSP_BITS((scr), 0, 32)\r
+\r
+#define MMC_RSP_BITS(resp, start, len) __bitfield((resp), (start), (len))\r
+static inline int\r
+__bitfield(uint32_t *src, int start, int len)\r
+{\r
+       uint8_t *sp;\r
+       uint32_t dst, mask;\r
+       int shift, bs, bc;\r
+\r
+       if (start < 0 || len < 0 || len > 32)\r
+               return 0;\r
+\r
+       dst = 0;\r
+       mask = len % 32 ? UINT_MAX >> (32 - (len % 32)) : UINT_MAX;\r
+       shift = 0;\r
+\r
+       while (len > 0) {\r
+               sp = (uint8_t *)src + start / 8;\r
+               bs = start % 8;\r
+               bc = 8 - bs;\r
+               if (bc > len)\r
+                       bc = len;\r
+               dst |= (*sp >> bs) << shift;\r
+               shift += bc;\r
+               start += bc;\r
+               len -= bc;\r
+       }\r
+\r
+       dst &= mask;\r
+       return (int)dst;\r
+}
\ No newline at end of file
diff --git a/arm_chainloader/drivers/sdhost.cc b/arm_chainloader/drivers/sdhost.cc
deleted file mode 100755 (executable)
index 1239b9e..0000000
+++ /dev/null
@@ -1,336 +0,0 @@
-/*=============================================================================\r
-Copyright (C) 2016 Kristina Brooks\r
-All rights reserved.\r
-\r
-This program is free software; you can redistribute it and/or\r
-modify it under the terms of the GNU General Public License\r
-as published by the Free Software Foundation; either version 2\r
-of the License, or (at your option) any later version.\r
-\r
-This program is distributed in the hope that it will be useful,\r
-but WITHOUT ANY WARRANTY; without even the implied warranty of\r
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
-GNU General Public License for more details.\r
-\r
-FILE DESCRIPTION\r
-SDHOST driver. This used to be known as ALTMMC.\r
-\r
-=============================================================================*/\r
-\r
-#include <chainloader.h>\r
-#include <hardware.h>\r
-\r
-#include "sd2.hpp"\r
-\r
-#define SDEDM_WRITE_THRESHOLD_SHIFT 9\r
-#define SDEDM_READ_THRESHOLD_SHIFT 14\r
-#define SDEDM_THRESHOLD_MASK     0x1f\r
-\r
-#define SAFE_READ_THRESHOLD     4\r
-#define SAFE_WRITE_THRESHOLD    4\r
-\r
-#define VOLTAGE_SUPPLY_RANGE 0x100\r
-#define CHECK_PATTERN 0x55\r
-\r
-\r
-#define SDHSTS_BUSY_IRPT                0x400\r
-#define SDHSTS_BLOCK_IRPT               0x200\r
-#define SDHSTS_SDIO_IRPT                0x100\r
-#define SDHSTS_REW_TIME_OUT             0x80\r
-#define SDHSTS_CMD_TIME_OUT             0x40\r
-#define SDHSTS_CRC16_ERROR              0x20\r
-#define SDHSTS_CRC7_ERROR               0x10\r
-#define SDHSTS_FIFO_ERROR               0x08\r
-\r
-#define SDHSTS_TRANSFER_ERROR_MASK      (SDHSTS_CRC7_ERROR|SDHSTS_CRC16_ERROR|SDHSTS_REW_TIME_OUT|SDHSTS_FIFO_ERROR)\r
-#define SDHSTS_ERROR_MASK               (SDHSTS_CMD_TIME_OUT|SDHSTS_TRANSFER_ERROR_MASK)\r
-\r
-#define logf(fmt, ...) printf("[sdhost::%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
-\r
-struct sdhost_t {\r
-       bool is_sdhc;\r
-       bool is_high_capacity;\r
-\r
-       uint32_t ocr;\r
-       uint32_t rca;\r
-\r
-       uint32_t cid[4];\r
-       uint32_t csd[4];\r
-\r
-       uint32_t r[4];\r
-\r
-       void set_power(bool on) {\r
-               SH_VDD = on ? SH_VDD_POWER_ON_SET : 0x0;\r
-       }\r
-\r
-       bool wait(uint32_t timeout = 10000) {\r
-               uint32_t t = timeout;\r
-\r
-               while(SH_CMD & SH_CMD_NEW_FLAG_SET) {\r
-                       if (t == 0) {\r
-                               logf("timed out after %dus!\n", timeout)\r
-                               return false;\r
-                       }\r
-                       t--;\r
-                       udelay(10);\r
-               }\r
-\r
-               return true;\r
-       }\r
-\r
-       bool send_raw(uint32_t command, uint32_t arg = 0) {\r
-               uint32_t sts;\r
-\r
-               wait();\r
-\r
-               sts = SH_HSTS;\r
-               if (sts & SDHSTS_ERROR_MASK)\r
-                       SH_HSTS = sts; \r
-\r
-               SH_ARG = arg;\r
-               SH_CMD = command | SH_CMD_NEW_FLAG_SET;\r
-\r
-               mfence();\r
-\r
-               return true;\r
-       }\r
-\r
-       bool send(uint32_t command, uint32_t arg = 0) {\r
-               return send_raw(command & SH_CMD_COMMAND_SET, arg);\r
-       }\r
-\r
-       void configure_pinmux() {\r
-               GP_FSEL4 = 0x24000000;\r
-               GP_FSEL5 = 0x924;\r
-               GP_PUD = 2;\r
-\r
-               logf("GPIOs set!\n");\r
-       }\r
-\r
-       void reset() {\r
-               logf("resetting controller ...\n");\r
-               set_power(false);\r
-\r
-               SH_CMD = 0;\r
-               SH_ARG = 0;\r
-               SH_TOUT = 0xF00000;\r
-               SH_CDIV = 0;\r
-               SH_HSTS = 0x7f8;\r
-               SH_HCFG = 0;\r
-               SH_HBCT = 0;\r
-               SH_HBLC = 0;\r
-       \r
-               uint32_t temp = SH_EDM;\r
-\r
-               temp &= ~((SDEDM_THRESHOLD_MASK<<SDEDM_READ_THRESHOLD_SHIFT) |\r
-                 (SDEDM_THRESHOLD_MASK<<SDEDM_WRITE_THRESHOLD_SHIFT));\r
-               temp |= (SAFE_READ_THRESHOLD << SDEDM_READ_THRESHOLD_SHIFT) |\r
-                       (SAFE_WRITE_THRESHOLD << SDEDM_WRITE_THRESHOLD_SHIFT);\r
-\r
-               SH_EDM = temp;\r
-               udelay(300);\r
-\r
-               set_power(true);\r
-\r
-               udelay(300);\r
-               mfence();\r
-       }\r
-\r
-       inline void get_response() {\r
-               r[0] = SH_RSP0;\r
-               r[1] = SH_RSP1;\r
-               r[2] = SH_RSP2;\r
-               r[3] = SH_RSP3;\r
-       }\r
-\r
-       bool wait_and_get_response() {\r
-               if (!wait())\r
-                       return false;\r
-\r
-               get_response();\r
-\r
-               logf("HSTS: 0x%x, RSP: %08x %08x %08x %08x\n", SH_HSTS, r[0], r[1], r[2], r[3]);\r
-\r
-               return true;\r
-       }\r
-\r
-       bool query_voltage_and_type() {\r
-               uint32_t t;\r
-\r
-               /* identify */\r
-               send(SD_SEND_IF_COND, 0x1AA);\r
-               wait_and_get_response();\r
-\r
-               /* set voltage */\r
-               t = MMC_OCR_3_2V_3_3V;\r
-               if (r[0] == 0x1AA) {\r
-                       t |= MMC_OCR_HCS;\r
-                       is_sdhc = true;\r
-               }\r
-\r
-               /* query voltage and type */\r
-               for (;;) {\r
-                       send(MMC_APP_CMD);\r
-                       send(SD_APP_OP_COND, t);\r
-\r
-                       if (!wait_and_get_response())\r
-                               return false;\r
-\r
-                       if (r[0] & MMC_OCR_MEM_READY)\r
-                               break;\r
-\r
-                       logf("waiting for sd card (0x%x) ...\n", r[0]);\r
-                       udelay(100);\r
-               }\r
-\r
-               logf("sd card has arrived!\n", r);\r
-\r
-               is_high_capacity = (r[0] & MMC_OCR_HCS) == MMC_OCR_HCS;\r
-\r
-               if (is_high_capacity)\r
-                       logf("this is an SDHC card!\n");\r
-\r
-               return true;\r
-\r
-       }\r
-\r
-       bool identify_card() {\r
-               logf("identifying card ...\n");\r
-               \r
-               send_raw(SH_CMD_LONG_RESPONSE_SET | MMC_ALL_SEND_CID);\r
-               if (!wait_and_get_response())\r
-                       return false;\r
-\r
-               cid[0] = r[0];\r
-               cid[1] = r[1];\r
-               cid[2] = r[2];\r
-               cid[3] = r[3];\r
-\r
-               send(MMC_SET_RELATIVE_ADDR);\r
-\r
-               if (!wait_and_get_response())\r
-                       return false;\r
-\r
-               rca = SD_R6_RCA(r);\r
-\r
-               /* get card specific data */\r
-\r
-               send_raw(SH_CMD_LONG_RESPONSE_SET | MMC_SEND_CSD, MMC_ARG_RCA(rca));\r
-               if (!wait_and_get_response())\r
-                       return false;\r
-\r
-               csd[0] = r[0];\r
-               csd[1] = r[1];\r
-               csd[2] = r[2];\r
-               csd[3] = r[3];\r
-\r
-               return true;\r
-       }\r
-\r
-       bool select_card() {\r
-               send(MMC_SELECT_CARD, MMC_ARG_RCA(rca));\r
-\r
-               if (!wait())\r
-                       return false;\r
-\r
-               return true;\r
-       }\r
-\r
-       bool init_card() {\r
-               char pnm[8];\r
-\r
-               send(MMC_GO_IDLE_STATE);\r
-\r
-               if (!query_voltage_and_type()) {\r
-                       logf("ERROR: Failed to query card voltage!\n");\r
-                       return false;\r
-               }\r
-\r
-               if (!identify_card()) {\r
-                       logf("ERROR: Failed to identify card!\n");\r
-                       return false;\r
-               }\r
-\r
-               SD_CID_PNM_CPY(cid, pnm);\r
-               logf("Product : %s\n", &pnm);\r
-               logf("MMC Ver : %d\n", SD_CSD_MMCVER(csd));\r
-\r
-               logf("MMC Size: %d\n", MMC_CSD_C_SIZE(csd));\r
-               logf("MMC Cap : %d\n", MMC_CSD_CAPACITY(csd));\r
-               \r
-               if (SD_CSD_CSDVER(csd) == SD_CSD_CSDVER_2_0) {\r
-                       logf("CSD     : version 2.0\n");\r
-                       logf("Capacity: %d\n", SD_CSD_V2_CAPACITY(csd));\r
-                       logf("Size    : %d\n", SD_CSD_V2_C_SIZE(csd));\r
-               }\r
-               else if (SD_CSD_CSDVER(csd) == SD_CSD_CSDVER_1_0) {\r
-                       logf("CSD     : version 1.0\n");\r
-                       logf("Capacity: %d\n", SD_CSD_CAPACITY(csd));\r
-                       logf("Size    : %d\n", SD_CSD_C_SIZE(csd));\r
-               }\r
-               else {\r
-                       logf("ERROR: unknown CSD version 0x%x!\n", SD_CSD_CSDVER(csd));\r
-                       return false;\r
-               }\r
-\r
-               if (!select_card()) {\r
-                       logf("ERROR: Failed to select card!\n");\r
-                       return false;\r
-               }\r
-\r
-               logf("setting block length to 512 ...\n");\r
-               send(MMC_SET_BLOCKLEN, 512);\r
-               if (!wait()) {\r
-                       logf("ERROR: Failed to set block length!\n");\r
-                       return false;\r
-               }\r
-\r
-               logf("card initialization complete!\n");\r
-\r
-               return true;\r
-       }\r
-\r
-       sdhost_t() {\r
-               is_sdhc = false;\r
-\r
-               logf("starting up ...\n");\r
-\r
-               configure_pinmux();\r
-               reset();\r
-\r
-               SH_HCFG = SH_HCFG_SLOW_CARD_SET | SH_HCFG_WIDE_INT_BUS_SET;\r
-               SH_CDIV = 0x96;\r
-\r
-               udelay(300);\r
-\r
-               logf("sdhost controller ready, initializing SD card ...\n");\r
-\r
-               if (init_card()) {\r
-                       \r
-               }\r
-       }\r
-};\r
-\r
-sdhost_t STATIC_DRIVER g_SDHostDriver {};\r
-\r
-/*\r
- * abstractions for fatfs.\r
- */\r
-\r
-#include "fatfs/diskio.h"\r
-\r
-DSTATUS disk_initialize (BYTE pdrv) {\r
-       return 0;\r
-}\r
-\r
-DSTATUS disk_status (BYTE pdrv) {\r
-       return 0;\r
-}\r
-\r
-DRESULT disk_read (BYTE pdrv, BYTE* buff, DWORD sector, UINT count) {\r
-       return (DRESULT)0;\r
-}\r
-\r
-DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void* buff) {\r
-       return (DRESULT)0;\r
-}
\ No newline at end of file
diff --git a/arm_chainloader/drivers/sdhost_impl.cc b/arm_chainloader/drivers/sdhost_impl.cc
new file mode 100644 (file)
index 0000000..42099d6
--- /dev/null
@@ -0,0 +1,451 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+SDHOST driver. This used to be known as ALTMMC.\r
+\r
+=============================================================================*/\r
+\r
+#include <chainloader.h>\r
+#include <hardware.h>\r
+\r
+#include "sd_proto.hpp"\r
+#include "block_device.hpp"\r
+\r
+#define SDEDM_WRITE_THRESHOLD_SHIFT 9\r
+#define SDEDM_READ_THRESHOLD_SHIFT 14\r
+#define SDEDM_THRESHOLD_MASK     0x1f\r
+\r
+#define SAFE_READ_THRESHOLD     4\r
+#define SAFE_WRITE_THRESHOLD    4\r
+\r
+#define VOLTAGE_SUPPLY_RANGE 0x100\r
+#define CHECK_PATTERN 0x55\r
+\r
+#define SDHSTS_BUSY_IRPT                0x400\r
+#define SDHSTS_BLOCK_IRPT               0x200\r
+#define SDHSTS_SDIO_IRPT                0x100\r
+#define SDHSTS_REW_TIME_OUT             0x80\r
+#define SDHSTS_CMD_TIME_OUT             0x40\r
+#define SDHSTS_CRC16_ERROR              0x20\r
+#define SDHSTS_CRC7_ERROR               0x10\r
+#define SDHSTS_FIFO_ERROR               0x08\r
+\r
+#define SDHSTS_TRANSFER_ERROR_MASK      (SDHSTS_CRC7_ERROR|SDHSTS_CRC16_ERROR|SDHSTS_REW_TIME_OUT|SDHSTS_FIFO_ERROR)\r
+#define SDHSTS_ERROR_MASK               (SDHSTS_CMD_TIME_OUT|SDHSTS_TRANSFER_ERROR_MASK)\r
+\r
+#define logf(fmt, ...) printf("[EMMC:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+\r
+struct SdhostImpl : BlockDevice {\r
+       bool is_sdhc;\r
+       bool is_high_capacity;\r
+       bool card_ready;\r
+\r
+       uint32_t ocr;\r
+       uint32_t rca;\r
+\r
+       uint32_t cid[4];\r
+       uint32_t csd[4];\r
+\r
+       uint32_t capacity_bytes;\r
+\r
+       uint32_t r[4];\r
+\r
+       uint32_t current_cmd;\r
+\r
+       void set_power(bool on) {\r
+               SH_VDD = on ? SH_VDD_POWER_ON_SET : 0x0;\r
+       }\r
+\r
+       bool wait(uint32_t timeout = 100000) {\r
+               uint32_t t = timeout;\r
+\r
+               while(SH_CMD & SH_CMD_NEW_FLAG_SET) {\r
+                       if (t == 0) {\r
+                               logf("timed out after %dus!\n", timeout)\r
+                               return false;\r
+                       }\r
+                       t--;\r
+                       udelay(10);\r
+               }\r
+\r
+               return true;\r
+       }\r
+\r
+       bool send_raw(uint32_t command, uint32_t arg = 0) {\r
+               uint32_t sts;\r
+\r
+               wait();\r
+\r
+               sts = SH_HSTS;\r
+               if (sts & SDHSTS_ERROR_MASK)\r
+                       SH_HSTS = sts; \r
+\r
+               current_cmd = command & SH_CMD_COMMAND_SET;\r
+\r
+               SH_ARG = arg;\r
+               SH_CMD = command | SH_CMD_NEW_FLAG_SET;\r
+\r
+               mfence();\r
+\r
+               return true;\r
+       }\r
+\r
+       bool send(uint32_t command, uint32_t arg = 0) {\r
+               return send_raw(command & SH_CMD_COMMAND_SET, arg);\r
+       }\r
+\r
+       bool send_136_resp(uint32_t command, uint32_t arg = 0) {\r
+               return send_raw((command & SH_CMD_COMMAND_SET) | SH_CMD_LONG_RESPONSE_SET, arg);\r
+       }\r
+\r
+       bool send_no_resp(uint32_t command, uint32_t arg = 0) {\r
+               return send_raw((command & SH_CMD_COMMAND_SET) | SH_CMD_NO_RESPONSE_SET, arg);\r
+       }\r
+\r
+       void configure_pinmux() {\r
+               GP_FSEL4 = 0x24000000;\r
+               GP_FSEL5 = 0x924;\r
+               GP_PUD = 2;\r
+\r
+               logf("GPIOs set!\n");\r
+\r
+               mfence();\r
+       }\r
+\r
+       void reset() {\r
+               logf("resetting controller ...\n");\r
+               set_power(false);\r
+\r
+               SH_CMD = 0;\r
+               SH_ARG = 0;\r
+               SH_TOUT = 0xF00000;\r
+               SH_CDIV = 0;\r
+               SH_HSTS = 0x7f8;\r
+               SH_HCFG = 0;\r
+               SH_HBCT = 0;\r
+               SH_HBLC = 0;\r
+       \r
+               uint32_t temp = SH_EDM;\r
+\r
+               temp &= ~((SDEDM_THRESHOLD_MASK<<SDEDM_READ_THRESHOLD_SHIFT) |\r
+                 (SDEDM_THRESHOLD_MASK<<SDEDM_WRITE_THRESHOLD_SHIFT));\r
+               temp |= (SAFE_READ_THRESHOLD << SDEDM_READ_THRESHOLD_SHIFT) |\r
+                       (SAFE_WRITE_THRESHOLD << SDEDM_WRITE_THRESHOLD_SHIFT);\r
+\r
+               SH_EDM = temp;\r
+               udelay(300);\r
+\r
+               set_power(true);\r
+\r
+               udelay(300);\r
+               mfence();\r
+       }\r
+\r
+       inline void get_response() {\r
+               r[0] = SH_RSP0;\r
+               r[1] = SH_RSP1;\r
+               r[2] = SH_RSP2;\r
+               r[3] = SH_RSP3;\r
+       }\r
+\r
+       bool wait_and_get_response() {\r
+               if (!wait())\r
+                       return false;\r
+\r
+               get_response();\r
+\r
+               //printf("Cmd: 0x%x Resp: %08x %08x %08x %08x\n", current_cmd, r[0], r[1], r[2], r[3]);\r
+\r
+               if (SH_CMD & SH_CMD_FAIL_FLAG_SET) {\r
+                       if (SH_HSTS & SDHSTS_ERROR_MASK) {\r
+                               logf("ERROR: sdhost status: 0x%x\n", SH_HSTS);\r
+                               return false;\r
+                       }\r
+                       logf("ERROR: unknown error, SH_CMD=0x%x\n", SH_CMD);\r
+                       return false;\r
+               }\r
+               \r
+\r
+               return true;\r
+       }\r
+\r
+       bool query_voltage_and_type() {\r
+               uint32_t t;\r
+\r
+               /* identify */\r
+               send(SD_SEND_IF_COND, 0x1AA);\r
+               wait_and_get_response();\r
+\r
+               /* set voltage */\r
+               t = MMC_OCR_3_2V_3_3V;\r
+               if (r[0] == 0x1AA) {\r
+                       t |= MMC_OCR_HCS;\r
+                       is_sdhc = true;\r
+               }\r
+\r
+               /* query voltage and type */\r
+               for (;;) {\r
+                       send(MMC_APP_CMD);\r
+                       send(SD_APP_OP_COND, t);\r
+\r
+                       if (!wait_and_get_response())\r
+                               return false;\r
+\r
+                       if (r[0] & MMC_OCR_MEM_READY)\r
+                               break;\r
+\r
+                       logf("waiting for SD (0x%x) ...\n", r[0]);\r
+                       udelay(100);\r
+               }\r
+\r
+               logf("SD card has arrived!\n", r);\r
+\r
+               is_high_capacity = (r[0] & MMC_OCR_HCS) == MMC_OCR_HCS;\r
+\r
+               if (is_high_capacity)\r
+                       logf("This is an SDHC card!\n");\r
+\r
+               return true;\r
+\r
+       }\r
+\r
+       inline void copy_136_to(uint32_t* dest) {\r
+               dest[0] = r[0];\r
+               dest[1] = r[1];\r
+               dest[2] = r[2];\r
+               dest[3] = r[3];\r
+       }\r
+\r
+       bool identify_card() {\r
+               logf("identifying card ...\n");\r
+               \r
+               send_136_resp(MMC_ALL_SEND_CID);\r
+               if (!wait_and_get_response())\r
+                       return false;\r
+\r
+               /* for SD this gets RCA */\r
+               send(MMC_SET_RELATIVE_ADDR);\r
+               if (!wait_and_get_response())\r
+                       return false;\r
+               rca = SD_R6_RCA(r);\r
+\r
+               logf("RCA = 0x%x\n", rca);\r
+\r
+               send_136_resp(MMC_SEND_CID, MMC_ARG_RCA(rca));\r
+               if (!wait_and_get_response())\r
+                       return false;\r
+\r
+               copy_136_to(cid);\r
+\r
+               /* get card specific data */\r
+               send_136_resp(MMC_SEND_CSD, MMC_ARG_RCA(rca));\r
+               if (!wait_and_get_response())\r
+                       return false;\r
+\r
+               copy_136_to(csd);\r
+\r
+               return true;\r
+       }\r
+\r
+       inline bool wait_for_data_xfer() {\r
+               for (int i = 0; i < 1000; i++) {\r
+                       if (SH_HSTS & SH_HSTS_DATA_FLAG_SET) {\r
+                               SH_HSTS = SH_HSTS_DATA_FLAG_SET;\r
+                               return true;\r
+                       }       \r
+                       udelay(100);\r
+               }\r
+               return false;\r
+       }\r
+\r
+//#define DUMP_READ\r
+\r
+       virtual bool read_block(uint32_t sector, uint32_t* buf) override {\r
+               if (!card_ready)\r
+                       panic("card not ready");\r
+\r
+#ifdef DUMP_READ\r
+               logf("Reading %d bytes from sector %d ...\n", block_size, sector);\r
+#endif\r
+\r
+               if (!is_high_capacity)\r
+                       sector <<= 9;\r
+\r
+               send_raw(MMC_READ_BLOCK_SINGLE | SH_CMD_READ_CMD_SET | SH_CMD_BUSY_CMD_SET, sector);\r
+               if (!wait()) {\r
+                       return false;\r
+               }\r
+\r
+               uint32_t remaining = block_size;\r
+\r
+#ifdef DUMP_READ\r
+               printf("----------------------------------------------------\n");\r
+#endif\r
+\r
+               while (remaining) {\r
+                       if (!wait_for_data_xfer()) {\r
+#ifdef DUMP_READ\r
+                               putchar('\n');\r
+#endif\r
+                               logf("ERROR: Timeout while reading block\n");\r
+                               break;\r
+                       }\r
+                       \r
+                       *buf = SH_DATA;\r
+#ifdef DUMP_READ\r
+                       printf("%08x ", *buf);\r
+#endif\r
+                       buf++;\r
+                       remaining -= 4;\r
+               }\r
+\r
+#ifdef DUMP_READ\r
+               printf("\n----------------------------------------------------\n");\r
+#endif\r
+\r
+               send(MMC_STOP_TRANSMISSION);\r
+\r
+               uint32_t sdhsts_errs = SH_HSTS & SDHSTS_ERROR_MASK;\r
+               if (sdhsts_errs) {\r
+                       logf("ERROR: Transfer error, status: 0x%x\n", SH_HSTS);\r
+               }\r
+\r
+               if (remaining) {\r
+                       logf("ERROR: Read failed, %d bytes left to copy\n", remaining);\r
+                       return false;\r
+               }\r
+               else {\r
+#ifdef DUMP_READ\r
+                       logf("Completed read for %d\n", sector);\r
+#endif\r
+                       return true;\r
+               }\r
+       }\r
+\r
+       bool select_card() {\r
+               send(MMC_SELECT_CARD, MMC_ARG_RCA(rca));\r
+\r
+               if (!wait())\r
+                       return false;\r
+\r
+               return true;\r
+       }\r
+\r
+       bool init_card() {\r
+               char pnm[8];\r
+               uint32_t block_length;\r
+\r
+               send_no_resp(MMC_GO_IDLE_STATE | SH_CMD_NO_RESPONSE_SET);\r
+\r
+               if (!query_voltage_and_type()) {\r
+                       logf("ERROR: Failed to query card voltage!\n");\r
+                       return false;\r
+               }\r
+\r
+               if (!identify_card()) {\r
+                       logf("ERROR: Failed to identify card!\n");\r
+                       return false;\r
+               }\r
+\r
+               SD_CID_PNM_CPY(cid, pnm);\r
+\r
+               logf("Detected SD card:\n");\r
+               printf("    Product : %s\n", &pnm);\r
+\r
+               if (SD_CSD_CSDVER(csd) == SD_CSD_CSDVER_2_0) {\r
+                       printf("    CSD     : Ver 2.0\n");\r
+                       printf("    Capacity: %d\n", SD_CSD_V2_CAPACITY(csd));\r
+                       printf("    Size    : %d\n", SD_CSD_V2_C_SIZE(csd));\r
+\r
+                       block_length = SD_CSD_V2_BL_LEN;\r
+               }\r
+               else if (SD_CSD_CSDVER(csd) == SD_CSD_CSDVER_1_0) {\r
+                       printf("    CSD     : Ver 1.0\n");\r
+                       printf("    Capacity: %d\n", SD_CSD_CAPACITY(csd));\r
+                       printf("    Size    : %d\n", SD_CSD_C_SIZE(csd));\r
+\r
+                       block_length = 1 << SD_CSD_READ_BL_LEN(csd);\r
+               }\r
+               else {\r
+                       printf("ERROR: Unknown CSD version 0x%x!\n", SD_CSD_CSDVER(csd));\r
+\r
+\r
+                       return false;\r
+               }\r
+       \r
+               printf("    BlockLen: 0x%x\n", block_length);\r
+\r
+               /* work out the capacity of the card in bytes */\r
+               capacity_bytes = (SD_CSD_CAPACITY(csd) * block_length);\r
+\r
+               if (!select_card()) {\r
+                       logf("ERROR: Failed to select card!\n");\r
+                       return false;\r
+               }\r
+\r
+               if (SD_CSD_CSDVER(csd) == SD_CSD_CSDVER_1_0) {\r
+                       /*\r
+                        * only needed for 1.0 ones, the 2.0 ones have this \r
+                        * fixed at 512.\r
+                        */\r
+                       logf("Setting block length to 512 ...\n");\r
+                       send(MMC_SET_BLOCKLEN, 512);\r
+                       if (!wait()) {\r
+                               logf("ERROR: Failed to set block length!\n");\r
+                               return false;\r
+                       }\r
+               }\r
+\r
+               block_size = 512;\r
+\r
+               logf("Card iniitlization complete: %s %dMB SD%s Card\n", &pnm, capacity_bytes >> 20, is_high_capacity ? "HC" : "");\r
+\r
+               return true;\r
+       }\r
+\r
+       void restart_controller() {\r
+               is_sdhc = false;\r
+\r
+               logf("Restarting the eMMC controller ...\n");\r
+\r
+               configure_pinmux();\r
+               reset();\r
+\r
+               SH_HCFG &= ~SH_HCFG_WIDE_EXT_BUS_SET;\r
+               SH_HCFG = SH_HCFG_SLOW_CARD_SET | SH_HCFG_WIDE_INT_BUS_SET;\r
+               SH_CDIV = 0x96;\r
+\r
+               udelay(300);\r
+               mfence();\r
+\r
+               if (init_card()) {\r
+                       card_ready = true;\r
+               }\r
+               else {\r
+                       panic("failed to reinitialize the eMMC controller");\r
+               }\r
+       }\r
+\r
+       SdhostImpl() {\r
+               restart_controller();\r
+               logf("eMMC driver sucessfully started!\n");\r
+       }\r
+};\r
+\r
+SdhostImpl STATIC_DRIVER g_SDHostDriver {};\r
+\r
+BlockDevice* get_sdhost_device() {\r
+       return &g_SDHostDriver;\r
+}
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/arm_chainloader/lib/arm_bcopy.s b/arm_chainloader/lib/arm_bcopy.s
new file mode 100644 (file)
index 0000000..c818353
--- /dev/null
@@ -0,0 +1,397 @@
+/*\r
+ * Copyright (c) 2006, 2009 Apple Inc. All rights reserved.\r
+ *\r
+ * @APPLE_LICENSE_HEADER_START@\r
+ * \r
+ * This file contains Original Code and/or Modifications of Original Code\r
+ * as defined in and that are subject to the Apple Public Source License\r
+ * Version 2.0 (the 'License'). You may not use this file except in\r
+ * compliance with the License. Please obtain a copy of the License at\r
+ * http://www.opensource.apple.com/apsl/ and read it before using this\r
+ * file.\r
+ * \r
+ * The Original Code and all software distributed under the License are\r
+ * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER\r
+ * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,\r
+ * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,\r
+ * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.\r
+ * Please see the License for the specific language governing rights and\r
+ * limitations under the License.\r
+ * \r
+ * @APPLE_LICENSE_HEADER_END@\r
+ */\r
+\r
+.text\r
+.align 2\r
+       \r
+       .globl memcpy\r
+       .globl bcopy\r
+       .globl _memmove\r
+\r
+bcopy:         /* void bcopy(const void *src, void *dest, size_t len); */\r
+       mov             r3, r0\r
+       mov             r0, r1\r
+       mov             r1, r3\r
+\r
+memcpy:                /* void *memcpy(void *dest, const void *src, size_t len); */\r
+memmove:       /* void *memmove(void *dest, const void *src, size_t len); */\r
+       /* check for zero len or if the pointers are the same */\r
+       cmp             r2, #0\r
+       cmpne   r0, r1\r
+       bxeq    lr\r
+\r
+       /* save r0 (return value), r4 (scratch), and r5 (scratch) */\r
+       stmfd   sp!, { r0, r4, r5, r7, lr }\r
+       add     r7, sp, #12\r
+       \r
+       /* check for overlap. r3 <- distance between src & dest */\r
+       subhs   r3, r0, r1\r
+       sublo   r3, r1, r0\r
+       cmp             r3, r2                  /* if distance(src, dest) < len, we have overlap */\r
+       blo             Loverlap\r
+\r
+Lnormalforwardcopy:\r
+       /* are src and dest dissimilarly word aligned? */\r
+       mov             r12, r0, lsl #30\r
+       cmp             r12, r1, lsl #30\r
+       bne             Lnonwordaligned_forward\r
+\r
+       /* if len < 64, do a quick forward copy */\r
+       cmp             r2, #64\r
+       blt             Lsmallforwardcopy\r
+\r
+       /* check for 16 byte src/dest unalignment */\r
+       tst             r0, #0xf\r
+       bne             Lsimilarlyunaligned\r
+\r
+       /* check for 32 byte dest unalignment */\r
+       tst             r0, #(1<<4)\r
+       bne             Lunaligned_32\r
+\r
+Lmorethan64_aligned:\r
+       /* save some more registers to use in the copy */\r
+       stmfd   sp!, { r6, r8, r10, r11 }\r
+\r
+       /* pre-subtract 64 from the len counter to avoid an extra compare in the loop */\r
+       sub             r2, r2, #64\r
+\r
+L64loop:\r
+       /* copy 64 bytes at a time */\r
+       ldmia   r1!, { r3, r4, r5, r6, r8, r10, r11, r12 }\r
+#ifdef _ARM_ARCH_6\r
+       pld             [r1, #32]\r
+#endif\r
+       stmia   r0!, { r3, r4, r5, r6, r8, r10, r11, r12 }\r
+       ldmia   r1!, { r3, r4, r5, r6, r8, r10, r11, r12 }\r
+       subs    r2, r2, #64\r
+#ifdef _ARM_ARCH_6\r
+       pld             [r1, #32]\r
+#endif\r
+       stmia   r0!, { r3, r4, r5, r6, r8, r10, r11, r12 }\r
+       bge             L64loop\r
+\r
+       /* restore the scratch registers we just saved */\r
+       ldmfd   sp!, { r6, r8, r10, r11 }\r
+\r
+       /* fix up the len counter (previously subtracted an extra 64 from it) and test for completion */\r
+       adds    r2, r2, #64\r
+       beq             Lexit\r
+\r
+Llessthan64_aligned:\r
+       /* copy 16 bytes at a time until we have < 16 bytes */\r
+       cmp             r2, #16\r
+       ldmgeia r1!, { r3, r4, r5, r12 }\r
+       stmgeia r0!, { r3, r4, r5, r12 }\r
+       subges  r2, r2, #16\r
+       bgt             Llessthan64_aligned\r
+       beq             Lexit\r
+       \r
+Llessthan16_aligned:\r
+       mov             r2, r2, lsl #28\r
+       msr             cpsr_f, r2\r
+\r
+       ldmmiia r1!, { r2, r3 }\r
+       ldreq   r4, [r1], #4\r
+       ldrcsh  r5, [r1], #2\r
+       ldrvsb  r12, [r1], #1\r
+\r
+       stmmiia r0!, { r2, r3 }\r
+       streq   r4, [r0], #4\r
+       strcsh  r5, [r0], #2\r
+       strvsb  r12, [r0], #1\r
+       b               Lexit\r
+\r
+Lsimilarlyunaligned:\r
+       /* both src and dest are unaligned in similar ways, align to dest on 32 byte boundary */\r
+       mov             r12, r0, lsl #28\r
+       rsb             r12, r12, #0\r
+       msr             cpsr_f, r12\r
+\r
+       ldrvsb  r3, [r1], #1\r
+       ldrcsh  r4, [r1], #2\r
+       ldreq   r5, [r1], #4\r
+\r
+       strvsb  r3, [r0], #1\r
+       strcsh  r4, [r0], #2\r
+       streq   r5, [r0], #4\r
+\r
+       ldmmiia r1!, { r3, r4 }\r
+       stmmiia r0!, { r3, r4 }\r
+\r
+       subs    r2, r2, r12, lsr #28\r
+       beq             Lexit\r
+\r
+Lunaligned_32:\r
+       /* bring up to dest 32 byte alignment */\r
+       tst             r0, #(1 << 4)\r
+       ldmneia r1!, { r3, r4, r5, r12 }\r
+       stmneia r0!, { r3, r4, r5, r12 }\r
+       subne   r2, r2, #16\r
+\r
+       /* we should now be aligned, see what copy method we should use */\r
+       cmp             r2, #64\r
+       bge             Lmorethan64_aligned\r
+       b               Llessthan64_aligned\r
+       \r
+Lbytewise2:\r
+       /* copy 2 bytes at a time */\r
+       subs    r2, r2, #2\r
+\r
+       ldrb    r3, [r1], #1\r
+       ldrplb  r4, [r1], #1\r
+\r
+       strb    r3, [r0], #1\r
+       strplb  r4, [r0], #1\r
+\r
+       bhi             Lbytewise2\r
+       b               Lexit\r
+\r
+Lbytewise:\r
+       /* simple bytewise forward copy */\r
+       ldrb    r3, [r1], #1\r
+       subs    r2, r2, #1\r
+       strb    r3, [r0], #1\r
+       bne             Lbytewise\r
+       b               Lexit\r
+\r
+Lsmallforwardcopy:\r
+       /* src and dest are word aligned similarly, less than 64 bytes to copy */\r
+       cmp             r2, #4\r
+       blt             Lbytewise2\r
+\r
+       /* bytewise copy until word aligned */\r
+       tst             r1, #3\r
+Lwordalignloop:\r
+       ldrneb  r3, [r1], #1\r
+       strneb  r3, [r0], #1\r
+       subne   r2, r2, #1\r
+       tstne   r1, #3\r
+       bne             Lwordalignloop\r
+\r
+       cmp             r2, #16\r
+       bge             Llessthan64_aligned\r
+       blt             Llessthan16_aligned\r
+\r
+Loverlap:\r
+       /* src and dest overlap in some way, len > 0 */\r
+       cmp             r0, r1                          /* if dest > src */\r
+       bhi             Loverlap_srclower\r
+\r
+Loverlap_destlower:\r
+       /* dest < src, see if we can still do a fast forward copy or fallback to slow forward copy */\r
+       cmp             r3, #64\r
+       bge             Lnormalforwardcopy      /* overlap is greater than one stride of the copy, use normal copy */\r
+\r
+       cmp             r3, #2\r
+       bge             Lbytewise2\r
+       b               Lbytewise\r
+\r
+       /* the following routines deal with having to copy in the reverse direction */\r
+Loverlap_srclower:\r
+       /* src < dest, with overlap */\r
+\r
+       /* src += len; dest += len; */\r
+       add             r0, r0, r2\r
+       add             r1, r1, r2\r
+\r
+       /* we have to copy in reverse no matter what, test if we can we use a large block reverse copy */\r
+       cmp             r2, #64                         /* less than 64 bytes to copy? */\r
+       cmpgt   r3, #64                         /* less than 64 bytes of nonoverlap? */\r
+       blt             Lbytewise_reverse\r
+\r
+       /* test of src and dest are nonword aligned differently */\r
+       mov             r3, r0, lsl #30\r
+       cmp             r3, r1, lsl #30\r
+       bne             Lbytewise_reverse\r
+\r
+       /* test if src and dest are non word aligned or dest is non 16 byte aligned */\r
+       tst             r0, #0xf\r
+       bne             Lunaligned_reverse_similarly\r
+\r
+       /* test for dest 32 byte alignment */\r
+       tst             r0, #(1<<4)\r
+       bne             Lunaligned_32_reverse_similarly\r
+\r
+       /* 64 byte reverse block copy, src and dest aligned */\r
+Lmorethan64_aligned_reverse:\r
+       /* save some more registers to use in the copy */\r
+       stmfd   sp!, { r6, r8, r10, r11 }\r
+\r
+       /* pre-subtract 64 from the len counter to avoid an extra compare in the loop */\r
+       sub             r2, r2, #64\r
+\r
+L64loop_reverse:\r
+       /* copy 64 bytes at a time */\r
+       ldmdb   r1!, { r3, r4, r5, r6, r8, r10, r11, r12 }\r
+#ifdef _ARM_ARCH_6\r
+       pld             [r1, #-32]\r
+#endif\r
+       stmdb   r0!, { r3, r4, r5, r6, r8, r10, r11, r12 }      \r
+       ldmdb   r1!, { r3, r4, r5, r6, r8, r10, r11, r12 }      \r
+       subs    r2, r2, #64\r
+#ifdef _ARM_ARCH_6\r
+       pld             [r1, #-32]\r
+#endif\r
+       stmdb   r0!, { r3, r4, r5, r6, r8, r10, r11, r12 }      \r
+       bge             L64loop_reverse\r
+\r
+       /* restore the scratch registers we just saved */\r
+       ldmfd   sp!, { r6, r8, r10, r11 }\r
+\r
+       /* fix up the len counter (previously subtracted an extra 64 from it) and test for completion */\r
+       adds    r2, r2, #64\r
+       beq             Lexit\r
+\r
+Lbytewise_reverse:\r
+       ldrb    r3, [r1, #-1]!\r
+       strb    r3, [r0, #-1]!\r
+       subs    r2, r2, #1\r
+       bne             Lbytewise_reverse\r
+       b               Lexit\r
+\r
+Lunaligned_reverse_similarly:\r
+       /* both src and dest are unaligned in similar ways, align to dest on 32 byte boundary */\r
+       mov             r12, r0, lsl #28\r
+       msr             cpsr_f, r12\r
+\r
+       ldrvsb  r3, [r1, #-1]!\r
+       ldrcsh  r4, [r1, #-2]!\r
+       ldreq   r5, [r1, #-4]!\r
+\r
+       strvsb  r3, [r0, #-1]!\r
+       strcsh  r4, [r0, #-2]!\r
+       streq   r5, [r0, #-4]!\r
+\r
+       ldmmidb r1!, { r3, r4 }\r
+       stmmidb r0!, { r3, r4 }\r
+\r
+       subs    r2, r2, r12, lsr #28\r
+       beq             Lexit\r
+\r
+Lunaligned_32_reverse_similarly:\r
+       /* bring up to dest 32 byte alignment */\r
+       tst             r0, #(1 << 4)\r
+       ldmnedb r1!, { r3, r4, r5, r12 }\r
+       stmnedb r0!, { r3, r4, r5, r12 }\r
+       subne   r2, r2, #16\r
+\r
+       /* we should now be aligned, see what copy method we should use */\r
+       cmp             r2, #64\r
+       bge             Lmorethan64_aligned_reverse\r
+       b               Lbytewise_reverse\r
+\r
+       /* the following routines deal with non word aligned copies */\r
+Lnonwordaligned_forward:\r
+       cmp             r2, #8\r
+       blt             Lbytewise2                      /* not worth the effort with less than 24 bytes total */\r
+\r
+       /* bytewise copy until src word aligned */\r
+       tst             r1, #3\r
+Lwordalignloop2:\r
+       ldrneb  r3, [r1], #1\r
+       strneb  r3, [r0], #1\r
+       subne   r2, r2, #1\r
+       tstne   r1, #3\r
+       bne             Lwordalignloop2\r
+\r
+       /* figure out how the src and dest are unaligned */\r
+       and             r3, r0, #3\r
+       cmp             r3, #2\r
+       blt             Lalign1_forward\r
+       beq             Lalign2_forward\r
+       bgt             Lalign3_forward\r
+\r
+Lalign1_forward:\r
+       /* the dest pointer is 1 byte off from src */\r
+       mov             r12, r2, lsr #2         /* number of words we should copy */\r
+       sub             r0, r0, #1\r
+\r
+       /* prime the copy */\r
+       ldrb    r4, [r0]                        /* load D[7:0] */\r
+\r
+Lalign1_forward_loop:\r
+       ldr             r3, [r1], #4            /* load S */\r
+       orr             r4, r4, r3, lsl #8      /* D[31:8] = S[24:0] */\r
+       str             r4, [r0], #4            /* save D */\r
+       mov             r4, r3, lsr #24         /* D[7:0] = S[31:25] */\r
+       subs    r12, r12, #1\r
+       bne             Lalign1_forward_loop\r
+\r
+       /* finish the copy off */\r
+       strb    r4, [r0], #1            /* save D[7:0] */\r
+\r
+       ands    r2, r2, #3\r
+       beq             Lexit\r
+       b               Lbytewise2\r
+\r
+Lalign2_forward:\r
+       /* the dest pointer is 2 bytes off from src */\r
+       mov             r12, r2, lsr #2         /* number of words we should copy */\r
+       sub             r0, r0, #2\r
+\r
+       /* prime the copy */\r
+       ldrh    r4, [r0]                        /* load D[15:0] */\r
+\r
+Lalign2_forward_loop:\r
+       ldr             r3, [r1], #4            /* load S */\r
+       orr             r4, r4, r3, lsl #16     /* D[31:16] = S[15:0] */\r
+       str             r4, [r0], #4            /* save D */\r
+       mov             r4, r3, lsr #16         /* D[15:0] = S[31:15] */\r
+       subs    r12, r12, #1\r
+       bne             Lalign2_forward_loop\r
+\r
+       /* finish the copy off */\r
+       strh    r4, [r0], #2            /* save D[15:0] */\r
+\r
+       ands    r2, r2, #3\r
+       beq             Lexit\r
+       b               Lbytewise2\r
+\r
+Lalign3_forward:\r
+       /* the dest pointer is 3 bytes off from src */\r
+       mov             r12, r2, lsr #2         /* number of words we should copy */\r
+       sub             r0, r0, #3\r
+\r
+       /* prime the copy */\r
+       ldr             r4, [r0]\r
+       and             r4, r4, #0x00ffffff     /* load D[24:0] */\r
+\r
+Lalign3_forward_loop:\r
+       ldr             r3, [r1], #4            /* load S */\r
+       orr             r4, r4, r3, lsl #24     /* D[31:25] = S[7:0] */\r
+       str             r4, [r0], #4            /* save D */\r
+       mov             r4, r3, lsr #8          /* D[24:0] = S[31:8] */\r
+       subs    r12, r12, #1\r
+       bne             Lalign3_forward_loop\r
+\r
+       /* finish the copy off */\r
+       strh    r4, [r0], #2            /* save D[15:0] */\r
+       mov             r4, r4, lsr #16\r
+       strb    r4, [r0], #1            /* save D[23:16] */\r
+\r
+       ands    r2, r2, #3\r
+       beq             Lexit\r
+       b               Lbytewise2\r
+\r
+Lexit:\r
+       ldmfd   sp!, {r0, r4, r5, r7, pc}\r
diff --git a/arm_chainloader/lib/arm_bzero.s b/arm_chainloader/lib/arm_bzero.s
new file mode 100644 (file)
index 0000000..c3123a9
--- /dev/null
@@ -0,0 +1,157 @@
+/*\r
+ * Copyright (c) 2006, 2009 Apple Inc. All rights reserved.\r
+ *\r
+ * @APPLE_LICENSE_HEADER_START@\r
+ * \r
+ * This file contains Original Code and/or Modifications of Original Code\r
+ * as defined in and that are subject to the Apple Public Source License\r
+ * Version 2.0 (the 'License'). You may not use this file except in\r
+ * compliance with the License. Please obtain a copy of the License at\r
+ * http://www.opensource.apple.com/apsl/ and read it before using this\r
+ * file.\r
+ * \r
+ * The Original Code and all software distributed under the License are\r
+ * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER\r
+ * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,\r
+ * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,\r
+ * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.\r
+ * Please see the License for the specific language governing rights and\r
+ * limitations under the License.\r
+ * \r
+ * @APPLE_LICENSE_HEADER_END@\r
+ */\r
+       \r
+/* \r
+ * A reasonably well-optimized bzero/memset. Should work equally well on arm11 and arm9 based\r
+ * cores. \r
+ *\r
+ * The algorithm is to align the destination pointer on a 32 byte boundary and then\r
+ * blast data 64 bytes at a time, in two stores of 32 bytes per loop.\r
+ */\r
+       .text\r
+       .align 2\r
+\r
+       .globl memset\r
+/* void *memset(void *ptr, int c, size_t len); */\r
+memset:\r
+       /* move len into r1, unpack c into r2 */\r
+       mov             r3, r2\r
+       and             r1, r1, #0xff\r
+       orr             r1, r1, r1, lsl #8\r
+       orr             r2, r1, r1, lsl #16\r
+       mov             r1, r3\r
+       b               Lbzeroengine\r
+\r
+       .globl bzero\r
+/* void bzero(void *ptr, size_t len); */\r
+bzero:\r
+       /* zero out r2 so we can be just like memset(0) */\r
+       mov             r2, #0\r
+\r
+Lbzeroengine:\r
+       /* move the base pointer into r12 and leave r0 alone so that we return the original pointer */\r
+       mov             r12, r0\r
+\r
+       /* copy r2 into r3 for 64-bit stores */\r
+       mov             r3, r2\r
+\r
+       /* check for zero len */\r
+       cmp             r1, #0\r
+       bxeq    lr\r
+\r
+       /* fall back to a bytewise store for less than 32 bytes */\r
+       cmp             r1, #32\r
+       blt             L_bytewise\r
+\r
+       /* check for 32 byte unaligned ptr */\r
+       tst             r12, #0x1f\r
+       bne             L_unaligned\r
+\r
+       /* make sure we have more than 64 bytes to zero */\r
+       cmp             r1, #64\r
+       blt             L_lessthan64aligned\r
+\r
+       /* >= 64 bytes of len, 32 byte aligned */\r
+L_64ormorealigned:\r
+\r
+       /* we need some registers, avoid r7 (frame pointer) and r9 (thread register) */\r
+       stmfd   sp!, { r4-r6, r8, r10-r11 }\r
+       mov             r4, r2\r
+       mov             r5, r2\r
+       mov             r6, r2\r
+       mov             r8, r2\r
+       mov             r10, r2\r
+       mov             r11, r2\r
+\r
+       /* pre-subtract 64 from the len to avoid an extra compare in the loop */\r
+       sub             r1, r1, #64\r
+\r
+L_64loop:\r
+       stmia   r12!, { r2-r6, r8, r10-r11 }\r
+       subs    r1, r1, #64\r
+       stmia   r12!, { r2-r6, r8, r10-r11 }\r
+       bge             L_64loop\r
+\r
+       /* restore the saved regs */\r
+       ldmfd   sp!, { r4-r6, r8, r10-r11 }\r
+\r
+       /* check for completion (had previously subtracted an extra 64 from len) */\r
+       adds    r1, r1, #64\r
+       bxeq    lr\r
+\r
+L_lessthan64aligned:\r
+       /* do we have 16 or more bytes left */\r
+       cmp             r1, #16\r
+       stmgeia r12!, { r2-r3 }\r
+       stmgeia r12!, { r2-r3 }\r
+       subges  r1, r1, #16\r
+       bgt             L_lessthan64aligned\r
+       bxeq    lr\r
+\r
+L_lessthan16aligned:\r
+       /* store 0 to 15 bytes */\r
+       mov             r1, r1, lsl #28         /* move the remaining len bits [3:0] to the flags area of cpsr */\r
+       msr             cpsr_f, r1\r
+\r
+       stmmiia r12!, { r2-r3 }         /* n is set, store 8 bytes */\r
+       streq   r2, [r12], #4           /* z is set, store 4 bytes */\r
+       strcsh  r2, [r12], #2           /* c is set, store 2 bytes */\r
+       strvsb  r2, [r12], #1           /* v is set, store 1 byte */\r
+       bx              lr\r
+\r
+L_bytewise:\r
+       /* bytewise copy, 2 bytes at a time, alignment not guaranteed */        \r
+       subs    r1, r1, #2\r
+       strb    r2, [r12], #1\r
+       strplb  r2, [r12], #1\r
+       bhi             L_bytewise\r
+       bx              lr\r
+\r
+L_unaligned:\r
+       /* unaligned on 32 byte boundary, store 1-15 bytes until we're 16 byte aligned */\r
+       mov             r3, r12, lsl #28\r
+       rsb     r3, r3, #0x00000000\r
+       msr             cpsr_f, r3\r
+\r
+       strvsb  r2, [r12], #1           /* v is set, unaligned in the 1s column */\r
+       strcsh  r2, [r12], #2           /* c is set, unaligned in the 2s column */\r
+       streq   r2, [r12], #4           /* z is set, unaligned in the 4s column */\r
+       strmi   r2, [r12], #4           /* n is set, unaligned in the 8s column */\r
+       strmi   r2, [r12], #4\r
+\r
+       subs    r1, r1, r3, lsr #28\r
+       bxeq    lr\r
+\r
+       /* we had previously trashed r3, restore it */\r
+       mov             r3, r2\r
+\r
+       /* now make sure we're 32 byte aligned */\r
+       tst             r12, #(1 << 4)\r
+       stmneia r12!, { r2-r3 }\r
+       stmneia r12!, { r2-r3 }\r
+       subnes  r1, r1, #16\r
+\r
+       /* we're now aligned, check for >= 64 bytes left */\r
+       cmp             r1, #64\r
+       bge             L_64ormorealigned\r
+       b               L_lessthan64aligned
\ No newline at end of file
diff --git a/arm_chainloader/lib/arm_locore.s b/arm_chainloader/lib/arm_locore.s
new file mode 100644 (file)
index 0000000..b9659b6
--- /dev/null
@@ -0,0 +1,2 @@
+.text\r
+.align 2\r
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/arm_chainloader/loader.cc b/arm_chainloader/loader.cc
new file mode 100644 (file)
index 0000000..feee35c
--- /dev/null
@@ -0,0 +1,55 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+Second stage bootloader.\r
+\r
+=============================================================================*/\r
+\r
+#include <drivers/fatfs/ff.h>\r
+#include <chainloader.h>\r
+\r
+#define logf(fmt, ...) printf("[LDR:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+\r
+FATFS g_BootVolumeFs;\r
+\r
+#define ROOT_VOLUME_PREFIX "0:"\r
+\r
+static const char* g_BootFiles32[] = {\r
+       "zImage",\r
+       "kernel.img",\r
+};\r
+\r
+struct LoaderImpl {\r
+       inline bool file_exists(const char* path) {\r
+               return f_stat(path) == FR_OK;\r
+       }\r
+\r
+       bool read_file(const char* path, uintptr_t dest) {\r
+\r
+       }\r
+\r
+       LoaderImpl() {\r
+               logf("Mounting boot partitiion ...\n");\r
+               FRESULT r = f_mount(&g_BootVolumeFs, ROOT_VOLUME_PREFIX, 1);\r
+               if (r != FR_OK) {\r
+                       panic("failed to mount boot partition, error: %d", (int)r);\r
+               }\r
+               logf("Boot partition mounted!\n");\r
+\r
+\r
+       }\r
+};\r
+\r
+static LoaderImpl STATIC_APP g_Loader {};
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
index 9732fb5..4106fdd
@@ -4,10 +4,11 @@
 \r
 extern uintptr_t* __init_array_start;\r
 extern uintptr_t* __init_array_end;\r
+extern uintptr_t* _end;\r
 \r
-#define logf(fmt, ...) printf("[startup::%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+#define logf(fmt, ...) printf("[BRINGUP:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
 \r
-void cxx_init() {\r
+static void cxx_init() {\r
        unsigned ctor_count = (unsigned)(&__init_array_end - &__init_array_start);\r
        void (*static_ctor)();\r
 \r
@@ -20,15 +21,30 @@ void cxx_init() {
        }\r
 }\r
 \r
+static void heap_init() {\r
+       void* start_of_heap = (void*)&_end;\r
+       size_t hs = 0x100000;\r
+\r
+       logf("Initializing heap at 0x%x with size 0x%x\n", start_of_heap, hs);\r
+\r
+       init_memory_pool(hs, start_of_heap);\r
+}\r
+\r
 void main() {\r
-       logf("started on ARM, continuing boot from here ...\n", __FUNCTION__);\r
+       logf("Started on ARM, continuing boot from here ...\n", __FUNCTION__);\r
+\r
+       heap_init();\r
 \r
        /* c++ runtime */\r
        cxx_init();\r
 \r
+       panic("Nothing else to do!");\r
+\r
+#if 0\r
        printf("Done ");\r
        for(;;) {\r
                printf(".");\r
                udelay(1000000);\r
        }\r
+#endif\r
 }
\ No newline at end of file
diff --git a/arm_chainloader/minicxx.cc b/arm_chainloader/minicxx.cc
new file mode 100644 (file)
index 0000000..9688513
--- /dev/null
@@ -0,0 +1,34 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+Crappy C++ runtime.\r
+\r
+=============================================================================*/\r
+\r
+#include <stdint.h>\r
+#include <chainloader.h>\r
+#include <hardware.h>\r
+\r
+void* operator new[] (size_t sz) {\r
+       return malloc(sz);\r
+}\r
+\r
+void* operator new (size_t sz) {\r
+       return malloc(sz);\r
+}\r
+\r
+extern "C" void __cxa_pure_virtual() {\r
+       panic("__cxa_pure_virtual!");\r
+}
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
index 4afedca..9d1d977
@@ -21,7 +21,7 @@ ARM initialization stuff.
 #include "hardware.h"\r
 \r
 \r
-#define logf(fmt, ...) printf("[ARM::%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+#define logf(fmt, ...) printf("[ARMLDR:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
 \r
 extern char L_arm_code_start;\r
 extern char L_arm_code_end;\r
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
index ff66790..610d993
@@ -23,10 +23,12 @@ that are missing from the release. This is also used by ARM.
 #define VPU_KILL_COMMAND 0xAAAAFFFF\r
 \r
 #define VC4_PERIPH_BASE 0x7E000000\r
-#define ARM_PERIPH_BASE 0x20000000\r
+#define ARM_PERIPH_BASE 0x3F000000\r
 \r
 #define VC4_TO_ARM_PERIPH(addr) ((addr - VC4_PERIPH_BASE) + ARM_PERIPH_BASE)\r
 \r
+#define VC4_CPUID_BCM2709_PLUS 0x40\r
+\r
 #ifdef __arm__\r
        #define HW_REGISTER_RW(addr) (*(volatile unsigned int *)(VC4_TO_ARM_PERIPH(addr)))  \r
        #define HW_REGISTER_RO(addr) (*(const volatile unsigned int *)(VC4_TO_ARM_PERIPH(addr)))\r
@@ -44,6 +46,13 @@ that are missing from the release. This is also used by ARM.
  * this is not included by hardware_vc4.h\r
  */\r
 #include "bcm2708_chip/aux_io.h"\r
+#include "bcm2708_chip/testbus.h"\r
+\r
+#define RAM_SIZE_1GB 0\r
+#define RAM_SIZE_512MB 1\r
+#define RAM_SIZE_256MB 2\r
+#define RAM_SIZE_128MB 3\r
+#define RAM_SIZE_UNKNOWN 4\r
 \r
 /*\r
  * LPDDR mode registers.\r
@@ -52,6 +61,7 @@ that are missing from the release. This is also used by ARM.
 #define LPDDR2_MR_DEVICE_FEATURE_1 1\r
 #define LPDDR2_MR_DEVICE_FEATURE_2 2\r
 #define LPDDR2_MR_IO_CONFIG        3\r
+#define LPDDR2_MR_TEMPERATURE      4\r
 #define LPDDR2_MR_MANUFACTURER_ID  5\r
 #define LPDDR2_MR_REV_1            6\r
 #define LPDDR2_MR_REV_2            7\r
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/lib/common.h b/lib/common.h
deleted file mode 100755 (executable)
index 4ee290a..0000000
+++ /dev/null
@@ -1,55 +0,0 @@
-#pragma once\r
-\r
-#include "xprintf.h"\r
-#include "stdarg.h"\r
-\r
-typedef unsigned long long u64;\r
-typedef unsigned long long uint64_t;\r
-typedef long long int64_t;\r
-\r
-typedef unsigned int u32;\r
-typedef unsigned int uint32_t;\r
-typedef int int32_t;\r
-\r
-typedef unsigned short u16;\r
-typedef unsigned short uint16_t;\r
-typedef short int16_t;\r
-\r
-typedef unsigned char u8;\r
-typedef unsigned char uint8_t;\r
-\r
-typedef int bool;\r
-\r
-#define true 1\r
-#define false 0\r
-\r
-# define ALWAYS_INLINE __attribute__((always_inline)) inline\r
-\r
-extern void panic(const char* fmt,  ...)\r
-       __attribute__((noreturn))\r
-       __attribute__ ((format (printf, 1, 2)));\r
-\r
-#define panic_plain(ex, ...) \\r
-       (panic)(ex, ## __VA_ARGS__)\r
-#define __STRINGIFY(x) #x\r
-#define LINE_NUMBER(x) __STRINGIFY(x)\r
-#define PANIC_LOCATION __FILE__ ":" LINE_NUMBER(__LINE__)\r
-#define panic(ex, ...) \\r
-       (panic)(# ex "@" PANIC_LOCATION, ## __VA_ARGS__)\r
-\r
-#define printf xprintf\r
-\r
-#define _OPEN_SOURCE\r
-\r
-extern void udelay(uint32_t time);\r
-extern uint32_t __div64_32(uint64_t *n, uint32_t base);\r
-\r
-#define do_div __div64_32\r
-\r
-/*\r
- * this is done like that because clang likes using __builtin_memcpy\r
- * which makes LLC choke in a fabulous way.\r
- */\r
-extern void *__memcpy(void *_dst, const void *_src, unsigned len);\r
-#define bcopy(s,d,l) __memcpy(d,s,l)\r
-#define memcpy(d,s,l) __memcpy(d,s,l)
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
index 576be62..eda1575
@@ -8,4 +8,17 @@ void *__memcpy(void *_dst, const void *_src, unsigned len)
                *dst++ = *src++;\r
        }\r
        return _dst;\r
+}\r
+\r
+void *\r
+memset(void *s, int c, unsigned int n)\r
+{\r
+       char *s1 = s;\r
+       if (n>0) {\r
+                       n++;\r
+                       while (--n > 0) {\r
+                                       *s1++ = c;\r
+                       }\r
+       }\r
+       return s;\r
 }
\ No newline at end of file
diff --git a/lib/panic.c b/lib/panic.c
new file mode 100644 (file)
index 0000000..b211609
--- /dev/null
@@ -0,0 +1,41 @@
+/*=============================================================================\r
+Copyright (C) 2016 Kristina Brooks\r
+All rights reserved.\r
+\r
+This program is free software; you can redistribute it and/or\r
+modify it under the terms of the GNU General Public License\r
+as published by the Free Software Foundation; either version 2\r
+of the License, or (at your option) any later version.\r
+\r
+This program is distributed in the hope that it will be useful,\r
+but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
+GNU General Public License for more details.\r
+\r
+FILE DESCRIPTION\r
+Panic routine.\r
+\r
+=============================================================================*/\r
+\r
+#if defined(__VIDEOCORE4__)\r
+       #include <common.h>\r
+       #include <cpu.h>\r
+#else\r
+       #include <chainloader.h>\r
+#endif\r
+\r
+#undef panic\r
+\r
+void panic(const char* fmt,  ...) {\r
+       printf("panic(): ");\r
+\r
+       va_list arp;\r
+\r
+       va_start(arp, fmt);\r
+       vprintf(fmt, arp);\r
+       va_end(arp);\r
+\r
+       putchar('\n');\r
+\r
+       hang_cpu();\r
+}
\ No newline at end of file
diff --git a/lib/panic.h b/lib/panic.h
new file mode 100644 (file)
index 0000000..084bce8
--- /dev/null
@@ -0,0 +1,21 @@
+#pragma once\r
+\r
+#ifdef __cplusplus\r
+extern "C" {\r
+#endif\r
+\r
+extern void panic(const char* fmt,  ...)\r
+       __attribute__((noreturn))\r
+       __attribute__ ((format (printf, 1, 2)));\r
+\r
+#define panic_plain(ex, ...) \\r
+       (panic)(ex, ## __VA_ARGS__)\r
+#define __STRINGIFY(x) #x\r
+#define LINE_NUMBER(x) __STRINGIFY(x)\r
+#define PANIC_LOCATION __FILE__ ":" LINE_NUMBER(__LINE__)\r
+#define panic(ex, ...) \\r
+       (panic)(# ex "@" PANIC_LOCATION, ## __VA_ARGS__)\r
+\r
+#ifdef __cplusplus\r
+}\r
+#endif
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/lib/tlsf/target.h b/lib/tlsf/target.h
new file mode 100644 (file)
index 0000000..1afd62a
--- /dev/null
@@ -0,0 +1,12 @@
+#ifndef _TARGET_H_
+#define _TARGET_H_
+
+#include <pthread.h>
+
+#define TLSF_MLOCK_T            pthread_mutex_t
+#define TLSF_CREATE_LOCK(l)     pthread_mutex_init (l, NULL)
+#define TLSF_DESTROY_LOCK(l)    pthread_mutex_destroy(l)
+#define TLSF_ACQUIRE_LOCK(l)    pthread_mutex_lock(l)
+#define TLSF_RELEASE_LOCK(l)    pthread_mutex_unlock(l)
+
+#endif
diff --git a/lib/tlsf/tlsf.c b/lib/tlsf/tlsf.c
new file mode 100644 (file)
index 0000000..bbda12e
--- /dev/null
@@ -0,0 +1,1015 @@
+/* 
+ * Two Levels Segregate Fit memory allocator (TLSF)
+ * Version 2.4.6
+ *
+ * Written by Miguel Masmano Tello <mimastel@doctor.upv.es>
+ *
+ * Thanks to Ismael Ripoll for his suggestions and reviews
+ *
+ * Copyright (C) 2008, 2007, 2006, 2005, 2004
+ *
+ * This code is released using a dual license strategy: GPL/LGPL
+ * You can choose the licence that better fits your requirements.
+ *
+ * Released under the terms of the GNU General Public License Version 2.0
+ * Released under the terms of the GNU Lesser General Public License Version 2.1
+ *
+ */
+
+/*
+ * Code contributions:
+ *
+ * (Jul 28 2007)  Herman ten Brugge <hermantenbrugge@home.nl>:
+ *
+ * - Add 64 bit support. It now runs on x86_64 and solaris64.
+ * - I also tested this on vxworks/32and solaris/32 and i386/32 processors.
+ * - Remove assembly code. I could not measure any performance difference 
+ *   on my core2 processor. This also makes the code more portable.
+ * - Moved defines/typedefs from tlsf.h to tlsf.c
+ * - Changed MIN_BLOCK_SIZE to sizeof (free_ptr_t) and BHDR_OVERHEAD to 
+ *   (sizeof (bhdr_t) - MIN_BLOCK_SIZE). This does not change the fact 
+ *    that the minumum size is still sizeof 
+ *   (bhdr_t).
+ * - Changed all C++ comment style to C style. (// -> /.* ... *./)
+ * - Used ls_bit instead of ffs and ms_bit instead of fls. I did this to 
+ *   avoid confusion with the standard ffs function which returns 
+ *   different values.
+ * - Created set_bit/clear_bit fuctions because they are not present 
+ *   on x86_64.
+ * - Added locking support + extra file target.h to show how to use it.
+ * - Added get_used_size function (REMOVED in 2.4)
+ * - Added rtl_realloc and rtl_calloc function
+ * - Implemented realloc clever support.
+ * - Added some test code in the example directory.
+ * - Bug fixed (discovered by the rockbox project: www.rockbox.org).       
+ *
+ * (Oct 23 2006) Adam Scislowicz: 
+ *
+ * - Support for ARMv5 implemented
+ *
+ */
+
+/*#define USE_SBRK        (0) */
+/*#define USE_MMAP        (0) */
+
+#ifndef USE_PRINTF
+#define USE_PRINTF      (1)
+#endif
+
+#include <string.h>
+
+#ifndef TLSF_USE_LOCKS
+#define        TLSF_USE_LOCKS  (0)
+#endif
+
+#ifndef TLSF_STATISTIC
+#define        TLSF_STATISTIC  (0)
+#endif
+
+#ifndef USE_MMAP
+#define        USE_MMAP        (0)
+#endif
+
+#ifndef USE_SBRK
+#define        USE_SBRK        (0)
+#endif
+
+
+#if TLSF_USE_LOCKS
+#include "target.h"
+#else
+#define TLSF_CREATE_LOCK(_unused_)   do{}while(0)
+#define TLSF_DESTROY_LOCK(_unused_)  do{}while(0) 
+#define TLSF_ACQUIRE_LOCK(_unused_)  do{}while(0)
+#define TLSF_RELEASE_LOCK(_unused_)  do{}while(0)
+#endif
+
+#if TLSF_STATISTIC
+#define        TLSF_ADD_SIZE(tlsf, b) do {                                                                     \
+               tlsf->used_size += (b->size & BLOCK_SIZE) + BHDR_OVERHEAD;      \
+               if (tlsf->used_size > tlsf->max_size)                                           \
+                       tlsf->max_size = tlsf->used_size;                                               \
+               } while(0)
+
+#define        TLSF_REMOVE_SIZE(tlsf, b) do {                                                          \
+               tlsf->used_size -= (b->size & BLOCK_SIZE) + BHDR_OVERHEAD;      \
+       } while(0)
+#else
+#define        TLSF_ADD_SIZE(tlsf, b)       do{}while(0)
+#define        TLSF_REMOVE_SIZE(tlsf, b)    do{}while(0)
+#endif
+
+#if USE_MMAP || USE_SBRK
+#include <unistd.h>
+#endif
+
+#if USE_MMAP
+#include <sys/mman.h>
+#endif
+
+#include "tlsf.h"
+
+#if !defined(__GNUC__)
+#ifndef __inline__
+#define __inline__
+#endif
+#endif
+
+/* The  debug functions  only can  be used  when _DEBUG_TLSF_  is set. */
+#ifndef _DEBUG_TLSF_
+#define _DEBUG_TLSF_  (0)
+#endif
+
+/*************************************************************************/
+/* Definition of the structures used by TLSF */
+
+
+/* Some IMPORTANT TLSF parameters */
+/* Unlike the preview TLSF versions, now they are statics */
+#define BLOCK_ALIGN (sizeof(void *) * 2)
+
+#define MAX_FLI                (30)
+#define MAX_LOG2_SLI   (5)
+#define MAX_SLI                (1 << MAX_LOG2_SLI)     /* MAX_SLI = 2^MAX_LOG2_SLI */
+
+#define FLI_OFFSET     (6)     /* tlsf structure just will manage blocks bigger */
+/* than 128 bytes */
+#define SMALL_BLOCK    (128)
+#define REAL_FLI       (MAX_FLI - FLI_OFFSET)
+#define MIN_BLOCK_SIZE (sizeof (free_ptr_t))
+#define BHDR_OVERHEAD  (sizeof (bhdr_t) - MIN_BLOCK_SIZE)
+#define TLSF_SIGNATURE (0x2A59FA59)
+
+#define        PTR_MASK        (sizeof(void *) - 1)
+#define BLOCK_SIZE     (0xFFFFFFFF - PTR_MASK)
+
+#define GET_NEXT_BLOCK(_addr, _r) ((bhdr_t *) ((char *) (_addr) + (_r)))
+#define        MEM_ALIGN                 ((BLOCK_ALIGN) - 1)
+#define ROUNDUP_SIZE(_r)          (((_r) + MEM_ALIGN) & ~MEM_ALIGN)
+#define ROUNDDOWN_SIZE(_r)        ((_r) & ~MEM_ALIGN)
+#define ROUNDUP(_x, _v)           ((((~(_x)) + 1) & ((_v)-1)) + (_x))
+
+#define BLOCK_STATE    (0x1)
+#define PREV_STATE     (0x2)
+
+/* bit 0 of the block size */
+#define FREE_BLOCK     (0x1)
+#define USED_BLOCK     (0x0)
+
+/* bit 1 of the block size */
+#define PREV_FREE      (0x2)
+#define PREV_USED      (0x0)
+
+
+#define DEFAULT_AREA_SIZE (1024*10)
+
+#ifdef USE_MMAP
+#define PAGE_SIZE (getpagesize())
+#endif
+
+#ifdef USE_PRINTF
+#include <stdio.h>
+# define PRINT_MSG(fmt, args...) printf(fmt, ## args)
+# define ERROR_MSG(fmt, args...) printf(fmt, ## args)
+#else
+# if !defined(PRINT_MSG)
+#  define PRINT_MSG(fmt, args...)
+# endif
+# if !defined(ERROR_MSG)
+#  define ERROR_MSG(fmt, args...)
+# endif
+#endif
+
+typedef unsigned int u32_t;     /* NOTE: Make sure that this type is 4 bytes long on your computer */
+typedef unsigned char u8_t;     /* NOTE: Make sure that this type is 1 byte on your computer */
+
+typedef struct free_ptr_struct {
+    struct bhdr_struct *prev;
+    struct bhdr_struct *next;
+} free_ptr_t;
+
+typedef struct bhdr_struct {
+    /* This pointer is just valid if the first bit of size is set */
+    struct bhdr_struct *prev_hdr;
+    /* The size is stored in bytes */
+    size_t size;                /* bit 0 indicates whether the block is used and */
+    /* bit 1 allows to know whether the previous block is free */
+    union {
+        struct free_ptr_struct free_ptr;
+        u8_t buffer[1];         /*sizeof(struct free_ptr_struct)]; */
+    } ptr;
+} bhdr_t;
+
+/* This structure is embedded at the beginning of each area, giving us
+ * enough information to cope with a set of areas */
+
+typedef struct area_info_struct {
+    bhdr_t *end;
+    struct area_info_struct *next;
+} area_info_t;
+
+typedef struct TLSF_struct {
+    /* the TLSF's structure signature */
+    u32_t tlsf_signature;
+
+#if TLSF_USE_LOCKS
+    TLSF_MLOCK_T lock;
+#endif
+
+#if TLSF_STATISTIC
+    /* These can not be calculated outside tlsf because we
+     * do not know the sizes when freeing/reallocing memory. */
+    size_t used_size;
+    size_t max_size;
+#endif
+
+    /* A linked list holding all the existing areas */
+    area_info_t *area_head;
+
+    /* the first-level bitmap */
+    /* This array should have a size of REAL_FLI bits */
+    u32_t fl_bitmap;
+
+    /* the second-level bitmap */
+    u32_t sl_bitmap[REAL_FLI];
+
+    bhdr_t *matrix[REAL_FLI][MAX_SLI];
+} tlsf_t;
+
+
+/******************************************************************/
+/**************     Helping functions    **************************/
+/******************************************************************/
+static __inline__ void set_bit(int nr, u32_t * addr);
+static __inline__ void clear_bit(int nr, u32_t * addr);
+static __inline__ int ls_bit(int x);
+static __inline__ int ms_bit(int x);
+static __inline__ void MAPPING_SEARCH(size_t * _r, int *_fl, int *_sl);
+static __inline__ void MAPPING_INSERT(size_t _r, int *_fl, int *_sl);
+static __inline__ bhdr_t *FIND_SUITABLE_BLOCK(tlsf_t * _tlsf, int *_fl, int *_sl);
+static __inline__ bhdr_t *process_area(void *area, size_t size);
+#if USE_SBRK || USE_MMAP
+static __inline__ void *get_new_area(size_t * size);
+#endif
+
+static const int table[] = {
+    -1, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 4,
+    4, 4,
+    4, 4, 4, 4, 4, 4, 4,
+    5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
+    5,
+    5, 5, 5, 5, 5, 5, 5,
+    6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+    6,
+    6, 6, 6, 6, 6, 6, 6,
+    6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
+    6,
+    6, 6, 6, 6, 6, 6, 6,
+    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+    7,
+    7, 7, 7, 7, 7, 7, 7,
+    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+    7,
+    7, 7, 7, 7, 7, 7, 7,
+    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+    7,
+    7, 7, 7, 7, 7, 7, 7,
+    7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
+    7,
+    7, 7, 7, 7, 7, 7, 7
+};
+
+static __inline__ int ls_bit(int i)
+{
+    unsigned int a;
+    unsigned int x = i & -i;
+
+    a = x <= 0xffff ? (x <= 0xff ? 0 : 8) : (x <= 0xffffff ? 16 : 24);
+    return table[x >> a] + a;
+}
+
+static __inline__ int ms_bit(int i)
+{
+    unsigned int a;
+    unsigned int x = (unsigned int) i;
+
+    a = x <= 0xffff ? (x <= 0xff ? 0 : 8) : (x <= 0xffffff ? 16 : 24);
+    return table[x >> a] + a;
+}
+
+static __inline__ void set_bit(int nr, u32_t * addr)
+{
+    addr[nr >> 5] |= 1 << (nr & 0x1f);
+}
+
+static __inline__ void clear_bit(int nr, u32_t * addr)
+{
+    addr[nr >> 5] &= ~(1 << (nr & 0x1f));
+}
+
+static __inline__ void MAPPING_SEARCH(size_t * _r, int *_fl, int *_sl)
+{
+    int _t;
+
+    if (*_r < SMALL_BLOCK) {
+        *_fl = 0;
+        *_sl = *_r / (SMALL_BLOCK / MAX_SLI);
+    } else {
+        _t = (1 << (ms_bit(*_r) - MAX_LOG2_SLI)) - 1;
+        *_r = *_r + _t;
+        *_fl = ms_bit(*_r);
+        *_sl = (*_r >> (*_fl - MAX_LOG2_SLI)) - MAX_SLI;
+        *_fl -= FLI_OFFSET;
+        /*if ((*_fl -= FLI_OFFSET) < 0) // FL wil be always >0!
+         *_fl = *_sl = 0;
+         */
+        *_r &= ~_t;
+    }
+}
+
+static __inline__ void MAPPING_INSERT(size_t _r, int *_fl, int *_sl)
+{
+    if (_r < SMALL_BLOCK) {
+        *_fl = 0;
+        *_sl = _r / (SMALL_BLOCK / MAX_SLI);
+    } else {
+        *_fl = ms_bit(_r);
+        *_sl = (_r >> (*_fl - MAX_LOG2_SLI)) - MAX_SLI;
+        *_fl -= FLI_OFFSET;
+    }
+}
+
+
+static __inline__ bhdr_t *FIND_SUITABLE_BLOCK(tlsf_t * _tlsf, int *_fl, int *_sl)
+{
+    u32_t _tmp = _tlsf->sl_bitmap[*_fl] & (~0 << *_sl);
+    bhdr_t *_b = NULL;
+
+    if (_tmp) {
+        *_sl = ls_bit(_tmp);
+        _b = _tlsf->matrix[*_fl][*_sl];
+    } else {
+        *_fl = ls_bit(_tlsf->fl_bitmap & (~0 << (*_fl + 1)));
+        if (*_fl > 0) {         /* likely */
+            *_sl = ls_bit(_tlsf->sl_bitmap[*_fl]);
+            _b = _tlsf->matrix[*_fl][*_sl];
+        }
+    }
+    return _b;
+}
+
+
+#define EXTRACT_BLOCK_HDR(_b, _tlsf, _fl, _sl) do {                                    \
+               _tlsf -> matrix [_fl] [_sl] = _b -> ptr.free_ptr.next;          \
+               if (_tlsf -> matrix[_fl][_sl])                                                          \
+                       _tlsf -> matrix[_fl][_sl] -> ptr.free_ptr.prev = NULL;  \
+               else {                                                                                                          \
+                       clear_bit (_sl, &_tlsf -> sl_bitmap [_fl]);                             \
+                       if (!_tlsf -> sl_bitmap [_fl])                                                  \
+                               clear_bit (_fl, &_tlsf -> fl_bitmap);                           \
+               }                                                                                                                       \
+               _b -> ptr.free_ptr.prev =  NULL;                                \
+               _b -> ptr.free_ptr.next =  NULL;                                \
+       }while(0)
+
+
+#define EXTRACT_BLOCK(_b, _tlsf, _fl, _sl) do {                                                        \
+               if (_b -> ptr.free_ptr.next)                                                                    \
+                       _b -> ptr.free_ptr.next -> ptr.free_ptr.prev = _b -> ptr.free_ptr.prev; \
+               if (_b -> ptr.free_ptr.prev)                                                                    \
+                       _b -> ptr.free_ptr.prev -> ptr.free_ptr.next = _b -> ptr.free_ptr.next; \
+               if (_tlsf -> matrix [_fl][_sl] == _b) {                                                 \
+                       _tlsf -> matrix [_fl][_sl] = _b -> ptr.free_ptr.next;           \
+                       if (!_tlsf -> matrix [_fl][_sl]) {                                                      \
+                               clear_bit (_sl, &_tlsf -> sl_bitmap[_fl]);                              \
+                               if (!_tlsf -> sl_bitmap [_fl])                                                  \
+                                       clear_bit (_fl, &_tlsf -> fl_bitmap);                           \
+                       }                                                                                                                       \
+               }                                                                                                                               \
+               _b -> ptr.free_ptr.prev = NULL;                                 \
+               _b -> ptr.free_ptr.next = NULL;                                 \
+       } while(0)
+
+#define INSERT_BLOCK(_b, _tlsf, _fl, _sl) do {                                                 \
+               _b -> ptr.free_ptr.prev = NULL; \
+               _b -> ptr.free_ptr.next = _tlsf -> matrix [_fl][_sl]; \
+               if (_tlsf -> matrix [_fl][_sl])                                                                 \
+                       _tlsf -> matrix [_fl][_sl] -> ptr.free_ptr.prev = _b;           \
+               _tlsf -> matrix [_fl][_sl] = _b;                                                                \
+               set_bit (_sl, &_tlsf -> sl_bitmap [_fl]);                                               \
+               set_bit (_fl, &_tlsf -> fl_bitmap);                                                             \
+       } while(0)
+
+#if USE_SBRK || USE_MMAP
+static __inline__ void *get_new_area(size_t * size) 
+{
+    void *area;
+
+#if USE_SBRK
+    area = (void *)sbrk(0);
+    if (((void *)sbrk(*size)) != ((void *) -1))
+        return area;
+#endif
+
+#ifndef MAP_ANONYMOUS
+/* https://dev.openwrt.org/ticket/322 */
+# define MAP_ANONYMOUS MAP_ANON
+#endif
+
+
+#if USE_MMAP
+    *size = ROUNDUP(*size, PAGE_SIZE);
+    if ((area = mmap(0, *size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANONYMOUS, -1, 0)) != MAP_FAILED)
+        return area;
+#endif
+    return ((void *) ~0);
+}
+#endif
+
+static __inline__ bhdr_t *process_area(void *area, size_t size)
+{
+    bhdr_t *b, *lb, *ib;
+    area_info_t *ai;
+
+    ib = (bhdr_t *) area;
+    ib->size =
+        (sizeof(area_info_t) <
+         MIN_BLOCK_SIZE) ? MIN_BLOCK_SIZE : ROUNDUP_SIZE(sizeof(area_info_t)) | USED_BLOCK | PREV_USED;
+    b = (bhdr_t *) GET_NEXT_BLOCK(ib->ptr.buffer, ib->size & BLOCK_SIZE);
+    b->size = ROUNDDOWN_SIZE(size - 3 * BHDR_OVERHEAD - (ib->size & BLOCK_SIZE)) | USED_BLOCK | PREV_USED;
+    b->ptr.free_ptr.prev = b->ptr.free_ptr.next = 0;
+    lb = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+    lb->prev_hdr = b;
+    lb->size = 0 | USED_BLOCK | PREV_FREE;
+    ai = (area_info_t *) ib->ptr.buffer;
+    ai->next = 0;
+    ai->end = lb;
+    return ib;
+}
+
+/******************************************************************/
+/******************** Begin of the allocator code *****************/
+/******************************************************************/
+
+static char *mp = NULL;         /* Default memory pool. */
+
+/******************************************************************/
+size_t init_memory_pool(size_t mem_pool_size, void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf;
+    bhdr_t *b, *ib;
+
+    if (!mem_pool || !mem_pool_size || mem_pool_size < sizeof(tlsf_t) + BHDR_OVERHEAD * 8) {
+        ERROR_MSG("init_memory_pool (): memory_pool invalid\n");
+        return -1;
+    }
+
+    if (((unsigned long) mem_pool & PTR_MASK)) {
+        ERROR_MSG("init_memory_pool (): mem_pool must be aligned to a word\n");
+        return -1;
+    }
+    tlsf = (tlsf_t *) mem_pool;
+    /* Check if already initialised */
+    if (tlsf->tlsf_signature == TLSF_SIGNATURE) {
+        mp = mem_pool;
+        b = GET_NEXT_BLOCK(mp, ROUNDUP_SIZE(sizeof(tlsf_t)));
+        return b->size & BLOCK_SIZE;
+    }
+
+    mp = mem_pool;
+
+    /* Zeroing the memory pool */
+    memset(mem_pool, 0, sizeof(tlsf_t));
+
+    tlsf->tlsf_signature = TLSF_SIGNATURE;
+
+    TLSF_CREATE_LOCK(&tlsf->lock);
+
+    ib = process_area(GET_NEXT_BLOCK
+                      (mem_pool, ROUNDUP_SIZE(sizeof(tlsf_t))), ROUNDDOWN_SIZE(mem_pool_size - sizeof(tlsf_t)));
+    b = GET_NEXT_BLOCK(ib->ptr.buffer, ib->size & BLOCK_SIZE);
+    free_ex(b->ptr.buffer, tlsf);
+    tlsf->area_head = (area_info_t *) ib->ptr.buffer;
+
+#if TLSF_STATISTIC
+    tlsf->used_size = mem_pool_size - (b->size & BLOCK_SIZE);
+    tlsf->max_size = tlsf->used_size;
+#endif
+
+    return (b->size & BLOCK_SIZE);
+}
+
+/******************************************************************/
+size_t add_new_area(void *area, size_t area_size, void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf = (tlsf_t *) mem_pool;
+    area_info_t *ptr, *ptr_prev, *ai;
+    bhdr_t *ib0, *b0, *lb0, *ib1, *b1, *lb1, *next_b;
+
+    memset(area, 0, area_size);
+    ptr = tlsf->area_head;
+    ptr_prev = 0;
+
+    ib0 = process_area(area, area_size);
+    b0 = GET_NEXT_BLOCK(ib0->ptr.buffer, ib0->size & BLOCK_SIZE);
+    lb0 = GET_NEXT_BLOCK(b0->ptr.buffer, b0->size & BLOCK_SIZE);
+
+    /* Before inserting the new area, we have to merge this area with the
+       already existing ones */
+
+    while (ptr) {
+        ib1 = (bhdr_t *) ((char *) ptr - BHDR_OVERHEAD);
+        b1 = GET_NEXT_BLOCK(ib1->ptr.buffer, ib1->size & BLOCK_SIZE);
+        lb1 = ptr->end;
+
+        /* Merging the new area with the next physically contigous one */
+        if ((unsigned long) ib1 == (unsigned long) lb0 + BHDR_OVERHEAD) {
+            if (tlsf->area_head == ptr) {
+                tlsf->area_head = ptr->next;
+                ptr = ptr->next;
+            } else {
+                ptr_prev->next = ptr->next;
+                ptr = ptr->next;
+            }
+
+            b0->size =
+                ROUNDDOWN_SIZE((b0->size & BLOCK_SIZE) +
+                               (ib1->size & BLOCK_SIZE) + 2 * BHDR_OVERHEAD) | USED_BLOCK | PREV_USED;
+
+            b1->prev_hdr = b0;
+            lb0 = lb1;
+
+            continue;
+        }
+
+        /* Merging the new area with the previous physically contigous
+           one */
+        if ((unsigned long) lb1->ptr.buffer == (unsigned long) ib0) {
+            if (tlsf->area_head == ptr) {
+                tlsf->area_head = ptr->next;
+                ptr = ptr->next;
+            } else {
+                ptr_prev->next = ptr->next;
+                ptr = ptr->next;
+            }
+
+            lb1->size =
+                ROUNDDOWN_SIZE((b0->size & BLOCK_SIZE) +
+                               (ib0->size & BLOCK_SIZE) + 2 * BHDR_OVERHEAD) | USED_BLOCK | (lb1->size & PREV_STATE);
+            next_b = GET_NEXT_BLOCK(lb1->ptr.buffer, lb1->size & BLOCK_SIZE);
+            next_b->prev_hdr = lb1;
+            b0 = lb1;
+            ib0 = ib1;
+
+            continue;
+        }
+        ptr_prev = ptr;
+        ptr = ptr->next;
+    }
+
+    /* Inserting the area in the list of linked areas */
+    ai = (area_info_t *) ib0->ptr.buffer;
+    ai->next = tlsf->area_head;
+    ai->end = lb0;
+    tlsf->area_head = ai;
+    free_ex(b0->ptr.buffer, mem_pool);
+    return (b0->size & BLOCK_SIZE);
+}
+
+
+/******************************************************************/
+size_t get_used_size(void *mem_pool)
+{
+/******************************************************************/
+#if TLSF_STATISTIC
+    return ((tlsf_t *) mem_pool)->used_size;
+#else
+    return 0;
+#endif
+}
+
+/******************************************************************/
+size_t get_max_size(void *mem_pool)
+{
+/******************************************************************/
+#if TLSF_STATISTIC
+    return ((tlsf_t *) mem_pool)->max_size;
+#else
+    return 0;
+#endif
+}
+
+/******************************************************************/
+void destroy_memory_pool(void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf = (tlsf_t *) mem_pool;
+
+    tlsf->tlsf_signature = 0;
+
+    TLSF_DESTROY_LOCK(&tlsf->lock);
+
+}
+
+
+/******************************************************************/
+void *tlsf_malloc(size_t size)
+{
+/******************************************************************/
+    void *ret;
+
+#if USE_MMAP || USE_SBRK
+    if (!mp) {
+        size_t area_size;
+        void *area;
+
+        area_size = sizeof(tlsf_t) + BHDR_OVERHEAD * 8; /* Just a safety constant */
+        area_size = (area_size > DEFAULT_AREA_SIZE) ? area_size : DEFAULT_AREA_SIZE;
+        area = get_new_area(&area_size);
+        if (area == ((void *) ~0))
+            return NULL;        /* Not enough system memory */
+        init_memory_pool(area_size, area);
+    }
+#endif
+
+    TLSF_ACQUIRE_LOCK(&((tlsf_t *)mp)->lock);
+
+    ret = malloc_ex(size, mp);
+
+    TLSF_RELEASE_LOCK(&((tlsf_t *)mp)->lock);
+
+    return ret;
+}
+
+/******************************************************************/
+void tlsf_free(void *ptr)
+{
+/******************************************************************/
+
+    TLSF_ACQUIRE_LOCK(&((tlsf_t *)mp)->lock);
+
+    free_ex(ptr, mp);
+
+    TLSF_RELEASE_LOCK(&((tlsf_t *)mp)->lock);
+
+}
+
+/******************************************************************/
+void *tlsf_realloc(void *ptr, size_t size)
+{
+/******************************************************************/
+    void *ret;
+
+#if USE_MMAP || USE_SBRK
+       if (!mp) {
+               return tlsf_malloc(size);
+       }
+#endif
+
+    TLSF_ACQUIRE_LOCK(&((tlsf_t *)mp)->lock);
+
+    ret = realloc_ex(ptr, size, mp);
+
+    TLSF_RELEASE_LOCK(&((tlsf_t *)mp)->lock);
+
+    return ret;
+}
+
+/******************************************************************/
+void *tlsf_calloc(size_t nelem, size_t elem_size)
+{
+/******************************************************************/
+    void *ret;
+
+    TLSF_ACQUIRE_LOCK(&((tlsf_t *)mp)->lock);
+
+    ret = calloc_ex(nelem, elem_size, mp);
+
+    TLSF_RELEASE_LOCK(&((tlsf_t *)mp)->lock);
+
+    return ret;
+}
+
+/******************************************************************/
+void *malloc_ex(size_t size, void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf = (tlsf_t *) mem_pool;
+    bhdr_t *b, *b2, *next_b;
+    int fl, sl;
+    size_t tmp_size;
+
+    size = (size < MIN_BLOCK_SIZE) ? MIN_BLOCK_SIZE : ROUNDUP_SIZE(size);
+
+    /* Rounding up the requested size and calculating fl and sl */
+    MAPPING_SEARCH(&size, &fl, &sl);
+
+    /* Searching a free block, recall that this function changes the values of fl and sl,
+       so they are not longer valid when the function fails */
+    b = FIND_SUITABLE_BLOCK(tlsf, &fl, &sl);
+#if USE_MMAP || USE_SBRK
+    if (!b) {
+        size_t area_size;
+        void *area;
+        /* Growing the pool size when needed */
+        area_size = size + BHDR_OVERHEAD * 8;   /* size plus enough room for the requered headers. */
+        area_size = (area_size > DEFAULT_AREA_SIZE) ? area_size : DEFAULT_AREA_SIZE;
+        area = get_new_area(&area_size);        /* Call sbrk or mmap */
+        if (area == ((void *) ~0))
+            return NULL;        /* Not enough system memory */
+        add_new_area(area, area_size, mem_pool);
+        /* Rounding up the requested size and calculating fl and sl */
+        MAPPING_SEARCH(&size, &fl, &sl);
+        /* Searching a free block */
+        b = FIND_SUITABLE_BLOCK(tlsf, &fl, &sl);
+    }
+#endif
+    if (!b)
+        return NULL;            /* Not found */
+
+    EXTRACT_BLOCK_HDR(b, tlsf, fl, sl);
+
+    /*-- found: */
+    next_b = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+    /* Should the block be split? */
+    tmp_size = (b->size & BLOCK_SIZE) - size;
+    if (tmp_size >= sizeof(bhdr_t)) {
+        tmp_size -= BHDR_OVERHEAD;
+        b2 = GET_NEXT_BLOCK(b->ptr.buffer, size);
+        b2->size = tmp_size | FREE_BLOCK | PREV_USED;
+        next_b->prev_hdr = b2;
+        MAPPING_INSERT(tmp_size, &fl, &sl);
+        INSERT_BLOCK(b2, tlsf, fl, sl);
+
+        b->size = size | (b->size & PREV_STATE);
+    } else {
+        next_b->size &= (~PREV_FREE);
+        b->size &= (~FREE_BLOCK);       /* Now it's used */
+    }
+
+    TLSF_ADD_SIZE(tlsf, b);
+
+    return (void *) b->ptr.buffer;
+}
+
+/******************************************************************/
+void free_ex(void *ptr, void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf = (tlsf_t *) mem_pool;
+    bhdr_t *b, *tmp_b;
+    int fl = 0, sl = 0;
+
+    if (!ptr) {
+        return;
+    }
+    b = (bhdr_t *) ((char *) ptr - BHDR_OVERHEAD);
+    b->size |= FREE_BLOCK;
+
+    TLSF_REMOVE_SIZE(tlsf, b);
+
+    b->ptr.free_ptr.prev = NULL;
+    b->ptr.free_ptr.next = NULL;
+    tmp_b = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+    if (tmp_b->size & FREE_BLOCK) {
+        MAPPING_INSERT(tmp_b->size & BLOCK_SIZE, &fl, &sl);
+        EXTRACT_BLOCK(tmp_b, tlsf, fl, sl);
+        b->size += (tmp_b->size & BLOCK_SIZE) + BHDR_OVERHEAD;
+    }
+    if (b->size & PREV_FREE) {
+        tmp_b = b->prev_hdr;
+        MAPPING_INSERT(tmp_b->size & BLOCK_SIZE, &fl, &sl);
+        EXTRACT_BLOCK(tmp_b, tlsf, fl, sl);
+        tmp_b->size += (b->size & BLOCK_SIZE) + BHDR_OVERHEAD;
+        b = tmp_b;
+    }
+    MAPPING_INSERT(b->size & BLOCK_SIZE, &fl, &sl);
+    INSERT_BLOCK(b, tlsf, fl, sl);
+
+    tmp_b = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+    tmp_b->size |= PREV_FREE;
+    tmp_b->prev_hdr = b;
+}
+
+/******************************************************************/
+void *realloc_ex(void *ptr, size_t new_size, void *mem_pool)
+{
+/******************************************************************/
+    tlsf_t *tlsf = (tlsf_t *) mem_pool;
+    void *ptr_aux;
+    unsigned int cpsize;
+    bhdr_t *b, *tmp_b, *next_b;
+    int fl, sl;
+    size_t tmp_size;
+
+    if (!ptr) {
+        if (new_size)
+            return (void *) malloc_ex(new_size, mem_pool);
+        if (!new_size)
+            return NULL;
+    } else if (!new_size) {
+        free_ex(ptr, mem_pool);
+        return NULL;
+    }
+
+    b = (bhdr_t *) ((char *) ptr - BHDR_OVERHEAD);
+    next_b = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+    new_size = (new_size < MIN_BLOCK_SIZE) ? MIN_BLOCK_SIZE : ROUNDUP_SIZE(new_size);
+    tmp_size = (b->size & BLOCK_SIZE);
+    if (new_size <= tmp_size) {
+       TLSF_REMOVE_SIZE(tlsf, b);
+        if (next_b->size & FREE_BLOCK) {
+            MAPPING_INSERT(next_b->size & BLOCK_SIZE, &fl, &sl);
+            EXTRACT_BLOCK(next_b, tlsf, fl, sl);
+            tmp_size += (next_b->size & BLOCK_SIZE) + BHDR_OVERHEAD;
+            next_b = GET_NEXT_BLOCK(next_b->ptr.buffer, next_b->size & BLOCK_SIZE);
+            /* We allways reenter this free block because tmp_size will
+               be greater then sizeof (bhdr_t) */
+        }
+        tmp_size -= new_size;
+        if (tmp_size >= sizeof(bhdr_t)) {
+            tmp_size -= BHDR_OVERHEAD;
+            tmp_b = GET_NEXT_BLOCK(b->ptr.buffer, new_size);
+            tmp_b->size = tmp_size | FREE_BLOCK | PREV_USED;
+            next_b->prev_hdr = tmp_b;
+            next_b->size |= PREV_FREE;
+            MAPPING_INSERT(tmp_size, &fl, &sl);
+            INSERT_BLOCK(tmp_b, tlsf, fl, sl);
+            b->size = new_size | (b->size & PREV_STATE);
+        }
+       TLSF_ADD_SIZE(tlsf, b);
+        return (void *) b->ptr.buffer;
+    }
+    if ((next_b->size & FREE_BLOCK)) {
+        if (new_size <= (tmp_size + (next_b->size & BLOCK_SIZE))) {
+                       TLSF_REMOVE_SIZE(tlsf, b);
+            MAPPING_INSERT(next_b->size & BLOCK_SIZE, &fl, &sl);
+            EXTRACT_BLOCK(next_b, tlsf, fl, sl);
+            b->size += (next_b->size & BLOCK_SIZE) + BHDR_OVERHEAD;
+            next_b = GET_NEXT_BLOCK(b->ptr.buffer, b->size & BLOCK_SIZE);
+            next_b->prev_hdr = b;
+            next_b->size &= ~PREV_FREE;
+            tmp_size = (b->size & BLOCK_SIZE) - new_size;
+            if (tmp_size >= sizeof(bhdr_t)) {
+                tmp_size -= BHDR_OVERHEAD;
+                tmp_b = GET_NEXT_BLOCK(b->ptr.buffer, new_size);
+                tmp_b->size = tmp_size | FREE_BLOCK | PREV_USED;
+                next_b->prev_hdr = tmp_b;
+                next_b->size |= PREV_FREE;
+                MAPPING_INSERT(tmp_size, &fl, &sl);
+                INSERT_BLOCK(tmp_b, tlsf, fl, sl);
+                b->size = new_size | (b->size & PREV_STATE);
+            }
+                       TLSF_ADD_SIZE(tlsf, b);
+            return (void *) b->ptr.buffer;
+        }
+    }
+
+    if (!(ptr_aux = malloc_ex(new_size, mem_pool))){
+        return NULL;
+    }      
+    
+    cpsize = ((b->size & BLOCK_SIZE) > new_size) ? new_size : (b->size & BLOCK_SIZE);
+
+    memcpy(ptr_aux, ptr, cpsize);
+
+    free_ex(ptr, mem_pool);
+    return ptr_aux;
+}
+
+
+/******************************************************************/
+void *calloc_ex(size_t nelem, size_t elem_size, void *mem_pool)
+{
+/******************************************************************/
+    void *ptr;
+
+    if (nelem <= 0 || elem_size <= 0)
+        return NULL;
+
+    if (!(ptr = malloc_ex(nelem * elem_size, mem_pool)))
+        return NULL;
+    memset(ptr, 0, nelem * elem_size);
+
+    return ptr;
+}
+
+
+
+#if _DEBUG_TLSF_
+
+/***************  DEBUG FUNCTIONS   **************/
+
+/* The following functions have been designed to ease the debugging of */
+/* the TLSF  structure.  For non-developing  purposes, it may  be they */
+/* haven't too much worth.  To enable them, _DEBUG_TLSF_ must be set.  */
+
+extern void dump_memory_region(unsigned char *mem_ptr, unsigned int size);
+extern void print_block(bhdr_t * b);
+extern void print_tlsf(tlsf_t * tlsf);
+void print_all_blocks(tlsf_t * tlsf);
+
+void dump_memory_region(unsigned char *mem_ptr, unsigned int size)
+{
+
+    unsigned long begin = (unsigned long) mem_ptr;
+    unsigned long end = (unsigned long) mem_ptr + size;
+    int column = 0;
+
+    begin >>= 2;
+    begin <<= 2;
+
+    end >>= 2;
+    end++;
+    end <<= 2;
+
+    PRINT_MSG("\nMemory region dumped: 0x%lx - 0x%lx\n\n", begin, end);
+
+    column = 0;
+    PRINT_MSG("0x%lx ", begin);
+
+    while (begin < end) {
+        if (((unsigned char *) begin)[0] == 0)
+            PRINT_MSG("00");
+        else
+            PRINT_MSG("%02x", ((unsigned char *) begin)[0]);
+        if (((unsigned char *) begin)[1] == 0)
+            PRINT_MSG("00 ");
+        else
+            PRINT_MSG("%02x ", ((unsigned char *) begin)[1]);
+        begin += 2;
+        column++;
+        if (column == 8) {
+            PRINT_MSG("\n0x%lx ", begin);
+            column = 0;
+        }
+
+    }
+    PRINT_MSG("\n\n");
+}
+
+void print_block(bhdr_t * b)
+{
+    if (!b)
+        return;
+    PRINT_MSG(">> [%p] (", b);
+    if ((b->size & BLOCK_SIZE))
+        PRINT_MSG("%lu bytes, ", (unsigned long) (b->size & BLOCK_SIZE));
+    else
+        PRINT_MSG("sentinel, ");
+    if ((b->size & BLOCK_STATE) == FREE_BLOCK)
+        PRINT_MSG("free [%p, %p], ", b->ptr.free_ptr.prev, b->ptr.free_ptr.next);
+    else
+        PRINT_MSG("used, ");
+    if ((b->size & PREV_STATE) == PREV_FREE)
+        PRINT_MSG("prev. free [%p])\n", b->prev_hdr);
+    else
+        PRINT_MSG("prev used)\n");
+}
+
+void print_tlsf(tlsf_t * tlsf)
+{
+    bhdr_t *next;
+    int i, j;
+
+    PRINT_MSG("\nTLSF at %p\n", tlsf);
+
+    PRINT_MSG("FL bitmap: 0x%x\n\n", (unsigned) tlsf->fl_bitmap);
+
+    for (i = 0; i < REAL_FLI; i++) {
+        if (tlsf->sl_bitmap[i])
+            PRINT_MSG("SL bitmap 0x%x\n", (unsigned) tlsf->sl_bitmap[i]);
+        for (j = 0; j < MAX_SLI; j++) {
+            next = tlsf->matrix[i][j];
+            if (next)
+                PRINT_MSG("-> [%d][%d]\n", i, j);
+            while (next) {
+                print_block(next);
+                next = next->ptr.free_ptr.next;
+            }
+        }
+    }
+}
+
+void print_all_blocks(tlsf_t * tlsf)
+{
+    area_info_t *ai;
+    bhdr_t *next;
+    PRINT_MSG("\nTLSF at %p\nALL BLOCKS\n\n", tlsf);
+    ai = tlsf->area_head;
+    while (ai) {
+        next = (bhdr_t *) ((char *) ai - BHDR_OVERHEAD);
+        while (next) {
+            print_block(next);
+            if ((next->size & BLOCK_SIZE))
+                next = GET_NEXT_BLOCK(next->ptr.buffer, next->size & BLOCK_SIZE);
+            else
+                next = NULL;
+        }
+        ai = ai->next;
+    }
+}
+
+#endif
diff --git a/lib/tlsf/tlsf.h b/lib/tlsf/tlsf.h
new file mode 100644 (file)
index 0000000..3a7ea69
--- /dev/null
@@ -0,0 +1,52 @@
+/*
+ * Two Levels Segregate Fit memory allocator (TLSF)
+ * Version 2.4.6
+ *
+ * Written by Miguel Masmano Tello <mimastel@doctor.upv.es>
+ *
+ * Thanks to Ismael Ripoll for his suggestions and reviews
+ *
+ * Copyright (C) 2008, 2007, 2006, 2005, 2004
+ *
+ * This code is released using a dual license strategy: GPL/LGPL
+ * You can choose the licence that better fits your requirements.
+ *
+ * Released under the terms of the GNU General Public License Version 2.0
+ * Released under the terms of the GNU Lesser General Public License Version 2.1
+ *
+ */
+
+#ifndef _TLSF_H_
+#define _TLSF_H_
+
+#include <sys/types.h>
+
+#define tlsf_malloc malloc
+#define tlsf_free free
+#define tlsf_realloc realloc
+#define tlsf_calloc calloc
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern size_t init_memory_pool(size_t, void *);
+extern size_t get_used_size(void *);
+extern size_t get_max_size(void *);
+extern void destroy_memory_pool(void *);
+extern size_t add_new_area(void *, size_t, void *);
+extern void *malloc_ex(size_t, void *);
+extern void free_ex(void *, void *);
+extern void *realloc_ex(void *, size_t, void *);
+extern void *calloc_ex(size_t, size_t, void *);
+
+extern void *tlsf_malloc(size_t size);
+extern void tlsf_free(void *ptr);
+extern void *tlsf_realloc(void *ptr, size_t size);
+extern void *tlsf_calloc(size_t nelem, size_t elem_size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
old mode 100755 (executable)
new mode 100644 (file)
index ba13d32..3231073
@@ -18,9 +18,9 @@ Simple timer based delay routine.
 =============================================================================*/\r
 \r
 #ifdef __arm__\r
-#include <stdint.h>\r
+       #include <stdint.h>\r
 #else\r
-#include "common.h"\r
+       #include <common.h>\r
 #endif\r
 \r
 #include "../hardware.h"\r
@@ -28,7 +28,6 @@ Simple timer based delay routine.
 \r
 void udelay(uint32_t t) {\r
        uint32_t tv = ST_CLO;\r
-\r
        for (;;) {\r
                /* nop still takes a cycle i think? */\r
                __asm__ __volatile__ ("nop" :::);\r
old mode 100755 (executable)
new mode 100644 (file)
index 7f9b7db..53462a0
@@ -19,9 +19,9 @@ extern void uart_putc(unsigned int ch);
 /* Put a character                              */\r
 /*----------------------------------------------*/\r
 \r
-void xputc (char c)\r
+int putchar (int c)\r
 {\r
-       if (_CR_CRLF && c == '\n') xputc('\r');         /* CR -> CRLF */\r
+       if (_CR_CRLF && c == '\n') putchar('\r');               /* CR -> CRLF */\r
 \r
        uart_putc(c);\r
 }\r
@@ -30,12 +30,12 @@ void xputc (char c)
 /* Put a null-terminated string                 */\r
 /*----------------------------------------------*/\r
 \r
-void xputs (                                   /* Put a string to the default device */\r
+int puts (                                     /* Put a string to the default device */\r
        const char* str                         /* Pointer to the string */\r
 )\r
 {\r
        while (*str)\r
-               xputc(*str++);\r
+               putchar(*str++);\r
 }\r
 \r
 /*----------------------------------------------*/\r
@@ -55,7 +55,7 @@ void xputs (                                  /* Put a string to the default device */
     xprintf("%f", 10.0);            <xprintf lacks floating point support>\r
 */\r
 \r
-void xvprintf (\r
+int vprintf (\r
        const char*     fmt,    /* Pointer to the format string */\r
        va_list arp                     /* Pointer to arguments */\r
 )\r
@@ -69,7 +69,7 @@ void xvprintf (
                c = *fmt++;                                     /* Get a char */\r
                if (!c) break;                          /* End of format? */\r
                if (c != '%') {                         /* Pass through it if not a % sequense */\r
-                       xputc(c); continue;\r
+                       putchar(c); continue;\r
                }\r
                f = 0;\r
                c = *fmt++;                                     /* Get first char of the sequense */\r
@@ -92,12 +92,12 @@ void xvprintf (
                case 'S' :                                      /* String */\r
                        p = va_arg(arp, char*);\r
                        for (j = 0; p[j]; j++) ;\r
-                       while (!(f & 2) && j++ < w) xputc(' ');\r
-                       xputs(p);\r
-                       while (j++ < w) xputc(' ');\r
+                       while (!(f & 2) && j++ < w) putchar(' ');\r
+                       puts(p);\r
+                       while (j++ < w) putchar(' ');\r
                        continue;\r
                case 'C' :                                      /* Character */\r
-                       xputc((char)va_arg(arp, int)); continue;\r
+                       putchar((char)va_arg(arp, int)); continue;\r
                case 'B' :                                      /* Binary */\r
                        r = 2; break;\r
                case 'O' :                                      /* Octal */\r
@@ -108,7 +108,7 @@ void xvprintf (
                case 'X' :                                      /* Hexdecimal */\r
                        r = 16; break;\r
                default:                                        /* Unknown type (passthrough) */\r
-                       xputc(c); continue;\r
+                       putchar(c); continue;\r
                }\r
 \r
                /* Get an argument and put it in numeral */\r
@@ -125,14 +125,14 @@ void xvprintf (
                } while (v && i < sizeof(s));\r
                if (f & 8) s[i++] = '-';\r
                j = i; d = (f & 1) ? '0' : ' ';\r
-               while (!(f & 2) && j++ < w) xputc(d);\r
-               do xputc(s[--i]); while(i);\r
-               while (j++ < w) xputc(' ');\r
+               while (!(f & 2) && j++ < w) putchar(d);\r
+               do putchar(s[--i]); while(i);\r
+               while (j++ < w) putchar(' ');\r
        }\r
 }\r
 \r
 \r
-void xprintf (                 /* Put a formatted string to the default device */\r
+int printf (                   /* Put a formatted string to the default device */\r
        const char*     fmt,    /* Pointer to the format string */\r
        ...                                     /* Optional arguments */\r
 )\r
@@ -140,7 +140,7 @@ void xprintf (                      /* Put a formatted string to the default device */
        va_list arp;\r
 \r
        va_start(arp, fmt);\r
-       xvprintf(fmt, arp);\r
+       vprintf(fmt, arp);\r
        va_end(arp);\r
 }\r
 \r
@@ -162,30 +162,30 @@ void put_dump (
        const unsigned long *lp;\r
 \r
 \r
-       xprintf("%08lX ", addr);                /* address */\r
+       printf("%08lX ", addr);         /* address */\r
 \r
        switch (width) {\r
        case DW_CHAR:\r
                bp = buff;\r
                for (i = 0; i < len; i++)               /* Hexdecimal dump */\r
-                       xprintf(" %02X", bp[i]);\r
-               xputc(' ');\r
+                       printf(" %02X", bp[i]);\r
+               putchar(' ');\r
                for (i = 0; i < len; i++)               /* ASCII dump */\r
-                       xputc((bp[i] >= ' ' && bp[i] <= '~') ? bp[i] : '.');\r
+                       putchar((bp[i] >= ' ' && bp[i] <= '~') ? bp[i] : '.');\r
                break;\r
        case DW_SHORT:\r
                sp = buff;\r
                do                                                              /* Hexdecimal dump */\r
-                       xprintf(" %04X", *sp++);\r
+                       printf(" %04X", *sp++);\r
                while (--len);\r
                break;\r
        case DW_LONG:\r
                lp = buff;\r
                do                                                              /* Hexdecimal dump */\r
-                       xprintf(" %08X", *lp++);\r
+                       printf(" %08X", *lp++);\r
                while (--len);\r
                break;\r
        }\r
 \r
-       xputc('\n');\r
+       putchar('\n');\r
 }
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
index 32863c7..80c6d2c
 extern "C" {\r
 #endif\r
 \r
-void xputc (char c);\r
-void xputs (const char* str);\r
-void xfputs (void (*func)(unsigned char), const char* str);\r
-void xprintf (const char* fmt, ...) __attribute__ ((format (printf, 1, 2)));\r
-void xsprintf (char* buff, const char* fmt, ...);\r
-void xfprintf (void (*func)(unsigned char), const char*        fmt, ...);\r
+int putchar (int c);\r
+int puts (const char* str);\r
+int printf (const char* fmt, ...) __attribute__ ((format (printf, 1, 2)));\r
 void put_dump (const void* buff, unsigned long addr, int len, int width);\r
 \r
 \r
-void xvprintf (\r
+int vprintf (\r
        const char*     fmt,    /* Pointer to the format string */\r
        va_list arp                     /* Pointer to arguments */\r
 );\r
diff --git a/panic.c b/panic.c
deleted file mode 100755 (executable)
index fa57c53..0000000
--- a/panic.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/*=============================================================================\r
-Copyright (C) 2016 Kristina Brooks\r
-All rights reserved.\r
-\r
-This program is free software; you can redistribute it and/or\r
-modify it under the terms of the GNU General Public License\r
-as published by the Free Software Foundation; either version 2\r
-of the License, or (at your option) any later version.\r
-\r
-This program is distributed in the hope that it will be useful,\r
-but WITHOUT ANY WARRANTY; without even the implied warranty of\r
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
-GNU General Public License for more details.\r
-\r
-FILE DESCRIPTION\r
-Panic routine.\r
-\r
-=============================================================================*/\r
-\r
-#include <common.h>\r
-#include <cpu.h>\r
-\r
-#undef panic\r
-\r
-void panic(const char* fmt,  ...) {\r
-       printf("panic(): ");\r
-\r
-       va_list arp;\r
-\r
-       va_start(arp, fmt);\r
-       vprintf(fmt, arp);\r
-       va_end(arp);\r
-\r
-       putchar('\n');\r
-\r
-       hang_cpu();\r
-}
\ No newline at end of file
old mode 100755 (executable)
new mode 100644 (file)
index 1bd0b08..c98dee9
@@ -17,8 +17,8 @@ VideoCoreIV first stage bootloader.
 \r
 =============================================================================*/\r
 \r
-#include "lib/common.h"\r
-#include "hardware.h"\r
+#include <common.h>\r
+#include <hardware.h>\r
 \r
 uint32_t g_CPUID;\r
 \r
@@ -146,6 +146,11 @@ void switch_vpu_to_pllc() {
 \r
 extern void sdram_init();\r
 extern void arm_init();\r
+extern void monitor_start();\r
+\r
+void print_crap() {\r
+       printf("TB_BOOT_OPT = 0x%X\n", TB_BOOT_OPT);\r
+}\r
 \r
 int _main(unsigned int cpuid, unsigned int load_address) {\r
        switch_vpu_to_pllc();\r
@@ -169,6 +174,8 @@ int _main(unsigned int cpuid, unsigned int load_address) {
        printf("CPUID    = 0x%X\n", cpuid);\r
        printf("LoadAddr = 0x%X\n", load_address);\r
 \r
+       print_crap();\r
+\r
        g_CPUID = cpuid;\r
        \r
        /* bring up SDRAM */\r
@@ -178,6 +185,9 @@ int _main(unsigned int cpuid, unsigned int load_address) {
        /* bring up ARM */\r
        arm_init();\r
 \r
+       /* start vpu monitor */\r
+       monitor_start();\r
+\r
        panic("main exiting!");\r
 }\r
 \r
diff --git a/sdram.c b/sdram.c
old mode 100755 (executable)
new mode 100644 (file)
index 59b8a85..0866f63
--- a/sdram.c
+++ b/sdram.c
@@ -51,7 +51,7 @@ extern uint32_t g_CPUID;
 \r
 #define PVT_calibrate_request 0x1\r
 \r
-#define logf(fmt, ...) printf("[SDRAM::%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
+#define logf(fmt, ...) printf("[SDRAM:%s]: " fmt, __FUNCTION__, ##__VA_ARGS__);\r
 \r
 unsigned g_RAMSize = RAM_SIZE_UNKNOWN;\r
 \r
diff --git a/start.s b/start.s
old mode 100755 (executable)
new mode 100644 (file)
diff --git a/trap.c b/trap.c
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
index 706d153..18a0714
@@ -28,23 +28,13 @@ typedef uint32_t size_t;
 \r
 # define ALWAYS_INLINE __attribute__((always_inline)) inline\r
 \r
-extern void panic(const char* fmt,  ...)\r
-       __attribute__((noreturn))\r
-       __attribute__ ((format (printf, 1, 2)));\r
-\r
-#define panic_plain(ex, ...) \\r
-       (panic)(ex, ## __VA_ARGS__)\r
-#define __STRINGIFY(x) #x\r
-#define LINE_NUMBER(x) __STRINGIFY(x)\r
-#define PANIC_LOCATION __FILE__ ":" LINE_NUMBER(__LINE__)\r
-#define panic(ex, ...) \\r
-       (panic)(# ex "@" PANIC_LOCATION, ## __VA_ARGS__)\r
-\r
 #define _OPEN_SOURCE\r
 \r
 extern void udelay(uint32_t time);\r
 extern uint32_t __div64_32(uint64_t *n, uint32_t base);\r
 \r
+#include "../lib/panic.h"\r
+\r
 #define do_div __div64_32\r
 \r
 /*\r
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
old mode 100755 (executable)
new mode 100644 (file)
This page took 0.380904 seconds and 4 git commands to generate.