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