#define _GNU_SOURCE
#include <mach.h>
#include <device/device.h>
#include "mach_i386.h"
#include <argp.h>               /* struct argp */
#include <unistd.h>             /* sleep */
#include "util.h"            /* _ */

static __inline void
outb (unsigned char value, unsigned short int port)
{
  __asm__ __volatile__ ("outb %b0,%w1": :"a" (value), "Nd" (port));
}

const char *argp_program_version = "ioport (" PACKAGE ") " VERSION;
const char *argp_program_bug_address = "<tosi@stekt.oulu.fi>";

#define VIDMMAP_BEGIN 0xB8000
#define VIDMMAP_SIZE (0xC0000 - 0xB8000)
#define VIDMMAP_KDOFS 0xA0000 /* == kd_bitmap_start in mach/i386/i386at/kd.c */

int
main (int argc, char **argv)
{
  kern_return_t err;
  mach_port_t device_master, kd_device;
  unsigned short *videomem = NULL;
  vm_address_t mapped;
  memory_object_t kd_mem = MACH_PORT_NULL;

  static const struct argp argp = { 0, 0, 0,
    N_("Move the cursor to the top left corner for a second "
       "by outputting values to EGA I/O ports.") };
  argp_parse (&argp, argc, argv, 0, 0, 0);

  err = get_privileged_ports (0, &device_master);
  if (err)
    error (1, err, "get_privileged_ports");

  err = device_open (device_master, D_WRITE, "kd", &kd_device);
  if (err)
    error (1, err, "device_open");

  err = device_map (kd_device, VM_PROT_READ | VM_PROT_WRITE,
                    VIDMMAP_BEGIN - VIDMMAP_KDOFS, VIDMMAP_SIZE,
                    &kd_mem, 0);
  if (err)
    error (1, err, "device_map");

  err = vm_map (mach_task_self (), &mapped, VIDMMAP_SIZE,
                0, 1, kd_mem, VIDMMAP_BEGIN - VIDMMAP_KDOFS, 0,
                VM_PROT_READ | VM_PROT_WRITE, VM_PROT_READ | VM_PROT_WRITE,
                VM_INHERIT_NONE);
  if (err)
    error (1, err, "vm_map");

  videomem = (unsigned short *) mapped;
  if (! videomem)
    error (1, err, "XXX");

  err = i386_io_port_add (mach_thread_self (), kd_device);
  if (err)
    error (1, err, "i386_io_port_add");

  outb (0x0E, 0x3D4);           /* cursor position high byte register */
  outb (0, 0x3D5);
  outb (0x0F, 0x3D4);           /* cursor position low byte register */
  outb (0, 0x3D5);

  sleep (1);
  return 0;
}