sys/arm/arm/elf_trampoline.c

   1 /*-
   2  * Copyright (c) 2005 Olivier Houchard.  All rights reserved.
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  * 1. Redistributions of source code must retain the above copyright
   8  *    notice, this list of conditions and the following disclaimer.
   9  * 2. Redistributions in binary form must reproduce the above copyright
  10  *    notice, this list of conditions and the following disclaimer in the
  11  *    documentation and/or other materials provided with the distribution.
  12  *
  13  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  14  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  15  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  16  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  17  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  18  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  19  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  20  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  21  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  22  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  23  */
  24
  25 /*
  26  * Since we are compiled outside of the normal kernel build process, we
  27  * need to include opt_global.h manually.
  28  */
  29 #include "opt_global.h"
  30 #include "opt_kernname.h"
  31
  32 #include <sys/cdefs.h>
  33 __FBSDID("$FreeBSD$");
  34 #include <machine/asm.h>
  35 #include <sys/param.h>
  36 #include <sys/elf32.h>
  37 #include <sys/inflate.h>
  38 #include <machine/elf.h>
  39 #include <machine/pte-v4.h>
  40 #include <machine/cpufunc.h>
  41 #include <machine/armreg.h>
  42 #include <machine/cpu.h>
  43
  44 extern char kernel_start[];
  45 extern char kernel_end[];
  46
  47 extern void *_end;
  48
  49 void _start(void);
  50 void __start(void);
  51 void __startC(unsigned r0, unsigned r1, unsigned r2, unsigned r3);
  52
  53 extern unsigned int cpu_ident(void);
  54 extern void armv6_idcache_wbinv_all(void);
  55 extern void armv7_idcache_wbinv_all(void);
  56 extern void do_call(void *, void *, void *, int);
  57
  58 #define GZ_HEAD 0xa
  59
  60 #if defined(CPU_ARM9)
  61 #define cpu_idcache_wbinv_all   arm9_idcache_wbinv_all
  62 extern void arm9_idcache_wbinv_all(void);
  63 #elif defined(CPU_FA526)
  64 #define cpu_idcache_wbinv_all   fa526_idcache_wbinv_all
  65 extern void fa526_idcache_wbinv_all(void);
  66 #elif defined(CPU_ARM9E)
  67 #define cpu_idcache_wbinv_all   armv5_ec_idcache_wbinv_all
  68 extern void armv5_ec_idcache_wbinv_all(void);
  69 #elif defined(CPU_ARM1176)
  70 #define cpu_idcache_wbinv_all   armv6_idcache_wbinv_all
  71 #elif defined(CPU_XSCALE_PXA2X0) || defined(CPU_XSCALE_IXP425)
  72 #define cpu_idcache_wbinv_all   xscale_cache_purgeID
  73 extern void xscale_cache_purgeID(void);
  74 #elif defined(CPU_XSCALE_81342)
  75 #define cpu_idcache_wbinv_all   xscalec3_cache_purgeID
  76 extern void xscalec3_cache_purgeID(void);
  77 #elif defined(CPU_MV_PJ4B)
  78 #if !defined(SOC_MV_ARMADAXP)
  79 #define cpu_idcache_wbinv_all   armv6_idcache_wbinv_all
  80 extern void armv6_idcache_wbinv_all(void);
  81 #else
  82 #define cpu_idcache_wbinv_all() armadaxp_idcache_wbinv_all
  83 #endif
  84 #endif /* CPU_MV_PJ4B */
  85 #ifdef CPU_XSCALE_81342
  86 #define cpu_l2cache_wbinv_all   xscalec3_l2cache_purge
  87 extern void xscalec3_l2cache_purge(void);
  88 #elif defined(SOC_MV_KIRKWOOD) || defined(SOC_MV_DISCOVERY)
  89 #define cpu_l2cache_wbinv_all   sheeva_l2cache_wbinv_all
  90 extern void sheeva_l2cache_wbinv_all(void);
  91 #elif defined(CPU_CORTEXA8) || defined(CPU_CORTEXA_MP) || defined(CPU_KRAIT)
  92 #define cpu_idcache_wbinv_all   armv7_idcache_wbinv_all
  93 #define cpu_l2cache_wbinv_all()
  94 #else
  95 #define cpu_l2cache_wbinv_all()
  96 #endif
  97
  98 static void armadaxp_idcache_wbinv_all(void);
  99
 100 int     arm_picache_size;
 101 int     arm_picache_line_size;
 102 int     arm_picache_ways;
 103
 104 int     arm_pdcache_size;       /* and unified */
 105 int     arm_pdcache_line_size = 32;
 106 int     arm_pdcache_ways;
 107
 108 int     arm_pcache_type;
 109 int     arm_pcache_unified;
 110
 111 int     arm_dcache_align;
 112 int     arm_dcache_align_mask;
 113
 114 int     arm_dcache_min_line_size = 32;
 115 int     arm_icache_min_line_size = 32;
 116 int     arm_idcache_min_line_size = 32;
 117
 118 u_int   arm_cache_level;
 119 u_int   arm_cache_type[14];
 120 u_int   arm_cache_loc;
 121
 122 /* Additional cache information local to this file.  Log2 of some of the
 123       above numbers.  */
 124 static int      arm_dcache_l2_nsets;
 125 static int      arm_dcache_l2_assoc;
 126 static int      arm_dcache_l2_linesize;
 127
 128 /*
 129  * Boot parameters
 130  */
 131 static struct arm_boot_params s_boot_params;
 132
 133 extern int arm9_dcache_sets_inc;
 134 extern int arm9_dcache_sets_max;
 135 extern int arm9_dcache_index_max;
 136 extern int arm9_dcache_index_inc;
 137
 138 static __inline void *
 139 memcpy(void *dst, const void *src, int len)
 140 {
 141         const char *s = src;
 142         char *d = dst;
 143
 144         while (len) {
 145                 if (0 && len >= 4 && !((vm_offset_t)d & 3) &&
 146                     !((vm_offset_t)s & 3)) {
 147                         *(uint32_t *)d = *(uint32_t *)s;
 148                         s += 4;
 149                         d += 4;
 150                         len -= 4;
 151                 } else {
 152                         *d++ = *s++;
 153                         len--;
 154                 }
 155         }
 156         return (dst);
 157 }
 158
 159 static __inline void
 160 bzero(void *addr, int count)
 161 {
 162         char *tmp = (char *)addr;
 163
 164         while (count > 0) {
 165                 if (count >= 4 && !((vm_offset_t)tmp & 3)) {
 166                         *(uint32_t *)tmp = 0;
 167                         tmp += 4;
 168                         count -= 4;
 169                 } else {
 170                         *tmp = 0;
 171                         tmp++;
 172                         count--;
 173                 }
 174         }
 175 }
 176
 177 static void arm9_setup(void);
 178
 179 void
 180 _startC(unsigned r0, unsigned r1, unsigned r2, unsigned r3)
 181 {
 182         int tmp1;
 183         unsigned int sp = ((unsigned int)&_end & ~3) + 4;
 184         unsigned int pc, kernphysaddr;
 185
 186         s_boot_params.abp_r0 = r0;
 187         s_boot_params.abp_r1 = r1;
 188         s_boot_params.abp_r2 = r2;
 189         s_boot_params.abp_r3 = r3;
 190
 191         /*
 192          * Figure out the physical address the kernel was loaded at.  This
 193          * assumes the entry point (this code right here) is in the first page,
 194          * which will always be the case for this trampoline code.
 195          */
 196         __asm __volatile("mov %0, pc\n"
 197             : "=r" (pc));
 198         kernphysaddr = pc & ~PAGE_MASK;
 199
 200 #if defined(FLASHADDR) && defined(PHYSADDR) && defined(LOADERRAMADDR)
 201         if ((FLASHADDR > LOADERRAMADDR && pc >= FLASHADDR) ||
 202             (FLASHADDR < LOADERRAMADDR && pc < LOADERRAMADDR)) {
 203                 /*
 204                  * We're running from flash, so just copy the whole thing
 205                  * from flash to memory.
 206                  * This is far from optimal, we could do the relocation or
 207                  * the unzipping directly from flash to memory to avoid this
 208                  * needless copy, but it would require to know the flash
 209                  * physical address.
 210                  */
 211                 unsigned int target_addr;
 212                 unsigned int tmp_sp;
 213                 uint32_t src_addr = (uint32_t)&_start - PHYSADDR + FLASHADDR
 214                     + (pc - FLASHADDR - ((uint32_t)&_startC - PHYSADDR)) & 0xfffff000;
 215
 216                 target_addr = (unsigned int)&_start - PHYSADDR + LOADERRAMADDR;
 217                 tmp_sp = target_addr + 0x100000 +
 218                     (unsigned int)&_end - (unsigned int)&_start;
 219                 memcpy((char *)target_addr, (char *)src_addr,
 220                     (unsigned int)&_end - (unsigned int)&_start);
 221                 /* Temporary set the sp and jump to the new location. */
 222                 __asm __volatile(
 223                     "mov sp, %1\n"
 224                     "mov r0, %2\n"
 225                     "mov r1, %3\n"
 226                     "mov r2, %4\n"
 227                     "mov r3, %5\n"
 228                     "mov pc, %0\n"
 229                     : : "r" (target_addr), "r" (tmp_sp),
 230                     "r" (s_boot_params.abp_r0), "r" (s_boot_params.abp_r1),
 231                     "r" (s_boot_params.abp_r2), "r" (s_boot_params.abp_r3)
 232                     : "r0", "r1", "r2", "r3");
 233
 234         }
 235 #endif
 236 #ifdef KZIP
 237         sp += KERNSIZE + 0x100;
 238         sp &= ~(L1_TABLE_SIZE - 1);
 239         sp += 2 * L1_TABLE_SIZE;
 240 #endif
 241         sp += 1024 * 1024; /* Should be enough for a stack */
 242
 243         __asm __volatile("adr %0, 2f\n"
 244                          "bic %0, %0, #0xff000000\n"
 245                          "and %1, %1, #0xff000000\n"
 246                          "orr %0, %0, %1\n"
 247                          "mrc p15, 0, %1, c1, c0, 0\n" /* CP15_SCTLR(%1)*/
 248                          "bic %1, %1, #1\n" /* Disable MMU */
 249                          "orr %1, %1, #(4 | 8)\n" /* Add DC enable,
 250                                                      WBUF enable */
 251                          "orr %1, %1, #0x1000\n" /* Add IC enable */
 252                          "orr %1, %1, #(0x800)\n" /* BPRD enable */
 253
 254                          "mcr p15, 0, %1, c1, c0, 0\n" /* CP15_SCTLR(%1)*/
 255                          "nop\n"
 256                          "nop\n"
 257                          "nop\n"
 258                          "mov pc, %0\n"
 259                          "2: nop\n"
 260                          "mov sp, %2\n"
 261                          : "=r" (tmp1), "+r" (kernphysaddr), "+r" (sp));
 262 #ifndef KZIP
 263 #ifdef CPU_ARM9
 264         /* So that idcache_wbinv works; */
 265         if ((cpu_ident() & 0x0000f000) == 0x00009000)
 266                 arm9_setup();
 267 #endif
 268 #endif
 269         __start();
 270 }
 271
 272 static void
 273 get_cachetype_cp15()
 274 {
 275         u_int ctype, isize, dsize, cpuid;
 276         u_int clevel, csize, i, sel;
 277         u_int multiplier;
 278         u_char type;
 279
 280         __asm __volatile("mrc p15, 0, %0, c0, c0, 1"
 281                 : "=r" (ctype));
 282
 283         cpuid = cpu_ident();
 284         /*
 285          * ...and thus spake the ARM ARM:
 286          *
 287          * If an <opcode2> value corresponding to an unimplemented or
 288          * reserved ID register is encountered, the System Control
 289          * processor returns the value of the main ID register.
 290          */
 291         if (ctype == cpuid)
 292                 goto out;
 293
 294         if (CPU_CT_FORMAT(ctype) == CPU_CT_ARMV7) {
 295                 /* Resolve minimal cache line sizes */
 296                 arm_dcache_min_line_size = 1 << (CPU_CT_DMINLINE(ctype) + 2);
 297                 arm_icache_min_line_size = 1 << (CPU_CT_IMINLINE(ctype) + 2);
 298                 arm_idcache_min_line_size =
 299                     (arm_dcache_min_line_size > arm_icache_min_line_size ?
 300                     arm_icache_min_line_size : arm_dcache_min_line_size);
 301
 302                 __asm __volatile("mrc p15, 1, %0, c0, c0, 1"
 303                     : "=r" (clevel));
 304                 arm_cache_level = clevel;
 305                 arm_cache_loc = CPU_CLIDR_LOC(arm_cache_level) + 1;
 306                 i = 0;
 307                 while ((type = (clevel & 0x7)) && i < 7) {
 308                         if (type == CACHE_DCACHE || type == CACHE_UNI_CACHE ||
 309                             type == CACHE_SEP_CACHE) {
 310                                 sel = i << 1;
 311                                 __asm __volatile("mcr p15, 2, %0, c0, c0, 0"
 312                                     : : "r" (sel));
 313                                 __asm __volatile("mrc p15, 1, %0, c0, c0, 0"
 314                                     : "=r" (csize));
 315                                 arm_cache_type[sel] = csize;
 316                         }
 317                         if (type == CACHE_ICACHE || type == CACHE_SEP_CACHE) {
 318                                 sel = (i << 1) | 1;
 319                                 __asm __volatile("mcr p15, 2, %0, c0, c0, 0"
 320                                     : : "r" (sel));
 321                                 __asm __volatile("mrc p15, 1, %0, c0, c0, 0"
 322                                     : "=r" (csize));
 323                                 arm_cache_type[sel] = csize;
 324                         }
 325                         i++;
 326                         clevel >>= 3;
 327                 }
 328         } else {
 329                 if ((ctype & CPU_CT_S) == 0)
 330                         arm_pcache_unified = 1;
 331
 332                 /*
 333                  * If you want to know how this code works, go read the ARM ARM.
 334                  */
 335
 336                 arm_pcache_type = CPU_CT_CTYPE(ctype);
 337
 338                 if (arm_pcache_unified == 0) {
 339                         isize = CPU_CT_ISIZE(ctype);
 340                         multiplier = (isize & CPU_CT_xSIZE_M) ? 3 : 2;
 341                         arm_picache_line_size = 1U << (CPU_CT_xSIZE_LEN(isize) + 3);
 342                         if (CPU_CT_xSIZE_ASSOC(isize) == 0) {
 343                                 if (isize & CPU_CT_xSIZE_M)
 344                                         arm_picache_line_size = 0; /* not present */
 345                                 else
 346                                         arm_picache_ways = 1;
 347                         } else {
 348                                 arm_picache_ways = multiplier <<
 349                                     (CPU_CT_xSIZE_ASSOC(isize) - 1);
 350                         }
 351                         arm_picache_size = multiplier << (CPU_CT_xSIZE_SIZE(isize) + 8);
 352                 }
 353
 354                 dsize = CPU_CT_DSIZE(ctype);
 355                 multiplier = (dsize & CPU_CT_xSIZE_M) ? 3 : 2;
 356                 arm_pdcache_line_size = 1U << (CPU_CT_xSIZE_LEN(dsize) + 3);
 357                 if (CPU_CT_xSIZE_ASSOC(dsize) == 0) {
 358                         if (dsize & CPU_CT_xSIZE_M)
 359                                 arm_pdcache_line_size = 0; /* not present */
 360                         else
 361                                 arm_pdcache_ways = 1;
 362                 } else {
 363                         arm_pdcache_ways = multiplier <<
 364                             (CPU_CT_xSIZE_ASSOC(dsize) - 1);
 365                 }
 366                 arm_pdcache_size = multiplier << (CPU_CT_xSIZE_SIZE(dsize) + 8);
 367
 368                 arm_dcache_align = arm_pdcache_line_size;
 369
 370                 arm_dcache_l2_assoc = CPU_CT_xSIZE_ASSOC(dsize) + multiplier - 2;
 371                 arm_dcache_l2_linesize = CPU_CT_xSIZE_LEN(dsize) + 3;
 372                 arm_dcache_l2_nsets = 6 + CPU_CT_xSIZE_SIZE(dsize) -
 373                     CPU_CT_xSIZE_ASSOC(dsize) - CPU_CT_xSIZE_LEN(dsize);
 374
 375         out:
 376                 arm_dcache_align_mask = arm_dcache_align - 1;
 377         }
 378 }
 379
 380 static void
 381 arm9_setup(void)
 382 {
 383
 384         get_cachetype_cp15();
 385         arm9_dcache_sets_inc = 1U << arm_dcache_l2_linesize;
 386         arm9_dcache_sets_max = (1U << (arm_dcache_l2_linesize +
 387             arm_dcache_l2_nsets)) - arm9_dcache_sets_inc;
 388         arm9_dcache_index_inc = 1U << (32 - arm_dcache_l2_assoc);
 389         arm9_dcache_index_max = 0U - arm9_dcache_index_inc;
 390 }
 391
 392 static void
 393 armadaxp_idcache_wbinv_all(void)
 394 {
 395         uint32_t feat;
 396
 397         __asm __volatile("mrc p15, 0, %0, c0, c1, 0" : "=r" (feat));
 398         if (feat & ARM_PFR0_THUMBEE_MASK)
 399                 armv7_idcache_wbinv_all();
 400         else
 401                 armv6_idcache_wbinv_all();
 402
 403 }
 404 #ifdef KZIP
 405 static  unsigned char *orig_input, *i_input, *i_output;
 406
 407
 408 static u_int memcnt;            /* Memory allocated: blocks */
 409 static size_t memtot;           /* Memory allocated: bytes */
 410 /*
 411  * Library functions required by inflate().
 412  */
 413
 414 #define MEMSIZ 0x8000
 415
 416 /*
 417  * Allocate memory block.
 418  */
 419 unsigned char *
 420 kzipmalloc(int size)
 421 {
 422         void *ptr;
 423         static u_char mem[MEMSIZ];
 424
 425         if (memtot + size > MEMSIZ)
 426                 return NULL;
 427         ptr = mem + memtot;
 428         memtot += size;
 429         memcnt++;
 430         return ptr;
 431 }
 432
 433 /*
 434  * Free allocated memory block.
 435  */
 436 void
 437 kzipfree(void *ptr)
 438 {
 439         memcnt--;
 440         if (!memcnt)
 441                 memtot = 0;
 442 }
 443
 444 void
 445 putstr(char *dummy)
 446 {
 447 }
 448
 449 static int
 450 input(void *dummy)
 451 {
 452         if ((size_t)(i_input - orig_input) >= KERNCOMPSIZE) {
 453                 return (GZ_EOF);
 454         }
 455         return *i_input++;
 456 }
 457
 458 static int
 459 output(void *dummy, unsigned char *ptr, unsigned long len)
 460 {
 461
 462
 463         memcpy(i_output, ptr, len);
 464         i_output += len;
 465         return (0);
 466 }
 467
 468 static void *
 469 inflate_kernel(void *kernel, void *startaddr)
 470 {
 471         struct inflate infl;
 472         unsigned char slide[GZ_WSIZE];
 473
 474         orig_input = kernel;
 475         memcnt = memtot = 0;
 476         i_input = (unsigned char *)kernel + GZ_HEAD;
 477         if (((char *)kernel)[3] & 0x18) {
 478                 while (*i_input)
 479                         i_input++;
 480                 i_input++;
 481         }
 482         i_output = startaddr;
 483         bzero(&infl, sizeof(infl));
 484         infl.gz_input = input;
 485         infl.gz_output = output;
 486         infl.gz_slide = slide;
 487         inflate(&infl);
 488         return ((char *)(((vm_offset_t)i_output & ~3) + 4));
 489 }
 490
 491 #endif
 492
 493 void *
 494 load_kernel(unsigned int kstart, unsigned int curaddr,unsigned int func_end,
 495     int d)
 496 {
 497         Elf32_Ehdr *eh;
 498         Elf32_Phdr phdr[64] /* XXX */, *php;
 499         Elf32_Shdr shdr[64] /* XXX */;
 500         int i,j;
 501         void *entry_point;
 502         int symtabindex = -1;
 503         int symstrindex = -1;
 504         vm_offset_t lastaddr = 0;
 505         Elf_Addr ssym = 0;
 506         Elf_Dyn *dp;
 507         struct arm_boot_params local_boot_params;
 508
 509         eh = (Elf32_Ehdr *)kstart;
 510         ssym = 0;
 511         entry_point = (void*)eh->e_entry;
 512         memcpy(phdr, (void *)(kstart + eh->e_phoff ),
 513             eh->e_phnum * sizeof(phdr[0]));
 514
 515         /* Determine lastaddr. */
 516         for (i = 0; i < eh->e_phnum; i++) {
 517                 if (lastaddr < (phdr[i].p_vaddr - KERNVIRTADDR + curaddr
 518                     + phdr[i].p_memsz))
 519                         lastaddr = phdr[i].p_vaddr - KERNVIRTADDR +
 520                             curaddr + phdr[i].p_memsz;
 521         }
 522
 523         /* Save the symbol tables, as there're about to be scratched. */
 524         memcpy(shdr, (void *)(kstart + eh->e_shoff),
 525             sizeof(*shdr) * eh->e_shnum);
 526         if (eh->e_shnum * eh->e_shentsize != 0 &&
 527             eh->e_shoff != 0) {
 528                 for (i = 0; i < eh->e_shnum; i++) {
 529                         if (shdr[i].sh_type == SHT_SYMTAB) {
 530                                 for (j = 0; j < eh->e_phnum; j++) {
 531                                         if (phdr[j].p_type == PT_LOAD &&
 532                                             shdr[i].sh_offset >=
 533                                             phdr[j].p_offset &&
 534                                             (shdr[i].sh_offset +
 535                                              shdr[i].sh_size <=
 536                                              phdr[j].p_offset +
 537                                              phdr[j].p_filesz)) {
 538                                                 shdr[i].sh_offset = 0;
 539                                                 shdr[i].sh_size = 0;
 540                                                 j = eh->e_phnum;
 541                                         }
 542                                 }
 543                                 if (shdr[i].sh_offset != 0 &&
 544                                     shdr[i].sh_size != 0) {
 545                                         symtabindex = i;
 546                                         symstrindex = shdr[i].sh_link;
 547                                 }
 548                         }
 549                 }
 550                 func_end = roundup(func_end, sizeof(long));
 551                 if (symtabindex >= 0 && symstrindex >= 0) {
 552                         ssym = lastaddr;
 553                         if (d) {
 554                                 memcpy((void *)func_end, (void *)(
 555                                     shdr[symtabindex].sh_offset + kstart),
 556                                     shdr[symtabindex].sh_size);
 557                                 memcpy((void *)(func_end +
 558                                     shdr[symtabindex].sh_size),
 559                                     (void *)(shdr[symstrindex].sh_offset +
 560                                     kstart), shdr[symstrindex].sh_size);
 561                         } else {
 562                                 lastaddr += shdr[symtabindex].sh_size;
 563                                 lastaddr = roundup(lastaddr,
 564                                     sizeof(shdr[symtabindex].sh_size));
 565                                 lastaddr += sizeof(shdr[symstrindex].sh_size);
 566                                 lastaddr += shdr[symstrindex].sh_size;
 567                                 lastaddr = roundup(lastaddr,
 568                                     sizeof(shdr[symstrindex].sh_size));
 569                         }
 570
 571                 }
 572         }
 573         if (!d)
 574                 return ((void *)lastaddr);
 575
 576         /*
 577          * Now the stack is fixed, copy boot params
 578          * before it's overrided
 579          */
 580         memcpy(&local_boot_params, &s_boot_params, sizeof(local_boot_params));
 581
 582         j = eh->e_phnum;
 583         for (i = 0; i < j; i++) {
 584                 volatile char c;
 585
 586                 if (phdr[i].p_type != PT_LOAD)
 587                         continue;
 588                 memcpy((void *)(phdr[i].p_vaddr - KERNVIRTADDR + curaddr),
 589                     (void*)(kstart + phdr[i].p_offset), phdr[i].p_filesz);
 590                 /* Clean space from oversized segments, eg: bss. */
 591                 if (phdr[i].p_filesz < phdr[i].p_memsz)
 592                         bzero((void *)(phdr[i].p_vaddr - KERNVIRTADDR +
 593                             curaddr + phdr[i].p_filesz), phdr[i].p_memsz -
 594                             phdr[i].p_filesz);
 595         }
 596         /* Now grab the symbol tables. */
 597         if (symtabindex >= 0 && symstrindex >= 0) {
 598                 *(Elf_Size *)lastaddr =
 599                     shdr[symtabindex].sh_size;
 600                 lastaddr += sizeof(shdr[symtabindex].sh_size);
 601                 memcpy((void*)lastaddr,
 602                     (void *)func_end,
 603                     shdr[symtabindex].sh_size);
 604                 lastaddr += shdr[symtabindex].sh_size;
 605                 lastaddr = roundup(lastaddr,
 606                     sizeof(shdr[symtabindex].sh_size));
 607                 *(Elf_Size *)lastaddr =
 608                     shdr[symstrindex].sh_size;
 609                 lastaddr += sizeof(shdr[symstrindex].sh_size);
 610                 memcpy((void*)lastaddr,
 611                     (void*)(func_end +
 612                             shdr[symtabindex].sh_size),
 613                     shdr[symstrindex].sh_size);
 614                 lastaddr += shdr[symstrindex].sh_size;
 615                 lastaddr = roundup(lastaddr,
 616                     sizeof(shdr[symstrindex].sh_size));
 617                 *(Elf_Addr *)curaddr = MAGIC_TRAMP_NUMBER;
 618                 *((Elf_Addr *)curaddr + 1) = ssym - curaddr + KERNVIRTADDR;
 619                 *((Elf_Addr *)curaddr + 2) = lastaddr - curaddr + KERNVIRTADDR;
 620         } else
 621                 *(Elf_Addr *)curaddr = 0;
 622         /* Invalidate the instruction cache. */
 623         __asm __volatile("mcr p15, 0, %0, c7, c5, 0\n"
 624                          "mcr p15, 0, %0, c7, c10, 4\n"
 625                          : : "r" (curaddr));
 626         __asm __volatile("mrc p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 627             "bic %0, %0, #1\n" /* MMU_ENABLE */
 628             "mcr p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 629             : "=r" (ssym));
 630         /* Jump to the entry point. */
 631         ((void(*)(unsigned, unsigned, unsigned, unsigned))
 632         (entry_point - KERNVIRTADDR + curaddr))
 633         (local_boot_params.abp_r0, local_boot_params.abp_r1,
 634         local_boot_params.abp_r2, local_boot_params.abp_r3);
 635         __asm __volatile(".globl func_end\n"
 636             "func_end:");
 637
 638         /* NOTREACHED */
 639         return NULL;
 640 }
 641
 642 extern char func_end[];
 643
 644
 645 #define PMAP_DOMAIN_KERNEL      0 /*
 646                                     * Just define it instead of including the
 647                                     * whole VM headers set.
 648                                     */
 649 int __hack;
 650 static __inline void
 651 setup_pagetables(unsigned int pt_addr, vm_paddr_t physstart, vm_paddr_t physend,
 652     int write_back)
 653 {
 654         unsigned int *pd = (unsigned int *)pt_addr;
 655         vm_paddr_t addr;
 656         int domain = (DOMAIN_CLIENT << (PMAP_DOMAIN_KERNEL * 2)) | DOMAIN_CLIENT;
 657         int tmp;
 658
 659         bzero(pd, L1_TABLE_SIZE);
 660         for (addr = physstart; addr < physend; addr += L1_S_SIZE) {
 661                 pd[addr >> L1_S_SHIFT] = L1_TYPE_S|L1_S_C|L1_S_AP(AP_KRW)|
 662                     L1_S_DOM(PMAP_DOMAIN_KERNEL) | addr;
 663                 if (write_back && 0)
 664                         pd[addr >> L1_S_SHIFT] |= L1_S_B;
 665         }
 666         /* XXX: See below */
 667         if (0xfff00000 < physstart || 0xfff00000 > physend)
 668                 pd[0xfff00000 >> L1_S_SHIFT] = L1_TYPE_S|L1_S_AP(AP_KRW)|
 669                     L1_S_DOM(PMAP_DOMAIN_KERNEL)|physstart;
 670         __asm __volatile("mcr p15, 0, %1, c2, c0, 0\n" /* set TTB */
 671                          "mcr p15, 0, %1, c8, c7, 0\n" /* Flush TTB */
 672                          "mcr p15, 0, %2, c3, c0, 0\n" /* Set DAR */
 673                          "mrc p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 674                          "orr %0, %0, #1\n" /* MMU_ENABLE */
 675                          "mcr p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 676                          "mrc p15, 0, %0, c2, c0, 0\n" /* CPWAIT */
 677                          "mov r0, r0\n"
 678                          "sub pc, pc, #4\n" :
 679                          "=r" (tmp) : "r" (pd), "r" (domain));
 680
 681         /*
 682          * XXX: This is the most stupid workaround I've ever wrote.
 683          * For some reason, the KB9202 won't boot the kernel unless
 684          * we access an address which is not in the
 685          * 0x20000000 - 0x20ffffff range. I hope I'll understand
 686          * what's going on later.
 687          */
 688         __hack = *(volatile int *)0xfffff21c;
 689 }
 690
 691 void
 692 __start(void)
 693 {
 694         void *curaddr;
 695         void *dst, *altdst;
 696         char *kernel = (char *)&kernel_start;
 697         int sp;
 698         int pt_addr;
 699
 700         __asm __volatile("mov %0, pc"  :
 701             "=r" (curaddr));
 702         curaddr = (void*)((unsigned int)curaddr & 0xfff00000);
 703 #ifdef KZIP
 704         if (*kernel == 0x1f && kernel[1] == 0x8b) {
 705                 pt_addr = L1_TABLE_SIZE +
 706                     rounddown2((int)&_end + KERNSIZE + 0x100, L1_TABLE_SIZE);
 707
 708 #ifdef CPU_ARM9
 709                 /* So that idcache_wbinv works; */
 710                 if ((cpu_ident() & 0x0000f000) == 0x00009000)
 711                         arm9_setup();
 712 #endif
 713                 setup_pagetables(pt_addr, (vm_paddr_t)curaddr,
 714                     (vm_paddr_t)curaddr + 0x10000000, 1);
 715                 /* Gzipped kernel */
 716                 dst = inflate_kernel(kernel, &_end);
 717                 kernel = (char *)&_end;
 718                 altdst = 4 + load_kernel((unsigned int)kernel,
 719                     (unsigned int)curaddr,
 720                     (unsigned int)&func_end + 800 , 0);
 721                 if (altdst > dst)
 722                         dst = altdst;
 723
 724                 /*
 725                  * Disable MMU.  Otherwise, setup_pagetables call below
 726                  * might overwrite the L1 table we are currently using.
 727                  */
 728                 cpu_idcache_wbinv_all();
 729                 cpu_l2cache_wbinv_all();
 730                 __asm __volatile("mrc p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 731                   "bic %0, %0, #1\n" /* MMU_DISABLE */
 732                   "mcr p15, 0, %0, c1, c0, 0\n" /* CP15_SCTLR(%0)*/
 733                   :"=r" (pt_addr));
 734         } else
 735 #endif
 736                 dst = 4 + load_kernel((unsigned int)&kernel_start,
 737             (unsigned int)curaddr,
 738             (unsigned int)&func_end, 0);
 739         dst = (void *)(((vm_offset_t)dst & ~3));
 740         pt_addr = L1_TABLE_SIZE + rounddown2((unsigned int)dst, L1_TABLE_SIZE);
 741         setup_pagetables(pt_addr, (vm_paddr_t)curaddr,
 742             (vm_paddr_t)curaddr + 0x10000000, 0);
 743         sp = pt_addr + L1_TABLE_SIZE + 8192;
 744         sp = sp &~3;
 745         dst = (void *)(sp + 4);
 746         memcpy((void *)dst, (void *)&load_kernel, (unsigned int)&func_end -
 747             (unsigned int)&load_kernel + 800);
 748         do_call(dst, kernel, dst + (unsigned int)(&func_end) -
 749             (unsigned int)(&load_kernel) + 800, sp);
 750 }
 751
 752 /* We need to provide these functions but never call them */
 753 void __aeabi_unwind_cpp_pr0(void);
 754 void __aeabi_unwind_cpp_pr1(void);
 755 void __aeabi_unwind_cpp_pr2(void);
 756
 757 __strong_reference(__aeabi_unwind_cpp_pr0, __aeabi_unwind_cpp_pr1);
 758 __strong_reference(__aeabi_unwind_cpp_pr0, __aeabi_unwind_cpp_pr2);
 759 void
 760 __aeabi_unwind_cpp_pr0(void)
 761 {
 762 }