1 files changed, 247 insertions, 6 deletions

diff --git a/src/lrmi/backend-vm86.c b/src/lrmi/backend-vm86.c
index 558a913..d4539f5 100644
--- a/src/lrmi/backend-vm86.c
+++ b/src/lrmi/backend-vm86.c
@@ -32,6 +32,10 @@ OTHER DEALINGS IN THE SOFTWARE.
 #include <inttypes.h>
 #include <sys/mman.h>
 #include <fcntl.h>
+#include <errno.h>
+#include <sys/ipc.h>
+#include <sys/shm.h>
+#include <unistd.h>
 
 
 #include "libx86.h"
@@ -44,6 +48,26 @@ OTHER DEALINGS IN THE SOFTWARE.
 #endif
 #define DEFAULT_STACK_SIZE 	0x1000
 
+#ifdef __sparc__
+#define DEV_MEM "/dev/fb"
+#else
+#define DEV_MEM "/dev/mem"
+#endif
+#define ALLOC_ENTRIES(x) ((V_RAM / x) - 1)
+#define SHMERRORPTR (void *)(-1)
+
+extern xf86Int10InfoPtr Int10Current;
+static int counter = 0;
+
+typedef struct {
+    int lowMem;
+    int highMem;
+    char* base;
+    char* base_high;
+    int screen;
+    char* alloc;
+} linuxInt10Priv;
+
 
 static uint8_t
 read_b(xf86Int10InfoPtr pInt, int addr)
@@ -98,17 +122,99 @@ set_bit(unsigned int bit, void *array)
 	a[bit / 8] |= (1 << (bit % 8));
 }
 
-#ifdef __sparc__
-#define DEV_MEM "/dev/fb"
-#else
-#define DEV_MEM "/dev/mem"
-#endif
+void
+LRMI_fini(xf86Int10InfoPtr pInt)
+{
+    if (!pInt)
+    return;
+
+    if (Int10Current == pInt) {
+    shmdt(0);
+    if (((linuxInt10Priv*)pInt->private)->highMem >= 0)
+        shmdt((char*)HIGH_MEM);
+    else
+        munmap((void *)V_BIOS, (SYS_BIOS - V_BIOS));
+    Int10Current = NULL;
+    }
+    
+    if (((linuxInt10Priv*)pInt->private)->base_high)
+    shmdt(((linuxInt10Priv*)pInt->private)->base_high);
+    shmdt(((linuxInt10Priv*)pInt->private)->base);
+    shmctl(((linuxInt10Priv*)pInt->private)->lowMem, IPC_RMID, NULL);
+    if (((linuxInt10Priv*)pInt->private)->highMem >= 0)
+    shmctl(((linuxInt10Priv*)pInt->private)->highMem, IPC_RMID, NULL);
+    free(((linuxInt10Priv*)pInt->private)->alloc);
+    free(pInt->private);
+    free(pInt);
+}
+
+int
+MapCurrentInt10(xf86Int10InfoPtr pInt)
+{
+    void *addr;
+    int fd = -1;
+    
+    if (Int10Current) {
+    shmdt(0);
+    if (((linuxInt10Priv*)Int10Current->private)->highMem >= 0)
+        shmdt((char*)HIGH_MEM);
+    else
+        munmap((void *)V_BIOS, (SYS_BIOS - V_BIOS));
+    }
+    addr = shmat(((linuxInt10Priv*)pInt->private)->lowMem, (char*)1, SHM_RND);
+    if (addr == SHMERRORPTR) {
+    fprintf(stderr, "Cannot shmat() low memory\n");
+    fprintf(stderr, "shmat(low_mem) error: %s\n",strerror(errno));
+    return 0;
+    }
+    if (mprotect((void*)0, V_RAM, PROT_READ|PROT_WRITE|PROT_EXEC) != 0)
+        fprintf(stderr,
+           "Cannot set EXEC bit on low memory: %s\n", strerror(errno));
+
+    if (((linuxInt10Priv*)pInt->private)->highMem >= 0) {
+    addr = shmat(((linuxInt10Priv*)pInt->private)->highMem,
+             (char*)HIGH_MEM, 0);
+    if (addr == SHMERRORPTR) {
+        fprintf(stderr, "Cannot shmat() high memory\n");
+        fprintf(stderr, "shmget error: %s\n",strerror(errno));
+        return 0;
+    }
+    if (mprotect((void*)HIGH_MEM, HIGH_MEM_SIZE,
+             PROT_READ|PROT_WRITE|PROT_EXEC) != 0)
+        fprintf(stderr, "Cannot set EXEC bit on high memory: %s\n",
+               strerror(errno));
+    } else {
+    if ((fd = open(DEV_MEM, O_RDWR, 0)) >= 0) {
+        if (mmap((void *)(V_BIOS), SYS_BIOS - V_BIOS,
+                 PROT_READ | PROT_WRITE | PROT_EXEC,
+                 MAP_SHARED | MAP_FIXED, fd, V_BIOS)
+        == MAP_FAILED) {
+        fprintf(stderr, "Cannot map V_BIOS\n");
+        close (fd);
+        return 0;
+        }
+    } else {
+        fprintf(stderr, "Cannot open %s\n",DEV_MEM);
+        return 0;
+    }
+    close (fd);
+    }
+    
+    return 1;
+}
+
 void
 LRMI_init(xf86Int10InfoPtr pInt)
 {
     int fd;
     static void* vidMem = NULL;
     static void* sysMem = NULL;
+    void* vMem = NULL;
+    int low_mem;
+    int high_mem = -1;
+    char *base = SHMERRORPTR;
+    char *base_high = SHMERRORPTR;
+    int videoBiosMapped = 0;
 
 
     /* TODO: should put this in common interface and to just run once instead
@@ -146,11 +252,93 @@ LRMI_init(xf86Int10InfoPtr pInt)
 
     pInt->mem = &linuxMem;
 
+
+    pInt->private = (void *)calloc(1, sizeof(linuxInt10Priv));
+    ((linuxInt10Priv*)pInt->private)->screen = pInt->scrnIndex;
+    ((linuxInt10Priv*)pInt->private)->alloc =
+    (void *)calloc(1, ALLOC_ENTRIES(getpagesize()));
+
+    if (!pci_device_is_boot_vga(pInt->dev)) {
+    fprintf(stderr, "Mapping high memory area\n");
+    if ((high_mem = shmget(counter++, HIGH_MEM_SIZE,
+                   IPC_CREAT | SHM_R | SHM_W)) == -1) {
+        if (errno == ENOSYS)
+        fprintf(stderr, "shmget error\n Please reconfigure"
+               " your kernel to include System V IPC support\n");
+        else
+        fprintf(stderr, "shmget(highmem) error: %s\n",strerror(errno));
+        goto error1;
+    }
+    } else {
+    fprintf(stderr, "Mapping Video BIOS\n");
+    videoBiosMapped = 1;
+    if ((fd = open(DEV_MEM, O_RDWR, 0)) >= 0) {
+        if ((vMem = mmap((void *)(V_BIOS), SYS_BIOS - V_BIOS,
+                 PROT_READ | PROT_WRITE | PROT_EXEC,
+                 MAP_SHARED | MAP_FIXED, fd, V_BIOS))
+        == MAP_FAILED) {
+        fprintf(stderr, "Cannot map V_BIOS\n");
+        close(fd);
+        goto error1;
+        }
+        close (fd);
+    } else
+        goto error1;
+    }
+    ((linuxInt10Priv*)pInt->private)->highMem = high_mem;
+
+    fprintf(stderr, "Mapping 640kB area\n");
+    if ((low_mem = shmget(counter++, V_RAM,
+              IPC_CREAT | SHM_R | SHM_W)) == -1) {
+    fprintf(stderr, "shmget(lowmem) error: %s\n",strerror(errno));
+    goto error2;
+    }
+
+    ((linuxInt10Priv*)pInt->private)->lowMem = low_mem;
+    base = shmat(low_mem, 0, 0);
+    if (base == SHMERRORPTR) {
+    fprintf(stderr, "shmat(low_mem) error: %s\n",strerror(errno));
+    goto error3;
+    }
+    ((linuxInt10Priv *)pInt->private)->base = base;
+    if (high_mem > -1) {
+    base_high = shmat(high_mem, 0, 0);
+    if (base_high == SHMERRORPTR) {
+        fprintf(stderr, "shmat(high_mem) error: %s\n",strerror(errno));
+        goto error3;
+    }
+    ((linuxInt10Priv*)pInt->private)->base_high = base_high;
+    } else
+    ((linuxInt10Priv*)pInt->private)->base_high = NULL;
+
+    if (!MapCurrentInt10(pInt))
+    goto error3;
+
+    Int10Current = pInt;
+
     return;
 
+error3:
+    if (base_high)
+    shmdt(base_high);
+    shmdt(base);
+    shmdt(0);
+    if (base_high)
+    shmdt((char*)HIGH_MEM);
+    shmctl(low_mem, IPC_RMID, NULL);
+    Int10Current = NULL;
+error2:
+    if (high_mem > -1)
+    shmctl(high_mem, IPC_RMID,NULL);
+error1:
+    if (vMem)
+    munmap(vMem, SYS_BIOS - V_BIOS);
+    free(((linuxInt10Priv*)pInt->private)->alloc);
+    free(pInt->private);
 error:
     free(pInt);
     return;
+
 #if 0
 	void *m;
 
@@ -343,9 +531,62 @@ LRMI_int(int i, struct LRMI_regs *r)
 }
 
 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 (((linuxInt10Priv*)pInt->private)->alloc[i] == 0) {
+        for (j = i; j < (num + i); j++)
+        if ((((linuxInt10Priv*)pInt->private)->alloc[j] != 0))
+            break;
+        if (j == (num + i))
+        break;
+        else
+        i = i + num;
+    }
+    }
+    if (i == (num_pages - num))
+    return NULL;
+
+    for (j = i; j < (i + num); j++)
+    ((linuxInt10Priv*)pInt->private)->alloc[j] = 1;
+
+    *off = (i + 1) * pagesize;
+
+    return ((linuxInt10Priv*)pInt->private)->base + ((i + 1) * pagesize);
+}
+
+void
+LRMI_free_real(xf86Int10InfoPtr pInt, void *pbase, int num)
+{
+    int pagesize = getpagesize();
+    int first = (((unsigned long)pbase
+         - (unsigned long)((linuxInt10Priv*)pInt->private)->base)
+    / pagesize) - 1;
+    int i;
+
+    for (i = first; i < (first + num); i++)
+    ((linuxInt10Priv*)pInt->private)->alloc[i] = 0;
+}
+
+void *
 LRMI_base_addr(xf86Int10InfoPtr pInt, unsigned long addr)
 {
-	return NULL;
+    if (addr < V_RAM)
+    return ((linuxInt10Priv*)pInt->private)->base + addr;
+    else if (addr < V_BIOS)
+    return (void *)addr;
+    else if (addr < SYS_BIOS) {
+    if (((linuxInt10Priv*)pInt->private)->base_high)
+        return (((linuxInt10Priv*)pInt->private)->base_high
+                 - V_BIOS + addr);
+    else
+        return (void *)addr;
+    } else
+    return (void *)addr;
 }
 
 #else /* (__linux__ || __NetBSD__ || __FreeBSD__ || __OpenBSD__) && __i386__ */