From 591f399b0e11f5ddf6f627b051c22c65f2b68eaa Mon Sep 17 00:00:00 2001 From: YuzukiTsuru Date: Sat, 3 Feb 2024 17:28:24 +0800 Subject: [PATCH] [board] support lt527x boot --- board/lt527x/start.S | 4 +- board/lt527x/syter_boot/bl33/syter_bl33.bin | Bin 0 -> 9500 bytes board/lt527x/syter_boot/main.c | 24 +- cmake/board/lt527x.cmake | 2 +- include/log.h | 2 +- scripts/genimage_t527.cfg | 33 + utils/bl33_t527/.gitignore | 2 + utils/bl33_t527/Makefile | 118 +++ utils/bl33_t527/include/byteorder.h | 84 ++ utils/bl33_t527/include/endian.h | 27 + utils/bl33_t527/include/io.h | 54 ++ utils/bl33_t527/include/linkage.h | 67 ++ utils/bl33_t527/include/rtc.h | 19 + utils/bl33_t527/include/stdarg.h | 34 + utils/bl33_t527/include/stddef.h | 49 + utils/bl33_t527/include/stdint.h | 31 + utils/bl33_t527/include/stdlib.h | 28 + utils/bl33_t527/include/string.h | 17 + utils/bl33_t527/include/types.h | 55 ++ utils/bl33_t527/include/xformat.h | 44 + utils/bl33_t527/link.ld | 27 + utils/bl33_t527/source/main.c | 67 ++ utils/bl33_t527/source/memcpy.S | 240 +++++ utils/bl33_t527/source/memset.S | 116 +++ utils/bl33_t527/source/printf.c | 83 ++ utils/bl33_t527/source/smccc-call.S | 9 + utils/bl33_t527/source/start.S | 85 ++ utils/bl33_t527/source/string.c | 31 + utils/bl33_t527/source/timer.c | 61 ++ utils/bl33_t527/source/xformat.c | 967 ++++++++++++++++++++ 30 files changed, 2369 insertions(+), 11 deletions(-) create mode 100755 board/lt527x/syter_boot/bl33/syter_bl33.bin create mode 100644 scripts/genimage_t527.cfg create mode 100644 utils/bl33_t527/.gitignore create mode 100644 utils/bl33_t527/Makefile create mode 100644 utils/bl33_t527/include/byteorder.h create mode 100644 utils/bl33_t527/include/endian.h create mode 100644 utils/bl33_t527/include/io.h create mode 100644 utils/bl33_t527/include/linkage.h create mode 100644 utils/bl33_t527/include/rtc.h create mode 100644 utils/bl33_t527/include/stdarg.h create mode 100644 utils/bl33_t527/include/stddef.h create mode 100644 utils/bl33_t527/include/stdint.h create mode 100644 utils/bl33_t527/include/stdlib.h create mode 100644 utils/bl33_t527/include/string.h create mode 100644 utils/bl33_t527/include/types.h create mode 100644 utils/bl33_t527/include/xformat.h create mode 100644 utils/bl33_t527/link.ld create mode 100644 utils/bl33_t527/source/main.c create mode 100644 utils/bl33_t527/source/memcpy.S create mode 100644 utils/bl33_t527/source/memset.S create mode 100644 utils/bl33_t527/source/printf.c create mode 100644 utils/bl33_t527/source/smccc-call.S create mode 100644 utils/bl33_t527/source/start.S create mode 100644 utils/bl33_t527/source/string.c create mode 100644 utils/bl33_t527/source/timer.c create mode 100644 utils/bl33_t527/source/xformat.c diff --git a/board/lt527x/start.S b/board/lt527x/start.S index 67a8921f..fd5bfc1e 100644 --- a/board/lt527x/start.S +++ b/board/lt527x/start.S @@ -77,8 +77,8 @@ _start: .long __spl_size /* spl size */ .long 0x30 /* boot header size */ .long 0x30303033 /* boot header version */ - .long 0x00020000 /* return value */ - .long 0x00020000 /* run address */ + .long 0x00044000 /* return value */ + .long 0x00044000 /* run address */ .long 0x0 /* eGON version */ .byte 0x00, 0x00, 0x00, 0x00 /* platform information - 8byte */ .byte 0x34, 0x2e, 0x30, 0x00 diff --git a/board/lt527x/syter_boot/bl33/syter_bl33.bin b/board/lt527x/syter_boot/bl33/syter_bl33.bin new file mode 100755 index 0000000000000000000000000000000000000000..dcca7039fef6ffbceefdd154a1ee3602cc55312b GIT binary patch literal 9500 zcmeHNe{dAneSdHF?sOK1w8sgo$P~Rp*ujc~S0JehCY}QcOfYUw5(dY?_6dV6*Q%`$ z(NR0h_)Z5Hdn6D7rykd`mtPr=%XCEROlQ*3%<;pH2a62YamRJ*&c0opBP5D1KblfE z_Vx4K+f^<&_N4#yj~qPS+xNcj`@X-w-?vLXpPy!|Tvlt$s$vaxHOoXIEE&nLq|I`- zORYS7O)1UTS~v{?NWt{b){0HdJ+b`j6v9ueTdh|C7(ZwlFi#5}6(=lsl-+ zV{Dq`gzeK0P5NSJakKJa%~r&!FiUj58Q#mV{&DQ+MU>kUpHRu&p-W&I9fQH!n{D4&%&;5a{M+X7Xx2 z51zx;`Fy@q)y_bUTWzAJma)=K#x4;5w|Kl0zSzHMA+biUWNf9W)?dl7(^JbD4N1JS z&eK!ARcX1T&5-_$d*rQ~3>nzESKhYWkR?6)F_n^Vu8foABTC)tK$!2}VbGDbZN-)nWK{$dZLl`dhqwqy31R7 z(OmnMNShs7?6b>73~~2xR%J2ske)K5Uoqqe-)3HS<%bwDGkaDM9p1T+>F3U6ji41W zjDQBBV?zJ!XUbPJUsm0DwG#3gVa#OOA(OG=R@oh$R~6PSRsE6)8>=O_Uf2?g!Dp-E zS2N%6+lPIssXFk+=itlA+plKo{m_SDKM{bvz|V^39>ts=y!w#auIKYBdyz}YR*PXC z1PtX^Z=Pr*d|QglZd#*JYeQkv6F0F9yU}83-Duhz zwjo!_Hu%tLS=`oHY+k8uL>jfx`9Xcuf0zr+Cl31>#gIR!5BVF}(0tB5$uhu_d>_|8 z%)SqvH25sX8Hi!iQ=tNG!hgMJOwTvA;M~GorQ6f`QjDE5AQK(3Xhfs)V(9D8R-p;3 zb%Q_nNL33P&Gy0XatFt!*07k>3|IzQ+F=uoVYZotD4EE6;;6iOy*fZeHQGBfvLs_K`wha60 zL!NC!9E|lM7UKC8&PwaF(JW^j5^z6IG~;xMmO^aVg8a0`gbihrLbd*Zvk$bykky(T za`3DGo*-g{j~Q}#e67W`>)Akjt$8QnkDqOkuqo73TO3}Hv)^sOnL*?PrS}=LrFSUX zL+2(&9o&_1qGhvPjyxXd-HWq3Y)&|P@3wBEx4}YY$U79iP&$@b6v!0g3MJ-wK)8K4r~y4!@dbU zLk~HX3dj+6t5Oa2Ky%~R7t_dhN3+n)bP)3d;}eG_HQ)|nO?CEn*fH26@_DI~W1+vF zp_XQ1o5k-;<4#ua|JBP>%Zc&AP}{^sKWJ2GA1{sA0m%DNeMqTQ=v=N1 zSd};@JU3$h3iOcelD(4sZgORzxUwMH6jvU=J6~TMh`he!V)!3Dd1Bu>F=SPO9+Hpe z7e01qavi(q@YCeNhIG^kzz3hpmLQ*mIIAkr&cO~K*Z5HOeR0OjiHnkC(9u1|UbJJq zJ@XyEZ9d&1Ug1-oXpKR(bT&{*5>rx`Hl;vd~u+vB`c6 zah>dsXi!LhklQlCtF=1%W8xh0z-V@95;}!E3poKc3z}|$&M@c1y11t+>?Qjy)~POx z2<&D)bQdB0u6AKn#61o0q^>G^Mq#&rIfqUV3cxG|3SnxOc4pUsnqs}x_8>#YFJ z{!M)CpN+4?R|#VJjj^6$Zu+zQg@d4l{J9G~N37~*A7;O%8EU;|M0Cg)cfeBeSEikH z{GR-uYblHTpK#Y{@IUNTk)x2q+_=`^#;Dz-X_Wjr!1Wune zGN}Pq5NG`yYn1cIUfVR(V}RpY+G@me40MxBslK@rdr9{cBm02=bv)a|(aRHq0^3*qG(3I%@XfUQBl^r7M7% zXCL&EL=26Zr(&DTP`oeWt4f^#%v*ptLv%6|wiDZAJ&1Q-&8!die&byw%-;db`+&L3 zV%sf&^(w|*3&oCDA;49{_E;jm-O`#2srw8W_9vwh<(3jYV;?UcHXACAnQ8N7dAvNs zJ$gcfhxXw8mJVLRKI{o|i{P1d)VdMgfz2L}I^e@iY1x2t2)@CW0p8^2@H5tE!`G{n zE^Z|m-W6&`O&$n?w>Aa!SZP2@A;&b)JvFR!sn&oP!=3avU=Uy9YYQ-X+PU9C$Fk;u zkV$%`x+9esmg&SuI13vlSrQ$PlkM?F>#$`GKi~wXVGrSeQmXi)!xA#90xpSKq)L5s zgkZyf4F~Ai#rJeY$}T5;6yc@0yKYX+o}+}LIG0T;4Kegka~bez~=$deEB!3KByJn%U=O_^lor-wzq3 z6b{)fK&*b#u%inGi5Jviy%y7?rSOQYHXE|0*_{_Vk(+Ic=y)7(khvwyU3g|cocCkY z-~NJ*BW1&8=ls(q=$IC$VTF0XiHQ-wD4D^UPQab3K5Z)PuqDPGV42#b9O(b}$CMVIQz- z;H^PCtz~$(UqN?N-B1xrz^3!%<(%l+~aUygU8}-V6RGwp)(xDM{H-v3Hg~$Ioa@DfpNlana+Q ze5m-De1P1E{qY6RRuqUKEtUR51y|*g7Z?#@!SH~2GaG@6?$VbRmKnq9R5JvqH z=Ifjo%W*DOZcfpej$RoI5Ok=(3EJ>Z_8!At4!fY(i+Ot=`1&FkrCo!aCDst!ZonlG z8_tGtzoNWWX2^Wwym@)wibz=`I*<6vFL{UJ2*;ca8fMQdyB9Sc*)HPXp8*dS!xT>( zdWa6P`{FsJJq-JYox6VJ(HwnyOs+x9QiWlkI)gYR2IkH7(E(HF+br!I^h!Ebp?}zr z0>7ra!5?zgA9B|Jr=0a=_V|!fL$#Tz88uNiNy;}H5oaj3@D9VSS-w{;#y2o!i6NsM zd!@j)HMX7VfHF(PdlrRl=AXt+Sn9?-btleaTuK|B`)J3z*N#xfDA)40+o_du3o7@^phCkKL7)c%Lfk z&W_1{rMnO66U2ln=F}#j?~Gx;M{xu5m~(QFSL?Or^UtZ7`%ERXnHA@@Ua#JhWma^- z0eLERXRlujCeye3(-R{7WWaSW=W$*45nBt={+9RF%;WL zU)|W_ttGMM)k*8IClwo(xN}sc<0JAR^!-?8854JU#J~(Fi|9(e%u4RSKYQ^ZS~@{Y>d+Z7h)=gN)fxpK3C`0Nu0+ zIlGoZ-UFUm!cRTo$Si);)*FiP6Yk=@gqP%vPd|;ZD?o zbhjt{DM>@F0v@Vy*l^*#-$M7q%s0&i_zo-*8?DObv=nBKRas}q7WkvWHbLL-p%#X| zfDbk6O6bB?P>X4ZNBW|aq*$V%c4PRz0@bk1V*>ts$AApsyyH6FX@L)dCpaJc&mTW& za@>_z{G`Q`_v4)tz01To!|$1RS5-R;gSG3hM;{wD8#fwq{-5lTP3!RgHBg&2ZN)Qa zt^mCi6NYRAtsxdS@qZRT>lRyQ&s65_F35@WputW_zNB05Fe6x!V&XJ-A-&-LpA=vh zqQ>FDxGjRHX|>g;X)*Spro~u;jSB@eEyh7{(84#QpTeh1!tYZRh2samb$)uo@)eCu z&41kT^{#D?Y~S%HO)h*X1x(W)+Eb54J*Mj&V8Zp=@W5G^ zqZ4Rc=xId1!W?=Be}PJa5}kk9w_~SRvvI|WC}w+}>e?=z*s=4; z9X&$tzg2JP7ky8OuCASrbT>DOKYwcbj%CZ1d2rkNwz)tw>EP__>)P2T?r{)!li$9z esb|-+TlMCiT@QZyn-2<-o%iehb^d8Gg#QJ+3E9K| literal 0 HcmV?d00001 diff --git a/board/lt527x/syter_boot/main.c b/board/lt527x/syter_boot/main.c index 7095a081..c1ed8ffc 100644 --- a/board/lt527x/syter_boot/main.c +++ b/board/lt527x/syter_boot/main.c @@ -38,11 +38,14 @@ #define CONFIG_BL31_FILENAME "bl31.bin" #define CONFIG_BL31_LOAD_ADDR (0x48000000) -#define CONFIG_DTB_FILENAME "sunxi.fex" -#define CONFIG_DTB_LOAD_ADDR (0x41f00000) +#define CONFIG_DTB_FILENAME "sunxi.dtb" +#define CONFIG_DTB_LOAD_ADDR (0x4a200000) #define CONFIG_KERNEL_FILENAME "Image" -#define CONFIG_KERNEL_LOAD_ADDR (0x4007f800) +#define CONFIG_KERNEL_LOAD_ADDR (0x40080000) + +#define CONFIG_BL33_FILENAME "syter_bl33.bin" +#define CONFIG_BL33_LOAD_ADDR (0x4a000000) #define CONFIG_SDMMC_SPEED_TEST_SIZE 1024// (unit: 512B sectors) @@ -88,9 +91,8 @@ typedef struct { uint8_t *of_dest; char of_filename[FILENAME_MAX_LEN]; - uint8_t *config_dest; - uint8_t is_config; - char config_filename[FILENAME_MAX_LEN]; + uint8_t *bl33_dest; + char bl33_filename[FILENAME_MAX_LEN]; } image_info_t; image_info_t image; @@ -180,6 +182,11 @@ static int load_sdcard(image_info_t *image) { if (ret) return ret; + printk(LOG_LEVEL_INFO, "FATFS: read %s addr=%x\n", image->bl33_filename, (uint32_t) image->bl33_dest); + ret = fatfs_loadimage(image->bl33_filename, image->bl33_dest); + if (ret) + return ret; + /* umount fs */ fret = f_mount(0, "", 0); if (fret != FR_OK) { @@ -247,7 +254,7 @@ msh_define_help(boot, "boot to linux", "Usage: boot\n"); int cmd_boot(int argc, const char **argv) { atf_head_t *atf_head = (atf_head_t *) image.bl31_dest; - atf_head->next_boot_base = CONFIG_KERNEL_LOAD_ADDR; + atf_head->next_boot_base = CONFIG_BL33_LOAD_ADDR; atf_head->dtb_base = CONFIG_DTB_LOAD_ADDR; atf_head->platform[0] = 0x00; @@ -348,10 +355,13 @@ int main(void) { image.bl31_dest = (uint8_t *) CONFIG_BL31_LOAD_ADDR; image.of_dest = (uint8_t *) CONFIG_DTB_LOAD_ADDR; image.kernel_dest = (uint8_t *) CONFIG_KERNEL_LOAD_ADDR; + image.bl33_dest = (uint8_t *) CONFIG_BL33_LOAD_ADDR; strcpy(image.bl31_filename, CONFIG_BL31_FILENAME); strcpy(image.of_filename, CONFIG_DTB_FILENAME); strcpy(image.kernel_filename, CONFIG_KERNEL_FILENAME); + strcpy(image.bl33_filename, CONFIG_BL33_FILENAME); + /* Initialize the SD host controller. */ if (sunxi_sdhci_init(&sdhci0) != 0) { printk(LOG_LEVEL_ERROR, "SMHC: %s controller init failed\n", sdhci0.name); diff --git a/cmake/board/lt527x.cmake b/cmake/board/lt527x.cmake index 2e0f21ac..fb14bd01 100644 --- a/cmake/board/lt527x.cmake +++ b/cmake/board/lt527x.cmake @@ -30,7 +30,7 @@ set(CMAKE_COMMON_FLAGS "-nostdlib -Os -mcpu=cortex-a55") set(CMAKE_C_DISABLE_WARN_FLAGS "-Wno-int-to-pointer-cast -Wno-implicit-function-declaration -Wno-discarded-qualifiers") set(CMAKE_CXX_DISABLE_WARN_FLAGS "-Wno-int-to-pointer-cast") -set(ARCH_BIN_START_ADDRESS "0x00020000") +set(ARCH_BIN_START_ADDRESS "0x00044000") set(ARCH_BIN_SRAM_LENGTH "128K") set(ARCH_FEL_START_ADDRESS "0x00020000") diff --git a/include/log.h b/include/log.h index 46b46d21..2ed2114a 100644 --- a/include/log.h +++ b/include/log.h @@ -28,7 +28,7 @@ enum { }; #ifndef LOG_LEVEL_DEFAULT -#define LOG_LEVEL_DEFAULT LOG_LEVEL_DEBUG +#define LOG_LEVEL_DEFAULT LOG_LEVEL_INFO #endif void set_timer_count(); diff --git a/scripts/genimage_t527.cfg b/scripts/genimage_t527.cfg new file mode 100644 index 00000000..ab28f48e --- /dev/null +++ b/scripts/genimage_t527.cfg @@ -0,0 +1,33 @@ +image boot.vfat { + vfat { + files = { + "Image", + "sunxi.dtb", + "../board/lt527x/syter_boot/bl31/bl31.bin", + "../utils/bl33/output/syter_bl33.bin" + } + } + size = 128M +} + +image sdcard.img { + hdimage {} + + partition boot0 { + in-partition-table = "no" + image = "../build/board/lt527x/syter_boot/syter_boot_bin_card.bin" + offset = 8K + } + + partition boot0-gpt { + in-partition-table = "no" + image = "../build/board/lt527x/syter_boot/syter_boot_bin_card.bin" + offset = 128K + } + + partition kernel { + partition-type = 0xC + bootable = "true" + image = "boot.vfat" + } +} diff --git a/utils/bl33_t527/.gitignore b/utils/bl33_t527/.gitignore new file mode 100644 index 00000000..2e75a798 --- /dev/null +++ b/utils/bl33_t527/.gitignore @@ -0,0 +1,2 @@ +output/ +.obj/ \ No newline at end of file diff --git a/utils/bl33_t527/Makefile b/utils/bl33_t527/Makefile new file mode 100644 index 00000000..30ad0b67 --- /dev/null +++ b/utils/bl33_t527/Makefile @@ -0,0 +1,118 @@ +# +# Top makefile +# + +CROSS ?= arm-none-eabi- +NAME := syter_bl33 + +# +# System environment variable. +# +ifeq ($(OS), Windows_NT) + HOSTOS := windows +else + ifneq (,$(findstring Linux, $(shell uname -a))) + HOSTOS := linux + endif +endif + +# +# Load default variables. +# +ASFLAGS := -g -ggdb -Wall -O3 +CFLAGS := -g -ggdb -Wall -O3 +CXXFLAGS := -g -ggdb -Wall -O3 +LDFLAGS := -T link.ld -nostdlib +ARFLAGS := -rcs +OCFLAGS := -v -O binary +ODFLAGS := +MCFLAGS := -nostdinc -nostdlib -mabi=aapcs-linux -march=armv7-a -Wno-builtin-declaration-mismatch -Wno-int-to-pointer-cast -Wno-pointer-to-int-cast + +LIBDIRS := +LIBS := +INCDIRS := +SRCDIRS := + +# +# Add external library +# +INCDIRS += include +SRCDIRS += source + +# +# You shouldn't need to change anything below this point. +# +AS := $(CROSS)gcc -x assembler-with-cpp +CC := $(CROSS)gcc +CXX := $(CROSS)g++ +LD := $(CROSS)ld +AR := $(CROSS)ar +OC := $(CROSS)objcopy +OD := $(CROSS)objdump +MKDIR := mkdir -p +CP := cp -af +RM := rm -fr +CD := cd +FIND := find + +# +# X variables +# +G_ASFLAGS := $(MCFLAGS) $(ASFLAGS) +G_CFLAGS := $(MCFLAGS) $(CFLAGS) +G_CXXFLAGS := $(MCFLAGS) $(CXXFLAGS) +G_LDFLAGS := $(LDFLAGS) +G_OCFLAGS := $(OCFLAGS) +G_LIBDIRS := $(LIBDIRS) +G_LIBS := $(LIBS) -lgcc + +G_OUT := output +G_NAME := $(patsubst %, $(G_OUT)/%, $(NAME)) +G_INCDIRS := $(patsubst %, -I %, $(INCDIRS)) +G_SRCDIRS := $(patsubst %, %, $(SRCDIRS)) +G_OBJDIRS := $(patsubst %, .obj/%, $(G_SRCDIRS)) + +G_SFILES := $(foreach dir, $(G_SRCDIRS), $(wildcard $(dir)/*.S)) +G_CFILES := $(foreach dir, $(G_SRCDIRS), $(wildcard $(dir)/*.c)) +G_CPPFILES := $(foreach dir, $(G_SRCDIRS), $(wildcard $(dir)/*.cpp)) + +G_SDEPS := $(patsubst %, .obj/%, $(G_SFILES:.S=.o.d)) +G_CDEPS := $(patsubst %, .obj/%, $(G_CFILES:.c=.o.d)) +G_CPPDEPS := $(patsubst %, .obj/%, $(G_CPPFILES:.cpp=.o.d)) +G_DEPS := $(G_SDEPS) $(G_CDEPS) $(G_CPPDEPS) + +G_SOBJS := $(patsubst %, .obj/%, $(G_SFILES:.S=.o)) +G_COBJS := $(patsubst %, .obj/%, $(G_CFILES:.c=.o)) +G_CPPOBJS := $(patsubst %, .obj/%, $(G_CPPFILES:.cpp=.o)) +G_OBJS := $(G_SOBJS) $(G_COBJS) $(G_CPPOBJS) + +VPATH := $(G_OBJDIRS) + +.PHONY: all clean +all : $(G_NAME) + +$(G_NAME) : $(G_OBJS) + @echo [LD] Linking $@.elf + @$(CC) $(G_LDFLAGS) $(G_LIBDIRS) -Wl,--cref,-Map=$@.map $^ -o $@.elf $(G_LIBS) + @echo [OC] Objcopying $@.bin + @$(OC) $(G_OCFLAGS) $@.elf $@.bin + +$(G_SOBJS) : .obj/%.o : %.S + @echo [AS] $< + @$(AS) $(G_ASFLAGS) -MD -MP -MF $@.d $(G_INCDIRS) -c $< -o $@ + +$(G_COBJS) : .obj/%.o : %.c + @echo [CC] $< + @$(CC) $(G_CFLAGS) -MD -MP -MF $@.d $(G_INCDIRS) -c $< -o $@ + +$(G_CPPOBJS) : .obj/%.o : %.cpp + @echo [CXX] $< + @$(CXX) $(G_CXXFLAGS) -MD -MP -MF $@.d $(G_INCDIRS) -c $< -o $@ + +clean: + @$(RM) .obj $(G_OUT) + +# +# Include the dependency files, should be place the last of makefile +# +sinclude $(shell $(MKDIR) $(G_OBJDIRS) $(G_OUT)) $(G_DEPS) diff --git a/utils/bl33_t527/include/byteorder.h b/utils/bl33_t527/include/byteorder.h new file mode 100644 index 00000000..52bb8e60 --- /dev/null +++ b/utils/bl33_t527/include/byteorder.h @@ -0,0 +1,84 @@ +#ifndef __BYTEORDER_H__ +#define __BYTEORDER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +static inline u16_t __swab16(u16_t x) +{ + return ( (x<<8) | (x>>8) ); +} + +static inline u32_t __swab32(u32_t x) +{ + return ( (x<<24) | (x>>24) | \ + ((x & (u32_t)0x0000ff00UL)<<8) | \ + ((x & (u32_t)0x00ff0000UL)>>8) ); +} + +static inline u64_t __swab64(u64_t x) +{ + return ( (x<<56) | (x>>56) | \ + ((x & (u64_t)0x000000000000ff00ULL)<<40) | \ + ((x & (u64_t)0x0000000000ff0000ULL)<<24) | \ + ((x & (u64_t)0x00000000ff000000ULL)<< 8) | \ + ((x & (u64_t)0x000000ff00000000ULL)>> 8) | \ + ((x & (u64_t)0x0000ff0000000000ULL)>>24) | \ + ((x & (u64_t)0x00ff000000000000ULL)>>40) ); +} + +/* + * swap bytes bizarrely. + * swahw32 - swap 16-bit half-words in a 32-bit word + */ +static inline u32_t __swahw32(u32_t x) +{ + return ( ((x & (u32_t)0x0000ffffUL)<<16) | ((x & (u32_t)0xffff0000UL)>>16) ); +} + +/* + * swap bytes bizarrely. + * swahb32 - swap 8-bit halves of each 16-bit half-word in a 32-bit word + */ +static inline u32_t __swahb32(u32_t x) +{ + return ( ((x & (u32_t)0x00ff00ffUL)<<8) | ((x & (u32_t)0xff00ff00UL)>>8) ); +} + +#if (BYTE_ORDER == BIG_ENDIAN) +#define cpu_to_le64(x) (__swab64((u64_t)(x))) +#define le64_to_cpu(x) (__swab64((u64_t)(x))) +#define cpu_to_le32(x) (__swab32((u32_t)(x))) +#define le32_to_cpu(x) (__swab32((u32_t)(x))) +#define cpu_to_le16(x) (__swab16((u16_t)(x))) +#define le16_to_cpu(x) (__swab16((u16_t)(x))) +#define cpu_to_be64(x) ((u64_t)(x)) +#define be64_to_cpu(x) ((u64_t)(x)) +#define cpu_to_be32(x) ((u32_t)(x)) +#define be32_to_cpu(x) ((u32_t)(x)) +#define cpu_to_be16(x) ((u16_t)(x)) +#define be16_to_cpu(x) ((u16_t)(x)) +#else +#define cpu_to_le64(x) ((u64_t)(x)) +#define le64_to_cpu(x) ((u64_t)(x)) +#define cpu_to_le32(x) ((u32_t)(x)) +#define le32_to_cpu(x) ((u32_t)(x)) +#define cpu_to_le16(x) ((u16_t)(x)) +#define le16_to_cpu(x) ((u16_t)(x)) +#define cpu_to_be64(x) (__swab64((u64_t)(x))) +#define be64_to_cpu(x) (__swab64((u64_t)(x))) +#define cpu_to_be32(x) (__swab32((u32_t)(x))) +#define be32_to_cpu(x) (__swab32((u32_t)(x))) +#define cpu_to_be16(x) (__swab16((u16_t)(x))) +#define be16_to_cpu(x) (__swab16((u16_t)(x))) +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __BYTEORDER_H__ */ diff --git a/utils/bl33_t527/include/endian.h b/utils/bl33_t527/include/endian.h new file mode 100644 index 00000000..7e74f435 --- /dev/null +++ b/utils/bl33_t527/include/endian.h @@ -0,0 +1,27 @@ +#ifndef __ARM32_ENDIAN_H__ +#define __ARM32_ENDIAN_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define LITTLE_ENDIAN (0x1234) +#define BIG_ENDIAN (0x4321) + +#if ( !defined(__LITTLE_ENDIAN) && !defined(__BIG_ENDIAN) ) +#define __LITTLE_ENDIAN +#endif + +#if defined(__LITTLE_ENDIAN) +#define BYTE_ORDER LITTLE_ENDIAN +#elif defined(__BIG_ENDIAN) +#define BYTE_ORDER BIG_ENDIAN +#else +#error "Unknown byte order!" +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* __ARM32_ENDIAN_H__ */ diff --git a/utils/bl33_t527/include/io.h b/utils/bl33_t527/include/io.h new file mode 100644 index 00000000..fbf25624 --- /dev/null +++ b/utils/bl33_t527/include/io.h @@ -0,0 +1,54 @@ +#ifndef __IO_H__ +#define __IO_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +static inline u8_t read8(virtual_addr_t addr) +{ + return( *((volatile u8_t *)(addr)) ); +} + +static inline u16_t read16(virtual_addr_t addr) +{ + return( *((volatile u16_t *)(addr)) ); +} + +static inline u32_t read32(virtual_addr_t addr) +{ + return( *((volatile u32_t *)(addr)) ); +} + +static inline u64_t read64(virtual_addr_t addr) +{ + return( *((volatile u64_t *)(addr)) ); +} + +static inline void write8(virtual_addr_t addr, u8_t value) +{ + *((volatile u8_t *)(addr)) = value; +} + +static inline void write16(virtual_addr_t addr, u16_t value) +{ + *((volatile u16_t *)(addr)) = value; +} + +static inline void write32(virtual_addr_t addr, u32_t value) +{ + *((volatile u32_t *)(addr)) = value; +} + +static inline void write64(virtual_addr_t addr, u64_t value) +{ + *((volatile u64_t *)(addr)) = value; +} + +#ifdef __cplusplus +} +#endif + +#endif /* __IO_H__ */ diff --git a/utils/bl33_t527/include/linkage.h b/utils/bl33_t527/include/linkage.h new file mode 100644 index 00000000..f44139d3 --- /dev/null +++ b/utils/bl33_t527/include/linkage.h @@ -0,0 +1,67 @@ +/* SPDX-License-Identifier: Apache-2.0 */ + +#ifndef __ARM32_LINKAGE_H__ +#define __ARM32_LINKAGE_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define ALIGN .align 0 +#define ALIGN_STR ".align 0" + +#define ENTRY(name) \ + .globl name; \ + ALIGN; \ + name: + +#define WEAK(name) \ + .weak name; \ + name: + +#define END(name) .size name, .- name + +#define ENDPROC(name) \ + .type name, % function; \ + END(name) + +/* + * CR1 bits (CP#15 CR1) + */ +#define CR_M (1 << 0) /* MMU enable */ +#define CR_A (1 << 1) /* Alignment abort enable */ +#define CR_C (1 << 2) /* Dcache enable */ +#define CR_W (1 << 3) /* Write buffer enable */ +#define CR_P (1 << 4) /* 32-bit exception handler */ +#define CR_D (1 << 5) /* 32-bit data address range */ +#define CR_L (1 << 6) /* Implementation defined */ +#define CR_B (1 << 7) /* Big endian */ +#define CR_S (1 << 8) /* System MMU protection */ +#define CR_R (1 << 9) /* ROM MMU protection */ +#define CR_F (1 << 10) /* Implementation defined */ +#define CR_Z (1 << 11) /* Implementation defined */ +#define CR_I (1 << 12) /* Icache enable */ +#define CR_V (1 << 13) /* Vectors relocated to 0xffff0000 */ +#define CR_RR (1 << 14) /* Round Robin cache replacement */ +#define CR_L4 (1 << 15) /* LDR pc can set T bit */ +#define CR_DT (1 << 16) +#define CR_IT (1 << 18) +#define CR_ST (1 << 19) +#define CR_FI (1 << 21) /* Fast interrupt (lower latency mode) */ +#define CR_U (1 << 22) /* Unaligned access operation */ +#define CR_XP (1 << 23) /* Extended page tables */ +#define CR_VE (1 << 24) /* Vectored interrupts */ +#define CR_EE (1 << 25) /* Exception (Big) Endian */ +#define CR_TRE (1 << 28) /* TEX remap enable */ +#define CR_AFE (1 << 29) /* Access flag enable */ +#define CR_TE (1 << 30) /* Thumb exception enable */ + +/* ARM relocs */ +#define R_ARM_NONE 0 /* No reloc */ +#define R_ARM_RELATIVE 23 /* Adjust by program base */ + +#ifdef __cplusplus +} +#endif + +#endif /* __ARM32_LINKAGE_H__ */ diff --git a/utils/bl33_t527/include/rtc.h b/utils/bl33_t527/include/rtc.h new file mode 100644 index 00000000..00642272 --- /dev/null +++ b/utils/bl33_t527/include/rtc.h @@ -0,0 +1,19 @@ +#ifndef __RTC_H__ +#define __RTC_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define SUNXI_RTC_BASE (0x07090000) +#define SUNXI_RTC_DATA_BASE (SUNXI_RTC_BASE + 0x100) + +#define RTC_FEL_INDEX 2 + +void set_timer_count(); + +#ifdef __cplusplus +} +#endif + +#endif /* __RTC_H__ */ diff --git a/utils/bl33_t527/include/stdarg.h b/utils/bl33_t527/include/stdarg.h new file mode 100644 index 00000000..247ccc6c --- /dev/null +++ b/utils/bl33_t527/include/stdarg.h @@ -0,0 +1,34 @@ +#ifndef __STDARG_H__ +#define __STDARG_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef __builtin_va_list va_list; + +/* + * prepare to access variable args + */ +#define va_start(v, l) __builtin_va_start(v, l) + +/* + * the caller will get the value of current argument + */ +#define va_arg(v, l) __builtin_va_arg(v, l) + +/* + * end for variable args + */ +#define va_end(v) __builtin_va_end(v) + +/* + * copy variable args + */ +#define va_copy(d, s) __builtin_va_copy(d, s) + +#ifdef __cplusplus +} +#endif + +#endif /* __STDARG_H__ */ diff --git a/utils/bl33_t527/include/stddef.h b/utils/bl33_t527/include/stddef.h new file mode 100644 index 00000000..ca3874cd --- /dev/null +++ b/utils/bl33_t527/include/stddef.h @@ -0,0 +1,49 @@ +#ifndef __STDDEF_H__ +#define __STDDEF_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(__cplusplus) +#define NULL (0) +#else +#define NULL ((void *)0) +#endif + +#if (defined(__GNUC__) && (__GNUC__ >= 4)) +#define offsetof(type, member) __builtin_offsetof(type, member) +#else +#define offsetof(type, field) ((size_t)(&((type *)0)->field)) +#endif +#define container_of(ptr, type, member) ({const typeof(((type *)0)->member) *__mptr = (ptr); (type *)((char *)__mptr - offsetof(type,member));}) + +#if (defined(__GNUC__) && (__GNUC__ >= 3)) +#define likely(expr) (__builtin_expect(!!(expr), 1)) +#define unlikely(expr) (__builtin_expect(!!(expr), 0)) +#else +#define likely(expr) (!!(expr)) +#define unlikely(expr) (!!(expr)) +#endif + +#define min(a, b) ({typeof(a) _amin = (a); typeof(b) _bmin = (b); (void)(&_amin == &_bmin); _amin < _bmin ? _amin : _bmin;}) +#define max(a, b) ({typeof(a) _amax = (a); typeof(b) _bmax = (b); (void)(&_amax == &_bmax); _amax > _bmax ? _amax : _bmax;}) +#define clamp(v, a, b) min(max(a, v), b) + +#define ifloor(x) ((x) > 0 ? (int)(x) : (int)((x) - 0.9999999999)) +#define iround(x) ((x) > 0 ? (int)((x) + 0.5) : (int)((x) - 0.5)) +#define iceil(x) ((x) > 0 ? (int)((x) + 0.9999999999) : (int)(x)) +#define idiv255(x) ((((int)(x) + 1) * 257) >> 16) + +#define X(...) ("" #__VA_ARGS__ "") + +enum { + FALSE = 0, + TRUE = 1, +}; + +#ifdef __cplusplus +} +#endif + +#endif /* __STDDEF_H__ */ diff --git a/utils/bl33_t527/include/stdint.h b/utils/bl33_t527/include/stdint.h new file mode 100644 index 00000000..328cfe22 --- /dev/null +++ b/utils/bl33_t527/include/stdint.h @@ -0,0 +1,31 @@ +#ifndef __STDINT_H__ +#define __STDINT_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef s8_t int8_t; +typedef u8_t uint8_t; + +typedef s16_t int16_t; +typedef u16_t uint16_t; + +typedef s32_t int32_t; +typedef u32_t uint32_t; + +typedef s64_t int64_t; +typedef u64_t uint64_t; + +#define UINT8_MAX (0xff) +#define UINT16_MAX (0xffff) +#define UINT32_MAX (0xffffffff) +#define UINT64_MAX (0xffffffffffffffffULL) + +#ifdef __cplusplus +} +#endif + +#endif /* __STDINT_H__ */ diff --git a/utils/bl33_t527/include/stdlib.h b/utils/bl33_t527/include/stdlib.h new file mode 100644 index 00000000..b4cc6f44 --- /dev/null +++ b/utils/bl33_t527/include/stdlib.h @@ -0,0 +1,28 @@ +#ifndef __STDLIB_H__ +#define __STDLIB_H__ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void sunxi_uart_init(uint32_t base); + +void sunxi_uart_putc(char c); + +void uart_printf(const char *fmt, ...); + +void printf(const char *fmt, ...); + +uint32_t time_ms(void); + +uint64_t time_us(void); + +void mdelay(uint32_t ms); + +#ifdef __cplusplus +} +#endif + +#endif /* __STDLIB_H__ */ diff --git a/utils/bl33_t527/include/string.h b/utils/bl33_t527/include/string.h new file mode 100644 index 00000000..cab76dee --- /dev/null +++ b/utils/bl33_t527/include/string.h @@ -0,0 +1,17 @@ +#ifndef __STRING_H__ +#define __STRING_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +void * memset(void * s, int c, size_t n); +void * memcpy(void * dest, const void * src, size_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* __STRING_H__ */ diff --git a/utils/bl33_t527/include/types.h b/utils/bl33_t527/include/types.h new file mode 100644 index 00000000..acc22346 --- /dev/null +++ b/utils/bl33_t527/include/types.h @@ -0,0 +1,55 @@ +#ifndef __ARM32_TYPES_H__ +#define __ARM32_TYPES_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef signed char s8_t; +typedef unsigned char u8_t; + +typedef signed short s16_t; +typedef unsigned short u16_t; + +typedef signed int s32_t; +typedef unsigned int u32_t; + +typedef signed long long s64_t; +typedef unsigned long long u64_t; + +typedef signed long long intmax_t; +typedef unsigned long long uintmax_t; + +typedef signed int ptrdiff_t; +typedef signed int intptr_t; +typedef unsigned int uintptr_t; + +typedef unsigned int size_t; +typedef signed int ssize_t; + +typedef signed int off_t; +typedef signed long long loff_t; + +typedef signed int bool_t; +typedef unsigned int irq_flags_t; + +typedef unsigned int virtual_addr_t; +typedef unsigned int virtual_size_t; +typedef unsigned int physical_addr_t; +typedef unsigned int physical_size_t; + +typedef unsigned long smc_call_arg_t; + +typedef struct { + volatile int counter; +} atomic_t; + +typedef struct { + volatile int lock; +} spinlock_t; + +#ifdef __cplusplus +} +#endif + +#endif /* __ARM32_TYPES_H__ */ diff --git a/utils/bl33_t527/include/xformat.h b/utils/bl33_t527/include/xformat.h new file mode 100644 index 00000000..8ee08af4 --- /dev/null +++ b/utils/bl33_t527/include/xformat.h @@ -0,0 +1,44 @@ +/** + * @file xformatc.h + * + * @brief Printf C declaration. + * + * @author Mario Viara + * + * @version 1.01 + * + * @copyright Copyright Mario Viara 2014 - License Open Source (LGPL) + * This is a free software and is opened for education, research and commercial + * developments under license policy of following terms: + * - This is a free software and there is NO WARRANTY. + * - No restriction on use. You can use, modify and redistribute it for personal, + * non-profit or commercial product UNDER YOUR RESPONSIBILITY. + * - Redistributions of source code must retain the above copyright notice. + * + */ +#ifndef __XFORMAT_H__ +#define __XFORMAT_H__ + +#include +#include + +/** + * Define internal parameters as volatile for 8 bit cpu define + * XCFG_FORMAT_STATIC=static to reduce stack usage. + */ +#define XCFG_FORMAT_STATIC + +/** + * Define XCFG_FORMAT_FLOAT=0 to remove floating point support + */ +#define XCFG_FORMAT_FLOAT 1 + +/** + * Define to 0 to support long long type (prefix ll) + */ +#define XCFG_FORMAT_LONGLONG 0 + +unsigned xvformat(void (*outchar)(void *arg, char), void *arg, const char *fmt, va_list args); +unsigned xformat(void (*outchar)(void *arg, char), void *arg, const char *fmt, ...); + +#endif diff --git a/utils/bl33_t527/link.ld b/utils/bl33_t527/link.ld new file mode 100644 index 00000000..950de1e8 --- /dev/null +++ b/utils/bl33_t527/link.ld @@ -0,0 +1,27 @@ +OUTPUT_FORMAT("elf32-littlearm", "elf32-littlearm", "elf32-littlearm") +OUTPUT_ARCH(arm) +ENTRY(_start) +SECTIONS +{ + . = 0x4a000000; + . = ALIGN(4); + .text : + { + .obj/source/start.o (*.text) + *(.text*) + } + + . = ALIGN(4); + .rodata : { *(SORT_BY_ALIGNMENT(SORT_BY_NAME(.rodata*))) } + + . = ALIGN(4); + .data : { + *(.data*) + } + + .interp : { *(.interp*) } + .gnu.hash : { *(.gnu.hash) } + .gnu : { *(.gnu*) } + .ARM.exidx : { *(.ARM.exidx*) } + .gnu.linkonce.armexidx : { *(.gnu.linkonce.armexidx.*) } +} diff --git a/utils/bl33_t527/source/main.c b/utils/bl33_t527/source/main.c new file mode 100644 index 00000000..4695ce24 --- /dev/null +++ b/utils/bl33_t527/source/main.c @@ -0,0 +1,67 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define ARM_SVC_CALL_COUNT 0x8000ff00 +#define ARM_SVC_UID 0x8000ff01 +#define ARM_SVC_VERSION 0x8000ff03 +#define ARM_SVC_RUNNSOS 0x8000ff04 +#define PSCI_CPU_OFF 0x84000002 +#define PSCI_CPU_ON_AARCH32 0x84000003 +#define PSCI_SYSTEM_OFF 0x84000008 +#define SUNXI_CPU_ON_AARCH32 0x84000010 +#define SUNXI_CPU_OFF_AARCH32 0x84000011 +#define SUNXI_CPU_WFI_AARCH32 0x84000012 +/* arisc */ +#define ARM_SVC_ARISC_STARTUP 0x8000ff10 +#define ARM_SVC_ARISC_WAIT_READY 0x8000ff11 +#define ARM_SVC_ARISC_READ_PMU 0x8000ff12 +#define ARM_SVC_ARISC_WRITE_PMU 0x8000ff13 +#define ARM_SVC_ARISC_FAKE_POWER_OFF_REQ_ARCH32 0x83000019 +#define ARM_SVC_FAKE_POWER_OFF 0x8000ff14 +#define ARM_SVC_UBOOT_POWER_OFF 0x8000ff15 +/* efuse */ +#define ARM_SVC_EFUSE_READ (0x8000fe00) +#define ARM_SVC_EFUSE_WRITE (0x8000fe01) +#define ARM_SVC_EFUSE_PROBE_SECURE_ENABLE_AARCH32 (0x8000fe03) +#define ARM_SVC_EFUSE_CUSTOMER_RESERVED_HANDLE (0x8000fe05) + +#define CONFIG_DTB_LOAD_ADDR (0x4a200000) +#define CONFIG_KERNEL_LOAD_ADDR (0x40080000) + +extern uint32_t __sunxi_smc_call(smc_call_arg_t arg0, smc_call_arg_t arg1, smc_call_arg_t arg2, smc_call_arg_t arg3); + +uint32_t sunxi_smc_call_atf(smc_call_arg_t arg0, smc_call_arg_t arg1, smc_call_arg_t arg2, smc_call_arg_t arg3, smc_call_arg_t pResult) { + return __sunxi_smc_call(arg0, arg1, arg2, arg3); +} + +uint32_t arm_svc_run_os(smc_call_arg_t kernel, smc_call_arg_t fdt, smc_call_arg_t arg2) { + return sunxi_smc_call_atf(ARM_SVC_RUNNSOS, kernel, fdt, arg2, 0); +} + +void print_banner(void) { + printf(" _____ _ _____ __ ___ ___ \n"); + printf("| __|_ _| |_ ___ ___| __ | | |_ |_ |\n"); + printf("|__ | | | _| -_| _| __ -| |__|_ |_ |\n"); + printf("|_____|_ |_| |___|_| |_____|_____|___|___|\n"); + printf(" |___| \n"); + printf("\n"); + printf("Hello Syter BL33!\n"); + printf("load kernel 0x%08x to aarch64 mode...\n", CONFIG_KERNEL_LOAD_ADDR); + printf("load dtb 0x%08x...\n\n", CONFIG_DTB_LOAD_ADDR); + + printf("Start Kernel...\n\n"); + + mdelay(10); +} + + +void sys_boot() { + arm_svc_run_os(CONFIG_KERNEL_LOAD_ADDR, CONFIG_DTB_LOAD_ADDR, 1); +} \ No newline at end of file diff --git a/utils/bl33_t527/source/memcpy.S b/utils/bl33_t527/source/memcpy.S new file mode 100644 index 00000000..137c3459 --- /dev/null +++ b/utils/bl33_t527/source/memcpy.S @@ -0,0 +1,240 @@ + +#define pull lsr +#define push lsl +#define PLD(code...) +#define CALGN(code...) code + +#define W(instr) instr + +#define LDR1W_SHIFT 0 +#define STR1W_SHIFT 0 + + .macro ldr1w ptr reg abort + W(ldr) \reg, [\ptr], #4 + .endm + + .macro ldr4w ptr reg1 reg2 reg3 reg4 abort + ldmia \ptr!, {\reg1, \reg2, \reg3, \reg4} + .endm + + .macro ldr8w ptr reg1 reg2 reg3 reg4 reg5 reg6 reg7 reg8 abort + ldmia \ptr!, {\reg1, \reg2, \reg3, \reg4, \reg5, \reg6, \reg7, \reg8} + .endm + + .macro ldr1b ptr reg cond=al abort + ldr\cond\()b \reg, [\ptr], #1 + .endm + + .macro str1w ptr reg abort + W(str) \reg, [\ptr], #4 + .endm + + .macro str8w ptr reg1 reg2 reg3 reg4 reg5 reg6 reg7 reg8 abort + stmia \ptr!, {\reg1, \reg2, \reg3, \reg4, \reg5, \reg6, \reg7, \reg8} + .endm + + .macro str1b ptr reg cond=al abort + str\cond\()b \reg, [\ptr], #1 + .endm + + .macro enter reg1 reg2 + stmdb sp!, {r0, \reg1, \reg2} + .endm + + .macro exit reg1 reg2 + ldmfd sp!, {r0, \reg1, \reg2} + .endm + + .text + +/* Prototype: void *memcpy(void *dest, const void *src, size_t n); */ + +.globl memcpy0 +memcpy0: + + cmp r0, r1 + moveq pc, lr + + enter r4, lr + + subs r2, r2, #4 + blt 8f + ands ip, r0, #3 + PLD( pld [r1, #0] ) + bne 9f + ands ip, r1, #3 + bne 10f + +1: subs r2, r2, #(28) + stmfd sp!, {r5 - r8} + blt 5f + + CALGN( ands ip, r0, #31 ) + CALGN( rsb r3, ip, #32 ) + CALGN( sbcnes r4, r3, r2 ) @ C is always set here + CALGN( bcs 2f ) + CALGN( adr r4, 6f ) + CALGN( subs r2, r2, r3 ) @ C gets set + CALGN( add pc, r4, ip ) + + PLD( pld [r1, #0] ) +2: PLD( subs r2, r2, #96 ) + PLD( pld [r1, #28] ) + PLD( blt 4f ) + PLD( pld [r1, #60] ) + PLD( pld [r1, #92] ) + +3: PLD( pld [r1, #124] ) +4: ldr8w r1, r3, r4, r5, r6, r7, r8, ip, lr, abort=20f + subs r2, r2, #32 + str8w r0, r3, r4, r5, r6, r7, r8, ip, lr, abort=20f + bge 3b + PLD( cmn r2, #96 ) + PLD( bge 4b ) + +5: ands ip, r2, #28 + rsb ip, ip, #32 +#if LDR1W_SHIFT > 0 + lsl ip, ip, #LDR1W_SHIFT +#endif + addne pc, pc, ip @ C is always clear here + b 7f +6: + .rept (1 << LDR1W_SHIFT) + W(nop) + .endr + ldr1w r1, r3, abort=20f + ldr1w r1, r4, abort=20f + ldr1w r1, r5, abort=20f + ldr1w r1, r6, abort=20f + ldr1w r1, r7, abort=20f + ldr1w r1, r8, abort=20f + ldr1w r1, lr, abort=20f + +#if LDR1W_SHIFT < STR1W_SHIFT + lsl ip, ip, #STR1W_SHIFT - LDR1W_SHIFT +#elif LDR1W_SHIFT > STR1W_SHIFT + lsr ip, ip, #LDR1W_SHIFT - STR1W_SHIFT +#endif + add pc, pc, ip + nop + .rept (1 << STR1W_SHIFT) + W(nop) + .endr + str1w r0, r3, abort=20f + str1w r0, r4, abort=20f + str1w r0, r5, abort=20f + str1w r0, r6, abort=20f + str1w r0, r7, abort=20f + str1w r0, r8, abort=20f + str1w r0, lr, abort=20f + + CALGN( bcs 2b ) + +7: ldmfd sp!, {r5 - r8} + +8: movs r2, r2, lsl #31 + ldr1b r1, r3, ne, abort=21f + ldr1b r1, r4, cs, abort=21f + ldr1b r1, ip, cs, abort=21f + str1b r0, r3, ne, abort=21f + str1b r0, r4, cs, abort=21f + str1b r0, ip, cs, abort=21f + + @exit r4, pc + + ldmfd sp!, {r0, r4, lr} + + blx lr + + +9: rsb ip, ip, #4 + cmp ip, #2 + ldr1b r1, r3, gt, abort=21f + ldr1b r1, r4, ge, abort=21f + ldr1b r1, lr, abort=21f + str1b r0, r3, gt, abort=21f + str1b r0, r4, ge, abort=21f + subs r2, r2, ip + str1b r0, lr, abort=21f + blt 8b + ands ip, r1, #3 + beq 1b + +10: bic r1, r1, #3 + cmp ip, #2 + ldr1w r1, lr, abort=21f + beq 17f + bgt 18f + + + .macro forward_copy_shift pull push + + subs r2, r2, #28 + blt 14f + + CALGN( ands ip, r0, #31 ) + CALGN( rsb ip, ip, #32 ) + CALGN( sbcnes r4, ip, r2 ) @ C is always set here + CALGN( subcc r2, r2, ip ) + CALGN( bcc 15f ) + +11: stmfd sp!, {r5 - r9} + + PLD( pld [r1, #0] ) + PLD( subs r2, r2, #96 ) + PLD( pld [r1, #28] ) + PLD( blt 13f ) + PLD( pld [r1, #60] ) + PLD( pld [r1, #92] ) + +12: PLD( pld [r1, #124] ) +13: ldr4w r1, r4, r5, r6, r7, abort=19f + mov r3, lr, pull #\pull + subs r2, r2, #32 + ldr4w r1, r8, r9, ip, lr, abort=19f + orr r3, r3, r4, push #\push + mov r4, r4, pull #\pull + orr r4, r4, r5, push #\push + mov r5, r5, pull #\pull + orr r5, r5, r6, push #\push + mov r6, r6, pull #\pull + orr r6, r6, r7, push #\push + mov r7, r7, pull #\pull + orr r7, r7, r8, push #\push + mov r8, r8, pull #\pull + orr r8, r8, r9, push #\push + mov r9, r9, pull #\pull + orr r9, r9, ip, push #\push + mov ip, ip, pull #\pull + orr ip, ip, lr, push #\push + str8w r0, r3, r4, r5, r6, r7, r8, r9, ip, , abort=19f + bge 12b + PLD( cmn r2, #96 ) + PLD( bge 13b ) + + ldmfd sp!, {r5 - r9} + +14: ands ip, r2, #28 + beq 16f + +15: mov r3, lr, pull #\pull + ldr1w r1, lr, abort=21f + subs ip, ip, #4 + orr r3, r3, lr, push #\push + str1w r0, r3, abort=21f + bgt 15b + CALGN( cmp r2, #0 ) + CALGN( bge 11b ) + +16: sub r1, r1, #(\push / 8) + b 8b + + .endm + + + forward_copy_shift pull=8 push=24 + +17: forward_copy_shift pull=16 push=16 + +18: forward_copy_shift pull=24 push=8 diff --git a/utils/bl33_t527/source/memset.S b/utils/bl33_t527/source/memset.S new file mode 100644 index 00000000..9675ac9e --- /dev/null +++ b/utils/bl33_t527/source/memset.S @@ -0,0 +1,116 @@ +#define pull lsr +#define push lsl +#define PLD(code...) +#define CALGN(code...) code + + .text + .align 5 + + .syntax unified + +.globl memset0 +memset0: + ands r3, r0, #3 @ 1 unaligned? + mov ip, r0 @ preserve r0 as return value + bne 6f @ 1 +/* + * we know that the pointer in ip is aligned to a word boundary. + */ +1: orr r1, r1, r1, lsl #8 + orr r1, r1, r1, lsl #16 + mov r3, r1 + cmp r2, #16 + blt 4f + +#if ! CALGN(1)+0 + +/* + * We need 2 extra registers for this loop - use r8 and the LR + */ + stmfd sp!, {r8, lr} + mov r8, r1 + mov lr, r1 + +2: subs r2, r2, #64 + stmiage ip!, {r1, r3, r8, lr} @ 64 bytes at a time. + stmiage ip!, {r1, r3, r8, lr} + stmiage ip!, {r1, r3, r8, lr} + stmiage ip!, {r1, r3, r8, lr} + bgt 2b + ldmfdeq sp!, {r8, pc} @ Now <64 bytes to go. +/* + * No need to correct the count; we're only testing bits from now on + */ + tst r2, #32 + stmiane ip!, {r1, r3, r8, lr} + stmiane ip!, {r1, r3, r8, lr} + tst r2, #16 + stmiane ip!, {r1, r3, r8, lr} + ldmfd sp!, {r8, lr} + +#else + +/* + * This version aligns the destination pointer in order to write + * whole cache lines at once. + */ + + stmfd sp!, {r4-r8, lr} + mov r4, r1 + mov r5, r1 + mov r6, r1 + mov r7, r1 + mov r8, r1 + mov lr, r1 + + cmp r2, #96 + tstgt ip, #31 + ble 3f + + and r8, ip, #31 + rsb r8, r8, #32 + sub r2, r2, r8 + movs r8, r8, lsl #(32 - 4) + stmiacs ip!, {r4, r5, r6, r7} + stmiami ip!, {r4, r5} + tst r8, #(1 << 30) + mov r8, r1 + strne r1, [ip], #4 + +3: subs r2, r2, #64 + stmiage ip!, {r1, r3-r8, lr} + stmiage ip!, {r1, r3-r8, lr} + bgt 3b + ldmfdeq sp!, {r4-r8, pc} + + tst r2, #32 + stmiane ip!, {r1, r3-r8, lr} + tst r2, #16 + stmiane ip!, {r4-r7} + ldmfd sp!, {r4-r8, lr} + +#endif + +4: tst r2, #8 + stmiane ip!, {r1, r3} + tst r2, #4 + strne r1, [ip], #4 +/* + * When we get here, we've got less than 4 bytes to zero. We + * may have an unaligned pointer as well. + */ +5: tst r2, #2 + strbne r1, [ip], #1 + strbne r1, [ip], #1 + tst r2, #1 + strbne r1, [ip], #1 + bx lr + +6: subs r2, r2, #4 @ 1 do we have enough + blt 5b @ 1 bytes to align with? + cmp r3, #2 @ 1 + strblt r1, [ip], #1 @ 1 + strble r1, [ip], #1 @ 1 + strb r1, [ip], #1 @ 1 + add r2, r2, r3 @ 1 (r2 = r2 - (4 - r3)) + b 1b diff --git a/utils/bl33_t527/source/printf.c b/utils/bl33_t527/source/printf.c new file mode 100644 index 00000000..e8c4f705 --- /dev/null +++ b/utils/bl33_t527/source/printf.c @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +typedef struct uart_serial { + volatile uint32_t rbr; /* 0 */ + volatile uint32_t ier; /* 1 */ + volatile uint32_t fcr; /* 2 */ + volatile uint32_t lcr; /* 3 */ + volatile uint32_t mcr; /* 4 */ + volatile uint32_t lsr; /* 5 */ + volatile uint32_t msr; /* 6 */ + volatile uint32_t sch; /* 7 */ +} uart_serial_t; + +#define thr rbr +#define dll rbr +#define dlh ier +#define iir fcr + +#define SUNXI_UART0_BASE 0x02500000 + +static uart_serial_t *uart_dbg; + +static uint32_t init_timestamp = 0; + +void set_timer_count() { + init_timestamp = read32(SUNXI_RTC_DATA_BASE + RTC_FEL_INDEX * 4); +} + +void sunxi_serial_init() { + uart_dbg = (uart_serial_t *) SUNXI_UART0_BASE; +} + +// Function to transmit a single character via UART +void sunxi_uart_putc(char c) { + while ((uart_dbg->lsr & (1 << 6)) == 0) + ; + uart_dbg->thr = c; +} + +void uart_log_putchar(void *arg, char c) { + if (c == '\n') { + /* If the character is a newline, transmit a carriage return before newline */ + sunxi_uart_putc('\r'); + } + /* Transmit the character */ + sunxi_uart_putc(c); +} + +// Output a formatted string to the standard output +void uart_printf(const char *fmt, ...) { + va_list args; + va_start(args, fmt); + va_list args_copy; + va_copy(args_copy, args); + xvformat(uart_log_putchar, NULL, fmt, args_copy); + va_end(args); + va_end(args_copy); +} + +void printf(const char *fmt, ...) { + uint32_t now_timestamp = time_us() - init_timestamp; + uint32_t seconds = now_timestamp / (1000 * 1000); + uint32_t milliseconds = now_timestamp % (1000 * 1000); + uart_printf("[%5lu.%06lu][I] ", seconds, milliseconds); + va_list args; + va_start(args, fmt); + va_list args_copy; + va_copy(args_copy, args); + xvformat(uart_log_putchar, NULL, fmt, args_copy); + va_end(args); + va_end(args_copy); +} diff --git a/utils/bl33_t527/source/smccc-call.S b/utils/bl33_t527/source/smccc-call.S new file mode 100644 index 00000000..508737da --- /dev/null +++ b/utils/bl33_t527/source/smccc-call.S @@ -0,0 +1,9 @@ +#include + +ENTRY(__sunxi_smc_call) + stmfd sp!, {r4-r12, lr} @ save reg state + .arch_extension sec + smc #0 + ldmfd sp!, {r4-r12, pc} @ restore saved regs and return +ENDPROC(__sunxi_smc_call) + diff --git a/utils/bl33_t527/source/start.S b/utils/bl33_t527/source/start.S new file mode 100644 index 00000000..813eaedc --- /dev/null +++ b/utils/bl33_t527/source/start.S @@ -0,0 +1,85 @@ +#include + +.globl _start +_start: + b reset + +reset: + /* + * disable interrupts (FIQ and IRQ), also set the cpu to SVC32 mode, + * except if in HYP mode already + */ + mrs r0, cpsr + and r1, r0, #0x1f @ mask mode bits + teq r1, #0x1a @ test for HYP mode + bicne r0, r0, #0x1f @ clear all mode bits + orrne r0, r0, #0x13 @ set SVC mode + orr r0, r0, #0xc0 @ disable FIQ and IRQ + msr cpsr,r0 + + /* Set V=0 in CP15 SCTLR register - for VBAR to point to vector */ + mrc p15, 0, r0, c1, c0, 0 @ Read CP15 SCTLR Register + bic r0, #CR_V @ V = 0 + mcr p15, 0, r0, c1, c0, 0 @ Write CP15 SCTLR Register + + /* Set vector address in CP15 VBAR register */ + ldr r0, =_start + mcr p15, 0, r0, c12, c0, 0 @Set VBAR + + /* the mask ROM code should have PLL and others stable */ + bl cpu_init_cp15 + + bl set_timer_count + + bl sunxi_serial_init + + bl print_banner + + bl sys_boot + +ENTRY(sys_main) + push {r4, lr} + mov r3, #1 + ldr r2, [pc, #16] + ldr r1, [pc, #16] + ldr r0, [pc, #16] + bl __sunxi_smc_call + mov r0, #0 + pop {r4, pc} + .word 0x4a200000 + .word 0x40080000 + .word 0x8000ff04 +ENDPROC(sys_main) + +ENTRY(cpu_init_cp15) + /* + * Invalidate L1 I/D + */ + mov r0, #0 @ set up for MCR + mcr p15, 0, r0, c8, c7, 0 @ invalidate TLBs + mcr p15, 0, r0, c7, c5, 0 @ invalidate icache + mcr p15, 0, r0, c7, c5, 6 @ invalidate BP array + mcr p15, 0, r0, c7, c10, 4 @ DSB + mcr p15, 0, r0, c7, c5, 4 @ ISB + + /* + * disable MMU stuff and caches + */ + mrc p15, 0, r0, c1, c0, 0 + bic r0, r0, #0x00002000 @ clear bits 13 (--V-) + bic r0, r0, #0x00000007 @ clear bits 2:0 (-CAM) + orr r0, r0, #0x00000002 @ set bit 1 (--A-) Align + orr r0, r0, #0x00000800 @ set bit 11 (Z---) BTB + orr r0, r0, #0x00001000 @ set bit 12 (I) I-cache + mcr p15, 0, r0, c1, c0, 0 + + mov r5, lr @ Store my Caller + mrc p15, 0, r1, c0, c0, 0 @ r1 has Read Main ID Register (MIDR) + mov r3, r1, lsr #20 @ get variant field + and r3, r3, #0xf @ r3 has CPU variant + and r4, r1, #0xf @ r4 has CPU revision + mov r2, r3, lsl #4 @ shift variant field for combined value + orr r2, r4, r2 @ r2 has combined CPU variant + revision + mov pc, r5 @ back to my caller +ENDPROC(cpu_init_cp15) + diff --git a/utils/bl33_t527/source/string.c b/utils/bl33_t527/source/string.c new file mode 100644 index 00000000..5bbacb13 --- /dev/null +++ b/utils/bl33_t527/source/string.c @@ -0,0 +1,31 @@ +/* SPDX-License-Identifier: MIT */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern void *memset0(void *s, int c, size_t count); + +void *memset(void *s, int c, size_t count) { + asm volatile("bx %0" + : + : "r"(memset0)); + + return s; +} + +extern void *memcpy0(void *dest, const void *src, size_t n); + +void *memcpy(void *dest, const void *src, size_t count) { + asm volatile("bx %0" + : + : "r"(memcpy0)); + + return dest; +} \ No newline at end of file diff --git a/utils/bl33_t527/source/timer.c b/utils/bl33_t527/source/timer.c new file mode 100644 index 00000000..45146654 --- /dev/null +++ b/utils/bl33_t527/source/timer.c @@ -0,0 +1,61 @@ +/* SPDX-License-Identifier: MIT */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Stub function for handling signal raising +int raise(int signum) { + return 0;// Always return 0 to indicate successful handling of the signal +} + +// Inline function to retrieve the current value of the architecture-specific counter +static inline uint64_t get_arch_counter(void) { + uint32_t low = 0, high = 0; + // Use assembly language to read the architecture-specific counter + asm volatile("mrrc p15, 0, %0, %1, c14" + : "=r"(low), "=r"(high) + : + : "memory"); + // Combine the low and high values to form a 64-bit counter value + return (((uint64_t) high) << 32 | low); +} + +// Delay function that waits for the specified number of microseconds +static void udelay(uint32_t us) { + uint64_t t1, t2; + + t1 = get_arch_counter();// Get the current value of the architecture-specific counter + t2 = t1 + us * 24; // Calculate the target end time based on the counter frequency (24 MHz) + do { + t1 = get_arch_counter();// Continuously update the current time until it reaches the target end time + } while (t2 >= t1); +} + +// Wrapper function for udelay with a different name +void __usdelay(uint32_t loop) { + udelay(loop);// Call the udelay function to introduce a microsecond delay +} + +void mdelay(uint32_t ms) { + udelay(ms * 1000); + uint32_t now; + + now = time_ms(); + while (time_ms() - now < ms) { + }; +} + +uint32_t time_ms(void) { + return get_arch_counter() / 24000; +} + +uint64_t time_us(void) { + return get_arch_counter() / 24; +} diff --git a/utils/bl33_t527/source/xformat.c b/utils/bl33_t527/source/xformat.c new file mode 100644 index 00000000..d30eb561 --- /dev/null +++ b/utils/bl33_t527/source/xformat.c @@ -0,0 +1,967 @@ +/** + * @file xformatc.c + * + * @brief Printf C implementation. + * + * Tested wuth the following operating systems / compilers : + * + * - Visual studio 6 + * - Visual studio 2008 / Windows CE + * - MinGw 32 + * - Linux i686 + * - Linux x86_64 + * - GCC wiith embedded ARM. + * - Linux armel + * - HCS08 with Freescale compiler. + * - SDCC (Z80 and 8051) + * + * @author Mario Viara + * + * + * @copyright Copyright Mario Viara 2014 - License Open Source (LGPL) + * This is a free software and is opened for education, research and commercial + * developments under license policy of following terms: + * + * - This is a free software and there is NO WARRANTY. + * - No restriction on use. You can use, modify and redistribute it for personal, + * non-profit or commercial product UNDER YOUR RESPONSIBILITY. + * - Redistributions of source code must retain the above copyright notice. + * + */ +#include "xformat.h" +/** + * Default largest int is long + */ +#ifndef LONG +#define LONG long +#endif + +/** + * Define the double type if not defined + */ +#ifndef DOUBLE +#define DOUBLE double +#endif + +/** + * Default long long type + */ +#ifndef LONGLONG +#define LONGLONG long long +#endif + +/** + * Definition to convert integer part of floating point + * numer if supported we use the long long type + */ +#if XCFG_FORMAT_LONGLONG +#define FLOAT_LONG LONGLONG +#define FLOAT_VALUE llvalue +#define FLOAT_TYPE FLAG_TYPE_LONGLONG +#else +#define FLOAT_LONG LONG +#define FLOAT_VALUE lvalue +#define FLOAT_TYPE FLAG_TYPE_LONG +#endif + +/** + * Structure with all parameter used + */ +struct param_s { + /** + * Buffer for current integer value and for temporary + * double value defined as union to save stack. + */ + union { + unsigned LONG lvalue; +#if XCFG_FORMAT_LONGLONG + unsigned LONGLONG llvalue; +#endif +#if XCFG_FORMAT_FLOAT + DOUBLE dvalue; +#endif + } values; + + /** + * Pointer to the output buffer + */ + char *out; + +#if XCFG_FORMAT_FLOAT + /** + * Floating point argument + */ + DOUBLE dbl; + + /** + * Fractional part of floating point + */ + unsigned FLOAT_LONG fPart; + +#endif + + /** + * Current length of the output buffer + */ + int length; + + /** + * Field precision + */ + int prec; + + /** + * Field width + */ + int width; + + /** + * Count the number of char emitted + */ + unsigned count; + + /** + * Parser flags + */ + unsigned flags; + +#define FLAG_TYPE_INT \ + 0x0000 /* Argument is integer */ +#define FLAG_TYPE_LONG \ + 0x0001 /* Argument is long */ +#define FLAG_TYPE_SIZEOF \ + 0x0002 /* Argument is size_t */ +#define FLAG_TYPE_LONGLONG \ + 0x0003 /* Argument is long long */ +#define FLAG_TYPE_MASK \ + 0x0003 /* Mask for field type */ +#define FLAG_PREC \ + 0x0004 /* Precision set */ +#define FLAG_LEFT \ + 0x0008 /* Left alignment */ +#define FLAG_BLANK 0x0010 /* Blank before positive integer number */ +#define FLAG_PREFIX \ + 0x0020 /* Prefix required */ +#define FLAG_PLUS 0x0040 /* Force a + before positive number */ +#define FLAG_UPPER \ + 0x0080 /* Output in upper case letter */ +#define FLAG_DECIMAL \ + 0x0100 /* Decimal field */ +#define FLAG_INTEGER \ + 0x0200 /* Integer field */ +#define FLAG_MINUS \ + 0x0400 /* Field is negative */ +#define FLAG_VALUE \ + 0x0800 /* Value set */ +#define FLAG_BUFFER \ + 0x1000 /* Buffer set */ + + /** + * Length of the prefix + */ + int prefixlen; + + /* Buffer to store the field prefix */ + char prefix[2]; + + /** Buffer to store the biggest integer number in binary */ +#if XCFG_FORMAT_LONGLONG + char buffer[sizeof(LONGLONG) * 8 + 1]; +#else + char buffer[sizeof(LONG) * 8 + 1]; +#endif + + /* Radix for ASCII integer conversion */ + unsigned char radix; + + /* char used for padding */ + char pad; + + /** + * Current state + */ + char state; +}; + +/** + * Enum for the internal state machine + */ +enum State { + /* Normal state outputting literal */ + ST_NORMAL = 0, + + /* Just read % */ + ST_PERCENT = 1, + + /* Just read flag */ + ST_FLAG = 2, + + /* Just read the width */ + ST_WIDTH = 3, + + /* Just read '.' */ + ST_DOT = 4, + + /* Just read the precision */ + ST_PRECIS = 5, + + /* Just read the size */ + ST_SIZE = 6, + + /* Just read the type specifier */ + ST_TYPE = 7 +}; + +/** + * Enum for char class + */ +enum CharClass { + /* Other char */ + CH_OTHER = 0, + /* The % char */ + CH_PERCENT = 1, + /* The . char */ + CH_DOT = 2, + /* The * char */ + CH_STAR = 3, + /* The 0 char */ + CH_ZERO = 4, + /* Digit 0-9 */ + CH_DIGIT = 5, + /* Flag chars */ + CH_FLAG = 6, + /* Size chars */ + CH_SIZE = 7, + /* Type chars */ + CH_TYPE = 8 +}; + +/** + * String used when %s is a null parameter + */ +static const char ms_null[] = "(null)"; +/* + * String for true value + */ +static const char ms_true[] = "True"; + +/** + * String for false value + */ +static const char ms_false[] = "False"; + +/* + * Digit values + */ +static const char ms_digits[] = "0123456789abcdef"; + +/* + * This table contains the next state for all char and it will be + * generate using xformattable.c + */ + +static const unsigned char formatStates[] = { + 0x06, 0x00, 0x00, 0x06, 0x00, 0x01, 0x00, 0x00, 0x10, 0x00, 0x03, 0x06, + 0x00, 0x06, 0x02, 0x10, 0x04, 0x45, 0x45, 0x45, 0x45, 0x05, 0x05, 0x05, + 0x05, 0x35, 0x30, 0x00, 0x50, 0x60, 0x00, 0x00, 0x00, 0x20, 0x28, 0x38, + 0x50, 0x50, 0x00, 0x00, 0x00, 0x30, 0x30, 0x30, 0x50, 0x50, 0x00, 0x00, + 0x08, 0x20, 0x20, 0x28, 0x20, 0x20, 0x20, 0x00, 0x08, 0x60, 0x60, 0x60, + 0x60, 0x60, 0x60, 0x00, 0x00, 0x70, 0x78, 0x78, 0x78, 0x70, 0x78, 0x00, + 0x07, 0x08, 0x00, 0x00, 0x07, 0x00, 0x00, 0x08, 0x08, 0x00, 0x00, 0x08, + 0x00, 0x08, 0x00, 0x00, 0x08, 0x00, 0x07 +}; + +/** + * Convert an unsigned value in one string + * + * All parameter are in the passed structure + * + * @param prec - Minimum precision + * @param lvalue - Unsigned value + * @param radix - Radix (Supported values 2/8/10/16) + * + * @param out - Buffer with the converted value. + */ +static void ulong2a(struct param_s *param) +{ + unsigned char digit; + + while (param->prec-- > 0 || param->values.lvalue) { + switch (param->radix) { + case 2: + digit = param->values.lvalue & 0x01; + param->values.lvalue >>= 1; + break; + + case 8: + digit = param->values.lvalue & 0x07; + param->values.lvalue >>= 3; + break; + + case 16: + digit = param->values.lvalue & 0x0F; + param->values.lvalue >>= 4; + break; + + default: + case 10: + // digit = param->values.lvalue % 10; + digit = param->values.lvalue % 10; + // param->values.lvalue /= 10; + param->values.lvalue = param->values.lvalue / 10; + break; + } + + *param->out-- = ms_digits[digit]; + param->length++; + } +} + +#if XCFG_FORMAT_LONGLONG +#ifdef XCFG_FORMAT_LONG_ARE_LONGLONG +#define ullong2a ulong2a +#else +static void ullong2a(struct param_s *param) +{ + unsigned char digit; + + while (param->prec-- > 0 || param->values.llvalue) { + switch (param->radix) { + case 2: + digit = param->values.llvalue & 0x01; + param->values.llvalue >>= 1; + break; + + case 8: + digit = param->values.llvalue & 0x07; + param->values.llvalue >>= 3; + break; + + case 16: + digit = param->values.llvalue & 0x0F; + param->values.llvalue >>= 4; + break; + + default: + case 10: + digit = param->values.llvalue % 10; + param->values.llvalue /= 10; + + break; + } + + *param->out-- = ms_digits[digit]; + param->length++; + } +} +#endif +#endif + +/** + * Printf like using variable arguments. + * + * @param outchar - Pointer to the function to output one new char. + * @param arg - Argument for the output function. + * @param fmt - Format options for the list of parameters. + * @param ... - Arguments + * + * @return The number of char emitted. + * + * @see xvformat + */ +unsigned xformat(void (*outchar)(void *, char), void *arg, const char *fmt, ...) +{ + va_list list; + unsigned count; + + va_start(list, fmt); + count = xvformat(outchar, arg, fmt, list); + va_end(list); + + (void)list; + + return count; +} + +/** + * We do not want use any library function. + * + * @param s - C string + * @return The length of the string + */ +static unsigned xstrlen(const char *s) +{ + const char *i; + + for (i = s; *i; i++) { + } + + return (unsigned)(i - s); +} + +static unsigned outBuffer(void (*myoutchar)(void *arg, char), void *arg, + const char *buffer, int len, unsigned toupper) +{ + unsigned count = 0; + int i; + char c; + + for (i = 0; i < len; i++) { + c = buffer[i]; + + if (toupper && (c >= 'a' && c <= 'z')) { + c -= 'a' - 'A'; + } + + (*myoutchar)(arg, c); + count++; + } + + return count; +} + +static unsigned outChars(void (*myoutchar)(void *arg, char), void *arg, char ch, + int len) +{ + unsigned count = 0; + + while (len-- > 0) { + (*myoutchar)(arg, ch); + count++; + } + + return count; +} + +/* + * Lint want declare list as const but list is an obscured pointer so + * the warning is disabled. + */ +/*lint -save -e818 */ + +/** + * Printf like format function. + * + * General format : + * + * %[width][.precision][flags]type + * + * - width Is the minimum size of the field. + * + * - precision Is the maximum size of the field. + * + * Supported flags : + * + * - l With integer number the argument will be of type long. + * - ll With integer number the argument will be of type long long. + * - Space for positive integer a space will be added before. + * - z Compatible with C99 the argument is size_t (aka sizeof(void *)) + * - + A + sign prefix positive number. + * - # A prefix will be printed (o for octal,0x for hex,0b for binary) + * - 0 Value will be padded with zero (default is spacwe) + * - - Left justify as default filed have rigth justification. + * + * Supported type : + * + * - s Null terminated string of char. + * - S Null terminated string of char in upper case. + * - i Integer number. + * - d Integer number. + * - u Unsigned number. + * - x Unsigned number in hex. + * - X Unsigned number in hex upper case. + * - b Binary number + * - o Octal number + * - p Pointer will be emitted with the prefix -> + * - P Pointer in upper case letter. + * - f Floating point number. + * - B Boolean value printed as True / False. + * + * @param outchar - Pointer to the function to output one char. + * @param arg - Argument for the output function. + * @param fmt - Format options for the list of parameters. + * @param args -List parameters. + * + * @return The number of char emitted. + */ +unsigned xvformat(void (*outchar)(void *, char), void *arg, const char *fmt, + va_list _args) +{ + XCFG_FORMAT_STATIC struct param_s param; + int i; + char c; + +#if XCFG_FORMAT_VA_COPY + va_list args; + + va_copy(args, _args); +#else +#define args _args +#endif + + param.count = 0; + param.state = ST_NORMAL; + + while (*fmt) { + c = *fmt++; + + if (c < ' ' || c > 'z') + i = (int)CH_OTHER; + else + i = formatStates[c - ' '] & 0x0F; + + param.state = formatStates[(i << 3) + param.state] >> 4; + + switch (param.state) { + default: + case ST_NORMAL: + (*outchar)(arg, c); + param.count++; + break; + + case ST_PERCENT: + param.length = param.prefixlen = param.width = + param.prec = 0; + param.flags = 0; + param.pad = ' '; + break; + + case ST_WIDTH: + if (c == '*') + param.width = (int)va_arg(args, int); + else + param.width = param.width * 10 + (c - '0'); + break; + + case ST_DOT: + break; + + case ST_PRECIS: + param.flags |= FLAG_PREC; + if (c == '*') + param.prec = (int)va_arg(args, int); + else + param.prec = param.prec * 10 + (c - '0'); + break; + + case ST_SIZE: + switch (c) { + default: + break; + case 'z': + param.flags &= ~FLAG_TYPE_MASK; + param.flags |= FLAG_TYPE_SIZEOF; + break; + + case 'l': +#if XCFG_FORMAT_LONGLONG + if ((param.flags & FLAG_TYPE_MASK) == + FLAG_TYPE_LONG) { + param.flags &= ~FLAG_TYPE_MASK; + param.flags |= FLAG_TYPE_LONGLONG; + } else { + param.flags &= ~FLAG_TYPE_MASK; + param.flags |= FLAG_TYPE_LONG; + } +#else + param.flags &= ~FLAG_TYPE_MASK; + param.flags |= FLAG_TYPE_LONG; +#endif + break; + } + break; + + case ST_FLAG: + switch (c) { + default: + break; + case '-': + param.flags |= FLAG_LEFT; + break; + case '0': + param.pad = '0'; + break; + case ' ': + param.flags |= FLAG_BLANK; + break; + case '#': + param.flags |= FLAG_PREFIX; + break; + case '+': + param.flags |= FLAG_PLUS; + break; + } + break; + + case ST_TYPE: + switch (c) { + default: + param.length = 0; + break; + + /* + * Pointer upper case + */ + case 'P': + param.flags |= FLAG_UPPER; + /* no break */ + /*lint -fallthrough */ + + /* + * Pointer + */ + case 'p': + param.flags |= FLAG_INTEGER; + param.flags &= ~FLAG_TYPE_MASK; + param.flags |= FLAG_TYPE_SIZEOF; + param.radix = 16; + param.prec = sizeof(void *) * 2; + param.prefix[0] = '-'; + param.prefix[1] = '>'; + param.prefixlen = 2; + break; + + /* + * Binary number + */ + case 'b': + param.flags |= FLAG_INTEGER; + param.radix = 2; + if (param.flags & FLAG_PREFIX) { + param.prefix[0] = '0'; + param.prefix[1] = 'b'; + param.prefixlen = 2; + } + break; + + /* + * Octal number + */ + case 'o': + param.flags |= FLAG_INTEGER; + param.radix = 8; + if (param.flags & FLAG_PREFIX) { + param.prefix[0] = '0'; + param.prefixlen = 1; + } + break; + + /* + * Hex number upper case letter. + */ + case 'X': + /* no break */ + param.flags |= FLAG_UPPER; + + /* no break */ + + /* lint -fallthrough */ + + /* + * Hex number lower case + */ + case 'x': + param.flags |= FLAG_INTEGER; + param.radix = 16; + if (param.flags & FLAG_PREFIX) { + param.prefix[0] = '0'; + param.prefix[1] = 'x'; + param.prefixlen = 2; + } + break; + + /* + * Integer number radix 10 + */ + case 'd': + case 'i': + param.flags |= FLAG_DECIMAL | FLAG_INTEGER; + param.radix = 10; + break; + + /* + * Unsigned number + */ + case 'u': + param.flags |= FLAG_INTEGER; + param.radix = 10; + break; + + /* + * Upper case string + */ + case 'S': + param.flags |= FLAG_UPPER; + /* no break */ + /*lint -fallthrough */ + + /* + * Normal string + */ + case 's': + param.out = va_arg(args, char *); + if (param.out == 0) + param.out = (char *)ms_null; + param.length = (int)xstrlen(param.out); + break; + + /* + * Upper case char + */ + case 'C': + param.flags |= FLAG_UPPER; + /* no break */ + /* lint -fallthrough */ + + /* + * Char + */ + case 'c': + param.out = param.buffer; + param.buffer[0] = (char)va_arg(args, int); + param.length = 1; + break; + +#if XCFG_FORMAT_FLOAT + /** + * Floating point number + */ + case 'f': + if (!(param.flags & FLAG_PREC)) { + param.prec = 6; + } + + param.dbl = va_arg(args, DOUBLE); + param.values.dvalue = 0.51; + for (i = 0; i < param.prec; i++) + param.values.dvalue /= 10.0; + + if (param.dbl < 0) { + param.flags |= FLAG_MINUS; + param.dbl -= param.values.dvalue; + param.fPart = (FLOAT_LONG)param.dbl; + param.dbl -= (FLOAT_LONG)param.fPart; + param.dbl = -param.dbl; + } else { + param.dbl += param.values.dvalue; + param.fPart = (FLOAT_LONG)param.dbl; + param.dbl -= param.fPart; + } + + for (i = 0; i < param.prec; i++) + param.dbl *= 10.0; + + param.values.lvalue = (unsigned LONG)param.dbl; + + param.out = + param.buffer + sizeof(param.buffer) - 1; + param.radix = 10; + if (param.prec) { + ulong2a(¶m); + *param.out-- = '.'; + param.length++; + } + param.flags |= FLAG_INTEGER | FLAG_BUFFER | + FLAG_DECIMAL | FLAG_VALUE | + FLOAT_TYPE; + + param.prec = 0; + param.values.FLOAT_VALUE = + (unsigned FLOAT_LONG)param.fPart; + break; +#endif + + /** + * Boolean value + */ + case 'B': + if (va_arg(args, int) != 0) + param.out = (char *)ms_true; + else + param.out = (char *)ms_false; + + param.length = (int)xstrlen(param.out); + break; + } + + /* + * Process integer number + */ + if (param.flags & FLAG_INTEGER) { + if (param.prec == 0) + param.prec = 1; + + if (!(param.flags & FLAG_VALUE)) { + if (param.flags & FLAG_DECIMAL) { + switch (param.flags & + FLAG_TYPE_MASK) { + case FLAG_TYPE_SIZEOF: + param.values.lvalue = + (unsigned LONG)va_arg( + args, + void *); + break; + case FLAG_TYPE_LONG: + param.values.lvalue = + (LONG)va_arg( + args, + long); + break; + case FLAG_TYPE_INT: + param.values.lvalue = + (LONG)va_arg( + args, + int); + break; +#if XCFG_FORMAT_LONGLONG + case FLAG_TYPE_LONGLONG: + param.values.llvalue = + (LONGLONG)va_arg( + args, + long long); + break; +#endif + } + + } else { + switch (param.flags & + FLAG_TYPE_MASK) { + case FLAG_TYPE_SIZEOF: + param.values.lvalue = + (unsigned LONG)va_arg( + args, + void *); + break; + case FLAG_TYPE_LONG: + param.values.lvalue = + (unsigned LONG)va_arg( + args, + unsigned long); + break; + case FLAG_TYPE_INT: + param.values.lvalue = + (unsigned LONG)va_arg( + args, + unsigned int); + break; +#if XCFG_FORMAT_LONGLONG + case FLAG_TYPE_LONGLONG: + param.values.llvalue = + (unsigned LONGLONG)va_arg( + args, + unsigned long long); + break; +#endif + } + } + } + + if ((param.flags & FLAG_PREFIX) && + param.values.lvalue == 0) { + param.prefixlen = 0; + } + + /* + * Manage signed integer + */ + if (param.flags & FLAG_DECIMAL) { +#if XCFG_FORMAT_LONGLONG + if ((param.flags & FLAG_TYPE_MASK) == + FLAG_TYPE_LONGLONG) { + if ((LONGLONG)param.values + .llvalue < 0) { + param.values.llvalue = + ~param.values + .llvalue + + 1; + param.flags |= + FLAG_MINUS; + } + } else { +#endif + if ((LONG)param.values.lvalue < + 0) { + param.values.lvalue = + ~param.values + .lvalue + + 1; + param.flags |= + FLAG_MINUS; + } +#if XCFG_FORMAT_LONGLONG + } +#endif + if (!(param.flags & FLAG_MINUS) && + (param.flags & FLAG_BLANK)) { + param.prefix[0] = ' '; + param.prefixlen = 1; + } + } + + if ((param.flags & FLAG_BUFFER) == 0) { + param.out = param.buffer + + sizeof(param.buffer) - 1; + } + +#if XCFG_FORMAT_LONGLONG + if ((param.flags & FLAG_TYPE_MASK) == + FLAG_TYPE_LONGLONG) + ullong2a(¶m); + else + ulong2a(¶m); +#else + + ulong2a(¶m); +#endif + param.out++; + + /* + * Check if a sign is required + */ + if (param.flags & (FLAG_MINUS | FLAG_PLUS)) { + c = param.flags & FLAG_MINUS ? '-' : + '+'; + + if (param.pad == '0') { + param.prefixlen = 1; + param.prefix[0] = c; + } else { + *--param.out = c; + param.length++; + } + } + + } else { + if (param.width && param.length > param.width) { + param.length = param.width; + } + } + + /* + * Now width contain the size of the pad + */ + param.width -= (param.length + param.prefixlen); + + param.count += outBuffer(outchar, arg, param.prefix, + param.prefixlen, + param.flags & FLAG_UPPER); + if (!(param.flags & FLAG_LEFT)) + param.count += outChars(outchar, arg, param.pad, + param.width); + param.count += outBuffer(outchar, arg, param.out, + param.length, + param.flags & FLAG_UPPER); + if (param.flags & FLAG_LEFT) + param.count += outChars(outchar, arg, param.pad, + param.width); + } + } + +#if XCFG_FORMAT_VA_COPY + va_end(args); +#endif + + return param.count; +} + +/*lint -restore */