/****************************** Routine: Description:******************************/static void clear_status(unsigned short *block_addr){#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS  #define CLEAR_CODE 0x00500050;  unsigned long *addr;  addr = (unsigned long *)((unsigned long)block_addr&0xfffffffc);  *addr = CLEAR_CODE;#else  #define CLEAR_CODE 0x50;  *block_addr = CLEAR_CODE;#endif  }/****************************** Routine: Description:******************************/static unsigned short read_status(unsigned short *block_addr){#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS  #define STATUS_CODE 0x00700070  unsigned long  l_status;  unsigned short status;  unsigned long *addr;  addr = (unsigned long *)((unsigned long)block_addr&0xfffffffc);  *addr = STATUS_CODE;  l_status = *addr;  // Next, take the upper 16bits and OR it with the  // lower 16bits forming a single 16bit return value.  status = (unsigned short)(l_status & 0x0000ffff);  status = status | (unsigned short)(l_status >> 16);  return status;#else  #define STATUS_CODE 0x70  unsigned short status;  *block_addr = STATUS_CODE;  status = *block_addr;  // util_printf("status read = %x\n",status); // *debug* temp  return status;#endif}/****************************** Routine: Description:******************************/static int is_busy(unsigned short *block_addr){#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS  #define STATUS_CODE 0x00700070  #define BUSY_BIT_POS 0x00800080  unsigned long status;  unsigned long *addr;  addr = (unsigned long *)((unsigned long)block_addr&0xfffffffc);  *addr = STATUS_CODE;  status = *addr;  if ((status & BUSY_BIT_POS) != BUSY_BIT_POS) {    // at least one of the bits is a zero indicating at    // at least one of the chips is busy.    return TRUE;  }  else {    return FALSE;  }#else    #define BUSY_BIT_POS 0x0080  unsigned short status;  status = read_status(block_addr);  if (0 == (BUSY_BIT_POS & status)) {    return TRUE;  }  else {    return FALSE;  }#endif  }/****************************** Routine: Description:******************************/static int was_error(unsigned short *block_addr){  #define ERASE_STATUS_BIT_POS      0x20  #define PROG_STATUS_BIT_POS       0x10  #define VOLT_STATUS_BIT_POS       0x08  #define PROG_SUSP_STATUS_BIT_POS  0x04  #define WRITE_PROT_STATUS_BIT_POS 0x02  unsigned short status;  unsigned char mask;  mask = 0;  mask |= WRITE_PROT_STATUS_BIT_POS;  mask |= PROG_SUSP_STATUS_BIT_POS;  mask |= VOLT_STATUS_BIT_POS;  mask |= PROG_STATUS_BIT_POS;  mask |= ERASE_STATUS_BIT_POS;  status = read_status(block_addr);  if (0 == (status & mask)) {    return 0;   // no errors.  }  else {    return status; // errors.  }}/****************************** Routine: Description:******************************/static int block_erase(unsigned short *block_addr){#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS  #define BLOCK_ERASE_CODE   0x00200020;  #define ERASE_CONFIRM_CODE 0x00D000D0;#else  #define BLOCK_ERASE_CODE   0x20;  #define ERASE_CONFIRM_CODE 0xD0;#endif  unsigned short status;  unsigned long *l_addr;  unsigned int retry_tally = 0;  l_addr = (unsigned long *)((unsigned long)block_addr&0xfffffffc);retry:    clear_status(block_addr);  if (0xBEEFFEED != FlashLockMode) {    util_printf("Error: Please Unlock Flash before trying to modify it.\n");    util_printf("       Giving up.\n");    return 1; // failed  }#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS  *l_addr = BLOCK_ERASE_CODE;  *l_addr = ERASE_CONFIRM_CODE;#else  *block_addr = BLOCK_ERASE_CODE;  *block_addr = ERASE_CONFIRM_CODE;#endif  while (is_busy(block_addr)) {}  if (was_error(block_addr)) {    status = read_status(block_addr);    util_printf("Erase error. block addr = 0x%X, status = 0x%x\n",block_addr,status);    util_printf("[retrying]\n");    if (retry_tally++ < 3) {      goto retry;    }    else {      util_printf("Giving up!\n");      enable_read_mode(block_addr);      return 1; // failed    }  }#if 0 /* stuff that was helpful for a while on C547X but now doesn't seem necessary. */  {    unsigned long val;    enable_read_mode(block_addr);    val = *l_addr;    if (0xffffffff != val) { // basic sanity check.      util_printf("Erase error. block addr = 0x%X, l_addr = 0x%X, val = 0x%X\n",block_addr,l_addr,val);      status = read_status(block_addr);      util_printf("[status = 0x%x]\n",status);      util_printf("[retrying]\n");      if (retry_tally++ < 3) {        goto retry;      }      else {        util_printf("Giving up!\n");        enable_read_mode(block_addr);        return 1; // failed      }    }  }#endif    enable_read_mode(block_addr);  return 0; // success}#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS   /******************************    Routine:    Description:   ******************************/   static int prog_data_32(unsigned long *block_addr, unsigned long data)   {     #define PROG_CODE_32 0x00400040;     unsigned short status;     // util_printf("0x%X = 0x%X\n", block_addr, data); // *debug* tmp     if ((*block_addr & data) != data) {       // we can only change bits from a "1" to "0".       util_printf("Error, not previously erased; addr = 0x%X, data = 0x%X\n",block_addr,*block_addr);       return 1; // failed     }     clear_status((unsigned short *)block_addr);     if (0xBEEFFEED != FlashLockMode) {       util_printf("Error: Please Unlock Flash before trying to modify it.\n");       util_printf("       Giving up.\n");       return 1; // failed     }     *block_addr = PROG_CODE_32; // Put each chip in program mode, and then...     *block_addr = data;         // 16bits+16bits=32bits; Give each chip a 16bit data value.     while (is_busy((unsigned short *)block_addr)) {}     if (was_error((unsigned short *)block_addr)) {       status = read_status((unsigned short *)block_addr);       util_printf("Prog error. addr = %X, status = %x\n",block_addr,status);       clear_status((unsigned short *)block_addr);  // *debug* temp       util_printf("Prog error. addr = %X, status = %x\n",block_addr,status);       SYSTEM_FATAL("Terminating.");     }     enable_read_mode((unsigned short *)block_addr);     return 0; // success   }    #else   /******************************    Routine:    Description:   ******************************/   static int prog_data_16(unsigned short *block_addr, unsigned short data)   {     #define PROG_CODE_16 0x40;     unsigned short status;     // util_printf("0x%X = 0x%x\n", block_addr, data); // *debug* tmp     if ((*block_addr & data) != data) {       // we can only change bits from a "1" to "0".       util_printf("Error, not previously erased; addr = 0x%X, data = 0x%X\n",block_addr,*block_addr);       return 1; // failed     }     clear_status(block_addr);     if (0xBEEFFEED != FlashLockMode) {       util_printf("Error: Please Unlock Flash before trying to modify it.\n");       util_printf("       Giving up.\n");       return 1; // failed     }     *block_addr = PROG_CODE_16;     *block_addr = data;     while (is_busy(block_addr)) {}     if (was_error(block_addr)) {       status = read_status(block_addr);       util_printf("Prog error. addr = %X, status = %x\n",block_addr,status);       clear_status(block_addr);  // *debug* temp       util_printf("Prog error. addr = %X, status = %x\n",block_addr,status);       SYSTEM_FATAL("Terminating.");     }     enable_read_mode(block_addr);     return 0; // success   }#endif/****************************** Routine: Description:   Note: See flash.h for description. ******************************/void flash_read(unsigned int offset,  // Of flash.                unsigned short *dest, // Destination buffer                unsigned int num_bytes,                PutValAtAddrCallBack_t CBack){  unsigned int i, num_words;  unsigned short fval, *s_addr, *d_addr;  s_addr = (unsigned short *)(BSPCONF_FLASH_BASE+offset);  d_addr = dest;  // Flash; read word at a time, not byte at a time.  num_words = ((num_bytes+1)>>1); // round up.  for (i=0; i<num_words; i++) {    fval = *s_addr;    if (CBack) {      (*CBack)(d_addr,fval); // write out the the 16bit value.    }    else {      *d_addr=fval; // write out the the 16bit value.    }    s_addr++;    d_addr++;  }}/****************************** Routine: Description:   Note: See flash.h for description. ******************************/int flash_write(unsigned int offset,  // Of flash.                 unsigned short *src,  // source buffer.                 unsigned int num_bytes,                 GetValAtAddrCallBack_t CBack){#ifdef TWO_16BIT_FLASH_CHIPS_INVOLVED_IN_EACH_32BIT_ACCESS