retval = retval >> 1;
        if (TDO) {
            retval |= (0x01 << (num_bits - 1));
        }
        
        if ((i == (num_bits - 1)) && (num_IR_bits_after == 0)) {
            TMS = 1;                              // move to Exit1_IR state
        }        

        JTAG_StrobeTCK();                         // move to Shift_IR state
                                                  // advance
    }

    TDI = 1; 
    for (i=0; i < num_IR_bits_after; i++) {       // now process IR bits after the
                                                  // isolated device
        if (i == (num_IR_bits_after - 1)) {
            TMS = 1;                              // move to Exit1_IR state
        }  
    
    
        JTAG_StrobeTCK();                         // fill the IR of the devices
                                                  // after the isolated device
    }                                             // with all ones, the BYPASS opcode.

    // navigate back to the RTI state
   
    TMS = 1;
    JTAG_StrobeTCK ();                            // move to Update_IR
    TMS = 0;
    JTAG_StrobeTCK ();                            // move to RTI state

    return retval;
}

//------------------------------------------------------------------------------------
// JTAG_DR_Scan
//------------------------------------------------------------------------------------
// This routine shifts <num_bits> of <data> into the Data Register of the isolated 
// device in the chain, and returns up to 32-bits of data read from its Data Register.
// 
// Assumes the JTAG state machine starts in the Run_Test/Idle state.
// Leaves in the Run_Test/Idle state.
//
unsigned long JTAG_DR_Scan (unsigned long dat, char num_bits)
{

    unsigned long retval;                        // JTAG return value
    char i;                                      // JTAG DR bit counter

    retval = 0x0L;

    // navigate the JTAG State Machine in all devices to the Shift_DR state
    TMS = 1;                            
    JTAG_StrobeTCK ();                           // move to SelectDR
    TMS = 0;
    JTAG_StrobeTCK ();                           // move to Capture_DR
    TMS = 0;
    JTAG_StrobeTCK ();                           // move to Shift_DR state 
    
    
    TDI = 0;       
    for (i=0; i < num_devices_before; i++) {
                                                  
       JTAG_StrobeTCK();                         // fill the BYPASS Register 
                                                 // of the devices before the 
    }                                            // isolated device with zeros.
                                                  
                                                   

    for (i=0; i < num_bits; i++) {
   
        TDI = (dat & 0x01);                      // determine the output 
        dat = dat >> 1;
        
        retval = retval >> 1;
        if (TDO) {
            retval |= (0x01L << (num_bits - 1));
        }
        
        if ((i == (num_bits - 1)) && (num_devices_after == 0)) {
            TMS = 1;                             // move to Exit1_IR state
        }        

        JTAG_StrobeTCK();                          
                                                 //output and get input
    }
     
    TDI = 0;
    for (i=0; i < num_devices_after; i++) {
  
        if (i == (num_devices_after - 1)) {
            TMS = 1;                             // move to Exit1_IR state
        }  
         
                                                 // move to Shift_DR state,
       JTAG_StrobeTCK();                         // fill the BYPASS Register 
                                                 // of the devices after the 
    }                                            // isolated device with zeros.
                                                     
    // navigate the JTAG State Machine in all devices to the RTI state
    TMS = 1;
    JTAG_StrobeTCK ();                           // move to Update_DR
    TMS = 0;
    JTAG_StrobeTCK ();                           // move to RTI state

    return retval;                               // retval is MSB aligned
}

//------------------------------------------------------------------------------------
// JTAG_IWrite
//------------------------------------------------------------------------------------
// This routine performs an indirect write to register <ireg>, containing <dat>, of
// <num_bits> in length.  It follows the write operation with a polling operation, and
// returns when the operation is completed.  Note: the polling implemented here refers
// to the JTAG register write operation being completed, NOT the FLASH write operation.
// Polling for the FLASH write operation is handled at a higher level
// Examples of valid indirect registers are:
//  FLASHCON - FLASH Control
//  FLASHSCL - FLASH Scale
//  FLASHADR - FLASH Address
//  FLASHDAT - FLASH Data
// Leaves in the Run_Test/Idle state.
//
void JTAG_IWrite (unsigned int ireg, unsigned long dat, int num_bits)
{
   
   bit done;                              // TRUE = write complete; FALSE otherwise
   
   JTAG_IR_Scan (ireg, INST_LENGTH);      // load IR with <ireg>

   dat |= (0x03L << num_bits);            // append 'WRITE' opcode to data
   
   // load DR with <dat>
   JTAG_DR_Scan (dat, num_bits + 2);      // initiate the JTAG write

   // load DR with '0', and check for BUSY bit to go to '0'.
   do {
      done = !(JTAG_DR_Scan (0x0L, 1));   // poll for JTAG_BUSY bit
   } while (!done);
}

//------------------------------------------------------------------------------------
// JTAG_IRead
//------------------------------------------------------------------------------------
// This routine performs an indirect read of register <ireg>, of <num_bits> in length.
// It follows the read operation with a polling operation, and returns when the 
// operation is completed.  Note: the polling implemented here refers to the JTAG 
// register read operation being completed, NOT the FLASH read operation.
// Polling for the FLASH read operation is handled at a higher level.
// Examples of valid indirect registers are:
//  FLASHCON - FLASH Control
//  FLASHSCL - FLASH Scale
//  FLASHADR - FLASH Address
//  FLASHDAT - FLASH Data
// Leaves JTAG in the Run_Test/Idle state.
//
unsigned long JTAG_IRead (unsigned int ireg, int num_bits) {
   
   unsigned long retval;                  // value returned from READ operation
   bit done;                              // TRUE = write complete; FALSE otherwise
   
   JTAG_IR_Scan (ireg, INST_LENGTH);      // load IR with <ireg>

   // load DR with read opcode (0x02)
   JTAG_DR_Scan (0x02L, 2);               // initiate the JTAG read

   do {
      done = !(JTAG_DR_Scan (0x0L, 1));   // poll for JTAG_BUSY bit
   } while (!done);
   
   retval = JTAG_DR_Scan (0x0L, num_bits + 1);  // allow poll operation to
                                                // read remainder of the bits
   retval = retval >> 1;                  // shift JTAG_BUSY bit off the end

   return retval;
}

//------------------------------------------------------------------------------------
// FLASH_ByteRead
//------------------------------------------------------------------------------------
// This routine reads the byte at <addr> and stores it at the address pointed to by
// <pdat>.
// Returns TRUE if the operation was successful; FALSE otherwise (page 
// read-protected).
//
int FLASH_ByteRead (unsigned int addr, unsigned char *pdat)
{
   unsigned long testval;                       // holds result of FLASHDAT read
   bit done;                                    // TRUE/FALSE flag
   int retval;                                  // TRUE if operation successful

   JTAG_IWrite (FLASHSCL, 0x86L, FLSC_LEN);     // set FLASHSCL based on SYSCLK 
                                                // frequency (2MHz = 0x86)

   // set FLASHADR to address to read from
   JTAG_IWrite (FLASHADR, (unsigned long) addr, FLA_LEN);

   JTAG_IWrite (FLASHCON, 0x02L, FLCN_LEN);     // set FLASHCON for FLASH Read 
                                                // operation (0x02)

   JTAG_IRead (FLASHDAT, FLD_RDLEN);            // initiate the read operation

   JTAG_IWrite (FLASHCON, 0x0L, FLCN_LEN);      // set FLASHCON for 'poll' operation

   do {
      done = !(JTAG_IRead (FLASHDAT, 1));       // poll for FLBUSY to de-assert
   } while (!done);

   testval = JTAG_IRead (FLASHDAT, FLD_RDLEN);  // read the resulting data

   retval = (testval & 0x02) ? FALSE: TRUE;     // FLFail is next to LSB

   testval = testval >> 2;                      // shift data.0 into LSB position

   *pdat = (unsigned char) testval;             // place data in return location

   return retval;                               // return FLASH Pass/Fail
}

//------------------------------------------------------------------------------------
// FLASH_ByteWrite
//------------------------------------------------------------------------------------
// This routine writes the data <dat> to FLASH at the address <addr>.
// Returns TRUE if the operation was successful; FALSE otherwise (page 
// write-protected).
//
int FLASH_ByteWrite (unsigned int addr, unsigned char dat)
{
   unsigned long testval;                       // holds result of FLASHDAT read
   int done;                                    // TRUE/FALSE flag
   int retval;                                  // TRUE if operation successful

   JTAG_IWrite (FLASHSCL, 0x86L, FLSC_LEN);     // set FLASHSCL based on SYSCLK 
                                                // frequency (2MHz = 0x86)

   // set FLASHADR to address to write to
   JTAG_IWrite (FLASHADR, (unsigned long) addr, FLA_LEN);

   JTAG_IWrite (FLASHCON, 0x10L, FLCN_LEN);     // set FLASHCON for FLASH Write 
                                                // operation (0x10)

   // initiate the write operation
   JTAG_IWrite (FLASHDAT, (unsigned long) dat, FLD_WRLEN);

   JTAG_IWrite (FLASHCON, 0x0L, FLCN_LEN);      // set FLASHCON for 'poll' operation

   do {
      done = !(JTAG_IRead (FLASHDAT, 1));       // poll for FLBusy to de-assert
   } while (!done);

   testval = JTAG_IRead (FLASHDAT, 2);          // read FLBusy and FLFail

   retval = (testval & 0x02) ? FALSE: TRUE;     // FLFail is next to LSB

   return retval;                               // return FLASH Pass/Fail
}

//------------------------------------------------------------------------------------
// FLASH_PageErase
//------------------------------------------------------------------------------------
// This routine performs an erase of the page in which <addr> is contained.
// This routine assumes that no FLASH operations are currently in progress.
// This routine exits with no FLASH operations currently in progress.
// Returns TRUE if the operation was successful; FALSE otherwise (page protected).
//
int FLASH_PageErase (unsigned int addr)
{
   unsigned long testval;                       // holds result of FLASHDAT read
   bit done;                                    // TRUE/FALSE flag
   int retval;                                  // TRUE if operation successful

   JTAG_IWrite (FLASHSCL, 0x86L, FLSC_LEN);     // set FLASHSCL based on SYSCLK 
                                                // frequency (2MHz = 0x86)

   // set FLASHADR to address within page to erase
   JTAG_IWrite (FLASHADR, (unsigned long) addr, FLA_LEN);   

   JTAG_IWrite (FLASHCON, 0x20L, FLCN_LEN);     // set FLASHCON for FLASH Erase 
                                                // operation (0x20)

   JTAG_IWrite (FLASHDAT, 0xa5L, FLD_WRLEN);    // set FLASHDAT to 0xa5 to initiate 
                                                // erase procedure

   JTAG_IWrite (FLASHCON, 0x0L, FLCN_LEN);      // set FLASHCON for 'poll' operation

   do {
      done = !(JTAG_IRead (FLASHDAT, 1));       // poll for FLBusy to de-assert
   } while (!done);

   testval = JTAG_IRead (FLASHDAT, 2);          // read FLBusy and FLFail

   retval = (testval & 0x02) ? FALSE: TRUE;     // FLFail is next to LSB

   // set return value based on FLFail bit
   return retval;                               // return FLASH Pass/Fail
}