/*
 * Run video BIOS code for various purposes
 *
 * Copyright Matthew Garrett <mjg59@srcf.ucam.org>, heavily based on
 * vbetest.c from the lrmi package and read-edid.c by John Fremlin
 *
 * This program is released under the terms of the GNU General Public License,
 * version 2
 */
#include <pciaccess.h>
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/io.h>
#include <sys/kd.h>
#include <sys/stat.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>

#include "x86emu/include/x86emu.h"
#include "lrmi.h"
#include "vbetool.h"

#define access_ptr_register(reg_frame,reg) (reg_frame -> reg)
#define access_seg_register(reg_frame,es) reg_frame.es
#define real_mode_int(interrupt,reg_frame_ptr) !LRMI_int(interrupt,reg_frame_ptr)

#define DPMS_STATE_ON 0x0000
#define DPMS_STATE_STANDBY 0x0100
#define DPMS_STATE_SUSPEND 0x0200
#define DPMS_STATE_OFF 0x0400
#define DPMS_STATE_LOW 0x0800

extern unsigned long GPU_IOPORT;
extern unsigned long GPU_IOSIZE;

static int primary_gpu_has_kernel_driver(void)
{
    struct pci_id_match dev_match = {
        PCI_MATCH_ANY, PCI_MATCH_ANY, PCI_MATCH_ANY, PCI_MATCH_ANY,
        (0x03 << 16), 0xff000, 0 };
    struct pci_device *dev;
    struct pci_device_iterator *iter;
    int ret = 0;

    iter = pci_id_match_iterator_create(&dev_match);
    if (iter == NULL) {
        return 1;
    }

    while ((dev = pci_device_next(iter)) != NULL) {
        int is_boot = pci_device_is_boot_vga(dev);
        if (!is_boot)
            continue;

        if (pci_device_has_kernel_driver(dev)) {
            ret = 1;
            break;
        }
    }
    pci_iterator_destroy(iter);
    return ret;
}

int vbetool_init (void) {
    if (!LRMI_init()) {
        fprintf(stderr, "Failed to initialise LRMI (Linux Real-Mode Interface).\n");
        exit(1);
    }

    ioperm(0, 1024, 1);
    iopl(3);

    pci_system_init();
    return 0;
}

#ifndef S2RAM
int main(int argc, char *argv[])
{
    /* Don't bother checking for privilege if they only want usage() */
    if (argc < 2)
        goto usage;

    vbetool_init();

    if (!strcmp(argv[1], "vbestate")) {
        /* VBE save/restore tends to break when done underneath X */
        int err = check_console();

        if (err) {
            return err;
        }

        if (primary_gpu_has_kernel_driver()) {
            fprintf(stderr,"no save or restore with kernel driver loaded\n");
            return 0;
        }
        if (!strcmp(argv[2], "save")) {
            save_state();
        } else if (!strcmp(argv[2], "restore")) {
            restore_state();
        } else {
            goto usage;
        }
    } else if (!strcmp(argv[1], "dpms")) {
        if (!strcmp(argv[2], "on")) {
            return do_blank(DPMS_STATE_ON);
        } else if (!strcmp(argv[2], "suspend")) {
            return do_blank(DPMS_STATE_SUSPEND);
        } else if (!strcmp(argv[2], "standby")) {
            return do_blank(DPMS_STATE_STANDBY);
        } else if (!strcmp(argv[2], "off")) {
            return do_blank(DPMS_STATE_OFF);
        } else if (!strcmp(argv[2], "reduced")) {
            return do_blank(DPMS_STATE_LOW);
        } else {
            goto usage;
        }
    } else if (!strcmp(argv[1], "vbemode")) {

        if (primary_gpu_has_kernel_driver()) {
            fprintf(stderr,"no save or restore with kernel driver loaded\n");
            return 0;
        }

        if (!strcmp(argv[2], "set")) {
            return do_set_mode(atoi(argv[3]),0);
        } else if (!strcmp(argv[2], "get")) {
            return do_get_mode();
        } else {
            goto usage;
        }
    } else if (!strcmp(argv[1], "vgamode")) {

        if (primary_gpu_has_kernel_driver()) {
            fprintf(stderr,"no save or restore with kernel driver loaded\n");
            return 0;
        }

        if (!strcmp(argv[2], "set")) {
            return do_set_mode(atoi(argv[3]),1);
        } else {
            return do_set_mode(atoi(argv[2]),1);
        }
    } else if (!strcmp(argv[1], "post")) {
        /* Again, we don't really want to do this while X is in
           control */
        int err = check_console();

        if (err) {
            return err;
        }

        if (argc >= 3) {
            void *rc;
            int romfd = open (argv[2], O_RDWR);

            munmap((void *)0xc0000, 64*1024);
            rc = mmap((void *)0xc0000, 64*1024,
                    PROT_READ|PROT_WRITE|PROT_EXEC,
                    MAP_FIXED|MAP_PRIVATE, romfd, 0);
        }

        return do_post(0);
    } else if (!strcmp(argv[1], "bootpost")) {
        int err = check_console();

        if (err) {
            return err;
        }
        return do_post(1);
    } else if (!strcmp(argv[1], "udevpost")) {

#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
        int err = check_console();

        if (err) {
            return err;
        }
        if (argc < 3)
            goto usage;

        return do_udev_post(argv[2]);
#else
        fprintf(stderr, "no vga arb support built in\n");
        return -1;
#endif
    } else if (!strcmp(argv[1], "vgastate")) {

        if (primary_gpu_has_kernel_driver()) {
            fprintf(stderr,"no save or restore with kernel driver loaded\n");
            return 0;
        }

        if (!strcmp(argv[2], "on")) {
            return enable_vga();
        } else {
            return disable_vga();
        }
    } else if (!strcmp(argv[1], "vbefp")) {
        if (!strcmp(argv[2], "id") || !strcmp(argv[2], "panelid")) {
            return do_get_panel_id(0);
        } else if (!strcmp(argv[2], "panelsize")) {
            return do_get_panel_id(1);
        } else if (!strcmp(argv[2], "getbrightness")) {
            return do_get_panel_brightness();
        } else if (!strcmp(argv[2], "setbrightness")) {
            return do_set_panel_brightness(atoi(argv[3]));
        } else if (!strcmp(argv[2], "invert")) {
            return do_invert_panel();
        } else {
            return 1;
        }
    } else {
usage:
        fprintf(stderr,
                "%s: Usage %s [[vbestate save|restore]|[vbemode set|get]|[vgamode]|[dpms on|off|standby|suspend|reduced]|[post [romfile]]|[bootpost]|[udevpost pciid]|[vgastate on|off]|[vbefp panelid|panelsize|getbrightness|setbrightness|invert]]\n",
                argv[0], argv[0]);
        return 1;
    }

    return 0;
}
#endif

int do_vbe_service(unsigned int AX, unsigned int BX, reg_frame * regs)
{
    const unsigned interrupt = 0x10;
    unsigned function_sup;
    unsigned success;
    regs->ds = 0x0040;

    access_ptr_register(regs, eax) = AX;
    access_ptr_register(regs, ebx) = BX;

    if (real_mode_int(interrupt, regs)) {
        fprintf(stderr,
                "Error: something went wrong performing real mode interrupt\n");
        return -1;
    }

    AX = access_ptr_register(regs, eax);

    function_sup = ((AX & 0xff) == 0x4f);
    success = ((AX & 0xff00) == 0);

    if (!success) {
        fprintf(stderr, "Real mode call failed\n");
        return -2;
    }

    if (!function_sup) {
        fprintf(stderr, "Function not supported\n");
        return -3;
    }

    return access_ptr_register(regs, ebx);
}

int do_real_post(unsigned pci_device)
{
    int error = 0;
    struct LRMI_regs r;
    memset(&r, 0, sizeof(r));

    /* Several machines seem to want the device that they're POSTing in
       here */
    r.eax = pci_device;

    /* 0xc000 is the video option ROM.  The init code for each
       option ROM is at 0x0003 - so jump to c000:0003 and start running */
    r.cs = 0xc000;
    r.ip = 0x0003;

    /* This is all heavily cargo culted but seems to work */
    r.edx = 0x80;
    r.ds = 0x0040;

    if (!LRMI_call(&r)) {
        fprintf(stderr,
                "Error: something went wrong performing real mode call\n");
        error = 1;
    }

    return error;
}

#define MAX_ROMSIZE 64*1024
void *rom_cseg;
unsigned char romfile[MAX_ROMSIZE];

int setup_rom_section(void)
{
    munmap((void *)0xc0000, MAX_ROMSIZE);
    rom_cseg = mmap((void *)0xc0000, MAX_ROMSIZE, PROT_READ|PROT_WRITE|PROT_EXEC,
            MAP_FIXED|MAP_ANONYMOUS|MAP_PRIVATE, -1, 0);
    if (!rom_cseg) {
        fprintf(stderr,"unable to setup fake rom\n");
        return -1;
    }
    return 0;
}

int do_device_post(struct pci_device *dev, int has_arb)
{
    int error = 0;
    /* need to pull the ROM file */
    unsigned int pci_id;
    int ret, i;

    /* Probe device to fill in regions informations. */
    pci_device_probe(dev);
    for (i = 0; i < 6; ++i) {
        if (dev->regions[i].is_IO) {
            GPU_IOPORT = dev->regions[i].base_addr;
            GPU_IOSIZE = dev->regions[i].size;
            printf("GPU_IO@0x%08x[0x%08x]\n", GPU_IOPORT, GPU_IOSIZE);
            break;
        }
    }

#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
    if (has_arb) {
        pci_device_vgaarb_set_target(dev);
        pci_device_vgaarb_lock();
        pci_device_enable(dev);

        ret = pci_device_read_rom(dev, romfile);
        if (ret) {
            pci_device_vgaarb_unlock();
            fprintf(stderr,"rom read returned %d\n", ret);
            return 0;
        }

        memcpy(rom_cseg, romfile, MAX_ROMSIZE);
    }
#endif
    pci_id = (dev->bus << 8) + (dev->dev << 3) +
        (dev->func & 0x7);
    error = do_real_post(pci_id);
#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
    if (has_arb)
        pci_device_vgaarb_unlock();
#endif
    return error;
}

/* 0000:01:0a.0 */
int pci_str_to_info(char *pci_string, struct pci_slot_match *match)
{
    int dom, bus, dev, func;
    char *tok;

    tok = strtok(pci_string, ":");
    if (!tok)
        return -1;

    dom = strtoul(tok, NULL, 16);
    tok = strtok(NULL, ":");
    if (!tok)
        return -1;
    bus = strtoul(tok, NULL, 16);
    tok = strtok(NULL, ".");
    if (!tok)
        return -1;
    dev = strtoul(tok, NULL, 16);
    tok = strtok(NULL, ".");
    if (!tok)
        return -1;
    func = strtoul(tok, NULL, 16);

    match->domain = dom;
    match->bus = bus;
    match->dev = dev;
    match->func = func;
    return 0;
}

#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
int do_udev_post(char *pci_string)
{
    struct pci_slot_match match;
    int is_boot;
    int error;
    struct pci_device *dev;

    error = pci_device_vgaarb_init();
    if (error)
        return -1;

    if (setup_rom_section())
        return -1;

    /* parse the PCI string */
    error = pci_str_to_info(pci_string, &match);
    if (error)
        return -1;

    dev = pci_device_find_by_slot(match.domain, match.bus,
            match.dev, match.func);
    if (!dev)
        return -1;

    error = pci_device_probe(dev);
    if (error)
        return -1;
    is_boot = pci_device_is_boot_vga(dev);
    if (is_boot)
        return 0;
    if (pci_device_has_kernel_driver(dev))
        return 0;
    error = do_device_post(dev, 1);
    if (error)
        return error;
    pci_device_vgaarb_set_target(NULL);
    pci_device_vgaarb_lock();
    pci_device_vgaarb_unlock();
    pci_device_vgaarb_fini();
    return 0;
}
#endif

int do_post(int boot_flag)
{
    int error;
    struct pci_id_match dev_match = {
        PCI_MATCH_ANY, PCI_MATCH_ANY, PCI_MATCH_ANY, PCI_MATCH_ANY,
        (0x03 << 16), 0xff000, 0 };
    struct pci_device *dev, *first_dev = NULL;
    struct pci_device_iterator *iter;
    int has_arb = 1;
    int ret;

#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
    ret = pci_device_vgaarb_init();
    if (ret)
        has_arb = 0;

    if (has_arb)
        if (setup_rom_section())
            return -1;
#else
    if (boot_flag == 1)
        return -1;
    has_arb = 0;
#endif

    iter = pci_id_match_iterator_create(&dev_match);
    if (iter == NULL) {
        return 1;
    }

    while ((dev = pci_device_next(iter)) != NULL) {
        int is_boot = pci_device_is_boot_vga(dev);
        if (!first_dev)
            first_dev = dev;

        if (!is_boot && !has_arb)
            continue;

        if (is_boot && boot_flag) {
            continue;
        }
#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
        if (pci_device_has_kernel_driver(dev)) {
            continue;
        }
#endif
        error = do_device_post(dev, has_arb);
        if (error)
            return error;
    }
    pci_iterator_destroy(iter);
#ifdef HAVE_PCI_DEVICE_VGAARB_INIT
    if (has_arb) {
        pci_device_vgaarb_set_target(first_dev);
        pci_device_vgaarb_lock();
        pci_device_vgaarb_unlock();
        pci_device_vgaarb_fini();
    }
#endif
    return 0;
}

void restore_state_from(char *data)
{
    struct LRMI_regs r;

    /* VGA BIOS mode 3 is text mode */
    do_set_mode(3,1);

    memset(&r, 0, sizeof(r));

    r.eax = 0x4f04;
    r.ecx = 0xf;        /* all states */
    r.edx = 2;        /* restore state */
    r.es = (unsigned long)(data - LRMI_base_addr()) >> 4;
    r.ebx = (unsigned long)(data - LRMI_base_addr()) & 0xf;
    r.ds = 0x0040;

    if (!LRMI_int(0x10, &r)) {
        fprintf(stderr,
                "Can't restore video state (vm86 failure)\n");
    } else if ((r.eax & 0xffff) != 0x4f) {
        fprintf(stderr, "Restore video state failed\n");
    }

    LRMI_free_real(data);

    ioctl(0, KDSETMODE, KD_TEXT);

}

void restore_state(void)
{

    char *data = NULL;
    char tmpbuffer[524288];
    int i, length = 0;

    /* We really, really don't want to fail to read the entire set */
    while ((i = read(0, tmpbuffer + length, sizeof(tmpbuffer)-length))) {
        if (i == -1) {
            if (errno != EAGAIN && errno != EINTR) {
                perror("Failed to read state - ");
                return;
            }
        } else {
            length += i;
        }
    }

    data = LRMI_alloc_real(length);
    memcpy(data, tmpbuffer, length);

    restore_state_from(data);
}

char *__save_state(int *psize)
{
    struct LRMI_regs r;
    char *buffer;
    unsigned int size;

    memset(&r, 0, sizeof(r));

    r.eax = 0x4f04;
    r.ecx = 0xf;        /* all states */
    r.edx = 0;        /* get buffer size */
    r.ds = 0x0040;

    if (!LRMI_int(0x10, &r)) {
        fprintf(stderr,
                "Can't get video state buffer size (vm86 failure)\n");
    }

    if ((r.eax & 0xffff) != 0x4f) {
        fprintf(stderr, "Get video state buffer size failed\n");
    }

    *psize = size = (r.ebx & 0xffff) * 64;

    buffer = LRMI_alloc_real(size);

    if (buffer == NULL) {
        fprintf(stderr, "Can't allocate video state buffer\n");
        return NULL;
    }

    memset(&r, 0, sizeof(r));

    fprintf(stderr, "Allocated buffer at %p (base is 0x%x)\n", buffer,
            LRMI_base_addr());

    r.eax = 0x4f04;
    r.ecx = 0xf;        /* all states */
    r.edx = 1;        /* save state */

    r.es = (unsigned long)(buffer - LRMI_base_addr()) >> 4;
    r.ebx = (unsigned long)(buffer - LRMI_base_addr()) & 0xf;
    r.ds = 0x0040;

    fprintf(stderr, "ES: 0x%04X EBX: 0x%04X\n", r.es, r.ebx);

    if (!LRMI_int(0x10, &r)) {
        fprintf(stderr, "Can't save video state (vm86 failure)\n");
    }

    if ((r.eax & 0xffff) != 0x4f) {
        fprintf(stderr, "Save video state failed\n");
    }
    return buffer;
}

void save_state(void)
{
    int size;
    char *buffer = __save_state(&size);
    ssize_t num_written;

    if (buffer)
        /* FIXME: should retry on short write); */
        num_written = write(1, buffer, size);
}

int do_blank(int state)
{
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));
    error = do_vbe_service(0x4f10, state |= 0x01, &regs);
    if (error<0) {
        return error;
    }
    return 0;
}

int do_set_mode (int mode, int vga) {
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));

    if (vga) {
        error = do_vbe_service(mode, 0, &regs);
    } else {
        error = do_vbe_service(0x4f02, mode, &regs);
    }

    if (error<0) {
        return error;
    }

    return 0;
}

int do_get_panel_brightness() {
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));

    error = do_vbe_service(0x4f11, 0x05, &regs);

    if (error<0) {
        return error;
    }

    printf("%d\n",regs.ecx);

    return 0;
}

int do_invert_panel() {
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));

    error = do_vbe_service(0x4f11, 0x02, &regs);

    if (error<0) {
        return error;
    }

    if ((regs.ebx & 0xff) == 0)
        regs.ecx = 3;
    else
        regs.ecx = 0;

    error = do_vbe_service(0x4f11, 0x0102, &regs);

    if (error<0) {
        return error;
    }

    return 0;
}

int do_set_panel_brightness(int brightness) {
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));

    regs.ecx = brightness;

    error = do_vbe_service(0x4f11, 0x0105, &regs);

    if (error<0) {
        return error;
    }

    printf("%d\n",regs.ecx);

    return 0;
}

int do_get_mode() {
    reg_frame regs;
    int error;

    memset(&regs, 0, sizeof(regs));
    error = do_vbe_service(0x4f03, 0, &regs);

    if (error<0) {
        return error;
    }

    printf("%d\n",error);
    return 0;
}

int check_console()
{
    struct stat stat;

    return 0;

    if (fstat(0, &stat) != 0) {
        fprintf(stderr, "Can't stat() stdin\n");
        return 10;
    }

    if ((stat.st_rdev & 0xff00) != 0x400 || (stat.st_rdev & 0xff) > 63) {
        fprintf(stderr, "To perform this operation, "
                "this program must be run from the console\n");
        return 11;
    }

    ioctl(0, KDSETMODE, KD_GRAPHICS);
    return 0;
}

int enable_vga() {
    outb(0x03 | inb(0x3CC),  0x3C2);
    outb(0x01 | inb(0x3C3),  0x3C3);
    outb(0x08 | inb(0x46e8), 0x46e8);
    outb(0x01 | inb(0x102),  0x102);
    return 0;
}

int disable_vga() {
    outb(~0x03 & inb(0x3CC),  0x3C2);
    outb(~0x01 & inb(0x3C3),  0x3C3);
    outb(~0x08 & inb(0x46e8), 0x46e8);
    outb(~0x01 & inb(0x102),  0x102);
    return 0;
}

/* Based on xserver-xorg-driver-i810/src/i830_driver.c */
struct panel_id {
    int hsize:16, vsize:16;
    int fptype:16;
    int redbpp:8, greenbpp:8, bluebpp:8, reservedbpp:8;
    int rsvdoffscrnmemsize:32, rsvdoffscrnmemptr:32;
    char reserved[14];
} __attribute__((packed));

int do_get_panel_id(int just_dimensions)
{
    reg_frame r = {
        .eax = 0x4f11,
        .ebx = 0x0001
    };
    struct panel_id *id = LRMI_alloc_real(sizeof(struct panel_id));
    r.es = (unsigned short)(((unsigned long)(id-LRMI_base_addr()) >> 4) & 0xffff);
    r.edi = (unsigned long)(id-LRMI_base_addr()) & 0xf;

    if(sizeof(struct panel_id) != 32)
        return fprintf(stderr, "oops: panel_id, sizeof struct panel_id != 32, it's %d...\n", sizeof(struct panel_id)), 7;

    if(real_mode_int(0x10, &r))
        return fprintf(stderr, "Can't get panel id (vm86 failure)\n"), 8;

    if((r.eax & 0xff) != 0x4f)
        return fprintf(stderr, "Panel id function not supported\n"), 9;

    if(r.eax & 0xff00)
    {
        if((r.eax & 0xff00) == 0x100)
            fprintf(stderr, "Panel id read failed\n");
        else
            fprintf(stderr, "Panel id function not successful\n");
        return 10;
    }

    if(!just_dimensions)
        printf("size:\t%d %d\n"
                "type:\t%d\n"
                "bpp:\t%d %d %d %d\n",
                id->hsize, id->vsize,
                id->fptype,
                id->redbpp, id->greenbpp, id->bluebpp, id->reservedbpp);
    else
        printf("%dx%d\n", id->hsize, id->vsize);

#if 0

    /* Don't have a use for these and they don't seem to be documented.
     * 320 appears to be 320kB of mapped memory and the following
     * pointer is 0x1ffb8000 which is kernel mapping + 0xb8000 offset.
     */
    printf("ram:\t%dkB\n"
            "offset:\t%p\n",
            id->rsvdoffscrnmemsize,
            (void *)id->rsvdoffscrnmemptr);
#endif

    return 0;
}