/*
  phat patch! woo -bushing
 */

#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#define WRITE(x,y) flash[((x & 0x7FFF)<<1) | ((x & 0x8000) >> 15)]=(y) 
#define READ(x) flash[((x & 0x7FFF)<<1) | ((x & 0x8000) >> 15)] 

#define UADDR1 (0x2aa)
#define UADDR2 (0x555)

#define UNLOCK flash[UADDR1] = 0xAA; flash[UADDR2] = 0x55

int main(int argc, char *argv[])
{
  int fd_mem;
  unsigned int addr;
  unsigned short val;
  volatile unsigned short *flash;

  printf("Test program...\n");

  if (argc<2) {
    printf("Usage: phatpatch addr val\n");
    exit(-1);
  }

  if ((fd_mem = open("/dev/mem", O_RDWR |O_SYNC)) < 0) {
    perror("Can not open /dev/mem");
    exit(1);
  }

  flash = mmap((void *)0x00000000, 0x20000, PROT_WRITE | PROT_READ, MAP_SHARED,
  fd_mem, (off_t) (0x00000000));
  if (flash == MAP_FAILED) {
    perror("Error MMAP /dev/mem");
    exit(1);
  }

  sscanf(argv[1], "%i", &addr);
  sscanf(argv[2], "%hi", &val);

  flash[addr] = val;

  //printf("Setting 0x%08x to 0x%04x (was 0x%04x)\n", addr, val, READ(addr));
  
  //UNLOCK;
//  WRITE_ADDR(UADDR1, 0xaa);
//  WRITE_ADDR(UADDR2, 0x55);
//  WRITE_ADDR(UADDR1, 0x90);
//  flash[UADDR1] = 0xaa;
//  flash[UADDR2] = 0x55;
//  flash[UADDR1] = 0x90;

//  printf("Values: %04x %04x %04x %04x\n", flash[0], flash[1], flash[2], flash[3]);
//  flash[UADDR1] = 0xF0;

#if 0
  WRITE_ADDR(UADDR1, 0xa0);
  //WRITE(UADDR1, 0xa0);
  WRITE(addr, val);

  while (
          ((READ(addr)>>7)&1) != ((val>>7)&1)
        )
  {
    if ( ( (READ(addr)>>5) &1) == 1)
    {
      if (
          ((READ(addr)>>7)&1) == ((val>>7)&1)
      )
      { // OK
        printf("Wrote %04x\n", val);
      } else {
        printf("Error writing %04x\n", val);
      }
      break;
    }
  }

#endif
  munmap((void *) flash, 0x20000);
  close(fd_mem);
  return 0;
}