/*
/*
  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 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;
  fflush(NULL);
  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);
    }
  }
}

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 off,valint,uaddr1,uaddr2;
  unsigned short val,wrote,word1,word2;

  printf("PhatPatch v0.7 - 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 *)0x00000000, 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=flash[0];
      word2=flash[2];
      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");
      fflush(NULL);
  
      WRITE_ADDR(0x555,0xaa);
      WRITE_ADDR(0x2aa,0x55);
      WRITE_ADDR(0x555,0x90);
  
      if (word1 != flash[0] || word2 != flash[2]) {
        printf("Flash chip reports manufacturer id=%04x, device id=%04x\n",flash[0],flash[2]);
        printf("offsets 0x555 and 0x2aa verified\n");
        uaddr1=0x555;
        uaddr2=0x2aa;
      } else {
        printf("testing offsets 0x5555 and 0x2aaa\n");
        printf("writing auto-id command (AA, 55, 90)\n");
        fflush(NULL);
  
        WRITE_ADDR(0x5555,0xaa);
        WRITE_ADDR(0x2aaa,0x55);
        WRITE_ADDR(0x5555,0x90);
        if (word1 != flash[0] || word2 != flash[2]) {
          printf("Flash chip reports manufacturer id=%04x, device id=%04x\n",flash[0],flash[2]);
          printf("offsets 0x5555 and 0x2aaa verified\n");
          uaddr1=0x5555;
          uaddr2=0x2aaa;
        } else {
          printf("Error: unable to unlock flash\n");
          exit(1);
        }
      }
      fflush(NULL);
  
      printf("Resetting flash.\n");
      fflush(NULL);
      WRITE_ADDR(0,0xF0);

      printf("Testing patch locations:\n");
      fflush(NULL);
  
      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);
        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(uaddr1,0xaa);
            WRITE_ADDR(uaddr2,0x55);
            WRITE_ADDR(uaddr1,0xa0);
  
            flash[patches[i].byte_offset/2]=patches[i].new_word1;
            sleep(2);
            fflush(NULL);
            while(flash[patches[i].byte_offset/2]!=patches[i].new_word1) {
              printf("Waiting...\n");
              fflush(NULL);
            }
            printf("Wrote %04x\n",patches[i].new_word1);
          }
  
          if(patches[i].original_word2!=patches[i].new_word2) {
            WRITE_ADDR(uaddr1,0xaa);
            WRITE_ADDR(uaddr2,0x55);
            WRITE_ADDR(uaddr1,0xa0);
  
            flash[patches[i].byte_offset/2+1]=patches[i].new_word2;
            sleep(2);
            fflush(NULL);
            while(flash[patches[i].byte_offset/2+1]!=patches[i].new_word2) {
              printf("Waiting...\n");
              fflush(NULL);
            }
            printf("Wrote %04x\n",patches[i].new_word2);
          }
        } 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");
          }
          fflush(NULL);
        }
        sleep(2);
      }
      break;

    case 'v':
      printf("Verifying:\n");
      count=0;
      fflush(NULL);
      for(i=0;i<num_patches;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 {	    
          count++;
          printf("Unverified!\n");
        }
      }
      ret = count;
      break;

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

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