/**
  PhatPatch...
 */

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

#define EP7211_VIRT_FLASH1              (0xfa000000)
#define EP7211_PHYS_FLASH1              (0x00000000)
#define FLASH_BASE EP7211_VIRT_FLASH1

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

#define UNLOCK(x) WRITE_ADDR(uaddr1, 0xaa); WRITE_ADDR(uaddr2, 0x55); WRITE_ADDR(uaddr1, x)
#define RESET 0xF0

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

struct patch_struct volvo_patches[] = {
  {"make drive signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0D90, 0x0033, 0x1A00, 0x0000, 0x1A00},
  {"make rc.sh signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0DC4, 0x0026, 0x1A00, 0x0000, 0x1A00},
  {"make phatd signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0DF8, 0x0019, 0x1A00, 0x0000, 0x1A00},
  {"make linux signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0E2C, 0x000C, 0x1A00, 0x0000, 0x1A00},
  {"make ramdisk invalid signature return 0 instead of 0xFFFFFFFF: [movlne r0, 0xFFFFFFFF -> movlne r0, #0]",
   0x051C, 0x0000, 0x13E0, 0x0000, 0x13A0},
  {"make ramdisk signature check verify 0 instead of 1: [cmp r0, #1 -> cmp r0, #0]",
   0x0E58, 0x0001, 0xE350, 0x0000, 0xE350},
  {"make ramdisk valid signature return 0 instead of 1: [moveq r0, #1 -> moveq r0, #0]",
   0x0520, 0x0001, 0x03A0, 0x0000, 0x03A0},
  {"don't try to read ramdisk.sig (boot without any .sig files): [bl sector_read_suzy -> bl PC+1]",
   0x04F4, 0x02E0, 0xEB00, 0x0000, 0xEB00},
  {"don't try to read linux.sig (boot without any .sig files): [bl sector_read_suzy -> bl PC+1]",
   0x0460, 0x0305, 0xEB00, 0x0000, 0xEB00}
};

struct patch_struct standard_patches[] = {
  {"make drive signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0BB8, 0x0033, 0x1A00, 0x0000, 0x1A00},
  {"make rc.sh signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0BEC, 0x0026, 0x1A00, 0x0000, 0x1A00},
  {"make phatd signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0C20, 0x0019, 0x1A00, 0x0000, 0x1A00},
  {"make linux signature check always succeed: [bne verify_sig_failed -> bne PC+1]",
   0x0C54, 0x000C, 0x1A00, 0x0000, 0x1A00},
  {"make ramdisk invalid signature return 0 instead of 0xFFFFFFFF: [movlne r0, 0xFFFFFFFF -> movlne r0, #0]",
   0x0354, 0x0000, 0x13E0, 0x0000, 0x13A0},
  {"make ramdisk signature check verify 0 instead of 1: [cmp r0, #1 -> cmp r0, #0]",
   0x0C80, 0x0001, 0xE350, 0x0000, 0xE350},
  {"make ramdisk valid signature return 0 instead of 1: [moveq r0, #1 -> moveq r0, #0]",
   0x0358, 0x0001, 0x03A0, 0x0000, 0x03A0},
  {"don't try to read ramdisk.sig (boot without any .sig files): [bl sector_read_suzy -> bl PC+1]",
   0x0330, 0x02DB, 0xEB00, 0x0000, 0xEB00},
  {"don't try to read linux.sig (boot without any .sig files): [bl sector_read_suzy -> bl PC+1]",
   0x02C0, 0x02F7, 0xEB00, 0x0000, 0xEB00}
};

int num_patches;

struct patch_struct *chose_patches(volatile unsigned short *flash)
{
  int verified,i;
  printf("Finding patch offsets:\n");
  verified=1;
  num_patches=sizeof(standard_patches)/sizeof(struct patch_struct);
  for(i=0;i<num_patches;i++) {
    if (!(
        (standard_patches[i].original_word1==flash[standard_patches[i].byte_offset/2] && standard_patches[i].original_word2==flash[standard_patches[i].byte_offset/2+1]) ||
        (standard_patches[i].new_word1==flash[standard_patches[i].byte_offset/2] && standard_patches[i].new_word2==flash[standard_patches[i].byte_offset/2+1]))) {
      verified=0;
      break;
    }
  }
  if (verified) {
    printf("Verified standard patch offsets\n");
    return standard_patches;
  } else {
    verified=1;
    num_patches=sizeof(volvo_patches)/sizeof(struct patch_struct);
    for(i=0;i<num_patches;i++) {
      if (!(
          (volvo_patches[i].original_word1==flash[volvo_patches[i].byte_offset/2] && volvo_patches[i].original_word2==flash[volvo_patches[i].byte_offset/2+1]) ||
          (volvo_patches[i].new_word1==flash[volvo_patches[i].byte_offset/2] && volvo_patches[i].new_word2==flash[volvo_patches[i].byte_offset/2+1]))) {
        verified=0;
        break;
      }
    }
    if (verified) {
      printf("Verified volvo patch offsets\n");
      return volvo_patches;
    } else {
      printf("ERROR: Unable to verify patch offsets\n");
      exit(1);
    }
  }
}

unsigned short write_word(unsigned int uaddr1, unsigned int uaddr2, volatile unsigned short flash[], unsigned int addr, unsigned short val)
{
  UNLOCK(0xa0);
  flash[addr] = val;

#define DQ7(x) ((x>>7)&1)
#define DQ5(x) ((x>>5)&1)

  while ( DQ7(flash[addr]) != DQ7(val) )
  {
    if ( DQ5(flash[addr]) == 1 )
    {
      if (DQ7(flash[addr]) == DQ7(val))
      { // OK
        return flash[addr];
      } else {
        return val ^ 0xffff;
      }
      break;
    }
  }
  return flash[addr];
}

int main(int argc, char *argv[])
{
  int fd_mem, fd_flash, i, count, ret=0;
  struct patch_struct *patches;
  volatile unsigned short *flash;
  unsigned long uaddr1,uaddr2;
  unsigned short word1,word2;

  printf("PhatPatch v0.8 - original code by bushing, additional patches by sbingner\n");

  if (argc<2) {
    printf("Usage: phatpatch OPT ARG\n\tOPTS:\n\t\tp = Patch flash\n\t\tv = Verify patched flash\n\t\ts filename = save flash to filename\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 *)FLASH_BASE, 0x20000, PROT_WRITE | PROT_READ, MAP_SHARED, fd_mem, (off_t) (0x00000000));
  if (flash == MAP_FAILED) {
    perror("Error MMAP /dev/mem");
    exit(1);
  }

  switch(*argv[1]) {
    case 's':

      if (argc<3) {
        printf("Error: provide name of file to save flash to\n");
        munmap((void *) flash, 0x20000);
        exit(1);
      } 
      printf("Saving current flash.\n");
      if ((fd_flash = open(argv[2], O_RDWR|O_CREAT|O_SYNC)) < 0) {
        perror("Can not create /dos/Data/flash.rom");
        munmap((void *) flash, 0x20000);
        exit(1);
      }
      write(fd_flash, (void *)(flash), 0x20000);
      close(fd_flash);
      break;
  
    case 'p':
      if (argc==3) {
        count=atoi(argv[2]);
      } else {
        count=100;
      }
      patches=chose_patches(flash);
      word1=READ_ADDR(0);
      word2=READ_ADDR(1);
      printf("first 2 words of flash=%04x %04x\n",word1,word2);
      printf("testing offsets 0x555 and 0x2aa\n");
      printf("writing auto-id command (AA, 55, 90)\n");

      uaddr1=0x555;
      uaddr2=0x2aa;
      UNLOCK(0x90);
  
      if (word1 != READ_ADDR(0) || word2 != READ_ADDR(1)) {
        printf("Flash chip reports manufacturer id=%04x, device id=%04x\n",READ_ADDR(0),READ_ADDR(1));
        printf("offsets 0x555 and 0x2aa verified\n");
      } else {
        printf("testing offsets 0x5555 and 0x2aaa\n");
        printf("writing auto-id command (AA, 55, 90)\n");
  
        uaddr1=0x5555;
        uaddr2=0x2aaa;
        UNLOCK(0x90);
        if (word1 != READ_ADDR(0) || word2 != READ_ADDR(1)) {
          printf("Flash chip reports manufacturer id=%04x, device id=%04x\n",READ_ADDR(0),READ_ADDR(1));
          printf("offsets 0x5555 and 0x2aaa verified\n");
        } else {
          printf("Error: unable to unlock flash\n");
          exit(1);
        }
      }
  
      printf("Resetting flash.\n");
      WRITE_ADDR(0,RESET);

      printf("Testing patch locations:\n");
  
      for(i=0;i<num_patches;i++) {
        if (i >= count)
          break;  // Only apply N patches -- this way we can automagically corrupt the ramdisk sig when necessary
        printf("Patch %d @ %04x: %s\n",i+1,patches[i].byte_offset,
        patches[i].desc);
        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]);
        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");
          if(patches[i].original_word1!=patches[i].new_word1) {
            if (write_word(uaddr1, uaddr2, flash, patches[i].byte_offset/2, patches[i].new_word1) == patches[i].new_word1) {
              printf("Wrote %04x\n",patches[i].new_word1);
//            } else {
//              printf("Error writing patch %d\n", i);
//              exit(-1);
            }
          }
  
          if(patches[i].original_word2!=patches[i].new_word2) {
            if (write_word(uaddr1, uaddr2, flash, patches[i].byte_offset/2+1, patches[i].new_word2) == patches[i].new_word2) {
              printf("Wrote %04x\n",patches[i].new_word2);
//            } else {
//              printf("Error writing patch %d\n", i);
//              exit(-1);
            }
          }
        } else {
          if (flash[patches[i].byte_offset/2]==patches[i].new_word1 && flash[patches[i].byte_offset/2+1]==patches[i].new_word2) {
            printf("Detected patch %d already applied\n", i+1);
          } else {
            printf("Mismatch!\n");
          }
        }
      }
      break;

    case 'v':
      patches=chose_patches(flash);
      printf("Verifying:\n");
      count=0;
      for(i=0;i<num_patches;i++) {
        printf("Patch %d @ %04x: %s\n",i+1,patches[i].byte_offset,
          patches[i].desc);
        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]);
        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 {	    
          count++;
          printf("Unverified!\n");
        }
      }
      ret = count;
      break;

    default:
      printf("Invalid option\n");
      break;
  }

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