tools/kvm/ioport.c

   1 #include "kvm/ioport.h"
   2
   3 #include "kvm/kvm.h"
   4 #include "kvm/util.h"
   5 #include "kvm/brlock.h"
   6 #include "kvm/rbtree-interval.h"
   7 #include "kvm/mutex.h"
   8
   9 #include <linux/kvm.h>  /* for KVM_EXIT_* */
  10 #include <linux/types.h>
  11
  12 #include <stdbool.h>
  13 #include <assert.h>
  14 #include <limits.h>
  15 #include <stdlib.h>
  16 #include <stdio.h>
  17
  18 #define ioport_node(n) rb_entry(n, struct ioport, node)
  19
  20 DEFINE_MUTEX(ioport_mutex);
  21
  22 static u16                      free_io_port_idx; /* protected by ioport_mutex */
  23
  24 static struct rb_root           ioport_tree = RB_ROOT;
  25 bool                            ioport_debug;
  26
  27 static u16 ioport__find_free_port(void)
  28 {
  29         u16 free_port;
  30
  31         mutex_lock(&ioport_mutex);
  32         free_port = IOPORT_START + free_io_port_idx * IOPORT_SIZE;
  33         free_io_port_idx++;
  34         mutex_unlock(&ioport_mutex);
  35
  36         return free_port;
  37 }
  38
  39 static struct ioport *ioport_search(struct rb_root *root, u64 addr)
  40 {
  41         struct rb_int_node *node;
  42
  43         node = rb_int_search_single(root, addr);
  44         if (node == NULL)
  45                 return NULL;
  46
  47         return ioport_node(node);
  48 }
  49
  50 static int ioport_insert(struct rb_root *root, struct ioport *data)
  51 {
  52         return rb_int_insert(root, &data->node);
  53 }
  54
  55 static bool debug_io_out(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size)
  56 {
  57         exit(EXIT_SUCCESS);
  58 }
  59
  60 static struct ioport_operations debug_ops = {
  61         .io_out         = debug_io_out,
  62 };
  63
  64 static bool dummy_io_in(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size)
  65 {
  66         return true;
  67 }
  68
  69 static bool dummy_io_out(struct ioport *ioport, struct kvm *kvm, u16 port, void *data, int size)
  70 {
  71         return true;
  72 }
  73
  74 static struct ioport_operations dummy_read_write_ioport_ops = {
  75         .io_in          = dummy_io_in,
  76         .io_out         = dummy_io_out,
  77 };
  78
  79 static struct ioport_operations dummy_write_only_ioport_ops = {
  80         .io_out         = dummy_io_out,
  81 };
  82
  83 u16 ioport__register(u16 port, struct ioport_operations *ops, int count, void *param)
  84 {
  85         struct ioport *entry;
  86
  87         br_write_lock();
  88         if (port == IOPORT_EMPTY)
  89                 port = ioport__find_free_port();
  90
  91         entry = ioport_search(&ioport_tree, port);
  92         if (entry) {
  93                 pr_warning("ioport re-registered: %x", port);
  94                 rb_int_erase(&ioport_tree, &entry->node);
  95         }
  96
  97         entry = malloc(sizeof(*entry));
  98         if (entry == NULL)
  99                 die("Failed allocating new ioport entry");
 100
 101         *entry = (struct ioport) {
 102                 .node   = RB_INT_INIT(port, port + count),
 103                 .ops    = ops,
 104                 .priv   = param,
 105         };
 106
 107         ioport_insert(&ioport_tree, entry);
 108
 109         br_write_unlock();
 110
 111         return port;
 112 }
 113
 114 static const char *to_direction(int direction)
 115 {
 116         if (direction == KVM_EXIT_IO_IN)
 117                 return "IN";
 118         else
 119                 return "OUT";
 120 }
 121
 122 static void ioport_error(u16 port, void *data, int direction, int size, u32 count)
 123 {
 124         fprintf(stderr, "IO error: %s port=%x, size=%d, count=%u\n", to_direction(direction), port, size, count);
 125 }
 126
 127 bool kvm__emulate_io(struct kvm *kvm, u16 port, void *data, int direction, int size, u32 count)
 128 {
 129         struct ioport_operations *ops;
 130         bool ret = false;
 131         struct ioport *entry;
 132         void *ptr = data;
 133
 134         br_read_lock();
 135         entry = ioport_search(&ioport_tree, port);
 136         if (!entry)
 137                 goto error;
 138
 139         ops     = entry->ops;
 140
 141         while (count--) {
 142                 if (direction == KVM_EXIT_IO_IN) {
 143                         if (ops->io_in)
 144                                 ret = ops->io_in(entry, kvm, port, ptr, size);
 145                 } else {
 146                         if (ops->io_out)
 147                                 ret = ops->io_out(entry, kvm, port, ptr, size);
 148                 }
 149
 150                 ptr += size;
 151         }
 152
 153         br_read_unlock();
 154
 155         if (!ret)
 156                 goto error;
 157
 158         return true;
 159 error:
 160         br_read_unlock();
 161
 162         if (ioport_debug)
 163                 ioport_error(port, data, direction, size, count);
 164
 165         return !ioport_debug;
 166 }
 167
 168 void ioport__setup_legacy(void)
 169 {
 170         /* 0x0020 - 0x003F - 8259A PIC 1 */
 171         ioport__register(0x0020, &dummy_read_write_ioport_ops, 2, NULL);
 172
 173         /* PORT 0040-005F - PIT - PROGRAMMABLE INTERVAL TIMER (8253, 8254) */
 174         ioport__register(0x0040, &dummy_read_write_ioport_ops, 4, NULL);
 175
 176         /* 0x00A0 - 0x00AF - 8259A PIC 2 */
 177         ioport__register(0x00A0, &dummy_read_write_ioport_ops, 2, NULL);
 178
 179         /* PORT 00E0-00EF are 'motherboard specific' so we use them for our
 180            internal debugging purposes.  */
 181         ioport__register(IOPORT_DBG, &debug_ops, 1, NULL);
 182
 183         /* PORT 00ED - DUMMY PORT FOR DELAY??? */
 184         ioport__register(0x00ED, &dummy_write_only_ioport_ops, 1, NULL);
 185
 186         /* 0x00F0 - 0x00FF - Math co-processor */
 187         ioport__register(0x00F0, &dummy_write_only_ioport_ops, 2, NULL);
 188
 189         /* PORT 03D4-03D5 - COLOR VIDEO - CRT CONTROL REGISTERS */
 190         ioport__register(0x03D4, &dummy_read_write_ioport_ops, 1, NULL);
 191         ioport__register(0x03D5, &dummy_write_only_ioport_ops, 1, NULL);
 192 }