#define _GNU_SOURCE

#include <errno.h>
#include <error.h>
#include <fcntl.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

/* CLOCK_CNTL_INDEX - RW - 32 bits - [IOReg,MMReg:0x8] */
#define CLOCK_CNTL_INDEX	0x8
#	define CLOCK_CNTL_INDEX__PLL_ADDR_MASK		0x3f
#	define CLOCK_CNTL_INDEX__PLL_ADDR_SHIFT		0
#	define CLOCK_CNTL_INDEX__PLL_WR_EN_MASK		0x1
#	define CLOCK_CNTL_INDEX__PLL_WR_EN_SHIFT	7
#	define CLOCK_CNTL_INDEX__PPLL_DIV_SEL_MASK	0x3
#	define CLOCK_CNTL_INDEX__PPLL_DIV_SEL_SHIFT	8
/* CLOCK_CNTL_DATA - RW - 32 bits - [IOReg,MMReg:0xC] */
#define CLOCK_CNTL_DATA		0xc

/* SCLK_CNTL - RW - 32 bits - CLKIND:0xD */
#define SCLK_CNTL		0xd
#define SCLK_CNTL__SCLK_SRC_SEL_MASK		0x7
#define SCLK_CNTL__SCLK_SRC_SEL_SHIFT		0
	/* ... */

/* GA_SOFT_RESET - RW - 32 bits - [MMReg:0x429C] */
#define GA_SOFT_RESET		0x429c

/* RBBM_SOFT_RESET - RW - 32 bits - [IOReg,MMReg:0xF0] [MMReg:0xE48] */
#define RBBM_SOFT_RESET		0xf0

int32_t *reg_mem_map = NULL;

int32_t
r32 (int32_t index)
{
  int32_t data;

  data = reg_mem_map[index >> 2];
  fprintf (stderr, "%s: INDEX=0x%08x DATA=0x%08x\n", __func__, index, data);

  return data;
}

void
w32 (int32_t index, int32_t data)
{
  fprintf (stderr, "%s: INDEX=0x%08x DATA=0x%08x\n", __func__, index, data);
  reg_mem_map[index >> 2] = data;
}

int32_t
R_CLOCK_CNTL (int32_t index)
{
  int32_t data;

  w32 (CLOCK_CNTL_INDEX,
       ((index & CLOCK_CNTL_INDEX__PLL_ADDR_MASK) << CLOCK_CNTL_INDEX__PLL_ADDR_SHIFT) |
       ((1 & CLOCK_CNTL_INDEX__PLL_WR_EN_MASK) << CLOCK_CNTL_INDEX__PLL_WR_EN_SHIFT) |
       ((0 & CLOCK_CNTL_INDEX__PPLL_DIV_SEL_MASK) << CLOCK_CNTL_INDEX__PPLL_DIV_SEL_SHIFT));
  data = r32 (CLOCK_CNTL_DATA);

  fprintf (stderr, "%s: INDEX=0x%08x DATA=0x%08x\n", __func__, index, data);

  return data;
}

void
W_CLOCK_CNTL (int32_t index, int32_t data)
{
  fprintf (stderr, "%s: INDEX=0x%08x DATA=0x%08x\n", __func__, index, data);

  w32 (CLOCK_CNTL_INDEX, ((index & CLOCK_CNTL_INDEX__PLL_ADDR_MASK) << CLOCK_CNTL_INDEX__PLL_ADDR_SHIFT) |
       ((1 & CLOCK_CNTL_INDEX__PLL_WR_EN_MASK) << CLOCK_CNTL_INDEX__PLL_WR_EN_SHIFT) |
       ((0 & CLOCK_CNTL_INDEX__PPLL_DIV_SEL_MASK) << CLOCK_CNTL_INDEX__PPLL_DIV_SEL_SHIFT));
  w32 (CLOCK_CNTL_DATA, data);
}

int
main (int argc, char **argv)
{
  int mem_fd;

  /* FIXME: Memory at dfff0000 (64-bit, non-prefetchable) [size=64K] */
  int reg_addr = 0xdfff0000;
  int reg_len = 0x10000;

  if ((mem_fd = open ("/dev/mem", O_RDWR)) < 0)
    {
      fprintf (stderr, "%s: %s:%d: %s\n", program_invocation_short_name,
	       __FILE__, __LINE__, strerror (errno));
      exit (EXIT_FAILURE);
    }

  if ((reg_mem_map =
       mmap (NULL, reg_len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd,
	     reg_addr)) == MAP_FAILED)
    {
      fprintf (stderr, "%s: %s:%d: %s\n", program_invocation_short_name,
	       __FILE__, __LINE__, strerror (errno));
      exit (EXIT_FAILURE);
    }

#if 1

  int32_t sclk_cntl;
  struct timespec timespec_15ms = { 0, 15000000 };	/* 15 Milliseconds */
  int i;

  sclk_cntl = R_CLOCK_CNTL (SCLK_CNTL);
  W_CLOCK_CNTL (SCLK_CNTL,
		sclk_cntl & ~(SCLK_CNTL__SCLK_SRC_SEL_MASK <<
			      SCLK_CNTL__SCLK_SRC_SEL_SHIFT));

  nanosleep (&timespec_15ms, NULL);
  w32 (GA_SOFT_RESET, 0x200);
  nanosleep (&timespec_15ms, NULL);

  for (i = 0; i < 16; i++)
    {
      w32 (RBBM_SOFT_RESET, /* FIXME */
	   1 << 0 | 1 << 1 | 1 << 2 | 1 << 5 | 1 << 7 | 1 << 8 | 1 << 10 | 1
	   << 11 | 1 << 12 | 1 << 13 | 1 << 14);
      nanosleep (&timespec_15ms, NULL);
    }

  W_CLOCK_CNTL (SCLK_CNTL, sclk_cntl);

#endif

  if (munmap (reg_mem_map, reg_len) < 0)
    {
      fprintf (stderr, "%s: %s:%d: %s\n", program_invocation_short_name,
	       __FILE__, __LINE__, strerror (errno));
      exit (EXIT_FAILURE);
    }

  if (close (mem_fd) < 0)
    {
      fprintf (stderr, "%s: %s:%d: %s\n", program_invocation_short_name,
	       __FILE__, __LINE__, strerror (errno));
      exit (EXIT_FAILURE);
    }

  return 0;
}