loader/loader_c.c

   1 /*
   2  * Copyright (c) 2007-2009 Josef 'Jeff' Sipek
   3  */
   4
   5 #include "loader.h"
   6 #include <binfmt_elf.h>
   7
   8 static unsigned char seek_data[6];
   9 static unsigned char search_data[5];
  10 static struct ccw ccw[4];
  11
  12 struct orb ORB = {
  13         .param          = 0x12345678,
  14         .f              = 1,
  15         .lpm            = 0xff,
  16         .addr           = 0xffffffff,
  17 };
  18
  19 static u8 *buf = (u8*) (16 * 1024);
  20 static u32 *ptrbuf = (u32*) (20 * 1024);
  21
  22 /*
  23  * halt the cpu
  24  *
  25  * NOTE: we don't care about not clobbering registers as when this
  26  * code executes, the CPU will be stopped.
  27  */
  28 static inline void die(void)
  29 {
  30         asm volatile(
  31                 "SR     %r1, %r1        # not used, but should be zero\n"
  32                 "SR     %r3, %r3        # CPU Address\n"
  33                 "SIGP   %r1, %r3, 0x05  # Signal, order 0x05\n"
  34         );
  35
  36         /*
  37          * Just in case SIGP fails
  38          */
  39         for(;;);
  40 }
  41
  42 static u64 pgm_new_psw[2] = {
  43         0x0000000180000000ULL, (u64) &PGMHANDLER,
  44 };
  45
  46 /*
  47  * determine amount of storage
  48  */
  49 static u64 sense_memsize(void)
  50 {
  51         u64 size;
  52         int cc;
  53
  54 #define SKIP_SIZE       (1024*1024ULL)
  55
  56         /* set new PGM psw */
  57         memcpy((void*)0x1d0, pgm_new_psw, 16);
  58
  59         for(size = 0; size < ((u64)~SKIP_SIZE)-1; size += SKIP_SIZE) {
  60                 asm volatile(
  61                         "lg     %%r1,%1\n"
  62                         "tprot  0(%%r1),0\n"
  63                         "ipm    %0\n"
  64                         "srl    %0,28\n"
  65                 : /* output */
  66                   "=d" (cc)
  67                 : /* input */
  68                   "m" (size)
  69                 : /* clobber */
  70                   "cc", "r1"
  71                 );
  72
  73                 /*
  74                  * we cheat here a little...if we try to tprot a location
  75                  * that isn't part of the configuration, a program exception
  76                  * fires off, but our handler sets the CC to 3, and resumes
  77                  * execution
  78                  */
  79                 if (cc == 3)
  80                         break;
  81         }
  82
  83         /* invalidate new PGM psw */
  84         memset((void*)0x1d0, 0, 16);
  85
  86         return size;
  87 }
  88
  89 static void read_blk(void *ptr, u32 lba)
  90 {
  91         u16 cc, hh, r;
  92
  93         if (lba < 1)
  94                 die();
  95
  96         memset(ccw, 0, sizeof(ccw));
  97
  98         lba--;
  99         cc = lba / RECORDS_PER_CYL;
 100         hh = (lba % RECORDS_PER_CYL) / RECORDS_PER_TRACK;
 101         r = (lba % RECORDS_PER_CYL) % RECORDS_PER_TRACK;
 102         r++;
 103
 104         ORB.addr = ADDR31(ccw);
 105
 106         /* SEEK */
 107         ccw[0].cmd = 0x07;
 108         ccw[0].flags = CCW_FLAG_CC | CCW_FLAG_SLI;
 109         ccw[0].count = 6;
 110         ccw[0].addr = ADDR31(seek_data);
 111
 112         seek_data[0] = 0;               /* zero */
 113         seek_data[1] = 0;               /* zero */
 114         seek_data[2] = cc >> 8;         /* Cc */
 115         seek_data[3] = cc & 0xff;       /* cC */
 116         seek_data[4] = hh >> 8;         /* Hh */
 117         seek_data[5] = hh & 0xff;       /* hH */
 118
 119         /* SEARCH */
 120         ccw[1].cmd = 0x31;
 121         ccw[1].flags = CCW_FLAG_CC | CCW_FLAG_SLI;
 122         ccw[1].count = 5;
 123         ccw[1].addr = ADDR31(search_data);
 124
 125         search_data[0] = cc >> 8;
 126         search_data[1] = cc & 0xff;
 127         search_data[2] = hh >> 8;
 128         search_data[3] = hh & 0xff;
 129         search_data[4] = r;
 130
 131         /* TIC */
 132         ccw[2].cmd = 0x08;
 133         ccw[2].flags = 0;
 134         ccw[2].count = 0;
 135         ccw[2].addr = ADDR31(&ccw[1]);
 136
 137         /* READ DATA */
 138         ccw[3].cmd = 0x86;
 139         ccw[3].flags = 0;
 140         ccw[3].count = 4096;
 141         ccw[3].addr = ADDR31(ptr);
 142
 143         /*
 144          * issue IO
 145          */
 146         __do_io();
 147 }
 148
 149 /*
 150  * read the entire nucleus into TEMP_BASE
 151  */
 152 static inline void readnucleus(void)
 153 {
 154         struct ADT *ADT = (struct ADT*) buf;
 155         struct FST *FST = (struct FST*) buf;
 156         struct FST fst;
 157         int i, found;
 158         u32 nfst;
 159
 160         read_blk(buf, EDF_LABEL_BLOCK_NO);
 161
 162         if ((ADT->IDENT != __ADTIDENT) ||
 163             (ADT->DBSIZ != EDF_SUPPORTED_BLOCK_SIZE) ||
 164             (ADT->OFFST != 0) ||
 165             (ADT->FSTSZ != sizeof(struct FST)))
 166                 die();
 167
 168         nfst = ADT->NFST;
 169
 170         read_blk(buf, ADT->DOP);
 171
 172         if (FST->NLVL != 0)
 173                die(); // FIXME
 174
 175         for(i=0,found=0; i<nfst; i++) {
 176                 if ((!memcmp(FST[i].FNAME, CP_FN, 8)) &&
 177                     (!memcmp(FST[i].FTYPE, CP_FT, 8))) {
 178                         memcpy(&fst, &FST[i], sizeof(struct FST));
 179                         found = 1;
 180                         break;
 181                 }
 182         }
 183
 184         if (!found)
 185                 die();
 186
 187         if (fst.PTRSZ != 4 ||
 188             fst.LRECL != 4096 ||
 189             fst.RECFM != FSTDFIX)
 190                 die();
 191
 192         /* Don't allow more than 3MB to be read */
 193         if ((FST->AIC * FST->LRECL) > (3ULL * 1024 * 1024))
 194                 die();
 195
 196         /* Since we're assuming that NLVL==1, there's only 1 pointer block */
 197         read_blk(ptrbuf, fst.FOP);
 198
 199         /* Read all the blocks pointed to by the ptr block */
 200         for(i=0; i<fst.AIC; i++)
 201                 read_blk(TEMP_BASE + (4096 * i), ptrbuf[i]);
 202 }
 203
 204 void load_nucleus(void)
 205 {
 206         /*
 207          * These are all stored in registers
 208          */
 209         register u32 iplsch;
 210         register int i, j;
 211         register Elf64_Ehdr *nucleus_elf;
 212         register Elf64_Shdr *section, *link_section, *tmp_section;
 213         register Elf64_Phdr *segment;
 214         register Elf64_Ehdr *elfcpy;
 215         register unsigned char *symtab;
 216         register void (*start_sym)(u64, u32, void*);
 217
 218         /* Save the IPL device subchannel id */
 219         iplsch = *((u32*) 0xb8);
 220
 221         /*
 222          * Read entire ELF to temporary location
 223          */
 224         readnucleus();
 225
 226         nucleus_elf = (Elf64_Ehdr*) TEMP_BASE;
 227
 228         /*
 229          * Check that this looks like a valid ELF
 230          */
 231         if (nucleus_elf->e_ident[0] != '\x7f' ||
 232             nucleus_elf->e_ident[1] != 'E' ||
 233             nucleus_elf->e_ident[2] != 'L' ||
 234             nucleus_elf->e_ident[3] != 'F' ||
 235             nucleus_elf->e_ident[EI_CLASS] != ELFCLASS64 ||
 236             nucleus_elf->e_ident[EI_DATA] != ELFDATA2MSB ||
 237             nucleus_elf->e_ident[EI_VERSION] != EV_CURRENT ||
 238             nucleus_elf->e_type != ET_EXEC ||
 239             nucleus_elf->e_machine != 0x16 || // FIXME: find the name for the #define
 240             nucleus_elf->e_version != EV_CURRENT)
 241                 die();
 242
 243         /* Iterate through each program header, and copy all PT_LOAD
 244          * segments to the final destinations.
 245          */
 246
 247         for(i=0; i<nucleus_elf->e_phnum; i++) {
 248                 segment = (Elf64_Phdr*) (TEMP_BASE +
 249                                          nucleus_elf->e_phoff +
 250                                          nucleus_elf->e_phentsize * i);
 251
 252                 if (segment->p_type != PT_LOAD)
 253                         continue;
 254
 255                 if (segment->p_filesz)
 256                         memcpy((void*) segment->p_vaddr,
 257                                TEMP_BASE + segment->p_offset,
 258                                segment->p_filesz);
 259
 260                 if (segment->p_filesz != segment->p_memsz)
 261                         memset((void*) (segment->p_vaddr + segment->p_filesz),
 262                                0, segment->p_memsz - segment->p_filesz);
 263         }
 264
 265         /*
 266          * Copy over the ELF header & tweak it.
 267          */
 268         symtab = SYMTAB_BASE;
 269         elfcpy = (Elf64_Ehdr*) symtab;
 270         memcpy(elfcpy, nucleus_elf, nucleus_elf->e_ehsize);
 271         symtab += nucleus_elf->e_ehsize;
 272
 273         /* the program headers start right after */
 274         elfcpy->e_phoff = symtab - SYMTAB_BASE;
 275
 276         /* copy over all the program headers */
 277         memcpy(symtab, TEMP_BASE + nucleus_elf->e_phoff,
 278                nucleus_elf->e_phentsize * nucleus_elf->e_phnum);
 279         symtab += nucleus_elf->e_phentsize * nucleus_elf->e_phnum;
 280
 281         /* the section headers start right after */
 282         elfcpy->e_shoff = symtab - SYMTAB_BASE;
 283
 284         /*
 285          * Iterate through each section to find the .symtab & .strtab (as
 286          * well as the null section), and copy the the headers over
 287          */
 288         elfcpy->e_shnum = 0;
 289
 290         for(i=0; i<nucleus_elf->e_shnum; i++) {
 291                 section = (Elf64_Shdr*) (TEMP_BASE +
 292                                          nucleus_elf->e_shoff +
 293                                          nucleus_elf->e_shentsize * i);
 294
 295                 switch (section->sh_type) {
 296                         case SHT_STRTAB:
 297                                 if (i == nucleus_elf->e_shstrndx)
 298                                         elfcpy->e_shstrndx = elfcpy->e_shnum;
 299
 300                         case SHT_NULL:
 301                         case SHT_SYMTAB:
 302                                 memcpy(symtab, section, nucleus_elf->e_shentsize);
 303                                 elfcpy->e_shnum++;
 304                                 symtab += nucleus_elf->e_shentsize;
 305                                 break;
 306                         default:
 307                                 /* Ignoring */
 308                                 break;
 309                 }
 310         }
 311
 312         for(i=0; i<elfcpy->e_shnum; i++) {
 313                 section = (Elf64_Shdr*) (SYMTAB_BASE +
 314                                          elfcpy->e_shoff +
 315                                          elfcpy->e_shentsize * i);
 316
 317                 if (!section->sh_link)
 318                         continue;
 319
 320                 tmp_section = (Elf64_Shdr*) (TEMP_BASE +
 321                                          nucleus_elf->e_shoff +
 322                                          nucleus_elf->e_shentsize *
 323                                          section->sh_link);
 324
 325                 for(j=0; j<elfcpy->e_shnum; j++) {
 326                         link_section = (Elf64_Shdr*) (SYMTAB_BASE +
 327                                                       elfcpy->e_shoff +
 328                                                       elfcpy->e_shentsize * j);
 329
 330                         if (memcmp(tmp_section, link_section, elfcpy->e_shentsize))
 331                                 continue;
 332
 333                         section->sh_link = j;
 334                         break;
 335                 }
 336         }
 337
 338         /*
 339          * Now that we know what sections and where, let's copy those over
 340          */
 341         for (i=1; i<elfcpy->e_shnum; i++) {
 342                 section = (Elf64_Shdr*) (SYMTAB_BASE +
 343                                          elfcpy->e_shoff +
 344                                          elfcpy->e_shentsize * i);
 345
 346                 memcpy(symtab, TEMP_BASE + section->sh_offset,
 347                        section->sh_size);
 348                 section->sh_offset = symtab - SYMTAB_BASE;
 349                 symtab += section->sh_size;
 350         }
 351
 352         /*
 353          * Now, jump to the nucleus entry point
 354          */
 355         start_sym = (void*) nucleus_elf->e_entry;
 356         start_sym(sense_memsize(), iplsch, SYMTAB_BASE);
 357 }