board/incaip/flash.c

   1 /*
   2  * (C) Copyright 2003
   3  * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
   4  *
   5  * See file CREDITS for list of people who contributed to this
   6  * project.
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License as
  10  * published by the Free Software Foundation; either version 2 of
  11  * the License, or (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  21  * MA 02111-1307 USA
  22  */
  23
  24 #include <common.h>
  25 #include <asm/inca-ip.h>
  26
  27 flash_info_t    flash_info[CONFIG_SYS_MAX_FLASH_BANKS]; /* info for FLASH chips */
  28
  29 /* NOTE - CONFIG_FLASH_16BIT means the CPU interface is 16-bit, it
  30  *        has nothing to do with the flash chip being 8-bit or 16-bit.
  31  */
  32 #ifdef CONFIG_FLASH_16BIT
  33 typedef unsigned short FLASH_PORT_WIDTH;
  34 typedef volatile unsigned short FLASH_PORT_WIDTHV;
  35 #define FLASH_ID_MASK   0xFFFF
  36 #else
  37 typedef unsigned long FLASH_PORT_WIDTH;
  38 typedef volatile unsigned long FLASH_PORT_WIDTHV;
  39 #define FLASH_ID_MASK   0xFFFFFFFF
  40 #endif
  41
  42 #define FPW     FLASH_PORT_WIDTH
  43 #define FPWV    FLASH_PORT_WIDTHV
  44
  45 #define ORMASK(size) ((-size) & OR_AM_MSK)
  46
  47 #if 0
  48 #define FLASH_CYCLE1    0x0555
  49 #define FLASH_CYCLE2    0x02aa
  50 #else
  51 #define FLASH_CYCLE1    0x0554
  52 #define FLASH_CYCLE2    0x02ab
  53 #endif
  54
  55 /*-----------------------------------------------------------------------
  56  * Functions
  57  */
  58 static ulong flash_get_size(FPWV *addr, flash_info_t *info);
  59 static void flash_reset(flash_info_t *info);
  60 static int write_word_intel(flash_info_t *info, FPWV *dest, FPW data);
  61 static int write_word_amd(flash_info_t *info, FPWV *dest, FPW data);
  62 static void flash_get_offsets(ulong base, flash_info_t *info);
  63 static flash_info_t *flash_get_info(ulong base);
  64
  65 /*-----------------------------------------------------------------------
  66  * flash_init()
  67  *
  68  * sets up flash_info and returns size of FLASH (bytes)
  69  */
  70 unsigned long flash_init (void)
  71 {
  72         unsigned long size = 0;
  73         int i;
  74
  75         /* Init: no FLASHes known */
  76         for (i=0; i < CONFIG_SYS_MAX_FLASH_BANKS; ++i) {
  77                 ulong flashbase = (i == 0) ? PHYS_FLASH_1 : PHYS_FLASH_2;
  78                 ulong * buscon = (ulong *)
  79                         ((i == 0) ? INCA_IP_EBU_EBU_BUSCON0 : INCA_IP_EBU_EBU_BUSCON2);
  80
  81                 /* Disable write protection */
  82                 *buscon &= ~INCA_IP_EBU_EBU_BUSCON1_WRDIS;
  83
  84 #if 1
  85                 memset(&flash_info[i], 0, sizeof(flash_info_t));
  86 #endif
  87
  88                 flash_info[i].size =
  89                         flash_get_size((FPW *)flashbase, &flash_info[i]);
  90
  91                 if (flash_info[i].flash_id == FLASH_UNKNOWN) {
  92                         printf ("## Unknown FLASH on Bank %d - Size = 0x%08lx\n",
  93                         i, flash_info[i].size);
  94                 }
  95
  96                 size += flash_info[i].size;
  97         }
  98
  99 #if CONFIG_SYS_MONITOR_BASE >= CONFIG_SYS_FLASH_BASE
 100         /* monitor protection ON by default */
 101         flash_protect(FLAG_PROTECT_SET,
 102                       CONFIG_SYS_MONITOR_BASE,
 103                       CONFIG_SYS_MONITOR_BASE+monitor_flash_len-1,
 104                       flash_get_info(CONFIG_SYS_MONITOR_BASE));
 105 #endif
 106
 107 #ifdef  CONFIG_ENV_IS_IN_FLASH
 108         /* ENV protection ON by default */
 109         flash_protect(FLAG_PROTECT_SET,
 110                       CONFIG_ENV_ADDR,
 111                       CONFIG_ENV_ADDR+CONFIG_ENV_SIZE-1,
 112                       flash_get_info(CONFIG_ENV_ADDR));
 113 #endif
 114
 115
 116         return size;
 117 }
 118
 119 /*-----------------------------------------------------------------------
 120  */
 121 static void flash_reset(flash_info_t *info)
 122 {
 123         FPWV *base = (FPWV *)(info->start[0]);
 124
 125         /* Put FLASH back in read mode */
 126         if ((info->flash_id & FLASH_VENDMASK) == FLASH_MAN_INTEL)
 127                 *base = (FPW)0x00FF00FF;        /* Intel Read Mode */
 128         else if ((info->flash_id & FLASH_VENDMASK) == FLASH_MAN_AMD)
 129                 *base = (FPW)0x00F000F0;        /* AMD Read Mode */
 130 }
 131
 132 /*-----------------------------------------------------------------------
 133  */
 134 static void flash_get_offsets (ulong base, flash_info_t *info)
 135 {
 136         int i;
 137
 138         /* set up sector start address table */
 139         if ((info->flash_id & FLASH_VENDMASK) == FLASH_MAN_INTEL
 140             && (info->flash_id & FLASH_BTYPE)) {
 141                 int bootsect_size;      /* number of bytes/boot sector  */
 142                 int sect_size;          /* number of bytes/regular sector */
 143
 144                 bootsect_size = 0x00002000 * (sizeof(FPW)/2);
 145                 sect_size =     0x00010000 * (sizeof(FPW)/2);
 146
 147                 /* set sector offsets for bottom boot block type        */
 148                 for (i = 0; i < 8; ++i) {
 149                         info->start[i] = base + (i * bootsect_size);
 150                 }
 151                 for (i = 8; i < info->sector_count; i++) {
 152                         info->start[i] = base + ((i - 7) * sect_size);
 153                 }
 154         }
 155         else if ((info->flash_id & FLASH_VENDMASK) == FLASH_MAN_AMD
 156                  && (info->flash_id & FLASH_TYPEMASK) == FLASH_AM640U) {
 157
 158                 int sect_size;          /* number of bytes/sector */
 159
 160                 sect_size = 0x00010000 * (sizeof(FPW)/2);
 161
 162                 /* set up sector start address table (uniform sector type) */
 163                 for( i = 0; i < info->sector_count; i++ )
 164                         info->start[i] = base + (i * sect_size);
 165         }
 166 }
 167
 168 /*-----------------------------------------------------------------------
 169  */
 170
 171 static flash_info_t *flash_get_info(ulong base)
 172 {
 173         int i;
 174         flash_info_t * info;
 175
 176         for (i = 0; i < CONFIG_SYS_MAX_FLASH_BANKS; i ++) {
 177                 info = & flash_info[i];
 178                 if (info->start[0] <= base && base < info->start[0] + info->size)
 179                         break;
 180         }
 181
 182         return i == CONFIG_SYS_MAX_FLASH_BANKS ? 0 : info;
 183 }
 184
 185 /*-----------------------------------------------------------------------
 186  */
 187
 188 void flash_print_info (flash_info_t *info)
 189 {
 190         int i;
 191         uchar *boottype;
 192         uchar *bootletter;
 193         char *fmt;
 194         uchar botbootletter[] = "B";
 195         uchar topbootletter[] = "T";
 196         uchar botboottype[] = "bottom boot sector";
 197         uchar topboottype[] = "top boot sector";
 198
 199         if (info->flash_id == FLASH_UNKNOWN) {
 200                 printf ("missing or unknown FLASH type\n");
 201                 return;
 202         }
 203
 204         switch (info->flash_id & FLASH_VENDMASK) {
 205         case FLASH_MAN_AMD:     printf ("AMD ");                break;
 206         case FLASH_MAN_BM:      printf ("BRIGHT MICRO ");       break;
 207         case FLASH_MAN_FUJ:     printf ("FUJITSU ");            break;
 208         case FLASH_MAN_SST:     printf ("SST ");                break;
 209         case FLASH_MAN_STM:     printf ("STM ");                break;
 210         case FLASH_MAN_INTEL:   printf ("INTEL ");              break;
 211         default:                printf ("Unknown Vendor ");     break;
 212         }
 213
 214         /* check for top or bottom boot, if it applies */
 215         if (info->flash_id & FLASH_BTYPE) {
 216                 boottype = botboottype;
 217                 bootletter = botbootletter;
 218         }
 219         else {
 220                 boottype = topboottype;
 221                 bootletter = topbootletter;
 222         }
 223
 224         switch (info->flash_id & FLASH_TYPEMASK) {
 225         case FLASH_AM640U:
 226                 fmt = "29LV641D (64 Mbit, uniform sectors)\n";
 227                 break;
 228         case FLASH_28F800C3B:
 229         case FLASH_28F800C3T:
 230                 fmt = "28F800C3%s (8 Mbit, %s)\n";
 231                 break;
 232         case FLASH_INTEL800B:
 233         case FLASH_INTEL800T:
 234                 fmt = "28F800B3%s (8 Mbit, %s)\n";
 235                 break;
 236         case FLASH_28F160C3B:
 237         case FLASH_28F160C3T:
 238                 fmt = "28F160C3%s (16 Mbit, %s)\n";
 239                 break;
 240         case FLASH_INTEL160B:
 241         case FLASH_INTEL160T:
 242                 fmt = "28F160B3%s (16 Mbit, %s)\n";
 243                 break;
 244         case FLASH_28F320C3B:
 245         case FLASH_28F320C3T:
 246                 fmt = "28F320C3%s (32 Mbit, %s)\n";
 247                 break;
 248         case FLASH_INTEL320B:
 249         case FLASH_INTEL320T:
 250                 fmt = "28F320B3%s (32 Mbit, %s)\n";
 251                 break;
 252         case FLASH_28F640C3B:
 253         case FLASH_28F640C3T:
 254                 fmt = "28F640C3%s (64 Mbit, %s)\n";
 255                 break;
 256         case FLASH_INTEL640B:
 257         case FLASH_INTEL640T:
 258                 fmt = "28F640B3%s (64 Mbit, %s)\n";
 259                 break;
 260         default:
 261                 fmt = "Unknown Chip Type\n";
 262                 break;
 263         }
 264
 265         printf (fmt, bootletter, boottype);
 266
 267         printf ("  Size: %ld MB in %d Sectors\n",
 268                 info->size >> 20,
 269                 info->sector_count);
 270
 271         printf ("  Sector Start Addresses:");
 272
 273         for (i=0; i<info->sector_count; ++i) {
 274                 if ((i % 5) == 0) {
 275                         printf ("\n   ");
 276                 }
 277
 278                 printf (" %08lX%s", info->start[i],
 279                         info->protect[i] ? " (RO)" : "     ");
 280         }
 281
 282         printf ("\n");
 283 }
 284
 285 /*-----------------------------------------------------------------------
 286  */
 287
 288 /*
 289  * The following code cannot be run from FLASH!
 290  */
 291
 292 ulong flash_get_size (FPWV *addr, flash_info_t *info)
 293 {
 294         /* Write auto select command: read Manufacturer ID */
 295
 296         /* Write auto select command sequence and test FLASH answer */
 297         addr[FLASH_CYCLE1] = (FPW)0x00AA00AA;   /* for AMD, Intel ignores this */
 298         addr[FLASH_CYCLE2] = (FPW)0x00550055;   /* for AMD, Intel ignores this */
 299         addr[FLASH_CYCLE1] = (FPW)0x00900090;   /* selects Intel or AMD */
 300
 301         /* The manufacturer codes are only 1 byte, so just use 1 byte.
 302          * This works for any bus width and any FLASH device width.
 303          */
 304         switch (addr[1] & 0xff) {
 305
 306         case (uchar)AMD_MANUFACT:
 307                 info->flash_id = FLASH_MAN_AMD;
 308                 break;
 309
 310         case (uchar)INTEL_MANUFACT:
 311                 info->flash_id = FLASH_MAN_INTEL;
 312                 break;
 313
 314         default:
 315                 info->flash_id = FLASH_UNKNOWN;
 316                 info->sector_count = 0;
 317                 info->size = 0;
 318                 break;
 319         }
 320
 321         /* Check 16 bits or 32 bits of ID so work on 32 or 16 bit bus. */
 322         if (info->flash_id != FLASH_UNKNOWN) switch (addr[0]) {
 323
 324         case (FPW)AMD_ID_LV640U:        /* 29LV640 and 29LV641 have same ID */
 325                 info->flash_id += FLASH_AM640U;
 326                 info->sector_count = 128;
 327                 info->size = 0x00800000 * (sizeof(FPW)/2);
 328                 break;                          /* => 8 or 16 MB        */
 329
 330         case (FPW)INTEL_ID_28F800C3B:
 331                 info->flash_id += FLASH_28F800C3B;
 332                 info->sector_count = 23;
 333                 info->size = 0x00100000 * (sizeof(FPW)/2);
 334                 break;                          /* => 1 or 2 MB         */
 335
 336         case (FPW)INTEL_ID_28F800B3B:
 337                 info->flash_id += FLASH_INTEL800B;
 338                 info->sector_count = 23;
 339                 info->size = 0x00100000 * (sizeof(FPW)/2);
 340                 break;                          /* => 1 or 2 MB         */
 341
 342         case (FPW)INTEL_ID_28F160C3B:
 343                 info->flash_id += FLASH_28F160C3B;
 344                 info->sector_count = 39;
 345                 info->size = 0x00200000 * (sizeof(FPW)/2);
 346                 break;                          /* => 2 or 4 MB         */
 347
 348         case (FPW)INTEL_ID_28F160B3B:
 349                 info->flash_id += FLASH_INTEL160B;
 350                 info->sector_count = 39;
 351                 info->size = 0x00200000 * (sizeof(FPW)/2);
 352                 break;                          /* => 2 or 4 MB         */
 353
 354         case (FPW)INTEL_ID_28F320C3B:
 355                 info->flash_id += FLASH_28F320C3B;
 356                 info->sector_count = 71;
 357                 info->size = 0x00400000 * (sizeof(FPW)/2);
 358                 break;                          /* => 4 or 8 MB         */
 359
 360         case (FPW)INTEL_ID_28F320B3B:
 361                 info->flash_id += FLASH_INTEL320B;
 362                 info->sector_count = 71;
 363                 info->size = 0x00400000 * (sizeof(FPW)/2);
 364                 break;                          /* => 4 or 8 MB         */
 365
 366         case (FPW)INTEL_ID_28F640C3B:
 367                 info->flash_id += FLASH_28F640C3B;
 368                 info->sector_count = 135;
 369                 info->size = 0x00800000 * (sizeof(FPW)/2);
 370                 break;                          /* => 8 or 16 MB        */
 371
 372         case (FPW)INTEL_ID_28F640B3B:
 373                 info->flash_id += FLASH_INTEL640B;
 374                 info->sector_count = 135;
 375                 info->size = 0x00800000 * (sizeof(FPW)/2);
 376                 break;                          /* => 8 or 16 MB        */
 377
 378         default:
 379                 info->flash_id = FLASH_UNKNOWN;
 380                 info->sector_count = 0;
 381                 info->size = 0;
 382                 return (0);                     /* => no or unknown flash */
 383         }
 384
 385         flash_get_offsets((ulong)addr, info);
 386
 387         /* Put FLASH back in read mode */
 388         flash_reset(info);
 389
 390         return (info->size);
 391 }
 392
 393 /*-----------------------------------------------------------------------
 394  */
 395
 396 int     flash_erase (flash_info_t *info, int s_first, int s_last)
 397 {
 398         FPWV *addr;
 399         int flag, prot, sect;
 400         int intel = (info->flash_id & FLASH_VENDMASK) == FLASH_MAN_INTEL;
 401         ulong start, now, last;
 402         int rcode = 0;
 403
 404         if ((s_first < 0) || (s_first > s_last)) {
 405                 if (info->flash_id == FLASH_UNKNOWN) {
 406                         printf ("- missing\n");
 407                 } else {
 408                         printf ("- no sectors to erase\n");
 409                 }
 410                 return 1;
 411         }
 412
 413         switch (info->flash_id & FLASH_TYPEMASK) {
 414         case FLASH_INTEL800B:
 415         case FLASH_INTEL160B:
 416         case FLASH_INTEL320B:
 417         case FLASH_INTEL640B:
 418         case FLASH_28F800C3B:
 419         case FLASH_28F160C3B:
 420         case FLASH_28F320C3B:
 421         case FLASH_28F640C3B:
 422         case FLASH_AM640U:
 423                 break;
 424         case FLASH_UNKNOWN:
 425         default:
 426                 printf ("Can't erase unknown flash type %08lx - aborted\n",
 427                         info->flash_id);
 428                 return 1;
 429         }
 430
 431         prot = 0;
 432         for (sect=s_first; sect<=s_last; ++sect) {
 433                 if (info->protect[sect]) {
 434                         prot++;
 435                 }
 436         }
 437
 438         if (prot) {
 439                 printf ("- Warning: %d protected sectors will not be erased!\n",
 440                         prot);
 441         } else {
 442                 printf ("\n");
 443         }
 444
 445         last  = get_timer(0);
 446
 447         /* Start erase on unprotected sectors */
 448         for (sect = s_first; sect<=s_last && rcode == 0; sect++) {
 449
 450                 if (info->protect[sect] != 0)   /* protected, skip it */
 451                         continue;
 452
 453                 /* Disable interrupts which might cause a timeout here */
 454                 flag = disable_interrupts();
 455
 456                 addr = (FPWV *)(info->start[sect]);
 457                 if (intel) {
 458                         *addr = (FPW)0x00500050; /* clear status register */
 459                         *addr = (FPW)0x00200020; /* erase setup */
 460                         *addr = (FPW)0x00D000D0; /* erase confirm */
 461                 }
 462                 else {
 463                         /* must be AMD style if not Intel */
 464                         FPWV *base;             /* first address in bank */
 465
 466                         base = (FPWV *)(info->start[0]);
 467                         base[FLASH_CYCLE1] = (FPW)0x00AA00AA;   /* unlock */
 468                         base[FLASH_CYCLE2] = (FPW)0x00550055;   /* unlock */
 469                         base[FLASH_CYCLE1] = (FPW)0x00800080;   /* erase mode */
 470                         base[FLASH_CYCLE1] = (FPW)0x00AA00AA;   /* unlock */
 471                         base[FLASH_CYCLE2] = (FPW)0x00550055;   /* unlock */
 472                         *addr = (FPW)0x00300030;        /* erase sector */
 473                 }
 474
 475                 /* re-enable interrupts if necessary */
 476                 if (flag)
 477                         enable_interrupts();
 478
 479                 start = get_timer(0);
 480
 481                 /* wait at least 50us for AMD, 80us for Intel.
 482                  * Let's wait 1 ms.
 483                  */
 484                 udelay (1000);
 485
 486                 while ((*addr & (FPW)0x00800080) != (FPW)0x00800080) {
 487                         if ((now = get_timer(start)) > CONFIG_SYS_FLASH_ERASE_TOUT) {
 488                                 printf ("Timeout\n");
 489
 490                                 if (intel) {
 491                                         /* suspend erase        */
 492                                         *addr = (FPW)0x00B000B0;
 493                                 }
 494
 495                                 flash_reset(info);      /* reset to read mode */
 496                                 rcode = 1;              /* failed */
 497                                 break;
 498                         }
 499
 500                         /* show that we're waiting */
 501                         if ((get_timer(last)) > CONFIG_SYS_HZ) {/* every second */
 502                                 putc ('.');
 503                                 last = get_timer(0);
 504                         }
 505                 }
 506
 507                 /* show that we're waiting */
 508                 if ((get_timer(last)) > CONFIG_SYS_HZ) {        /* every second */
 509                         putc ('.');
 510                         last = get_timer(0);
 511                 }
 512
 513                 flash_reset(info);      /* reset to read mode   */
 514         }
 515
 516         printf (" done\n");
 517         return rcode;
 518 }
 519
 520 /*-----------------------------------------------------------------------
 521  * Copy memory to flash, returns:
 522  * 0 - OK
 523  * 1 - write timeout
 524  * 2 - Flash not erased
 525  */
 526 int write_buff (flash_info_t *info, uchar *src, ulong addr, ulong cnt)
 527 {
 528     FPW data = 0; /* 16 or 32 bit word, matches flash bus width on MPC8XX */
 529     int bytes;    /* number of bytes to program in current word         */
 530     int left;     /* number of bytes left to program                    */
 531     int i, res;
 532
 533     for (left = cnt, res = 0;
 534          left > 0 && res == 0;
 535          addr += sizeof(data), left -= sizeof(data) - bytes) {
 536
 537         bytes = addr & (sizeof(data) - 1);
 538         addr &= ~(sizeof(data) - 1);
 539
 540         /* combine source and destination data so can program
 541          * an entire word of 16 or 32 bits
 542          */
 543         for (i = 0; i < sizeof(data); i++) {
 544             data <<= 8;
 545             if (i < bytes || i - bytes >= left )
 546                 data += *((uchar *)addr + i);
 547             else
 548                 data += *src++;
 549         }
 550
 551         /* write one word to the flash */
 552         switch (info->flash_id & FLASH_VENDMASK) {
 553         case FLASH_MAN_AMD:
 554                 res = write_word_amd(info, (FPWV *)addr, data);
 555                 break;
 556         case FLASH_MAN_INTEL:
 557                 res = write_word_intel(info, (FPWV *)addr, data);
 558                 break;
 559         default:
 560                 /* unknown flash type, error! */
 561                 printf ("missing or unknown FLASH type\n");
 562                 res = 1;        /* not really a timeout, but gives error */
 563                 break;
 564         }
 565     }
 566
 567     return (res);
 568 }
 569
 570 /*-----------------------------------------------------------------------
 571  * Write a word to Flash for AMD FLASH
 572  * A word is 16 or 32 bits, whichever the bus width of the flash bank
 573  * (not an individual chip) is.
 574  *
 575  * returns:
 576  * 0 - OK
 577  * 1 - write timeout
 578  * 2 - Flash not erased
 579  */
 580 static int write_word_amd (flash_info_t *info, FPWV *dest, FPW data)
 581 {
 582     ulong start;
 583     int flag;
 584     int res = 0;        /* result, assume success       */
 585     FPWV *base;         /* first address in flash bank  */
 586
 587     /* Check if Flash is (sufficiently) erased */
 588     if ((*dest & data) != data) {
 589         return (2);
 590     }
 591
 592
 593     base = (FPWV *)(info->start[0]);
 594
 595     /* Disable interrupts which might cause a timeout here */
 596     flag = disable_interrupts();
 597
 598     base[FLASH_CYCLE1] = (FPW)0x00AA00AA;       /* unlock */
 599     base[FLASH_CYCLE2] = (FPW)0x00550055;       /* unlock */
 600     base[FLASH_CYCLE1] = (FPW)0x00A000A0;       /* selects program mode */
 601
 602     *dest = data;               /* start programming the data   */
 603
 604     /* re-enable interrupts if necessary */
 605     if (flag)
 606         enable_interrupts();
 607
 608     start = get_timer (0);
 609
 610     /* data polling for D7 */
 611     while (res == 0 && (*dest & (FPW)0x00800080) != (data & (FPW)0x00800080)) {
 612         if (get_timer(start) > CONFIG_SYS_FLASH_WRITE_TOUT) {
 613             *dest = (FPW)0x00F000F0;    /* reset bank */
 614             res = 1;
 615         }
 616     }
 617
 618     return (res);
 619 }
 620
 621 /*-----------------------------------------------------------------------
 622  * Write a word to Flash for Intel FLASH
 623  * A word is 16 or 32 bits, whichever the bus width of the flash bank
 624  * (not an individual chip) is.
 625  *
 626  * returns:
 627  * 0 - OK
 628  * 1 - write timeout
 629  * 2 - Flash not erased
 630  */
 631 static int write_word_intel (flash_info_t *info, FPWV *dest, FPW data)
 632 {
 633     ulong start;
 634     int flag;
 635     int res = 0;        /* result, assume success       */
 636
 637     /* Check if Flash is (sufficiently) erased */
 638     if ((*dest & data) != data) {
 639         return (2);
 640     }
 641
 642     /* Disable interrupts which might cause a timeout here */
 643     flag = disable_interrupts();
 644
 645     *dest = (FPW)0x00500050;    /* clear status register        */
 646     *dest = (FPW)0x00FF00FF;    /* make sure in read mode       */
 647     *dest = (FPW)0x00400040;    /* program setup                */
 648
 649     *dest = data;               /* start programming the data   */
 650
 651     /* re-enable interrupts if necessary */
 652     if (flag)
 653         enable_interrupts();
 654
 655     start = get_timer (0);
 656
 657     while (res == 0 && (*dest & (FPW)0x00800080) != (FPW)0x00800080) {
 658         if (get_timer(start) > CONFIG_SYS_FLASH_WRITE_TOUT) {
 659             *dest = (FPW)0x00B000B0;    /* Suspend program      */
 660             res = 1;
 661         }
 662     }
 663
 664     if (res == 0 && (*dest & (FPW)0x00100010))
 665         res = 1;        /* write failed, time out error is close enough */
 666
 667     *dest = (FPW)0x00500050;    /* clear status register        */
 668     *dest = (FPW)0x00FF00FF;    /* make sure in read mode       */
 669
 670     return (res);
 671 }