x86emu: import real-mode allocating functions from X

same idea as commit fda0acbc420efa876fc1ac9797305572f80a849f

Signed-off-by: Tiago Vignatti <tiago.vignatti@nokia.com>

1 files changed, 155 insertions, 1 deletions

diff --git a/src/lrmi/backend-x86emu.c b/src/lrmi/backend-x86emu.c
index 1c937d9..fa90df0 100644
--- a/src/lrmi/backend-x86emu.c
+++ b/src/lrmi/backend-x86emu.c
@@ -1,8 +1,10 @@
 #include <stdarg.h>
 #include <stdio.h>
 #include <string.h>
+#include <stdlib.h>
 #include <pciaccess.h>
 #include <inttypes.h>
+#include <sys/mman.h>
 
 #include "libx86.h"
 #include "common.h"
@@ -17,6 +19,7 @@ extern xf86Int10InfoPtr Int10Current;
 #define Bool int
 
 #define SHMERRORPTR (pointer)(-1)
+#define ALLOC_ENTRIES(x) ((V_RAM / x) - 1)
 
 char *mmap_addr = SHMERRORPTR;
 static void *stack;
@@ -37,6 +40,12 @@ typedef struct {
 } genericInt10Priv;
 
 #define INTPriv(x) ((genericInt10Priv*)x->private)
+#define GET_HIGH_BASE(x) (((V_BIOS + (x) + getpagesize() - 1)/getpagesize()) \
+                              * getpagesize())
+
+#define DEV_MEM "/dev/mem"
+#include <sys/stat.h>
+#include <fcntl.h>
 
 void
 printk(const char *fmt, ...)
@@ -221,11 +230,50 @@ void x_outl(uint16_t port, uint32_t val);
 void x_outb(uint16_t port, uint8_t val);
 void x_outw(uint16_t port, uint16_t val);
 
+/* Stolen from Xorg code */
+int
+read_bios(unsigned long Base, unsigned long Offset, unsigned char *Buf,
+         int Len)
+{
+    int fd;
+    unsigned char *ptr;
+    int psize;
+    int mlen;
+
+    if ((fd = open(DEV_MEM, O_RDONLY)) < 0)
+    {
+        fprintf(stderr, "xf86ReadBIOS: Failed to open %s\n",
+            DEV_MEM);
+        return(-1);
+    }
+    psize = getpagesize();
+    Offset += Base & (psize - 1);
+    Base &= ~(psize - 1);
+    mlen = (Offset + Len + psize - 1) & ~(psize - 1);
+    ptr = (unsigned char *)mmap((caddr_t)0, mlen, PROT_READ,
+                    MAP_SHARED, fd, (off_t)Base);
+    if (ptr == MAP_FAILED)
+    {
+        fprintf(stderr, "xf86ReadBIOS: %s mmap failed\n",
+            DEV_MEM);
+        close(fd);
+        return(-1);
+    }
+    (void)memcpy(Buf, (void *)(ptr + Offset), Len);
+    (void)munmap((caddr_t)ptr, mlen);
+    (void)close(fd);
+    return(Len);
+}
 
 void
 LRMI_init(xf86Int10InfoPtr pInt)
 {
-    int i;
+    int i, err;
+    void* sysMem = NULL;
+    void* base = 0;
+    int fd;
+
+
     X86EMU_intrFuncs intFuncs[256];
     X86EMU_pioFuncs pioFuncs = {
     (&x_inb),
@@ -257,6 +305,63 @@ LRMI_init(xf86Int10InfoPtr pInt)
 
     pInt->mem = &genericMem;
 
+    if ((fd = open(DEV_MEM, O_RDWR, 0)) >= 0) {
+        if (!sysMem) {
+        if ((sysMem = mmap((void *)0, BIOS_SIZE + SYS_BIOS - V_BIOS,
+                   PROT_READ | PROT_WRITE,
+                   MAP_SHARED | MAP_FIXED, fd, V_BIOS))
+            == MAP_FAILED) {
+            fprintf(stderr, "Cannot map SYS BIOS\n");
+            close(fd);
+        }
+        }
+    }
+
+    pInt->private = (void *)calloc(1, sizeof(genericInt10Priv));
+    INTPriv(pInt)->alloc = (void *)calloc(1, ALLOC_ENTRIES(getpagesize()));
+    base = INTPriv(pInt)->base = malloc(SYS_BIOS);
+
+    /*
+     * we need to map video RAM MMIO as some chipsets map mmio
+     * registers into this range.
+     */
+    INTPriv(pInt)->sysMem = sysMem;
+
+    if (read_bios(0, 0, base, LOW_PAGE_SIZE) < 0) {
+        fprintf(stderr, "Cannot read int vect\n");
+        goto error1;
+    }
+
+    memset((char *)base + V_BIOS, 0, SYS_BIOS - V_BIOS);
+    INTPriv(pInt)->highMemory = V_BIOS;
+
+    LRMI_set_exit_condition(pInt);
+
+    /*
+     * Allocate a segment for the stack
+     */
+    LRMI_alloc_real(pInt, 1, &pInt->stackseg);
+
+    if (pci_device_is_boot_vga(pInt->dev))
+        return;
+
+    err = pci_device_read_rom(pInt->dev, (unsigned char *)base + V_BIOS);
+    if (err) {
+        fprintf(stderr,"Cannot read V_BIOS (3) %s\n",
+               strerror(err));
+        goto error1;
+    }
+    INTPriv(pInt)->highMemory = GET_HIGH_BASE(pInt->dev->rom_size);
+
+    pInt->BIOSseg = V_BIOS >> 4;
+
+    return;
+
+error1:
+    free(base);
+    free(INTPriv(pInt)->alloc);
+    free(pInt->private);
+    return;
 #if 0
 	int i;
 
@@ -282,6 +387,55 @@ LRMI_init(xf86Int10InfoPtr pInt)
 void
 LRMI_fini(xf86Int10InfoPtr pInt)
 {
+    if (!pInt)
+      return;
+
     if (Int10Current == pInt)
     Int10Current = NULL;
+
+    free(INTPriv(pInt)->base);
+    free(INTPriv(pInt)->alloc);
+    free(pInt->private);
+    free(pInt);
+}
+
+void *
+LRMI_alloc_real(xf86Int10InfoPtr pInt, int num, int *off)
+{
+    int pagesize = getpagesize();
+    int num_pages = ALLOC_ENTRIES(pagesize);
+    int i,j;
+
+    for (i = 0; i < (num_pages - num); i++) {
+    if (INTPriv(pInt)->alloc[i] == 0) {
+        for (j = i; j < (num + i); j++)
+        if (INTPriv(pInt)->alloc[j] != 0)
+            break;
+        if (j == (num + i))
+        break;
+        i += num;
+    }
+    }
+    if (i == (num_pages - num))
+    return NULL;
+
+    for (j = i; j < (i + num); j++)
+    INTPriv(pInt)->alloc[j] = 1;
+
+    *off = (i + 1) * pagesize;
+
+    return (char *)INTPriv(pInt)->base + *off;
+}
+
+void
+LRMI_free_real(xf86Int10InfoPtr pInt, void *pbase, int num)
+{
+    int pagesize = getpagesize();
+    int first = (((char *)pbase - (char *)INTPriv(pInt)->base) / pagesize) -
+1;
+    int i;
+
+    for (i = first; i < (first + num); i++)
+    INTPriv(pInt)->alloc[i] = 0;
 }
+