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(const u8 *addr, u32 len)
 115 {
 116         memset(vec_buf, 0, sizeof(vec_buf));
 117
 118         if (len < sizeof(Elf64_Ehdr))
 119                 return -1;
 120
 121         Elf64_Ehdr *ehdr = (Elf64_Ehdr *) addr;
 122
 123         if (memcmp("\x7F" "ELF\x02\x02\x01", ehdr->e_ident, 7)) {
 124                 printf("load_elf_kernel: invalid ELF header 0x%02x 0x%02x 0x%02x 0x%02x\n",
 125                                                 ehdr->e_ident[0], ehdr->e_ident[1],
 126                                                 ehdr->e_ident[2], ehdr->e_ident[3]);
 127                 return -1;
 128         }
 129
 130         if (ehdr->e_phoff == 0 || ehdr->e_phnum == 0) {
 131                 printf("load_elf_kernel: ELF has no program headers\n");
 132                 return -1;
 133         }
 134
 135         int count = ehdr->e_phnum;
 136         if (len < ehdr->e_phoff + count * sizeof(Elf64_Phdr)) {
 137                 printf("load_elf_kernel: image too short for phdrs\n");
 138                 return -1;
 139         }
 140
 141         Elf64_Phdr *phdr = (Elf64_Phdr *) &addr[ehdr->e_phoff];
 142
 143         while (count--) {
 144                 if (phdr->p_type != PT_LOAD) {
 145                         printf("load_elf_kernel: skipping PHDR of type %d\n", phdr->p_type);
 146                 } else {
 147                         if ((phdr->p_paddr+phdr->p_filesz) > ADDR_LIMIT) {
 148                                 printf("PHDR out of bounds [0x%lx...0x%lx] 0x%lx\n",
 149                                            phdr->p_paddr, phdr->p_paddr + phdr->p_filesz,
 150                                            ADDR_LIMIT);
 151                                 return -1;
 152                         }
 153
 154                         printf("load_elf_kernel: LOAD 0x%lx @0x%lx [0x%lx/0x%lx]\n", phdr->p_offset,
 155                                    phdr->p_paddr, phdr->p_filesz, phdr->p_memsz);
 156
 157                         vecmemcpy(phdr->p_paddr, &addr[phdr->p_offset],
 158                                         phdr->p_filesz);
 159                 }
 160                 phdr++;
 161         }
 162
 163         ehdr->e_entry &= 0x3ffffffffffffffful;
 164
 165         entry[0] = (void*)ehdr->e_entry;
 166
 167         printf("load_elf_kernel: kernel loaded, entry at 0x%lx\n", ehdr->e_entry);
 168
 169         return 0;
 170 }
 171
 172 void kernel_build_cmdline(const char *parameters, const char *root)
 173 {
 174         bootargs[0] = 0;
 175
 176         if (root) {
 177                 strlcat(bootargs, "root=", MAX_CMDLINE_SIZE);
 178                 strlcat(bootargs, root, MAX_CMDLINE_SIZE);
 179                 strlcat(bootargs, " ", MAX_CMDLINE_SIZE);
 180         }
 181
 182         if (parameters)
 183                 strlcat(bootargs, parameters, MAX_CMDLINE_SIZE);
 184
 185         printf("Kernel command line: '%s'\n", bootargs);
 186 }
 187
 188 void kernel_set_initrd(void *start, size_t size)
 189 {
 190         printf("Initrd at %p/0x%lx: %ld bytes (%ldKiB)\n", start, \
 191         mm_addr_to_kernel(start), size, size/1024);
 192
 193         initrd_start = start;
 194         initrd_size = size;
 195 }
 196
 197 void kernel_launch(void)
 198 {
 199         devtree_prepare();
 200
 201         if (initrd_start && initrd_size)
 202                 mm_set_highmem_repo_info();
 203         else
 204                 mm_shutdown_highmem();
 205
 206         printf("Relocating vectors...\n");
 207         memcpy((void*)0, vec_buf, VECSIZE);
 208         sync_before_exec((void*)0, VECSIZE);
 209         printf("Letting thread1 run loose...\n");
 210         _thread1_vector = 0x60; /* this is __secondary_hold in Linux */
 211         _thread1_release = 1;
 212         printf("Taking the plunge...\n");
 213         debug_shutdown();
 214         ((kernel_entry)entry)(__devtree, entry[0], NULL);
 215         lv1_panic(0);
 216 }