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 #define strace(...)
  14 //#define strace printf
  15
  16 static const unsigned int sig_open[] = {
  17   0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900005
  18 };
  19
  20 static const unsigned int sig_mmap[] = {
  21   0xe92d000f, 0xe1a0000d, 0xef90005a, 0xe28dd010
  22 };
  23
  24 static const unsigned int sig_mmap_[] = {
  25   0xe52d5004, 0xe59d5008, 0xe52d4004, 0xe59d4008,
  26   0xe1b0ca05, 0x1a000006, 0xe1a05625, 0xef9000c0
  27 };
  28
  29 static const unsigned int sig_read[] = {
  30   0xe59fc080, 0xe59cc000, 0xe33c0000, 0x1a000003, 0xef900003
  31 };
  32
  33 enum {
  34   FAKEDEV_MEM = 10001,
  35   FAKEDEV_GPIO,
  36   FAKEDEV_FB0,
  37   FAKEDEV_FB1,
  38   FAKEDEV_MMUHACK,
  39 };
  40
  41 static const struct {
  42   const char *name;
  43   int fd;
  44 } takeover_devs[] = {
  45   { "/dev/mem",     FAKEDEV_MEM },
  46   { "/dev/GPIO",    FAKEDEV_GPIO },
  47   { "/dev/fb0",     FAKEDEV_FB0 },
  48   { "/dev/fb/0",    FAKEDEV_FB0 },
  49   { "/dev/fb1",     FAKEDEV_FB1 },
  50   { "/dev/fb/1",    FAKEDEV_FB1 },
  51   { "/dev/mmuhack", FAKEDEV_MMUHACK },
  52 };
  53
  54 static int w_open(const char *pathname, int flags, mode_t mode)
  55 {
  56   int i, ret;
  57
  58   for (i = 0; i < ARRAY_SIZE(takeover_devs); i++) {
  59     if (strcmp(pathname, takeover_devs[i].name) == 0) {
  60       ret = takeover_devs[i].fd;
  61       break;
  62     }
  63   }
  64
  65   if (i == ARRAY_SIZE(takeover_devs))
  66     ret = open(pathname, flags, mode);
  67
  68   strace("open(%s) = %d\n", pathname, ret);
  69   return ret;
  70 }
  71
  72 static void *w_mmap(void *addr, size_t length, int prot, int flags, int fd, off_t offset)
  73 {
  74   void *ret;
  75   if (fd != FAKEDEV_MEM)
  76     ret = mmap(addr, length, prot, flags, fd, offset);
  77   else
  78     ret = emu_mmap_dev(length, prot, flags, offset);
  79
  80   // threads are using heap before they mmap their stack
  81   // printf needs valid stack for pthtead/errno
  82   if (((long)&ret & 0xf0000000) == 0xb0000000)
  83     strace("mmap(%p, %x, %x, %x, %d, %lx) = %p\n", addr, length, prot, flags, fd, (long)offset, ret);
  84   return ret;
  85 }
  86 #define w_mmap_ w_mmap
  87
  88 ssize_t w_read(int fd, void *buf, size_t count)
  89 {
  90   ssize_t ret;
  91   if (fd != FAKEDEV_GPIO)
  92     return read(fd, buf, count);
  93
  94   ret = emu_read_gpiodev(buf, count);
  95   //printf("read(%d, %p, %d) = %d\n", fd, buf, count, ret);
  96   return ret;
  97 }
  98
  99 #define PATCH(f) { sig_##f, ARRAY_SIZE(sig_##f), w_##f }
 100
 101 static const struct {
 102   const unsigned int *sig;
 103   size_t sig_cnt;
 104   void *func;
 105 } patches[] = {
 106   PATCH(open),
 107   PATCH(mmap),
 108   PATCH(mmap_), // mmap using mmap2 syscall
 109   PATCH(read),
 110 };
 111
 112 void do_patches(void *ptr, unsigned int size)
 113 {
 114   int i, s;
 115
 116   for (i = 0; i < ARRAY_SIZE(patches); i++) {
 117     const unsigned int *sig = patches[i].sig;
 118     unsigned int *seg = (void *)(((long)ptr + 3) & ~3);
 119     unsigned int *seg_end = seg + size / 4;
 120     unsigned int sig0 = sig[0];
 121
 122     for (; seg < seg_end; seg++) {
 123       if (*seg != sig0)
 124         continue;
 125
 126       for (s = 1; s < patches[i].sig_cnt; s++)
 127         if (seg[s] != sig[s])
 128           break;
 129
 130       if (s == patches[i].sig_cnt)
 131         goto found;
 132     }
 133     continue;
 134
 135 found:
 136     printf("  patch #%i @ %08x\n", i, (int)seg);
 137     seg[0] = 0xe59ff000; // ldr pc, [pc]
 138     seg[1] = 0;
 139     seg[2] = (unsigned int)patches[i].func;
 140   }
 141
 142   sys_cacheflush(ptr, (char *)ptr + size);
 143 }
 144