Signed-off-by: Hervé Poussineau <hpous...@reactos.org>
---
hw/isa-bus.c | 127 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
hw/isa.h | 2 +-
2 files changed, 125 insertions(+), 4 deletions(-)
diff --git a/hw/isa-bus.c b/hw/isa-bus.c
index 86b0bbd..bcf7cd4 100644
--- a/hw/isa-bus.c
+++ b/hw/isa-bus.c
@@ -104,19 +104,140 @@ void isa_register_ioport(ISADevice *dev, MemoryRegion
*io, uint16_t start)
isa_init_ioport(dev, start);
}
+typedef struct PortioState {
+ const char *name; /* debug purposes */
+ uint16_t start;
+ uint16_t offset;
+ const MemoryRegionPortio *pio_start;
+ void *opaque;
+} PortioState;
+
+static const MemoryRegionPortio *portio_find(const MemoryRegionPortio *mrp,
+ uint64_t offset,
+ unsigned int width, bool write,
+ bool smaller)
+{
+ for (; mrp->size; ++mrp) {
+ if (offset >= mrp->offset && offset < mrp->offset + mrp->len
+ && (width == mrp->size || (smaller && width < mrp->size))
+ && (write ? (bool)mrp->write : (bool)mrp->read)) {
+ return mrp;
+ }
+ }
+ return NULL;
+}
+
+static uint64_t portio_read(void *opaque, hwaddr addr, unsigned int size)
+{
+ const PortioState *s = opaque;
+ const MemoryRegionPortio *mrp;
+
+ addr += s->offset;
+ mrp = portio_find(s->pio_start, addr, size, false, false);
+ if (mrp) {
+ return mrp->read(s->opaque, s->start + addr);
+ } else if (size == 2) {
+ uint64_t data;
+ mrp = portio_find(s->pio_start, addr, 1, false, false);
+ assert(mrp);
+ data = mrp->read(s->opaque, s->start + addr) |
+ (mrp->read(s->opaque, s->start + addr + 1) << 8);
+ return data;
+ }
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: invalid read from 0x%x size=%d",
+ s->name, s->start + (int)addr, size);
+ return -1U;
+}
+
+static void portio_write(void *opaque, hwaddr addr, uint64_t data,
+ unsigned int size)
+{
+ const PortioState *s = opaque;
+ const MemoryRegionPortio *mrp;
+
+ addr += s->offset;
+ mrp = portio_find(s->pio_start, addr, size, true, false);
+ if (mrp) {
+ mrp->write(s->opaque, s->start + addr, data);
+ return;
+ } else if (size == 2) {
+ mrp = portio_find(s->pio_start, addr, 1, true, false);
+ assert(mrp);
+ mrp->write(s->opaque, s->start + addr, data & 0xff);
+ mrp->write(s->opaque, s->start + addr + 1, data >> 8);
+ return;
+ }
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: invalid write to 0x%x size=%d",
+ s->name, s->start + (int)addr, size);
+}
+
+static bool portio_accepts(void *opaque, hwaddr addr, unsigned int size,
+ bool is_write)
+{
+ const PortioState *s = opaque;
+ const MemoryRegionPortio *mrp;
+
+ addr += s->offset;
+ mrp = portio_find(s->pio_start, addr, size, is_write, true);
+ return (mrp != NULL);
+}
+
+const MemoryRegionOps portio_ops = {
+ .read = portio_read,
+ .write = portio_write,
+ .valid.accepts = portio_accepts,
+ .endianness = DEVICE_LITTLE_ENDIAN,
+};
+
+static void isa_register_portio_list_1(ISADevice *dev, uint16_t start,
+ uint16_t offset, uint16_t end,
+ const MemoryRegionPortio *pio_start,
+ void *opaque, const char *name)
+{
+ MemoryRegion *mr = g_new(MemoryRegion, 1);
+ PortioState *s = g_new(PortioState, 1);
+
+ s->name = name;
+ s->start = start;
+ s->offset = offset;
+ s->pio_start = pio_start;
+ s->opaque = opaque;
+ memory_region_init_io(mr, &portio_ops, s, name, end - offset);
+ memory_region_add_subregion(isa_address_space_io(dev),
+ start + offset, mr);
+}
+
void isa_register_portio_list(ISADevice *dev, uint16_t start,
const MemoryRegionPortio *pio_start,
void *opaque, const char *name)
{
- PortioList *piolist = g_new(PortioList, 1);
+ const MemoryRegionPortio *pio, *first;
+ uint16_t end;
/* START is how we should treat DEV, regardless of the actual
contents of the portio array. This is how the old code
actually handled e.g. the FDC device. */
isa_init_ioport(dev, start);
- portio_list_init(piolist, pio_start, opaque, name);
- portio_list_add(piolist, isabus->address_space_io, start);
+ assert(pio_start->size);
+
+ first = pio_start;
+ end = 0;
+ for (pio = pio_start; pio->size; pio++) {
+ assert(pio->offset >= first->offset);
+ if (pio->offset > first->offset + first->len) {
+ isa_register_portio_list_1(dev, start, first->offset, end,
+ pio_start, opaque, name);
+ first = pio;
+ end = 0;
+ }
+ if (pio->offset + pio->len > end) {
+ end = pio->offset + pio->len;
+ }
+ }
+
+ isa_register_portio_list_1(dev, start, first->offset, end,
+ pio_start, opaque, name);
}
static int isa_qdev_init(DeviceState *qdev)
diff --git a/hw/isa.h b/hw/isa.h
index 62e89d3..c3b01ea 100644
--- a/hw/isa.h
+++ b/hw/isa.h
@@ -73,7 +73,7 @@ void isa_register_ioport(ISADevice *dev, MemoryRegion *io,
uint16_t start);
* @dev: the ISADevice against which these are registered; may be NULL.
* @start: the base I/O port against which the portio->offset is applied.
* @portio: the ports, sorted by offset.
- * @opaque: passed into the old_portio callbacks.
+ * @opaque: passed into the portio callbacks.
* @name: passed into memory_region_init_io.
*/
void isa_register_portio_list(ISADevice *dev, uint16_t start,
--
1.7.10.4
