#include <sys/types.h>
#include <sys/mman.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <assert.h>
#include <fcntl.h>

#include "voodoo2.h"
#include "voodoo2_bits.h"
#include "voodoo2_config.h"
#include "voodoo2_config_bits.h"
#include "ics5342.h"

static inline bool _wait_graphics_busy(voodoo2_reg * voodoo2)
{
  int cnt = 0;
  for (int i = 0; i < 4096; i++) {
    if (voodoo2->status & STATUS__GRAPHICS_BUSY) {
      cnt = 0;
    } else {
      cnt += 1;
    }
    if (cnt >= 3)
      return true;
  }
  return false;
}

#define wait_graphics_busy(voodoo2) assert(_wait_graphics_busy(voodoo2));

static inline void dac_data_read(voodoo2_reg * voodoo2,
                                 int data,
                                 int rs)
{
  voodoo2->dacData
    = DACDATA__WRITE_DATA(data)
    | DACDATA__ADDRESS_2_0(rs >> 0)
    | DACDATA__READ_COMMAND
    | DACDATA__ADDRESS_4_3(rs >> 3)
    ;
}

static inline void dac_data_write(voodoo2_reg * voodoo2,
                                  int data,
                                  int rs)
{
  voodoo2->dacData
    = DACDATA__WRITE_DATA(data)
    | DACDATA__ADDRESS_2_0(rs >> 0)
    | DACDATA__ADDRESS_4_3(rs >> 3)
    ;
}

static inline void dac_write_pll_8(voodoo2_reg * voodoo2,
                                   int address,
                                   int parameter_m)
{
  dac_data_write(voodoo2, address, DAC__RS__PLL_ADDRESS_WRITE);
  wait_graphics_busy(voodoo2);

  dac_data_write(voodoo2, parameter_m, DAC__RS__PLL_PARAMETER);
  wait_graphics_busy(voodoo2);
}

static inline void dac_write_pll_16(voodoo2_reg * voodoo2,
                                    int address,
                                    int parameter_m,
                                    int parameter_n)
{
  dac_data_write(voodoo2, address, DAC__RS__PLL_ADDRESS_WRITE);
  wait_graphics_busy(voodoo2);

  dac_data_write(voodoo2, parameter_m, DAC__RS__PLL_PARAMETER);
  wait_graphics_busy(voodoo2);

  dac_data_write(voodoo2, parameter_n, DAC__RS__PLL_PARAMETER);
  wait_graphics_busy(voodoo2);
}

typedef struct mn {
  uint8_t m;
  uint8_t n;
} mn_t;

static inline mn_t dac_read_pll_16(voodoo2_reg * voodoo2,
                                   int address)
{
  dac_data_write(voodoo2, address, DAC__RS__PLL_ADDRESS_READ);
  wait_graphics_busy(voodoo2);

  dac_data_read(voodoo2, 0, DAC__RS__PLL_PARAMETER);
  wait_graphics_busy(voodoo2);
  int m = voodoo2->fbiInit2 & 0xff;

  dac_data_read(voodoo2, 0, DAC__RS__PLL_PARAMETER);
  wait_graphics_busy(voodoo2);
  int n = voodoo2->fbiInit2 & 0xff;

  return (mn_t){ m, n };
}

uint8_t read_fd_u8(int fd, uint32_t offset)
{
  off_t ret = lseek(fd, offset, SEEK_SET);
  assert(ret == offset);
  uint8_t value;
  ssize_t len = read(fd, &value, 1);
  assert(len == 1);
  return value;
}

uint16_t read_fd_u16(int fd, uint32_t offset)
{
  off_t ret = lseek(fd, offset, SEEK_SET);
  assert(ret == offset);
  uint16_t value;
  ssize_t len = read(fd, &value, 2);
  assert(len == 2);
  return value;
}

void write_fd_u8(int fd, uint32_t offset, uint8_t value)
{
  off_t ret = lseek(fd, offset, SEEK_SET);
  assert(ret == offset);
  ssize_t len = write(fd, &value, 1);
  assert(len == 1);
}

void write_fd_u32(int fd, uint32_t offset, uint32_t value)
{
  off_t ret = lseek(fd, offset, SEEK_SET);
  assert(ret == offset);
  ssize_t len = write(fd, &value, 4);
  assert(len == 4);
}

int main()
{
  const char * config_path = "/sys/bus/pci/devices/0000:02:0a.0/config";
  int config_fd = open(config_path, O_RDWR | O_SYNC);
  assert(config_fd >= 0);

  uint16_t vendor_id = read_fd_u16(config_fd, PCI__CONFIG__VENDOR_ID);
  uint16_t device_id = read_fd_u16(config_fd, PCI__CONFIG__DEVICE_ID);
  printf("Vendor ID: %04x\n", vendor_id);
  printf("Device ID: %04x\n", device_id);
  assert(vendor_id == 0x121a);
  assert(device_id == 0x0002);

  ////////////////////////////////////////////////////////////////////////
  // PCI enable
  ////////////////////////////////////////////////////////////////////////
  {
    const char * enable_path = "/sys/bus/pci/devices/0000:02:0a.0/enable";
    int enable_fd = open(enable_path, O_RDWR | O_SYNC);
    assert(enable_fd >= 0);

    if (read_fd_u8(enable_fd, 0) == '0') {
      write_fd_u8(enable_fd, 0, '1'); // enable
      assert(read_fd_u8(enable_fd, 0) != '0');
    }
    close(enable_fd);
  }

  ////////////////////////////////////////////////////////////////////////
  // PCI resource0
  ////////////////////////////////////////////////////////////////////////
  const char * resource0_path = "/sys/bus/pci/devices/0000:02:0a.0/resource0";
  int resource0_fd = open(resource0_path, O_RDWR | O_SYNC);
  assert(resource0_fd >= 0);

  uint32_t resource0_size = 16 * 1024 * 1024;
  void * resource0_base = mmap(0, resource0_size, PROT_READ | PROT_WRITE, MAP_SHARED, resource0_fd, 0);
  assert(resource0_base != MAP_FAILED);

  voodoo2_reg * voodoo2 = (voodoo2_reg *)resource0_base;

  int init_enable;

  ////////////////////////////////////////////////////////////////////////
  // reset
  ////////////////////////////////////////////////////////////////////////

  init_enable
    = INIT_ENABLE__ENABLE_WRITES_TO_HARDWARE_REGISTERS;
  write_fd_u32(config_fd, PCI__CONFIG__INIT_ENABLE, init_enable);
  //wait_graphics_busy(voodoo2);

  voodoo2->fbiInit0
    = FBIINIT0__CHUCK_GRAPHICS_RESET
    | FBIINIT0__CHUCK_FIFO_RESET
    ;
  wait_graphics_busy(voodoo2);

  voodoo2->fbiInit1
    = FBIINIT1__VIDEO_TIMING_RESET
    ;
  wait_graphics_busy(voodoo2);

  voodoo2->fbiInit2
    = 0; // disable DRAM refresh
  wait_graphics_busy(voodoo2);

  ////////////////////////////////////////////////////////////////////////
  // DAC initialization
  ////////////////////////////////////////////////////////////////////////

  init_enable
    = INIT_ENABLE__ENABLE_WRITES_TO_HARDWARE_REGISTERS
    | INIT_ENABLE__REMAP_FBIINIT2_FBIINIT3_TO_DACREAD_VIDEOCHECKSUM;
  write_fd_u32(config_fd, PCI__CONFIG__INIT_ENABLE, init_enable);
  wait_graphics_busy(voodoo2);

  int command = DAC__COMMAND__COLOR_MODE__16BIT_DIRECT_COLOR_WITH_BYPASS;
  dac_data_write(voodoo2, command, DAC__RS__COMMAND);
  wait_graphics_busy(voodoo2);
  dac_data_read(voodoo2, command, DAC__RS__COMMAND);
  wait_graphics_busy(voodoo2);
  int command_read = voodoo2->fbiInit2 & 0xff;
  printf("dac read: DAC__RS__COMMAND: m: %02x\n", command_read);

  /*
    pixel clock: 40MHz
    graphics/memory clock: 44.6MHz
  */

  int m = 54;
  int n = 67;
  dac_write_pll_16(voodoo2, DAC__PLL_PARAMETER__CLK0_f0_PLL, m, n);
  mn_t clk0_f0_res = dac_read_pll_16(voodoo2, DAC__PLL_PARAMETER__CLK0_f0_PLL);
  printf("dac read: DAC__PLL_PARAMETER__CLK0_f0_PLL: m: %02x n: %02x\n",
         clk0_f0_res.m, clk0_f0_res.n);

  int pll_control
    = DAC__PLL_CONTROL__CLK0_SELECT(0) // f0
    | DAC__PLL_CONTROL__CLK1_SELECT(0) // fA
    | DAC__PLL_CONTROL__ENABLE_INTERNAL_CLOCK_SELECT;
  dac_write_pll_8(voodoo2, DAC__PLL_PARAMETER__PLL_CONTROL, pll_control);
  mn_t pll_control_res = dac_read_pll_16(voodoo2, DAC__PLL_PARAMETER__PLL_CONTROL);
  printf("dac read: DAC__PLL_PARAMETER__PLL_CONTROL: m: %02x\n",
         pll_control_res.m);

  ////////////////////////////////////////////////////////////////////////
  // cleanup
  ////////////////////////////////////////////////////////////////////////
  close(config_fd);
  close(resource0_fd);

  return 0;
}