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 memreg1[] = {0, mm_bootmem_size};
  45         u64 memreg2[] = {mm_highmem_addr, mm_highmem_size};
  46
  47         res = fdt_open_into(dt_blob_start, __devtree, DT_BUFSIZE);
  48         if (res < 0)
  49                 fatal("fdt_open_into() failed");
  50
  51         node = fdt_path_offset(__devtree, "/chosen");
  52         if (node < 0)
  53                 fatal("/chosen node not found in devtree");
  54
  55         res = fdt_setprop(__devtree, node, "bootargs", bootargs, strlen(bootargs)+1);
  56         if (res < 0)
  57                 fatal("couldn't set chosen.bootargs property");
  58
  59         if (initrd_start && initrd_size)
  60         {
  61                 u64 start, end;
  62                 start = mm_addr_to_kernel(initrd_start);
  63                 res = fdt_setprop(__devtree, node, "linux,initrd-start", &start, sizeof(start));
  64                 if (res < 0)
  65                         fatal("couldn't set chosen.linux,initrd-start property");
  66
  67                 end = mm_addr_to_kernel(initrd_start + initrd_size);
  68                 res = fdt_setprop(__devtree, node, "linux,initrd-end", &end, sizeof(end));
  69                 if (res < 0)
  70                         fatal("couldn't set chosen.linux,initrd-end property");
  71
  72                 res = fdt_add_mem_rsv(__devtree, start, initrd_size);
  73                 if (res < 0)
  74                         fatal("couldn't add reservation for the initrd");
  75         }
  76
  77         node = fdt_path_offset(__devtree, "/memory");
  78         if (node < 0)
  79                 fatal("/memory node not found in devtree");
  80
  81         res = fdt_setprop(__devtree, node, "reg", memreg1, sizeof(memreg1));
  82         if (res < 0)
  83                 fatal("couldn't set memory.reg property");
  84
  85         res = fdt_setprop(__devtree, node, "sony,lv1-highmem", memreg2, sizeof(memreg2));
  86         if (res < 0)
  87                 fatal("couldn't set memory.sony,lv1-highmem property");
  88
  89         res = fdt_add_mem_rsv(__devtree, (u64)__devtree, DT_BUFSIZE);
  90         if (res < 0)
  91                 fatal("couldn't add reservation for the devtree");
  92
  93         res = fdt_pack(__devtree);
  94         if (res < 0)
  95                 fatal("fdt_pack() failed");
  96
  97         printf("Device tree prepared\n");
  98 }
  99
 100 static void vecmemclr(u64 dest, u64 size)
 101 {
 102         u64 end = dest+size;
 103         if (size && dest < VECSIZE) {
 104                 if (end <= VECSIZE)
 105                         return;
 106                 dest = VECSIZE;
 107                 size = end - dest;
 108         }
 109         memset((void*)dest, 0, size);
 110         sync_before_exec((void*)dest, size);
 111 }
 112
 113 static void vecmemcpy(u64 dest, const void *src, u64 size)
 114 {
 115         const u8 *p = src;
 116         u64 end = dest+size;
 117         if (size && dest < VECSIZE) {
 118                 if (end <= VECSIZE) {
 119                         memcpy(vec_buf+dest, p, size);
 120                         return;
 121                 } else {
 122                         memcpy(vec_buf+dest, p, VECSIZE-dest);
 123                         p += VECSIZE-dest;
 124                         dest = VECSIZE;
 125                         size = end - dest;
 126                 }
 127         }
 128         memcpy((void*)dest, p, size);
 129         sync_before_exec((void*)dest, size);
 130 }
 131
 132 int kernel_load(const u8 *addr, u32 len)
 133 {
 134         memset(vec_buf, 0, sizeof(vec_buf));
 135
 136         if (len < sizeof(Elf64_Ehdr))
 137                 return -1;
 138
 139         Elf64_Ehdr *ehdr = (Elf64_Ehdr *) addr;
 140
 141         if (memcmp("\x7F" "ELF\x02\x02\x01", ehdr->e_ident, 7)) {
 142                 printf("load_elf_kernel: invalid ELF header 0x%02x 0x%02x 0x%02x 0x%02x\n",
 143                                                 ehdr->e_ident[0], ehdr->e_ident[1],
 144                                                 ehdr->e_ident[2], ehdr->e_ident[3]);
 145                 return -1;
 146         }
 147
 148         if (ehdr->e_phoff == 0 || ehdr->e_phnum == 0) {
 149                 printf("load_elf_kernel: ELF has no program headers\n");
 150                 return -1;
 151         }
 152
 153         int count = ehdr->e_phnum;
 154         if (len < ehdr->e_phoff + count * sizeof(Elf64_Phdr)) {
 155                 printf("load_elf_kernel: image too short for phdrs\n");
 156                 return -1;
 157         }
 158
 159         Elf64_Phdr *phdr = (Elf64_Phdr *) &addr[ehdr->e_phoff];
 160
 161         while (count--) {
 162                 if (phdr->p_type != PT_LOAD) {
 163                         printf("load_elf_kernel: skipping PHDR of type %d\n", phdr->p_type);
 164                 } else {
 165                         if ((phdr->p_paddr+phdr->p_memsz) > ADDR_LIMIT) {
 166                                 printf("PHDR out of bounds [0x%lx...0x%lx]\n",
 167                                            phdr->p_paddr, phdr->p_paddr + phdr->p_memsz);
 168                                 return -1;
 169                         }
 170
 171                         printf("load_elf_kernel: LOAD 0x%lx @0x%lx [0x%lx/0x%lx]\n", phdr->p_offset,
 172                                    phdr->p_paddr, phdr->p_filesz, phdr->p_memsz);
 173
 174                         vecmemclr(phdr->p_paddr, phdr->p_memsz);
 175                         vecmemcpy(phdr->p_paddr, &addr[phdr->p_offset],
 176                                         phdr->p_filesz);
 177                 }
 178                 phdr++;
 179         }
 180
 181         ehdr->e_entry &= 0x3ffffffffffffffful;
 182
 183         entry[0] = (void*)ehdr->e_entry;
 184
 185         printf("load_elf_kernel: kernel loaded, entry at 0x%lx\n", ehdr->e_entry);
 186
 187         return 0;
 188 }
 189
 190 void kernel_build_cmdline(const char *parameters, const char *root)
 191 {
 192         bootargs[0] = 0;
 193
 194         if (root) {
 195                 strlcat(bootargs, "root=", MAX_CMDLINE_SIZE);
 196                 strlcat(bootargs, root, MAX_CMDLINE_SIZE);
 197                 strlcat(bootargs, " ", MAX_CMDLINE_SIZE);
 198         }
 199
 200         if (parameters)
 201                 strlcat(bootargs, parameters, MAX_CMDLINE_SIZE);
 202
 203         printf("Kernel command line: '%s'\n", bootargs);
 204 }
 205
 206 void kernel_set_initrd(void *start, size_t size)
 207 {
 208         printf("Initrd at %p/0x%lx: %ld bytes (%ldKiB)\n", start, \
 209         mm_addr_to_kernel(start), size, size/1024);
 210
 211         initrd_start = start;
 212         initrd_size = size;
 213 }
 214
 215 void kernel_launch(void)
 216 {
 217         devtree_prepare();
 218         printf("Relocating vectors...\n");
 219         memcpy((void*)0, vec_buf, VECSIZE);
 220         sync_before_exec((void*)0, VECSIZE);
 221         printf("Letting thread1 run loose...\n");
 222         _thread1_vector = 0x60; /* this is __secondary_hold in Linux */
 223         _thread1_release = 1;
 224         printf("Taking the plunge...\n");
 225         debug_shutdown();
 226         ((kernel_entry)entry)(__devtree, entry[0], NULL);
 227         lv1_panic(0);
 228 }