loader/patches.c

   1 // vim:shiftwidth=2:expandtab
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <sys/types.h>
   5 #include <sys/stat.h>
   6 #include <fcntl.h>
   7 #include <sys/mman.h>
   8 #include <unistd.h>
   9
  10 #include "header.h"
  11 #include "sys_cacheflush.h"
  12
  13 static const unsigned int sig_open[] = {
  14   0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900005
  15 };
  16
  17 static const unsigned int sig_mmap[] = {
  18   0xe92d000f, 0xe1a0000d, 0xef90005a, 0xe28dd010
  19 };
  20
  21 static const unsigned int sig_mmap_[] = {
  22   0xe52d5004, 0xe59d5008, 0xe52d4004, 0xe59d4008,
  23   0xe1b0ca05, 0x1a000006, 0xe1a05625, 0xef9000c0
  24 };
  25
  26 static const unsigned int sig_read[] = {
  27   0xe59fc080, 0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900003
  28 };
  29
  30 #define FAKE_DEVMEM_DEVICE      10001
  31 #define FAKE_DEVGPIO_DEVICE     10002
  32
  33 static int w_open(const char *pathname, int flags, mode_t mode)
  34 {
  35   int ret;
  36   if      (strcmp(pathname, "/dev/mem") == 0)
  37     ret = FAKE_DEVMEM_DEVICE;
  38   else if (strcmp(pathname, "/dev/GPIO") == 0)
  39     ret = FAKE_DEVGPIO_DEVICE;
  40   else
  41     ret = open(pathname, flags, mode);
  42
  43   printf("open(%s) = %d\n", pathname, ret);
  44   return ret;
  45 }
  46
  47 static void *w_mmap(void *addr, size_t length, int prot, int flags, int fd, off_t offset)
  48 {
  49   void *ret;
  50   if (fd != FAKE_DEVMEM_DEVICE)
  51     ret = mmap(addr, length, prot, flags, fd, offset);
  52   else
  53     ret = emu_mmap_dev(length, prot, flags, offset);
  54
  55   printf("mmap(%p, %x, %x, %x, %d, %lx) = %p\n", addr, length, prot, flags, fd, (long)offset, ret);
  56   return ret;
  57 }
  58 #define w_mmap_ w_mmap
  59
  60 ssize_t w_read(int fd, void *buf, size_t count)
  61 {
  62   ssize_t ret;
  63   if (fd != FAKE_DEVGPIO_DEVICE)
  64     return read(fd, buf, count);
  65
  66   ret = emu_read_gpiodev(buf, count);
  67   //printf("read(%d, %p, %d) = %d\n", fd, buf, count, ret);
  68   return ret;
  69 }
  70
  71 #define PATCH(f) { sig_##f, ARRAY_SIZE(sig_##f), w_##f }
  72
  73 static const struct {
  74   const unsigned int *sig;
  75   size_t sig_cnt;
  76   void *func;
  77 } patches[] = {
  78   PATCH(open),
  79   PATCH(mmap),
  80   PATCH(mmap_), // mmap using mmap2 syscall
  81   PATCH(read),
  82 };
  83
  84 void do_patches(void *ptr, unsigned int size)
  85 {
  86   int i, s;
  87
  88   for (i = 0; i < ARRAY_SIZE(patches); i++) {
  89     const unsigned int *sig = patches[i].sig;
  90     unsigned int *seg = (void *)(((long)ptr + 3) & ~3);
  91     unsigned int *seg_end = seg + size / 4;
  92     unsigned int sig0 = sig[0];
  93
  94     for (; seg < seg_end; seg++) {
  95       if (*seg != sig0)
  96         continue;
  97
  98       for (s = 1; s < patches[i].sig_cnt; s++)
  99         if (seg[s] != sig[s])
 100           break;
 101
 102       if (s == patches[i].sig_cnt)
 103         goto found;
 104     }
 105     continue;
 106
 107 found:
 108     printf("  patch #%i @ %08x\n", i, (int)seg);
 109     seg[0] = 0xe59ff000; // ldr pc, [pc]
 110     seg[1] = 0;
 111     seg[2] = (unsigned int)patches[i].func;
 112   }
 113
 114   sys_cacheflush(ptr, (char *)ptr + size);
 115 }
 116