sys/arm/arm/elf_trampoline.c

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