/*
  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_ADDR(x,y) flash[((x & 0x7FFF)<<1) | ((x & 0x8000) >> 15)]=(y) 
#define READ_ADDR(x) flash[((x & 0x7FFF)<<1) | ((x & 0x8000) >> 15)] 

struct patch_struct {
  char *desc;
  unsigned int byte_offset;
  unsigned short original_word1, original_word2, new_word1, new_word2;
};

/*struct patch_struct patches[] = {
  {"make drive signature check accept 0: [cmp r0, #1 -> cmp r0, #0]",
   0xBB0, 0x0001, 0xE350, 0x0000, 0xE350},
  {"make ramdisk check accept 0: [cmp r0, #1 -> cmp r0, #0]",
   0x350, 0x0001, 0xE350, 0x0000, 0xE350},
  {"make other file check accept 0: [cmp r0, #1 -> cmp r0, #0]",
   0x2E0, 0x0001, 0xE350, 0x0000, 0xE350},
  {"make crypto verification always return 0: [mov r0, #1 -> mov r0, #0]",
   0x1C80, 0x0001, 0xE3A0, 0x0000, 0xE3A0}};
*/

struct patch_struct patches[] = {
  {"make drive signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0xBB8, 0x0033, 0x1A00, 0x0000, 0x1A00}};

int main(int argc, char *argv[])
{
	int fd_mem, fd_flash, i, i2;
	volatile unsigned short *flash;
	unsigned long off,valint;
	unsigned short val, wrote;

	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);
	}
	
	printf("PhatPatch v0.2 -bushing\n");
	printf("first 2 words of flash=%04x %04x\n",flash[0],flash[2]);
	printf("writing auto-id command (AA, 55, 90)\n");
	fflush(NULL);

	WRITE_ADDR(0x555,0xaa);
	WRITE_ADDR(0x2aa,0x55);
	WRITE_ADDR(0x555,0x90);
	
	printf("Flash chip reports manufacturer id=%04x, device id=%04x\n",flash[0],flash[2]);
	fflush(NULL);
	
	printf("Resetting flash.\n");
	fflush(NULL);
	WRITE_ADDR(0,0xF0);

	printf("Testing patch locations:\n");
	fflush(NULL);
	
	for(i=0;i<sizeof(patches)/sizeof(struct patch_struct);i++) {
	  printf("Patch %d @ %04x: %s\n",i+1,patches[i].byte_offset,
		 patches[i].desc);
	fflush(NULL);
	  printf("Expected: %04x %04x    Actual: %04x %04x\n",
		 patches[i].original_word1, patches[i].original_word2,
		 flash[patches[i].byte_offset/2],
		 flash[patches[i].byte_offset/2+1]);
	  fflush(NULL);
	  if(patches[i].original_word1==flash[patches[i].byte_offset/2] &&
	     patches[i].original_word2==flash[patches[i].byte_offset/2+1]) {
	    printf("Match! Programming...\n");
	    fflush(NULL);
	    if(patches[i].original_word1!=patches[i].new_word1) {
	      WRITE_ADDR(0x555,0xaa);
	      WRITE_ADDR(0x2aa,0x55);
	      WRITE_ADDR(0x555,0xa0);
	      
	      flash[patches[i].byte_offset/2]=patches[i].new_word1;
	      while(flash[patches[i].byte_offset/2]!=patches[i].new_word1) {
		printf("Waiting...\n");
		fflush(NULL);
	      }

	      printf("Wrote %04x\n",patches[i].new_word1);
	      fflush(NULL);
	    }
	    
	    if(patches[i].original_word2!=patches[i].new_word2) {
	      WRITE_ADDR(0x555,0xaa);
	      WRITE_ADDR(0x2aa,0x55);
	      WRITE_ADDR(0x555,0xa0);
	      
	      flash[patches[i].byte_offset/2+1]=patches[i].new_word2;
	      printf("Wrote %04x\n",patches[i].new_word2);
	      fflush(NULL);
	      while(flash[patches[i].byte_offset/2+1]!=patches[i].new_word2) {
		printf("Waiting...\n");
		fflush(NULL);
	      }
	    }
	    
	    
	  } else {
	    printf("Mismatch!\n");
	    fflush(NULL);
	  }
	}

	printf("Verifying:\n");
		fflush(NULL);
	for(i=0;i<sizeof(patches)/sizeof(struct patch_struct);i++) {
	  printf("Patch %d @ %04x: %s\n",i+1,patches[i].byte_offset,
		 patches[i].desc);
		fflush(NULL);
	  printf("Expected: %04x %04x    Actual: %04x %04x\n",
		 patches[i].new_word1, patches[i].new_word2,
		 flash[patches[i].byte_offset/2],
		 flash[patches[i].byte_offset/2+1]);
		fflush(NULL);
	  if(patches[i].new_word1==flash[patches[i].byte_offset/2] &&
	     patches[i].new_word2==flash[patches[i].byte_offset/2+1]) {
	    printf("Verified!\n");
	  } else {	    
	    printf("Unverified!\n");
	  }
	}

	munmap((void *) flash, 0x20000);
	return 0;
}