palacios/src/palacios/vmm_io.c

   1 #include <palacios/vmm_io.h>
   2 #include <palacios/vmm_string.h>
   3 #include <palacios/vmm.h>
   4
   5
   6
   7
   8 #ifndef DEBUG_IO
   9 #undef PrintDebug
  10 #define PrintDebug(fmt, args...)
  11 #endif
  12
  13
  14 void init_vmm_io_map(struct guest_info * info) {
  15   struct vmm_io_map * io_map = &(info->io_map);
  16   io_map->num_ports = 0;
  17   io_map->head = NULL;
  18 }
  19
  20
  21
  22
  23
  24 static int add_io_hook(struct vmm_io_map * io_map, struct vmm_io_hook * io_hook) {
  25
  26   if (!(io_map->head)) {
  27     io_map->head = io_hook;
  28     io_map->num_ports = 1;
  29     return 0;
  30   } else if (io_map->head->port > io_hook->port) {
  31     io_hook->next = io_map->head;
  32
  33     io_map->head->prev = io_hook;
  34     io_map->head = io_hook;
  35     io_map->num_ports++;
  36
  37     return 0;
  38   } else {
  39     struct vmm_io_hook * tmp_hook = io_map->head;
  40
  41     while ((tmp_hook->next)  &&
  42            (tmp_hook->next->port <= io_hook->port)) {
  43         tmp_hook = tmp_hook->next;
  44     }
  45
  46     if (tmp_hook->port == io_hook->port) {
  47       //tmp_hook->read = io_hook->read;
  48       //tmp_hook->write = io_hook->write;
  49       //V3_Free(io_hook);
  50       return -1;
  51     } else {
  52       io_hook->prev = tmp_hook;
  53       io_hook->next = tmp_hook->next;
  54
  55       if (tmp_hook->next) {
  56         tmp_hook->next->prev = io_hook;
  57       }
  58
  59       tmp_hook->next = io_hook;
  60
  61       io_map->num_ports++;
  62       return 0;
  63     }
  64   }
  65   return -1;
  66 }
  67
  68 static int remove_io_hook(struct vmm_io_map * io_map, struct vmm_io_hook * io_hook) {
  69   if (io_map->head == io_hook) {
  70     io_map->head = io_hook->next;
  71   } else if (io_hook->prev) {
  72     io_hook->prev->next = io_hook->next;
  73   } else {
  74     return -1;
  75     // data corruption failure
  76   }
  77
  78   if (io_hook->next) {
  79     io_hook->next->prev = io_hook->prev;
  80   }
  81
  82   io_map->num_ports--;
  83
  84   return 0;
  85 }
  86
  87
  88
  89 /* FIX ME */
  90 static int default_write(ushort_t port, void *src, uint_t length, void * priv_data) {
  91   /*
  92
  93   if (length == 1) {
  94   __asm__ __volatile__ (
  95   "outb %b0, %w1"
  96   :
  97   : "a" (*dst), "Nd" (port)
  98   );
  99   } else if (length == 2) {
 100   __asm__ __volatile__ (
 101   "outw %b0, %w1"
 102   :
 103   : "a" (*dst), "Nd" (port)
 104   );
 105   } else if (length == 4) {
 106   __asm__ __volatile__ (
 107   "outw %b0, %w1"
 108   :
 109   : "a" (*dst), "Nd" (port)
 110   );
 111   }
 112   */
 113   return 0;
 114 }
 115
 116 static int default_read(ushort_t port, void * dst, uint_t length, void * priv_data)
 117 {
 118
 119   /*
 120         uchar_t value;
 121
 122     __asm__ __volatile__ (
 123         "inb %w1, %b0"
 124         : "=a" (value)
 125         : "Nd" (port)
 126     );
 127
 128     return value;
 129   */
 130
 131   return 0;
 132 }
 133
 134 int v3_hook_io_port(struct guest_info * info, uint_t port,
 135                     int (*read)(ushort_t port, void * dst, uint_t length, void * priv_data),
 136                     int (*write)(ushort_t port, void * src, uint_t length, void * priv_data),
 137                     void * priv_data) {
 138   struct vmm_io_map * io_map = &(info->io_map);
 139   struct vmm_io_hook * io_hook = (struct vmm_io_hook *)V3_Malloc(sizeof(struct vmm_io_hook));
 140
 141   io_hook->port = port;
 142
 143   if (!read) {
 144     io_hook->read = &default_read;
 145   } else {
 146     io_hook->read = read;
 147   }
 148
 149   if (!write) {
 150     io_hook->write = &default_write;
 151   } else {
 152     io_hook->write = write;
 153   }
 154
 155   io_hook->next = NULL;
 156   io_hook->prev = NULL;
 157
 158   io_hook->priv_data = priv_data;
 159
 160   if (add_io_hook(io_map, io_hook) != 0) {
 161     V3_Free(io_hook);
 162     return -1;
 163   }
 164
 165   return 0;
 166 }
 167
 168 int v3_unhook_io_port(struct guest_info * info, uint_t port) {
 169   struct vmm_io_map * io_map = &(info->io_map);
 170   struct vmm_io_hook * hook = v3_get_io_hook(io_map, port);
 171
 172   if (hook == NULL) {
 173     return -1;
 174   }
 175
 176   remove_io_hook(io_map, hook);
 177   return 0;
 178 }
 179
 180
 181 struct vmm_io_hook * v3_get_io_hook(struct vmm_io_map * io_map, uint_t port) {
 182   struct vmm_io_hook * tmp_hook;
 183   FOREACH_IO_HOOK(*io_map, tmp_hook) {
 184     if (tmp_hook->port == port) {
 185       return tmp_hook;
 186     }
 187   }
 188   return NULL;
 189 }
 190
 191
 192
 193 void PrintDebugIOMap(struct vmm_io_map * io_map) {
 194   struct vmm_io_hook * iter = io_map->head;
 195
 196   PrintDebug("VMM IO Map (Entries=%d)\n", io_map->num_ports);
 197
 198   while (iter) {
 199     PrintDebug("IO Port: %hu (Read=%x) (Write=%x)\n", iter->port, iter->read, iter->write);
 200   }
 201 }