// **************************************************************************
//
//  WRT54G.C - WRT54G EJTAG DeBrick Utility  v3.0
//
//  Note:
//  This program is for De-Bricking a WRT54G/GS version 1.x/2.x router.  It 
//  has only been tested (by me) on a WRT54G version 2.0 router.  
//  
//  New for v3.0 - the software has been changed to fully support units with
//                 a larger 8MB flash chip (i.e. - the GS Units).  It now
//                 has a "/8MB" command line switch to support these chips.
//                 Software also now uses Flash Window 2 (0x1C000000) instead
//                 of Flash Window 1 (0x1FC00000) in order to support the
//                 larger 8MB flash chips.
//
// **************************************************************************
//
//  wrt54g: read/write flash memory via EJTAG
//   usage: wrt54g [option] </noreset> </noerase> </notimestamp>
//              -backup:cfe
//              -backup:nvram
//              -backup:kernel
//              -backup:wholeflash
//              -erase:cfe
//              -erase:nvram
//              -erase:kernel
//              -erase:wholeflash
//              -flash:cfe
//              -flash:nvram
//              -flash:kernel
//              -flash:wholeflash
//
//              /noreset     (prevents CPU reset of Broadcom Chip ..... optional)
//              /noerase     (prevents forced erase before flashing ... optional)
//              /notimestamp (prevents timestamping of backups ........ optional)
//              /8MB         (used to support 8MB flash chips ......... optional)
//
//  Written by HairyDairyMaid (a.k.a. - lightbulb)
//  hairydairymaid@yahoo.com
// **************************************************************************
//
//  This program is copyright (C) 2004 HairyDairyMaid (a.k.a. Lightbulb)
//  This program is free software; you can redistribute it and/or modify it
//  under the terms of version 2 the GNU General Public License as published
//  by the Free Software Foundation.
//  This program is distributed in the hope that it will be useful, but WITHOUT
//  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
//  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
//  more details.
//  To view a copy of the license go to:
//  http://www.fsf.org/copyleft/gpl.html
//  To receive a copy of the GNU General Public License write the Free Software
//  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
// **************************************************************************

#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include "wrt54g.h"

int DEBUG_DMA_RW = 0;
int DEBUG_PRACC_RW = 0;
unsigned int SEGMENT_MASK = 0xFFFFFFFF;
int pfd;
static unsigned int ctrl_reg;
static unsigned int ctrl_out;
int chip_type;
int instruction_length;
int issue_reset     = 1;
int issue_erase     = 1;
int issue_timestamp = 1;
int flash_chip_type = FLASH_CHIP_UNKNOWN;
int flash_is_8MB    = 0;


void test_reset()
{
    clockin(1, 0);
    clockin(1, 0);
    clockin(1, 0);
    clockin(1, 0);
    clockin(1, 0);
}


void capture_dr()
{
    clockin(1, 0);  // Select DR scan
    clockin(0, 0);  // Capture DR
}


void capture_ir()
{
    clockin(1, 0);  // Select DR scan
    clockin(1, 0);  // Select IR scan
    clockin(0, 0);  // Capture IR
}


void set_instr(int instr)
{
    int i;
    static int curinstr = 0xFFFFFFFF;

    if (instr == curinstr)
        return;
        
    capture_ir();

    clockin(0,0);
    for (i=0; i < instruction_length; i++)
    {
        clockin(i==(instruction_length - 1), (instr>>i)&1);
    }

    clockin(1, 0);  // Update IR
    clockin(0, 0);  // Run-Test/Idle

    curinstr = instr;
}


void lpt_openport(void)
{
    pfd = open("/dev/parport0", O_RDWR);
    if (pfd < 0)
    {
        perror("Failed to open /dev/parport0");
        exit(0);
    }

    if ((ioctl(pfd, PPEXCL) < 0) || (ioctl(pfd, PPCLAIM) < 0))
    {
        perror("Failed to lock /dev/parport0");
        close(pfd);
        exit(0);
    }

}


void lpt_closeport(void)
{
    if (ioctl(pfd, PPRELEASE) < 0)
    {
        perror("Failed to release /dev/parport0");
        close(pfd);
        exit(0);
    }

    close(pfd);
}


void clockin(int tms, int tdi)
{
    unsigned char data;

    tms = tms ? 1 : 0;
    tdi = tdi ? 1 : 0;

    data = (1 << PROG) | (0 << TCK) | (tms << TMS) | (tdi << TDI);
    if (ioctl(pfd, PPWDATA, &data) < 0)
    {
        perror("Failed to write parport data");
        lpt_closeport();
        exit(0);
    }

    data = (1 << PROG) | (1 << TCK) | (tms << TMS) | (tdi << TDI);
    if (ioctl(pfd, PPWDATA, &data) < 0)
    {
        perror("Failed to write parport data");
        lpt_closeport();
        exit(0);
    }

    data = (1 << PROG) | (0 << TCK) | (tms << TMS) | (tdi << TDI);
    if (ioctl(pfd, PPWDATA, &data) < 0)
    {
        perror("Failed to write parport data");
        lpt_closeport();
        exit(0);
    }
}


unsigned char clockout(void)
{
    unsigned char data;

    data = (1 << PROG) | (0 << TCK);        /* Data input on the LEADING EDGE of clock so we can do this! */
    if (ioctl(pfd, PPWDATA, &data) < 0)
    {
        perror("Failed to write parport data");
        lpt_closeport();
        exit(0);
    }

    if (ioctl(pfd, PPRSTATUS, &data) < 0)
    {
        perror("Failed to read  parport data");
        lpt_closeport();
        exit(0);
    }

    data ^= 0x80;
    data >>= TDO;
    data &= 1;

    return data;
}


void ReadWriteData(unsigned char* ichain, unsigned char* ochain)
{
    int i;

    capture_dr();
    for(i = 0 ; i < 32 ; i++)
    {
        clockin((i == 31), ichain[i/8] >> (i%8) & 1);
        ochain[i/8] &= ~(1 << (i%8));
        ochain[i/8] |= (clockout() << (i%8));
    }
    clockin(1,0);           // enter update DR state
    clockin(0,0);           // enter run-test/idle state

}


void ReadData(unsigned char *read_chain)
{
    int i;


    capture_dr();
    clockin(0, 0);  // Shift DR
    for(i = 0 ; i < 32 ; i++)
    {
        read_chain[i/8] &= ~(1 << (i%8));
        read_chain[i/8] |= (clockout() << (i%8));
        clockin((i == 31), 0);
    }
    clockin(1,0);   // enter update DR state
    clockin(0,0);   // enter run-test/idle state
}


void WriteData(unsigned char *write_chain)
{
    int i;

    capture_dr();
    clockin(0,0);           // enter shift state
    for(i = 0 ; i < 32 ; i++)
    {
        clockin((i == 31), (write_chain[i/8] >> (i%8) & 1) );
    }
    clockin(1,0);           // enter update DR state
    clockin(0,0);           // enter run-test/idle state
}


void ShowData(unsigned int value)
{
    int i;
    for (i=0; i<32; i++)
        printf("%d", (value >> (31-i)) & 1);
    printf(" (%08X)\n", value);
}


void ejtag_issue_reset(void)
{
  set_instr(INSTR_CONTROL);

  printf("Resetting Processor... ");
  ctrl_reg = PRRST ;
  WriteData((unsigned char *) &ctrl_reg);
  ctrl_reg = ~(PRRST|PERRST) ;
  WriteData((unsigned char *) &ctrl_reg);
  printf("Done\n\n");
}


static unsigned int ejtag_dma_read(unsigned int addr)
{
    unsigned int srcaddr, data;

    srcaddr = addr & SEGMENT_MASK ;

    // Setup DMA Control
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Setup Address
    set_instr(INSTR_ADDRESS);
    WriteData((unsigned char *) &srcaddr);

    // Initiate DMA Read and set DSTRT
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | DRWN | DMA_WORD | DSTRT | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Test the DSTRT bit
    while (true)
    {
        ctrl_reg = DMAACC | PROBEN | PRACC;
        ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);
        if (ctrl_out & DSTRT)  continue;
        else  break;
    }

    // Read Data
    set_instr(INSTR_DATA);
    ReadData((unsigned char *) &data);

    // Clear DMA and Check DERR
    set_instr(INSTR_CONTROL);
    ctrl_reg = PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    if (ctrl_out & DERR)  printf("DMA Read Addr = %08x  Data = (%08x)ERROR ON READ\n", addr,data);
    else if (DEBUG_DMA_RW)    printf("DMA Read Addr = %08x  Data = %08x\n", addr, data);

    return(data);
}


static void ejtag_dma_write(unsigned int addr, unsigned int data)
{
    unsigned int srcaddr, srcdata;

    srcaddr = addr & SEGMENT_MASK ;
    srcdata = data;

    // Setup DMA Control
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Setup Address
    set_instr(INSTR_ADDRESS);
    WriteData((unsigned char *) &srcaddr);

    // Setup Data
    set_instr(INSTR_DATA);
    WriteData((unsigned char *) &srcdata);

    // Initiate DMA Write and set DSTRT
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | DMA_WORD | DSTRT | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Test the DSTRT bit
    while (true)
    {
        ctrl_reg = DMAACC | PROBEN | PRACC;
        ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);
        if (ctrl_out & DSTRT)  continue;
        else  break;
    }

    // Clear DMA and Check DERR
    set_instr(INSTR_CONTROL);
    ctrl_reg = PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    if (ctrl_out & DERR)  printf("DMA Write Addr = %08x  Data = ERROR ON WRITE\n", addr);
    else if (DEBUG_DMA_RW)    printf("DMA Write Addr = %08x  Data = %08x\n", addr, data);

    return;
}


static unsigned int ejtag_dma_read_h(unsigned int addr)
{
    unsigned int srcaddr, data;

    srcaddr = addr & SEGMENT_MASK ;

    // Setup DMA Control
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Setup Address
    set_instr(INSTR_ADDRESS);
    WriteData((unsigned char *) &srcaddr);

    // Initiate DMA Read and set DSTRT
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | DRWN | DMA_HALFWORD | DSTRT | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Test the DSTRT bit
    while (true)
    {
        ctrl_reg = DMAACC | PROBEN | PRACC;
        ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);
        if (ctrl_out & DSTRT)  continue;
        else  break;
    }

    // Read Data
    set_instr(INSTR_DATA);
    ReadData((unsigned char *) &data);

    // Clear DMA and Check DERR
    set_instr(INSTR_CONTROL);
    ctrl_reg = PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    /* handle the bigendian/littleendian */
    if ( addr & 0x2 )
      data = (data>>16)&0xffff ;
    else
      data = (data&0x0000ffff) ;

    if (ctrl_out & DERR)  printf("DMA Read Addr = %08x  Data = (%08x)ERROR ON READ\n", addr,data);
    else if (DEBUG_DMA_RW)    printf("DMA Read Addr = %08x  Data = %08x\n", addr, data);

    return(data);
}


static void ejtag_dma_write_h(unsigned int addr, unsigned int data)
{
    unsigned int srcaddr, srcdata;

    srcaddr = addr & SEGMENT_MASK ;
    srcdata = data;

    // Setup DMA Control
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Setup Address
    set_instr(INSTR_ADDRESS);
    WriteData((unsigned char *) &srcaddr);

    /* handle the bigendian/littleendian */
    //data = (data&0x0000ffff) | (data<<16) ;

    // Setup Data
    set_instr(INSTR_DATA);
    WriteData((unsigned char *) &srcdata);

    // Initiate DMA Write and set DSTRT
    set_instr(INSTR_CONTROL);
    ctrl_reg = DMAACC | DMA_HALFWORD | DSTRT | PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    // Test the DSTRT bit
    while (true)
    {
        ctrl_reg = DMAACC | PROBEN | PRACC;
        ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);
        if (ctrl_out & DSTRT)  continue;
        else  break;
    }

    // Clear DMA and Check DERR
    set_instr(INSTR_CONTROL);
    ctrl_reg = PROBEN | PRACC;
    ReadWriteData((unsigned char *) &ctrl_reg, (unsigned char *) &ctrl_out);

    if (ctrl_out & DERR)  printf("DMA Write Addr = %08x  Data = ERROR ON WRITE\n", addr);
    else if (DEBUG_DMA_RW)    printf("DMA Write Addr = %08x  Data = %08x\n", addr, data);

    return;
}


static void chip_detect(void)
{
    unsigned int id;

    printf("\nProbing bus... "); fflush(stdout);

    lpt_openport();

    // First, Attempt Detect of BCM4702 Chip
    test_reset();
    clockin(0,0); // enter run-test/idle state
    instruction_length = 0x05;
    set_instr(INSTR_IDCODE);
    ReadData((unsigned char *) &id);
    if (id == 0x0471017F)  // ID String: (Hi) 00000100011100010000000101111111 (Lo)
    {
        chip_type = BCM4702_CHIP;
        printf("\nCHIP ID: ");
        ShowData(id);
        printf("*** Found a Broadcom BCM4702 Rev 1 chip ***\n\n");
        return;
    }

    // If Not Found, Then Attempt Detect of BCM4712 Chip
    test_reset();
    clockin(0,0); // enter run-test/idle state
    instruction_length = 0x08;
    set_instr(INSTR_IDCODE);
    ReadData((unsigned char *) &id);
    if (id == 0x1471217F)  // ID String: (Hi) 00010100011100010010000101111111 (Lo)
    {
        chip_type = BCM4712_CHIP;
        printf("\nCHIP ID: ");
        ShowData(id);
        printf("*** Found a Broadcom BCM4712 Rev 1 chip ***\n\n");
        return;
    }
    
    printf("\nCHIP ID: ");
    ShowData(id);
    printf("*** Unrecognized Chip ***\n");
    printf("*** This is not a Broadcom BCM47XX chip ***\n");
    printf("*** Possible Causes:\n");
    printf("    1) WRT54G is not Connected.\n");
    printf("    2) WRT54G is not Powered On.\n");
    printf("    3) Improper JTAG Cable.\n");
    printf("    4) Unrecognized Chip Version of WRT54G.\n");
    lpt_closeport();
    exit(0);

}


static void chip_shutdown(void)
{
    fflush(stdout);
    test_reset();
    lpt_closeport();
}


void setup_Watchdog(void)
{
    // Clear Watchdog
    ejtag_dma_write(0xb8000080,0x00000000);
}


void setup_DCR(void)
{
    unsigned int old_dcr, new_dcr;

    printf("Enabling Memory Writes... ");
    fflush(stdout);
    old_dcr = ejtag_dma_read(0xff300000);
    // Clear the Following: Mask Soft Reset, Memory Protection, Mask Non-Maskable Interrupt, and Mask Interrupt
    new_dcr = old_dcr & ~((1<<1) | (1<<2) | (1<<3) | (1<<4));
    ejtag_dma_write(0xff300000, new_dcr);
    new_dcr = ejtag_dma_read(0xff300000);
    //printf("DEBUG CONTROL REGISTER: old value = %08x  new value = %08x\n",old_dcr,new_dcr);
    printf("Done\n\n");
    fflush(stdout);
}


void setup_memory(void)
{
    printf("Configuring Memory... ");
    
    if (chip_type == BCM4702_CHIP)
    {                            
        // bcm4702/bcm4710 chip sdram controller setup
        
        ejtag_dma_write(0xb8000F98,0x00030001);       // SD_SBTMSTATELOW
        ejtag_dma_write(0xb8000F98,0x00030000);       // SD_SBTMSTATELOW
        ejtag_dma_write(0xb8000F98,0x00010000);       // SD_SBTMSTATELOW
        ejtag_dma_write(0xb8000004,0x00000000);       // SD_CONFIG      
        ejtag_dma_write(0xb8000000,0x0000000a);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x0000000c);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000009);       // SD_INIT        
        ejtag_dma_write(0xb8000000,0x00000419);       // SD_INIT        
        ejtag_dma_write(0xb8000008,0x00008040);       // SD_REFRESH     
    }

    if (chip_type == BCM4712_CHIP)
    {                            
        // bcm4712 chip sdram controller setup
        
        ejtag_dma_write(0xb8006f98,0x00030001);
        ejtag_dma_write(0xb8006f98,0x00030000);
        ejtag_dma_write(0xb8006f98,0x00010000);
        ejtag_dma_write(0xb8006004,0x00048000);
        ejtag_dma_write(0xb800601c,0x000754da);
        ejtag_dma_write(0xb8006034,0x23232323);
        ejtag_dma_write(0xb8006038,0x14500200);
        ejtag_dma_write(0xb800603c,0x22021416);
        ejtag_dma_write(0xb8006000,0x00000002);
        ejtag_dma_write(0xb8006000,0x00000008);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006000,0x00000004);
        ejtag_dma_write(0xb8006008,0x0000840f);
        ejtag_dma_write(0xb8006010,0x00000032);
        ejtag_dma_write(0xb8006000,0x00000010);
        ejtag_dma_write(0xb8006000,0x00000001);
    }
    printf("Done\n\n");
}


void run_backup(char *filename, unsigned int start, unsigned int length)
{
    unsigned int addr, data;
    int fd;
    int counter = 0;
    int percent_complete = 0;
    char newfilename[128] = "";

    time_t t = time(0);
    struct tm* lt = localtime(&t);
    char time_str[15];
    
    sprintf(time_str, "%04d%02d%02d_%02d%02d%02d",
        lt->tm_year + 1900, lt->tm_mon + 1, lt->tm_mday,
        lt->tm_hour, lt->tm_min, lt->tm_sec
    );

    printf("\n*** You Selected to Backup the %s ***\n",filename);

    strcpy(newfilename,filename);
    strcat(newfilename,".SAVED");
    if (issue_timestamp)
    {
       strcat(newfilename,"_");
       strcat(newfilename,time_str);
    }

    fd=open(newfilename, O_RDWR|O_CREAT|O_TRUNC, 0666 );
    if (fd<=0)
    {
        fprintf(stderr,"Could not open %s for writing\n", newfilename);
        exit(1);
    }

    printf("=========================\n");
    printf("Backup Routine Started\n");
    printf("=========================\n");

    printf("\nSaving %s to Disk...\n",newfilename);
    for(addr=start; addr<(start+length); addr+=4)
    {
        counter += 4;
        percent_complete = (counter * 100 / length);
        if ((addr&0xF) == 0) {
            printf ("[%3d\%% Backed Up]   %08x: ", percent_complete, addr);
        }

        data = ejtag_dma_read(addr);
        write(fd, (unsigned char*) &data, sizeof(data));

        printf ("%08x%c", data, (addr&0xF)==0xC?'\n':' ');
      }
    close(fd);

    printf("Done  (%s saved to Disk OK)\n\n",newfilename);

    printf("=========================\n");
    printf("Backup Routine Complete\n");
    printf("=========================\n");
}


void run_flash(char *filename, unsigned int start, unsigned int length)
{
    unsigned int addr, data ;
    int fd ;
    int counter = 0;
    int percent_complete = 0;

    printf("\n*** You Selected to Flash the %s ***\n",filename);

    fd=open(filename, O_RDONLY );
    if (fd<=0)
    {
        fprintf(stderr,"Could not open %s for reading\n", filename);
        exit(1);
    }

    printf("=========================\n");
    printf("Flashing Routine Started\n");
    printf("=========================\n");

    if (issue_erase) sflash_erase_area(start,length);

    printf("\nLoading %s to Flash Memory...\n",filename);
    for(addr=start; addr<(start+length); addr+=4)
    {
        counter += 4;
        percent_complete = (counter * 100 / length);
        if ((addr&0xF) == 0) {
            printf ("[%3d\%% Flashed]   %08x: ", percent_complete, addr);
        }

        read(fd, (unsigned char*) &data, sizeof(data));
        // Erasing Flash Sets addresses to 0xFF's so we can avoid writing these (for speed)
        if (issue_erase) {
           if (!(data == 0xFFFFFFFF))
              sflash_write_word(addr, data);
        }
        else sflash_write_word(addr, data);  // Otherwise we gotta flash it all

        printf ("%08x%c", data, (addr&0xF)==0xC?'\n':' ');
        data = 0xFFFFFFFF;  // This is in case file is shorter than expected length
      }
    close(fd);
    printf("Done  (%s loaded into Flash Memory OK)\n\n",filename);

    sflash_end();

    printf("=========================\n");
    printf("Flashing Routine Complete\n");
    printf("=========================\n");
}


void run_erase(char *filename, unsigned int start, unsigned int length)
{
    printf("\n*** You Selected to Erase the %s ***\n",filename);

    printf("=========================\n");
    printf("Erasing Routine Started\n");
    printf("=========================\n");

    sflash_erase_area(start,length);
    sflash_end();

    printf("=========================\n");
    printf("Erasing Routine Complete\n");
    printf("=========================\n");
}


static int sflash_probe(void)
{
    unsigned int id;
    
    flash_chip_type = FLASH_CHIP_UNKNOWN;

    printf("Probing Flash Chip... ");

    // AMD Chip
    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00AA00AA);
    ejtag_dma_write_h(FLASH_MEMORY+(0x2AA << 1), 0x00550055);
    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00900090);
    id = ejtag_dma_read_h(FLASH_MEMORY+(0x0 << 1));
    if((id & 0xffff) == 0x0001)                     // Cheap AMD Check
    {
       flash_chip_type = FLASH_CHIP_AMD;
       printf("*** ID:(%08X) AMD Flash Chip Detected ***\n\n",id);
    }
    ejtag_dma_write_h(FLASH_MEMORY, 0x00F000F0);    // Set array to read mode


    // Intel Chip
    ejtag_dma_write_h(FLASH_MEMORY, 0x00900090);
    id = ejtag_dma_read_h(FLASH_MEMORY);
    if((id & 0x00ff) == 0x0089)                     // Cheap Intel Check
    {
       flash_chip_type = FLASH_CHIP_INTEL;
       printf("*** ID:(%08X) Intel Flash Chip Detected ***\n\n",id);
    }
    ejtag_dma_write_h(FLASH_MEMORY, 0x00ff00ff);    // Set array to read mode
    
    if (flash_chip_type == FLASH_CHIP_UNKNOWN)
    {
        printf("*** Unknown or No Flash Chip Detected ***\n\n");
        lpt_closeport();
        exit(1);
    }


    return 0;
}


static void sflash_erase_area(unsigned int start, unsigned int length)
{
    unsigned int addr;
    int cur_block  = 0;
    int tot_blocks = 0;
    int boot_block_count = 0;

    tot_blocks = (length/BLOCK_SIZE) + (length%BLOCK_SIZE?1:0);
    
    if ((flash_chip_type == FLASH_CHIP_INTEL) & (start == FLASH_MEMORY))
    {
       tot_blocks = tot_blocks + 7;
    }

    addr = start;
    while (addr < (start+length) )
    {
       cur_block++;
       printf("Erasing block: %d / %d (addr = %08x)...", cur_block, tot_blocks, addr);
       sflash_erase_block(addr);
       printf("Done.\n");
       if ((flash_chip_type == FLASH_CHIP_INTEL) & (start == FLASH_MEMORY) & (boot_block_count < 8)) 
       {
          addr = addr + BLOCK_SIZE_8;  // Special Small Boot Block 8K Code
          boot_block_count++;
       }
       else
       {
          addr = addr + BLOCK_SIZE;
       }
    }
}


static int sflash_erase_block(unsigned int addr)
{
    int res;

    if (flash_chip_type == FLASH_CHIP_AMD)
    {
        // AMD Chip
        
        //Unlock Block
	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00AA00AA);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x2AA << 1), 0x00550055);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00800080);
        printf("Unlocked...");

        //Erase Block
	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00AA00AA);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x2AA << 1), 0x00550055);
	    ejtag_dma_write_h(addr, 0x00300030);
        do {
            res = ejtag_dma_read_h(addr);
        } while((res & 0xFFFF) != 0xFFFF);
    }

    if (flash_chip_type == FLASH_CHIP_INTEL)
    {
        // Intel Chip
        
        //Unlock Block
        ejtag_dma_write_h(addr, 0x00500050);     // Clear Status Command
        ejtag_dma_write_h(addr, 0x00600060);     // Unlock Flash Block Command
        ejtag_dma_write_h(addr, 0x00D000D0);     // Confirm Command
        ejtag_dma_write_h(addr, 0x00700070);     // Check status
        do {
            res = ejtag_dma_read_h(addr);
        } while((res & 0x0080) != 0x0080);
        
        printf("Unlocked...");
        
        //Erase Block
        ejtag_dma_write_h(FLASH_MEMORY, 0x0050);    // Clear Status Command
        ejtag_dma_write_h(addr, 0x0020);            // Block Erase Command
        ejtag_dma_write_h(addr, 0x00D0);            // Confirm Command
        ejtag_dma_write_h(FLASH_MEMORY, 0x0070);    // Check status
        do {
            res = ejtag_dma_read_h(FLASH_MEMORY);
        } while((res & 0x0080) != 0x0080);
    }

    return 0;
}


static int sflash_write_word(unsigned int addr, unsigned int data)
{
    int res;

    if (flash_chip_type == FLASH_CHIP_AMD)
    {
        // AMD Chip
        
        // Handle Half Of Word
   	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00AA00AA);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x2AA << 1), 0x00550055);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00A000A0);
	    ejtag_dma_write_h(addr, data);
        do {
            res = ejtag_dma_read_h(addr);
        } while((res & 0x80) != (data & 0x80));

        // Now Handle Other Half Of Word
   	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00AA00AA);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x2AA << 1), 0x00550055);
	    ejtag_dma_write_h(FLASH_MEMORY+(0x555 << 1), 0x00A000A0);
	    ejtag_dma_write_h(addr+2, data);
        do {
            res = ejtag_dma_read_h(addr+2);
        } while((res & 0x80) != ((data >> 16) & 0x80));
    }

    if (flash_chip_type == FLASH_CHIP_INTEL)
    {
        // Intel Chip
        
        // Handle Half Of Word
        ejtag_dma_write_h(FLASH_MEMORY, 0x00500050);    // Clear Status Command
        ejtag_dma_write_h(addr, 0x00400040);            // Write Command
        ejtag_dma_write_h(addr, data);                  // Send HalfWord Data
        ejtag_dma_write_h(FLASH_MEMORY, 0x00700070);    // Check status
        do {
            res = ejtag_dma_read_h(FLASH_MEMORY);
        } while((res & 0x0080) != 0x0080);
        
        // Now Handle Other Half Of Word
        ejtag_dma_write_h(FLASH_MEMORY, 0x00500050);    // Clear Status Command
        ejtag_dma_write_h(addr+2, 0x00400040);          // Write Command
        ejtag_dma_write_h(addr+2, data);                // Send HalfWord Data
        ejtag_dma_write_h(FLASH_MEMORY, 0x00700070);    // Check status
        do {
            res = ejtag_dma_read_h(FLASH_MEMORY);
        } while((res & 0x0080) != 0x0080);
    }

    return 0;
}


static void sflash_end(void)
{
    if (flash_chip_type == FLASH_CHIP_AMD)
    {
        // AMD Chip
        
        printf("Making Flash Access Read-Only Again...");
        ejtag_dma_write_h(FLASH_MEMORY, 0x00F000F0);    // Set array to read mode
        printf("Done.\n");
    }
    
    if (flash_chip_type == FLASH_CHIP_INTEL)
    {
        // Intel Chip
        
        printf("Making Flash Access Read-Only Again...");
        ejtag_dma_write_h(FLASH_MEMORY, 0x00ff00ff);    // Set array to read mode
        printf("Done.\n");
    }
}


void show_usage(void)
{
   fprintf(stderr, " USAGE: wrt54g [option] </noreset> </noerase> </notimestamp>\n\n"
                   "            -backup:cfe\n"
                   "            -backup:nvram\n"
                   "            -backup:kernel\n"
                   "            -backup:wholeflash\n"
                   "            -erase:cfe\n"
                   "            -erase:nvram\n"
                   "            -erase:kernel\n"
                   "            -erase:wholeflash\n"
                   "            -flash:cfe\n"
                   "            -flash:nvram\n"
                   "            -flash:kernel\n"
                   "            -flash:wholeflash\n\n"
                   "            /noreset     (prevents CPU reset of Broadcom Chip ..... optional)\n"
                   "            /noerase     (prevents forced erase before flashing ... optional)\n"
                   "            /notimestamp (prevents timestamping of backups ........ optional)\n"
                   "            /8MB         (used to support 8MB flash chips ......... optional)\n\n"
                   " Note: If 'flashing' - the source filename must exist as follows:\n"
                   "       CFE.BIN or NVRAM.BIN or KERNEL.BIN or WHOLEFLASH.BIN\n\n"
                   " ***************************************************************************\n"
                   " * Flashing the KERNEL or WHOLEFLASH will take a very long time using JTAG *\n"
                   " * via this utility.  You are better off flashing the CFE and NVRAM files  *\n"
                   " * and then using the normal TFTP method to flash the KERNEL via ethernet. *\n"
                   " ***************************************************************************\n\n");
}


int main(int argc, char** argv)
{
    char choice[128];
    int run_option;
    int i = 0;
    int j;

    printf("\n");
    printf("====================================\n");
    printf("WRT54G EJTAG DeBrick Utility v3.0\n");
    printf("====================================\n");

    if (argc < 2)
    {
        printf("This program reads/writes flash memory on the WRT54G router via EJTAG\n\n");
        show_usage();
        exit(1);
    }

    strcpy(choice,argv[1]);
    i = 0;
    while (choice[i])
    {
        choice[i] = tolower(choice[i]);
        i++;
    }

    run_option = 0;

    if (strcmp(choice,"-backup:cfe")==0)         run_option = 1;
    if (strcmp(choice,"-backup:nvram")==0)       run_option = 2;
    if (strcmp(choice,"-backup:kernel")==0)      run_option = 3;
    if (strcmp(choice,"-backup:wholeflash")==0)  run_option = 4;

    if (strcmp(choice,"-erase:cfe")==0)          run_option = 5;
    if (strcmp(choice,"-erase:nvram")==0)        run_option = 6;
    if (strcmp(choice,"-erase:kernel")==0)       run_option = 7;
    if (strcmp(choice,"-erase:wholeflash")==0)   run_option = 8;

    if (strcmp(choice,"-flash:cfe")==0)          run_option = 9;
    if (strcmp(choice,"-flash:nvram")==0)        run_option = 10;
    if (strcmp(choice,"-flash:kernel")==0)       run_option = 11;
    if (strcmp(choice,"-flash:wholeflash")==0)   run_option = 12;

    if (run_option == 0)
    {
        show_usage();
        printf("\n*** ERROR - Invalid [option] specified ***\n\n");
        exit(1);
    }

    if (argc > 2)
    {
       j = 2;
       while (j < argc) 
       {
          strcpy(choice,argv[j]);
          i = 0;
          while (choice[i])
          {
              choice[i] = tolower(choice[i]);
              i++;
          }
          if (strcmp(choice,"/noreset")==0)  
             issue_reset = 0;
          else
          if (strcmp(choice,"/noerase")==0)  
             issue_erase = 0;
          else
          if (strcmp(choice,"/notimestamp")==0)  
             issue_timestamp = 0;
          else
          if (strcmp(choice,"/8mb")==0)  
             flash_is_8MB = 1;
          else
          {
             show_usage();
             printf("\n*** ERROR - Invalid <switch> specified ***\n\n");
             exit(1);
          }
          j++;
       }
    }


    // Start First Few DMA Commands in KSEG1 (uncached/unmapped area)
    SEGMENT_MASK = 0xFFFFFFFF;

    // Detect and Initialize
    chip_detect();

    // Enable Memory Writes
    setup_DCR();

    // Setup Unit Specific Memory Refresh Values (In case they are trashed)
    setup_memory();

    // Reset Processor
    if (issue_reset)  ejtag_issue_reset();

    // Clear Watchdog
    setup_Watchdog();

    // Rest of DMA Commands use USEG (cached/mapped area)
    SEGMENT_MASK = 0x1fffffff;

    // Check Flash Chip
    sflash_probe();

    if (flash_is_8MB == 1) {
       printf("8MB Flash Memory Option Selected\n");
       if (run_option == 1 )  run_backup("CFE.BIN", CFE_START_8MB, CFE_LENGTH_8MB);
       if (run_option == 2 )  run_backup("NVRAM.BIN", NVRAM_START_8MB, NVRAM_LENGTH_8MB);
       if (run_option == 3 )  run_backup("KERNEL.BIN", KERNEL_START_8MB, KERNEL_LENGTH_8MB);
       if (run_option == 4 )  run_backup("WHOLEFLASH.BIN", WHOLEFLASH_START_8MB, WHOLEFLASH_LENGTH_8MB);
       
       if (run_option == 5 )  run_erase("CFE.BIN", CFE_START_8MB, CFE_LENGTH_8MB);
       if (run_option == 6 )  run_erase("NVRAM.BIN", NVRAM_START_8MB, NVRAM_LENGTH_8MB);
       if (run_option == 7 )  run_erase("KERNEL.BIN", KERNEL_START_8MB, KERNEL_LENGTH_8MB);
       if (run_option == 8 )  run_erase("WHOLEFLASH.BIN", WHOLEFLASH_START_8MB, WHOLEFLASH_LENGTH_8MB);
       
       if (run_option == 9 )  run_flash("CFE.BIN", CFE_START_8MB, CFE_LENGTH_8MB);
       if (run_option == 10)  run_flash("NVRAM.BIN", NVRAM_START_8MB, NVRAM_LENGTH_8MB);
       if (run_option == 11)  run_flash("KERNEL.BIN", KERNEL_START_8MB, KERNEL_LENGTH_8MB);
       if (run_option == 12)  run_flash("WHOLEFLASH.BIN", WHOLEFLASH_START_8MB, WHOLEFLASH_LENGTH_8MB);
    } else {
       printf("4MB Flash Memory Option Defaulted\n");
       if (run_option == 1 )  run_backup("CFE.BIN", CFE_START_4MB, CFE_LENGTH_4MB);
       if (run_option == 2 )  run_backup("NVRAM.BIN", NVRAM_START_4MB, NVRAM_LENGTH_4MB);
       if (run_option == 3 )  run_backup("KERNEL.BIN", KERNEL_START_4MB, KERNEL_LENGTH_4MB);
       if (run_option == 4 )  run_backup("WHOLEFLASH.BIN", WHOLEFLASH_START_4MB, WHOLEFLASH_LENGTH_4MB);
       
       if (run_option == 5 )  run_erase("CFE.BIN", CFE_START_4MB, CFE_LENGTH_4MB);
       if (run_option == 6 )  run_erase("NVRAM.BIN", NVRAM_START_4MB, NVRAM_LENGTH_4MB);
       if (run_option == 7 )  run_erase("KERNEL.BIN", KERNEL_START_4MB, KERNEL_LENGTH_4MB);
       if (run_option == 8 )  run_erase("WHOLEFLASH.BIN", WHOLEFLASH_START_4MB, WHOLEFLASH_LENGTH_4MB);
       
       if (run_option == 9 )  run_flash("CFE.BIN", CFE_START_4MB, CFE_LENGTH_4MB);
       if (run_option == 10)  run_flash("NVRAM.BIN", NVRAM_START_4MB, NVRAM_LENGTH_4MB);
       if (run_option == 11)  run_flash("KERNEL.BIN", KERNEL_START_4MB, KERNEL_LENGTH_4MB);
       if (run_option == 12)  run_flash("WHOLEFLASH.BIN", WHOLEFLASH_START_4MB, WHOLEFLASH_LENGTH_4MB);
    }

    printf("\n\n *** REQUESTED OPERATION IS COMPLETE ***\n\n");

    chip_shutdown();

    return 0;
}