stage2/kernel.c

   1 /*  device.c - lv1 device functions
   2
   3 Copyright (C) 2010-2011  Hector Martin "marcan" <hector@marcansoft.com>
   4
   5 This code is licensed to you under the terms of the GNU GPL, version 2;
   6 see file COPYING or http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
   7 */
   8
   9 #include "types.h"
  10 #include "lv1call.h"
  11 #include "debug.h"
  12 #include "kernel.h"
  13 #include "mm.h"
  14 #include "libfdt.h"
  15 #include "string.h"
  16 #include "elf.h"
  17
  18 #define VECSIZE 65536
  19 static u8 vec_buf[VECSIZE];
  20
  21 #define DT_BUFSIZE 65536
  22
  23 extern char __base[];
  24 extern char __devtree[];
  25 extern char dt_blob_start[];
  26
  27 #define ADDR_LIMIT ((u64)__base)
  28
  29 static char bootargs[MAX_CMDLINE_SIZE];
  30
  31 static u64 *entry[3] = {NULL,NULL,NULL}; // function descriptor for the kernel entrypoint
  32
  33 typedef void (*kernel_entry)(void *devtree, void *self, void *null);
  34
  35 extern volatile u64 _thread1_release;
  36 extern volatile u64 _thread1_vector;
  37
  38 static u8 *initrd_start = NULL;
  39 static size_t initrd_size = 0;
  40
  41 static void devtree_prepare(void)
  42 {
  43         int res, node;
  44         u64 memreg[] = {0, mm_bootmem_size};
  45
  46         res = fdt_open_into(dt_blob_start, __devtree, DT_BUFSIZE);
  47         if (res < 0)
  48                 fatal("fdt_open_into() failed");
  49
  50         node = fdt_path_offset(__devtree, "/chosen");
  51         if (node < 0)
  52                 fatal("/chosen node not found in devtree");
  53
  54         res = fdt_setprop(__devtree, node, "bootargs", bootargs, strlen(bootargs)+1);
  55         if (res < 0)
  56                 fatal("couldn't set chosen.bootargs property");
  57
  58         if (initrd_start && initrd_size)
  59         {
  60                 u64 start, end;
  61                 start = mm_addr_to_kernel(initrd_start);
  62                 res = fdt_setprop(__devtree, node, "linux,initrd-start", &start, sizeof(start));
  63                 if (res < 0)
  64                         fatal("couldn't set chosen.linux,initrd-start property");
  65
  66                 end = mm_addr_to_kernel(initrd_start + initrd_size);
  67                 res = fdt_setprop(__devtree, node, "linux,initrd-end", &end, sizeof(end));
  68                 if (res < 0)
  69                         fatal("couldn't set chosen.linux,initrd-end property");
  70
  71                 res = fdt_add_mem_rsv(__devtree, start, initrd_size);
  72                 if (res < 0)
  73                         fatal("couldn't add reservation for the initrd");
  74         }
  75
  76         node = fdt_path_offset(__devtree, "/memory");
  77         if (node < 0)
  78                 fatal("/memory node not found in devtree");
  79
  80         res = fdt_setprop(__devtree, node, "reg", memreg, sizeof(memreg));
  81         if (res < 0)
  82                 fatal("couldn't set memory.reg property");
  83
  84         res = fdt_add_mem_rsv(__devtree, (u64)__devtree, DT_BUFSIZE);
  85         if (res < 0)
  86                 fatal("couldn't add reservation for the devtree");
  87
  88         res = fdt_pack(__devtree);
  89         if (res < 0)
  90                 fatal("fdt_pack() failed");
  91
  92         printf("Device tree prepared\n");
  93 }
  94
  95 static void vecmemcpy(u64 dest, const void *src, u64 size)
  96 {
  97         const u8 *p = src;
  98         u64 end = dest+size;
  99         if (size && dest < VECSIZE) {
 100                 if (end <= VECSIZE) {
 101                         memcpy(vec_buf+dest, p, size);
 102                         return;
 103                 } else {
 104                         memcpy(vec_buf+dest, p, VECSIZE-dest);
 105                         p += VECSIZE-dest;
 106                         dest = VECSIZE;
 107                         size = end - dest;
 108                 }
 109         }
 110         memcpy((void*)dest, p, size);
 111         sync_before_exec((void*)dest, size);
 112 }
 113
 114 int kernel_load_elf64(const u8 *addr, u32 len)
 115 {
 116         if (len < sizeof(Elf64_Ehdr))
 117                 return -1;
 118
 119         Elf64_Ehdr *ehdr = (Elf64_Ehdr *) addr;
 120
 121         if (ehdr->e_phoff == 0 || ehdr->e_phnum == 0) {
 122                 printf("load_elf_kernel: ELF has no program headers\n");
 123                 return -1;
 124         }
 125
 126         int count = ehdr->e_phnum;
 127         if (len < ehdr->e_phoff + count * sizeof(Elf64_Phdr)) {
 128                 printf("load_elf_kernel: image too short for phdrs\n");
 129                 return -1;
 130         }
 131
 132         Elf64_Phdr *phdr = (Elf64_Phdr *) &addr[ehdr->e_phoff];
 133
 134         while (count--) {
 135                 if (phdr->p_type != PT_LOAD) {
 136                         printf("load_elf_kernel: skipping PHDR of type %d\n", phdr->p_type);
 137                 } else {
 138                         if ((phdr->p_paddr+phdr->p_filesz) > ADDR_LIMIT) {
 139                                 printf("PHDR out of bounds [0x%lx...0x%lx] 0x%lx\n",
 140                                            phdr->p_paddr, phdr->p_paddr + phdr->p_filesz,
 141                                            ADDR_LIMIT);
 142                                 return -1;
 143                         }
 144
 145                         printf("load_elf_kernel: LOAD 0x%lx @0x%lx [0x%lx/0x%lx]\n", phdr->p_offset,
 146                                    phdr->p_paddr, phdr->p_filesz, phdr->p_memsz);
 147
 148                         vecmemcpy(phdr->p_paddr, &addr[phdr->p_offset],
 149                                         phdr->p_filesz);
 150                 }
 151                 phdr++;
 152         }
 153
 154         ehdr->e_entry &= 0x3ffffffffffffffful;
 155
 156         entry[0] = (void*)ehdr->e_entry;
 157
 158         printf("load_elf_kernel: kernel loaded, entry at 0x%lx\n", ehdr->e_entry);
 159
 160         return 0;
 161 }
 162
 163 int kernel_load_elf32(const u8 *addr, u32 len)
 164 {
 165         if (len < sizeof(Elf32_Ehdr)) {
 166                 printf("file smaller than Elf32_Ehdr\n");
 167                 return -1;
 168         }
 169
 170         Elf32_Ehdr *ehdr = (Elf32_Ehdr *) addr;
 171
 172         if (ehdr->e_ehsize != sizeof(Elf32_Ehdr)) {
 173                 printf("sizeof Elf32_Ehdr mismatch\n");
 174                 return -1;
 175         }
 176
 177         if (ehdr->e_phoff == 0 || ehdr->e_phnum == 0) {
 178                 printf("load_elf_kernel: ELF has no program headers\n");
 179                 return -1;
 180         }
 181
 182         int count = ehdr->e_phnum;
 183         if (len < ehdr->e_phoff + count * sizeof(Elf32_Phdr)) {
 184                 printf("load_elf_kernel: image too short for phdrs\n");
 185                 return -1;
 186         }
 187
 188         Elf32_Phdr *phdr = (Elf32_Phdr *) &addr[ehdr->e_phoff];
 189
 190         while (count--) {
 191                 if (phdr->p_type != PT_LOAD) {
 192                         printf("load_elf_kernel: skipping PHDR of type %d\n", phdr->p_type);
 193                 } else {
 194                         if ((phdr->p_paddr+phdr->p_filesz) > ADDR_LIMIT) {
 195                                 printf("PHDR out of bounds [0x%x...0x%x] 0x%lx\n",
 196                                            phdr->p_paddr, phdr->p_paddr + phdr->p_filesz,
 197                                            ADDR_LIMIT);
 198                                 return -1;
 199                         }
 200
 201                         printf("load_elf_kernel: LOAD 0x%x @0x%x [0x%x/0x%x]\n", phdr->p_offset,
 202                                    phdr->p_paddr, phdr->p_filesz, phdr->p_memsz);
 203
 204                         vecmemcpy(phdr->p_paddr, &addr[phdr->p_offset],
 205                                         phdr->p_filesz);
 206                 }
 207                 phdr++;
 208         }
 209
 210         ehdr->e_entry &= 0x3ffffffffffffffful;
 211
 212         entry[0] = (void*)(u64)ehdr->e_entry;
 213
 214         printf("load_elf_kernel: kernel loaded, entry at 0x%x\n", ehdr->e_entry);
 215
 216         return 0;
 217 }
 218
 219 int kernel_load(const u8 *addr, u32 len)
 220 {
 221         memset(vec_buf, 0, sizeof(vec_buf));
 222
 223         if (len < EI_NIDENT) {
 224                 printf("kernel too short\n");
 225                 return -1;
 226         }
 227
 228         if (!memcmp("\x7F" "ELF\x02\x02\x01", addr, 7)) {
 229                 printf("ELF64 kernel detected\n");
 230                 return kernel_load_elf64(addr, len);
 231         }
 232
 233         if (!memcmp("\x7F" "ELF\x01\x02\x01", addr, 7)) {
 234                 printf("ELF32 kernel detected\n");
 235                 return kernel_load_elf32(addr, len);
 236         }
 237
 238         printf("invalid ELF header %02x %02x %02x %02x %02x %02x %02x\n",
 239                         addr[0], addr[1], addr[2], addr[3], addr[4], addr[5], addr[6]);
 240         return -1;
 241 }
 242
 243 void kernel_build_cmdline(const char *parameters, const char *root)
 244 {
 245         bootargs[0] = 0;
 246
 247         if (root) {
 248                 strlcat(bootargs, "root=", MAX_CMDLINE_SIZE);
 249                 strlcat(bootargs, root, MAX_CMDLINE_SIZE);
 250                 strlcat(bootargs, " ", MAX_CMDLINE_SIZE);
 251         }
 252
 253         if (parameters)
 254                 strlcat(bootargs, parameters, MAX_CMDLINE_SIZE);
 255
 256         printf("Kernel command line: '%s'\n", bootargs);
 257 }
 258
 259 void kernel_set_initrd(void *start, size_t size)
 260 {
 261         printf("Initrd at %p/0x%lx: %ld bytes (%ldKiB)\n", start, \
 262         mm_addr_to_kernel(start), size, size/1024);
 263
 264         initrd_start = start;
 265         initrd_size = size;
 266 }
 267
 268 void kernel_launch(void)
 269 {
 270         devtree_prepare();
 271
 272         if (initrd_start && initrd_size)
 273                 mm_set_highmem_repo_info();
 274         else
 275                 mm_shutdown_highmem();
 276
 277         printf("Relocating vectors...\n");
 278         memcpy((void*)0, vec_buf, VECSIZE);
 279         sync_before_exec((void*)0, VECSIZE);
 280         printf("Letting thread1 run loose...\n");
 281         _thread1_vector = (u64)entry[0] + 0x60; /* this is __secondary_hold */
 282         _thread1_release = 1;
 283         printf("Taking the plunge...\n");
 284         debug_shutdown();
 285         ((kernel_entry)entry)(__devtree, entry[0], NULL);
 286         lv1_panic(0);
 287 }