elf_trampoline.c revision 154561
1150861Scognet/*- 2150861Scognet * Copyright (c) 2005 Olivier Houchard. All rights reserved. 3150861Scognet * 4150861Scognet * Redistribution and use in source and binary forms, with or without 5150861Scognet * modification, are permitted provided that the following conditions 6150861Scognet * are met: 7150861Scognet * 1. Redistributions of source code must retain the above copyright 8150861Scognet * notice, this list of conditions and the following disclaimer. 9150861Scognet * 2. Redistributions in binary form must reproduce the above copyright 10150861Scognet * notice, this list of conditions and the following disclaimer in the 11150861Scognet * documentation and/or other materials provided with the distribution. 12150861Scognet * 13150861Scognet * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 14150861Scognet * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 15150861Scognet * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 16150861Scognet * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 17150861Scognet * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 18150861Scognet * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 19150861Scognet * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 20150861Scognet * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21150861Scognet * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 22150861Scognet * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23150861Scognet */ 24150861Scognet 25150861Scognet#include <sys/cdefs.h> 26150861Scognet__FBSDID("$FreeBSD: head/sys/arm/arm/elf_trampoline.c 154561 2006-01-20 00:46:44Z cognet $"); 27150861Scognet#include <machine/asm.h> 28150861Scognet#include <sys/types.h> 29150861Scognet#include <sys/elf32.h> 30150861Scognet#include <sys/param.h> 31153112Scognet#include <sys/inflate.h> 32150861Scognet#include <machine/elf.h> 33154561Scognet#include <machine/pte.h> 34154561Scognet 35150861Scognet#include <stdlib.h> 36150861Scognet 37150861Scognet#include "opt_global.h" 38153112Scognet#include "opt_kernname.h" 39150861Scognet 40150861Scognetextern char kernel_start[]; 41150861Scognetextern char kernel_end[]; 42150861Scognet 43152743Scognetvoid __start(void); 44152743Scognet 45153112Scognet#define GZ_HEAD 0xa 46152743Scognet 47150861Scognetstatic __inline void * 48150861Scognetmemcpy(void *dst, const void *src, int len) 49150861Scognet{ 50150861Scognet const char *s = src; 51150861Scognet char *d = dst; 52153549Scognet 53153112Scognet while (len) { 54153549Scognet if (0 && len >= 4 && !((vm_offset_t)d & 3) && 55153549Scognet !((vm_offset_t)s & 3)) { 56153112Scognet *(uint32_t *)d = *(uint32_t *)s; 57153112Scognet s += 4; 58153112Scognet d += 4; 59153112Scognet len -= 4; 60153112Scognet } else { 61153112Scognet *d++ = *s++; 62153112Scognet len--; 63153112Scognet } 64150861Scognet } 65150861Scognet return (dst); 66150861Scognet} 67150861Scognet 68150861Scognetstatic __inline void 69153112Scognetbzero(void *addr, int count) 70150861Scognet{ 71153112Scognet char *tmp = (char *)addr; 72153112Scognet 73150861Scognet while (count > 0) { 74153112Scognet if (count >= 4 && !((vm_offset_t)tmp & 3)) { 75153112Scognet *(uint32_t *)tmp = 0; 76153112Scognet tmp += 4; 77153112Scognet count -= 4; 78153112Scognet } else { 79153112Scognet *tmp = 0; 80153112Scognet tmp++; 81153112Scognet count--; 82153112Scognet } 83150861Scognet } 84150861Scognet} 85150861Scognet 86153112Scognetvoid 87153112Scognet_start(void) 88153112Scognet{ 89153549Scognet int physaddr = KERNPHYSADDR; 90153549Scognet int tmp1; 91153549Scognet 92153549Scognet __asm __volatile("adr %0, 2f\n" 93153549Scognet "bic %0, %0, #0xff000000\n" 94153549Scognet "bic sp, sp, #0xff000000\n" 95153549Scognet "and %1, %1, #0xff000000\n" 96153549Scognet "orr %0, %0, %1\n" 97153549Scognet "orr sp, sp, %1\n" 98153549Scognet "mrc p15, 0, %1, c1, c0, 0\n" 99153549Scognet "bic %1, %1, #1\n" /* Disable MMU */ 100153549Scognet "orr %1, %1, #(4 | 8)\n" /* Add DC enable, 101153549Scognet WBUF enable */ 102153549Scognet "orr %1, %1, #0x1000\n" /* Add IC enable */ 103153549Scognet "orr %1, %1, #(0x800)\n" /* BPRD enable */ 104153549Scognet 105153549Scognet "mcr p15, 0, %1, c1, c0, 0\n" 106153549Scognet "nop\n" 107153549Scognet "nop\n" 108153549Scognet "nop\n" 109153549Scognet "mov pc, %0\n" 110153549Scognet "2: nop\n" 111153549Scognet : "=r" (tmp1), "+r" (physaddr)); 112153112Scognet __start(); 113153112Scognet} 114153112Scognet 115153112Scognet#ifdef KZIP 116153112Scognetstatic unsigned char *orig_input, *i_input, *i_output; 117153112Scognet 118153112Scognet 119153112Scognetstatic u_int memcnt; /* Memory allocated: blocks */ 120153112Scognetstatic size_t memtot; /* Memory allocated: bytes */ 121153112Scognet/* 122153112Scognet * Library functions required by inflate(). 123153112Scognet */ 124153112Scognet 125153112Scognet#define MEMSIZ 0x8000 126153112Scognet 127153112Scognet/* 128153112Scognet * Allocate memory block. 129153112Scognet */ 130153112Scognetunsigned char * 131153112Scognetkzipmalloc(int size) 132153112Scognet{ 133153112Scognet void *ptr; 134153112Scognet static u_char mem[MEMSIZ]; 135153112Scognet 136153112Scognet if (memtot + size > MEMSIZ) 137153112Scognet return NULL; 138153112Scognet ptr = mem + memtot; 139153112Scognet memtot += size; 140153112Scognet memcnt++; 141153112Scognet return ptr; 142153112Scognet} 143153112Scognet 144153112Scognet/* 145153112Scognet * Free allocated memory block. 146153112Scognet */ 147153112Scognetvoid 148153112Scognetkzipfree(void *ptr) 149153112Scognet{ 150153112Scognet memcnt--; 151153112Scognet if (!memcnt) 152153112Scognet memtot = 0; 153153112Scognet} 154153112Scognet 155153112Scognetvoid 156153112Scognetputstr(char *dummy) 157153112Scognet{ 158153112Scognet} 159153112Scognet 160153112Scognetstatic int 161153112Scognetinput(void *dummy) 162153112Scognet{ 163153112Scognet if ((size_t)(i_input - orig_input) >= KERNSIZE) { 164153112Scognet return (GZ_EOF); 165153112Scognet } 166153112Scognet return *i_input++; 167153112Scognet} 168153112Scognet 169153112Scognetstatic int 170153112Scognetoutput(void *dummy, unsigned char *ptr, unsigned long len) 171153112Scognet{ 172153112Scognet 173153112Scognet memcpy(i_output, ptr, len); 174153112Scognet i_output += len; 175153112Scognet return (0); 176153112Scognet} 177153112Scognet 178153112Scognetstatic void * 179153112Scognetinflate_kernel(void *kernel, void *startaddr) 180153112Scognet{ 181153112Scognet struct inflate infl; 182153112Scognet char slide[GZ_WSIZE]; 183153112Scognet 184153112Scognet orig_input = kernel; 185153549Scognet memcnt = memtot = 0; 186153112Scognet i_input = (char *)kernel + GZ_HEAD; 187153112Scognet if (((char *)kernel)[3] & 0x18) { 188153112Scognet while (*i_input) 189153112Scognet i_input++; 190153112Scognet i_input++; 191153112Scognet } 192153112Scognet i_output = startaddr; 193153112Scognet bzero(&infl, sizeof(infl)); 194153112Scognet infl.gz_input = input; 195153112Scognet infl.gz_output = output; 196153112Scognet infl.gz_slide = slide; 197153112Scognet inflate(&infl); 198153112Scognet return ((char *)(((vm_offset_t)i_output & ~3) + 4)); 199153112Scognet} 200153112Scognet 201153112Scognet#endif 202153112Scognet 203150861Scognetvoid * 204153112Scognetload_kernel(unsigned int kstart, unsigned int curaddr,unsigned int func_end, 205153112Scognet int d) 206150861Scognet{ 207150861Scognet Elf32_Ehdr *eh; 208150861Scognet Elf32_Phdr phdr[512] /* XXX */, *php; 209154561Scognet Elf32_Shdr shdr[512] /* XXX */; 210150861Scognet int i,j; 211150861Scognet void *entry_point; 212153112Scognet int symtabindex = -1; 213153112Scognet int symstrindex = -1; 214150861Scognet vm_offset_t lastaddr = 0; 215153112Scognet Elf_Addr ssym = 0, esym = 0; 216150861Scognet Elf_Dyn *dp; 217150861Scognet 218150861Scognet eh = (Elf32_Ehdr *)kstart; 219150861Scognet ssym = esym = 0; 220150861Scognet entry_point = (void*)eh->e_entry; 221150861Scognet memcpy(phdr, (void *)(kstart + eh->e_phoff ), 222150861Scognet eh->e_phnum * sizeof(phdr[0])); 223153549Scognet 224150861Scognet /* Determine lastaddr. */ 225150861Scognet for (i = 0; i < eh->e_phnum; i++) { 226150861Scognet if (lastaddr < (phdr[i].p_vaddr - KERNVIRTADDR + curaddr 227150861Scognet + phdr[i].p_memsz)) 228150861Scognet lastaddr = phdr[i].p_vaddr - KERNVIRTADDR + 229150861Scognet curaddr + phdr[i].p_memsz; 230150861Scognet } 231150861Scognet 232153112Scognet /* Save the symbol tables, as there're about to be scratched. */ 233153549Scognet memcpy(shdr, (void *)(kstart + eh->e_shoff), 234153549Scognet sizeof(*shdr) * eh->e_shnum); 235150861Scognet if (eh->e_shnum * eh->e_shentsize != 0 && 236150861Scognet eh->e_shoff != 0) { 237150861Scognet for (i = 0; i < eh->e_shnum; i++) { 238150861Scognet if (shdr[i].sh_type == SHT_SYMTAB) { 239150861Scognet for (j = 0; j < eh->e_phnum; j++) { 240150861Scognet if (phdr[j].p_type == PT_LOAD && 241150861Scognet shdr[i].sh_offset >= 242150861Scognet phdr[j].p_offset && 243150861Scognet (shdr[i].sh_offset + 244150861Scognet shdr[i].sh_size <= 245150861Scognet phdr[j].p_offset + 246150861Scognet phdr[j].p_filesz)) { 247150861Scognet shdr[i].sh_offset = 0; 248150861Scognet shdr[i].sh_size = 0; 249150861Scognet j = eh->e_phnum; 250150861Scognet } 251150861Scognet } 252150861Scognet if (shdr[i].sh_offset != 0 && 253150861Scognet shdr[i].sh_size != 0) { 254150861Scognet symtabindex = i; 255150861Scognet symstrindex = shdr[i].sh_link; 256150861Scognet } 257150861Scognet } 258150861Scognet } 259150861Scognet func_end = roundup(func_end, sizeof(long)); 260150861Scognet if (symtabindex >= 0 && symstrindex >= 0) { 261150861Scognet ssym = lastaddr; 262150861Scognet if (d) { 263150861Scognet memcpy((void *)func_end, (void *)( 264150861Scognet shdr[symtabindex].sh_offset + kstart), 265150861Scognet shdr[symtabindex].sh_size); 266150861Scognet memcpy((void *)(func_end + 267150861Scognet shdr[symtabindex].sh_size), 268150861Scognet (void *)(shdr[symstrindex].sh_offset + 269150861Scognet kstart), shdr[symstrindex].sh_size); 270153112Scognet } else { 271153112Scognet lastaddr += shdr[symtabindex].sh_size; 272153112Scognet lastaddr = roundup(lastaddr, 273153112Scognet sizeof(shdr[symtabindex].sh_size)); 274153112Scognet lastaddr += sizeof(shdr[symstrindex].sh_size); 275153112Scognet lastaddr += shdr[symstrindex].sh_size; 276153112Scognet lastaddr = roundup(lastaddr, 277153112Scognet sizeof(shdr[symstrindex].sh_size)); 278150861Scognet } 279150861Scognet 280150861Scognet } 281150861Scognet } 282150861Scognet if (!d) 283150861Scognet return ((void *)lastaddr); 284150861Scognet 285150861Scognet j = eh->e_phnum; 286150861Scognet for (i = 0; i < j; i++) { 287150861Scognet volatile char c; 288153112Scognet 289153112Scognet if (phdr[i].p_type != PT_LOAD) 290150861Scognet continue; 291150861Scognet memcpy((void *)(phdr[i].p_vaddr - KERNVIRTADDR + curaddr), 292150861Scognet (void*)(kstart + phdr[i].p_offset), phdr[i].p_filesz); 293150861Scognet /* Clean space from oversized segments, eg: bss. */ 294150861Scognet if (phdr[i].p_filesz < phdr[i].p_memsz) 295150861Scognet bzero((void *)(phdr[i].p_vaddr - KERNVIRTADDR + 296150861Scognet curaddr + phdr[i].p_filesz), phdr[i].p_memsz - 297150861Scognet phdr[i].p_filesz); 298150861Scognet } 299150861Scognet /* Now grab the symbol tables. */ 300153112Scognet if (symtabindex >= 0 && symstrindex >= 0) { 301153112Scognet *(Elf_Size *)lastaddr = 302153112Scognet shdr[symtabindex].sh_size; 303153112Scognet lastaddr += sizeof(shdr[symtabindex].sh_size); 304153112Scognet memcpy((void*)lastaddr, 305153112Scognet (void *)func_end, 306153112Scognet shdr[symtabindex].sh_size); 307153112Scognet lastaddr += shdr[symtabindex].sh_size; 308153112Scognet lastaddr = roundup(lastaddr, 309153112Scognet sizeof(shdr[symtabindex].sh_size)); 310153112Scognet *(Elf_Size *)lastaddr = 311153112Scognet shdr[symstrindex].sh_size; 312153112Scognet lastaddr += sizeof(shdr[symstrindex].sh_size); 313153112Scognet memcpy((void*)lastaddr, 314153112Scognet (void*)(func_end + 315153112Scognet shdr[symtabindex].sh_size), 316153112Scognet shdr[symstrindex].sh_size); 317153112Scognet lastaddr += shdr[symstrindex].sh_size; 318153112Scognet lastaddr = roundup(lastaddr, 319153112Scognet sizeof(shdr[symstrindex].sh_size)); 320153112Scognet *(Elf_Addr *)curaddr = MAGIC_TRAMP_NUMBER; 321153112Scognet *((Elf_Addr *)curaddr + 1) = ssym - curaddr + KERNVIRTADDR; 322153112Scognet *((Elf_Addr *)curaddr + 2) = lastaddr - curaddr + KERNVIRTADDR; 323153112Scognet } else 324153112Scognet *(Elf_Addr *)curaddr = 0; 325153549Scognet /* Invalidate the instruction cache. */ 326153549Scognet __asm __volatile("mcr p15, 0, %0, c7, c5, 0\n" 327153549Scognet "mcr p15, 0, %0, c7, c10, 4\n" 328153549Scognet : : "r" (curaddr)); 329150861Scognet /* Jump to the entry point. */ 330150861Scognet ((void(*)(void))(entry_point - KERNVIRTADDR + curaddr))(); 331150861Scognet __asm __volatile(".globl func_end\n" 332150861Scognet "func_end:"); 333150861Scognet 334150861Scognet} 335150861Scognet 336150861Scognetextern char func_end[]; 337150861Scognet 338153112Scognetextern void *_end; 339154561Scognet 340154561Scognet#define PMAP_DOMAIN_KERNEL 15 /* 341154561Scognet * Just define it instead of including the 342154561Scognet * whole VM headers set. 343154561Scognet */ 344154561Scognetint __hack; 345154561Scognetstatic __inline void 346154561Scognetsetup_pagetables(unsigned int pt_addr, vm_paddr_t physstart, vm_paddr_t physend) 347150861Scognet{ 348154561Scognet unsigned int *pd = (unsigned int *)pt_addr; 349154561Scognet vm_paddr_t addr; 350154561Scognet int domain = (DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL * 2)) | DOMAIN_CLIENT; 351154561Scognet int tmp; 352154561Scognet 353154561Scognet bzero(pd, L1_TABLE_SIZE); 354154561Scognet for (addr = physstart; addr < physend; addr += L1_S_SIZE) 355154561Scognet pd[addr >> L1_S_SHIFT] = L1_TYPE_S|L1_S_C|L1_S_AP(AP_KRW)| 356154561Scognet L1_S_DOM(PMAP_DOMAIN_KERNEL) | addr; 357154561Scognet /* XXX: See below */ 358154561Scognet if (0xfff00000 < physstart || 0xfff00000 > physend) 359154561Scognet pd[0xfff00000 >> L1_S_SHIFT] = L1_TYPE_S|L1_S_AP(AP_KRW)| 360154561Scognet L1_S_DOM(PMAP_DOMAIN_KERNEL)|physstart; 361154561Scognet __asm __volatile("mcr p15, 0, %1, c2, c0, 0\n" /* set TTB */ 362154561Scognet "mcr p15, 0, %1, c8, c7, 0\n" /* Flush TTB */ 363154561Scognet "mcr p15, 0, %2, c3, c0, 0\n" /* Set DAR */ 364154561Scognet "mrc p15, 0, %0, c1, c0, 0\n" 365154561Scognet "orr %0, %0, #1\n" /* MMU_ENABLE */ 366154561Scognet "mcr p15, 0, %0, c1, c0, 0\n" 367154561Scognet "mrc p15, 0, %0, c2, c0, 0\n" /* CPWAIT */ 368154561Scognet "mov r0, r0\n" 369154561Scognet "sub pc, pc, #4\n" : 370154561Scognet "=r" (tmp) : "r" (pd), "r" (domain)); 371154561Scognet 372154561Scognet /* 373154561Scognet * XXX: This is the most stupid workaround I've ever wrote. 374154561Scognet * For some reason, the KB9202 won't boot the kernel unless 375154561Scognet * we access an address which is not in the 376154561Scognet * 0x20000000 - 0x20ffffff range. I hope I'll understand 377154561Scognet * what's going on later. 378154561Scognet */ 379154561Scognet __hack = *(volatile int *)0xfffff21c; 380154561Scognet} 381154561Scognet 382154561Scognetvoid 383154561Scognet__start(void) 384154561Scognet{ 385150861Scognet void *curaddr; 386153112Scognet void *dst; 387153112Scognet char *kernel = (char *)&kernel_start; 388150861Scognet 389150861Scognet __asm __volatile("mov %0, pc" : 390150861Scognet "=r" (curaddr)); 391150861Scognet curaddr = (void*)((unsigned int)curaddr & 0xfff00000); 392153112Scognet#ifdef KZIP 393153112Scognet if (*kernel == 0x1f && kernel[1] == 0x8b) { 394154561Scognet int pt_addr = (((int)&_end + KERNSIZE + 0x100) & 395154561Scognet ~(L1_TABLE_SIZE - 1)) + L1_TABLE_SIZE; 396154561Scognet setup_pagetables(pt_addr, (vm_paddr_t)curaddr, 397154561Scognet (vm_paddr_t)curaddr + 0x10000000); 398153112Scognet /* Gzipped kernel */ 399153112Scognet dst = inflate_kernel(kernel, &_end); 400153112Scognet kernel = (char *)&_end; 401153112Scognet } else 402153112Scognet#endif 403153112Scognet dst = 4 + load_kernel((unsigned int)&kernel_start, 404153112Scognet (unsigned int)curaddr, 405150861Scognet (unsigned int)&func_end, 0); 406150861Scognet memcpy((void *)dst, (void *)&load_kernel, (unsigned int)&func_end - 407150861Scognet (unsigned int)&load_kernel); 408153112Scognet ((void (*)())dst)((unsigned int)kernel, 409153112Scognet (unsigned int)curaddr, 410153112Scognet dst + (unsigned int)(&func_end) - 411152743Scognet (unsigned int)(&load_kernel), 1); 412150861Scognet} 413