main.cpp

   1 //*----------------------------------------------------------------------------
   2 //*         ATMEL Microcontroller Software Support  -  ROUSSET  -
   3 //*----------------------------------------------------------------------------
   4 //* The software is delivered "AS IS" without warranty or condition of any
   5 //* kind, either express, implied or statutory. This includes without
   6 //* limitation any warranty or condition with respect to merchantability or
   7 //* fitness for any particular purpose, or against the infringements of
   8 //* intellectual property rights of others.
   9 //*----------------------------------------------------------------------------
  10 //* File Name           : main.c
  11 //* Object              :
  12 //* Creation            : HIi   10/10/2003
  13 //*----------------------------------------------------------------------------
  14 #include <AT91RM9200.h>
  15 #include <lib_AT91RM9200.h>
  16
  17 #include "com.h"
  18 #include "main.h"
  19 #include "dataflash.h"
  20
  21 #define AT91C_UBOOT_ADDR 0x21F00000
  22 #define AT91C_UBOOT_SIZE 512*1024
  23 #define AT91C_UBOOT_DATAFLASH_ADDR 0xC1000000
  24
  25 // crystal= 18.432MHz
  26 #define AT91C_PLLA_VALUE 0x20263E04 // -> 179.712MHz
  27 #define AT91C_PLLA_MCK 0x0000202
  28
  29 // crystal= 20.000MHz
  30 // #define AT91C_PLLA_VALUE 0x2023BE04 // -> 180MHz
  31 // #define AT91C_PLLA_MCK 0x0000202
  32
  33 #define DELAY_MAIN_FREQ 1000
  34 #define DISP_LINE_LEN 16
  35
  36 //* prototypes
  37 extern void AT91F_DBGU_Printk(char *);
  38 extern "C" void AT91F_ST_ASM_Handler(void);
  39 extern "C" void Jump(unsigned int addr);
  40
  41 const char *menu_separ = "*----------------------------------------*\n\r";
  42
  43 const char *menu_dataflash = {
  44   "1: Download Dataflash [addr]\n\r"
  45   "2: Read Dataflash [addr]\n\r"
  46   "3: Erase chip [nb]\n\r"
  47   "4: Clear bootloader section in Dataflash\n\r"
  48   "5: start Uboot\n\r"
  49   "6: Enable flash lock (flash_nb)\n\r"
  50   "7: Disable flash lock (fl_nb)\r\n"
  51   "8: Read lock status register (all)\r\n"
  52   "9: Read lockdown status register (all)\r\n"
  53   "a: Read security register (all)\r\n"
  54   "b: Program lock register (fl_nb) all to ff \r\n"
  55   "c: Erase lock register (fl_nb) all to 0 \r\n"
  56   "d: Display flash info\r\n"
  57 };
  58
  59 //* Globales variables
  60 char message[40];
  61 volatile char XmodemComplete = 0;
  62 unsigned int StTick;
  63
  64 AT91S_RomBoot const *pAT91;
  65 AT91S_SBuffer           sXmBuffer;
  66 AT91S_SvcXmodem         svcXmodem;
  67 AT91S_Pipe              xmodemPipe;
  68 AT91S_CtlTempo          ctlTempo;
  69 AT91S_SvcTempo          svcTempo;        // Link to a AT91S_Tempo object
  70
  71 void switch_led();
  72
  73 //*--------------------------------------------------------------------------------------
  74 //* Function Name       : GetTickCount()
  75 //* Object              : Return the number of systimer tick
  76 //* Input Parameters    :
  77 //* Output Parameters   :
  78 //*--------------------------------------------------------------------------------------
  79 unsigned int GetTickCount(void)
  80 {
  81   return StTick;
  82 }
  83
  84
  85 //*--------------------------------------------------------------------------------------
  86 //* Function Name       : AT91_XmodemComplete()
  87 //* Object              : Perform the remap and jump to appli in RAM
  88 //* Input Parameters    :
  89 //* Output Parameters   :
  90 //*--------------------------------------------------------------------------------------
  91 void AT91_XmodemComplete(AT91S_PipeStatus status, void *pVoid)
  92 {
  93   // stop the Xmodem tempo
  94   svcXmodem.tempo.Stop(&(svcXmodem.tempo));
  95   XmodemComplete = 1;
  96 }
  97
  98
  99 //*--------------------------------------------------------------------------------------
 100 //* Function Name       : AT91F_XmodemProtocol(AT91S_PipeStatus status, void *pVoid)
 101 //* Object              : Xmodem dispatcher
 102 //* Input Parameters    :
 103 //* Output Parameters   :
 104 //*--------------------------------------------------------------------------------------
 105 void XmodemProtocol(AT91S_PipeStatus status, void *pVoid)
 106 {
 107   AT91PS_SBuffer pSBuffer = (AT91PS_SBuffer) xmodemPipe.pBuffer->pChild;
 108   AT91PS_USART   pUsart     = svcXmodem.pUsart;
 109
 110   if (pSBuffer->szRdBuffer == 0) {
 111     // Start a tempo to wait the Xmodem protocol complete
 112     svcXmodem.tempo.Start(&(svcXmodem.tempo), 10, 0, AT91_XmodemComplete, pUsart);
 113   }
 114 }
 115
 116
 117 //*-------------------------- Interrupt handlers ----------------------------------------
 118 //*--------------------------------------------------------------------------------------
 119 //* Function Name       : irq1_c_handler()
 120 //* Object              : C Interrupt handler for Interrutp source 1
 121 //* Input Parameters    : none
 122 //* Output Parameters   : none
 123 //*--------------------------------------------------------------------------------------
 124 extern "C" void AT91F_ST_Handler(void);
 125
 126 void AT91F_ST_Handler(void)
 127 {
 128   volatile unsigned int csr = *AT91C_DBGU_CSR;
 129   unsigned int error;
 130
 131   /* ========== Systimer interrupt ============== */
 132   if (AT91C_BASE_ST->ST_SR & 0x01) {
 133     StTick++;
 134     //    switch_led();
 135     ctlTempo.CtlTempoTick(&ctlTempo);
 136     return;
 137   }
 138
 139   error = AT91F_US_Error((AT91PS_USART)AT91C_BASE_DBGU);
 140   if (csr & error) {
 141     // Stop previous Xmodem transmition
 142     *(AT91C_DBGU_CR) = AT91C_US_RSTSTA;
 143     AT91F_US_DisableIt((AT91PS_USART)AT91C_BASE_DBGU, AT91C_US_ENDRX);
 144     AT91F_US_EnableIt((AT91PS_USART)AT91C_BASE_DBGU, AT91C_US_RXRDY);
 145
 146   }
 147
 148   else if (csr & (AT91C_US_TXRDY | AT91C_US_ENDTX | AT91C_US_TXEMPTY |
 149                   AT91C_US_RXRDY | AT91C_US_ENDRX | AT91C_US_TIMEOUT |
 150                   AT91C_US_RXBUFF)) {
 151     if ( !(svcXmodem.eot) )
 152       svcXmodem.Handler(&svcXmodem, csr);
 153   }
 154 }
 155
 156
 157 //*-----------------------------------------------------------------------------
 158 //* Function Name       : AT91F_DisplayMenu()
 159 //* Object              :
 160 //* Input Parameters    :
 161 //* Return value                :
 162 //*-----------------------------------------------------------------------------
 163 void AT91F_DisplayMenu(void)
 164 {
 165   printf("\n\rATMEL LOADER %s %s %s\n\r", AT91C_VERSION, __DATE__, __TIME__);
 166   printf(menu_separ);
 167   printf(menu_separ);
 168   printf(menu_dataflash);
 169   printf(menu_separ);
 170 }
 171
 172 //*-----------------------------------------------------------------------------
 173 //* Function Name       : AsciiToHex()
 174 //* Object              : ascii to hexa conversion
 175 //* Input Parameters    :
 176 //* Return value                :
 177 //*-----------------------------------------------------------------------------
 178 unsigned int AsciiToHex(char *s, unsigned int *val)
 179 {
 180   int n;
 181
 182   *val=0;
 183
 184   if(s[0] == '0' && ((s[1] == 'x') || (s[1] == 'X')))
 185     s+=2;
 186   n = 0;
 187   while((n < 8) && (s[n] !=0))
 188     {
 189       *val <<= 4;
 190       if ( (s[n] >= '0') && (s[n] <='9'))
 191         *val += (s[n] - '0');
 192       else
 193         if ((s[n] >= 'a') && (s[n] <='f'))
 194           *val += (s[n] - 0x57);
 195         else
 196           if ((s[n] >= 'A') && (s[n] <='F'))
 197             *val += (s[n] - 0x37);
 198           else
 199             return 0;
 200       n++;
 201     }
 202
 203   return 1;
 204 }
 205
 206
 207 //*-----------------------------------------------------------------------------
 208 //* Function Name       : AT91F_MemoryDisplay()
 209 //* Object              : Display the content of the dataflash
 210 //* Input Parameters    :
 211 //* Return value                :
 212 //*-----------------------------------------------------------------------------
 213 int AT91F_MemoryDisplay(unsigned int addr, unsigned int size, unsigned int length)
 214 {
 215   unsigned long i, nbytes, linebytes;
 216   char  *cp;
 217   unsigned int  *uip;
 218   unsigned short        *usp;
 219   unsigned char         *ucp;
 220   char linebuf[DISP_LINE_LEN];
 221
 222   nbytes = length * size;
 223   do
 224     {
 225       uip = (unsigned int *)linebuf;
 226       usp = (unsigned short *)linebuf;
 227       ucp = (unsigned char *)linebuf;
 228
 229       printf("%08x:", addr);
 230       linebytes = (nbytes > DISP_LINE_LEN)?DISP_LINE_LEN:nbytes;
 231
 232       read_dataflash(addr, (linebytes/size)*size, linebuf);
 233       for (i=0; i<linebytes; i+= size)
 234         {
 235           if (size == 4)
 236             printf(" %08x", *uip++);
 237           else if (size == 2)
 238             printf(" %04x", *usp++);
 239           else
 240             printf(" %02x", *ucp++);
 241           addr += size;
 242         }
 243       printf("    ");
 244       cp = linebuf;
 245       for (i=0; i<linebytes; i++) {
 246         if ((*cp < 0x20) || (*cp > 0x7e))
 247           printf(".");
 248         else
 249           printf("%c", *cp);
 250         cp++;
 251       }
 252       printf("\n\r");
 253       nbytes -= linebytes;
 254     } while (nbytes > 0);
 255   return 0;
 256 }
 257
 258 #if 0
 259 //*-----------------------------------------------------------------------------
 260 //* Function Name       : AT91F_DramDisplay()
 261 //* Object              : Display the content of the sdram
 262 //* Input Parameters    :
 263 //* Return value                :
 264 //*-----------------------------------------------------------------------------
 265 int AT91F_SdramDisplay(unsigned int addr, unsigned int size, unsigned int length)
 266 {
 267   unsigned long i, nbytes, linebytes;
 268   char  *cp;
 269   unsigned int  *uip;
 270   unsigned short        *usp;
 271   unsigned char         *ucp;
 272
 273   nbytes = length * size;
 274   do
 275     {
 276       uip = (unsigned int *)addr;
 277       usp = (unsigned short *)addr;
 278       ucp = (unsigned char *)addr;
 279
 280       printf("%08x:", addr);
 281       linebytes = (nbytes > DISP_LINE_LEN)?DISP_LINE_LEN:nbytes;
 282
 283       for (i=0; i<linebytes; i+= size)
 284         {
 285           if (size == 4)
 286             printf(" %08x", *uip++);
 287           else if (size == 2)
 288             printf(" %04x", *usp++);
 289           else
 290             printf(" %02x", *ucp++);
 291           addr += size;
 292         }
 293       printf("    ");
 294       for (i=0; i<linebytes; i++) {
 295         if ((*cp < 0x20) || (*cp > 0x7e))
 296           printf(".");
 297         else
 298           printf("%c", *cp);
 299         cp++;
 300       }
 301       printf("\n\r");
 302       nbytes -= linebytes;
 303     } while (nbytes > 0);
 304   return 0;
 305 }
 306 #endif
 307
 308
 309 //*--------------------------------------------------------------------------------------
 310 //* Function Name       : AT91F_SetPLL
 311 //* Object              : Set the PLLA to 180Mhz and Master clock to 60 Mhz
 312 //* Input Parameters    :
 313 //* Output Parameters   :
 314 //*--------------------------------------------------------------------------------------
 315 void AT91F_SetPLL(void)
 316 {
 317   volatile int tmp = 0;
 318
 319   AT91PS_PMC pPmc = AT91C_BASE_PMC;
 320   AT91PS_CKGR pCkgr = AT91C_BASE_CKGR;
 321
 322   pPmc->PMC_IDR = 0xFFFFFFFF;
 323
 324   //* -Setup the PLL A
 325   pCkgr->CKGR_PLLAR = AT91C_PLLA_VALUE;
 326
 327   while(!(pPmc->PMC_SR & AT91C_PMC_MCKRDY) && (tmp++ < DELAY_MAIN_FREQ));
 328
 329   //* - Commuting Master Clock from PLLB to PLLA/3
 330   pPmc->PMC_MCKR = AT91C_PLLA_MCK;
 331 }
 332
 333
 334 //*--------------------------------------------------------------------------------------
 335 //* Function Name       : AT91F_ResetRegisters
 336 //* Object              : Restore the initial state to registers
 337 //* Input Parameters    :
 338 //* Output Parameters   :
 339 //*--------------------------------------------------------------------------------------
 340 static void AT91F_ResetRegisters(void)
 341 {
 342   volatile int i = 0;
 343
 344   //* set the PIOs in input
 345   *AT91C_PIOA_ODR = 0xFFFFFFFF; /* Disables all the output pins */
 346   *AT91C_PIOA_PER = 0xFFFFFFFF; /* Enables the PIO to control all the pins */
 347
 348   AT91F_AIC_DisableIt (AT91C_BASE_AIC, AT91C_ID_SYS);
 349
 350   /* close all peripheral clocks */
 351   AT91C_BASE_PMC->PMC_PCDR = 0xFFFFFFFC;
 352
 353   //* Disable core interrupts and set supervisor mode
 354   __asm__ ("msr CPSR_c, #0xDF"); //* ARM_MODE_SYS(0x1F) | I_BIT(0x80) | F_BIT(0x40)
 355
 356   //* Clear all the interrupts
 357   *AT91C_AIC_ICCR = 0xffffffff;
 358
 359   /* read the AIC_IVR and AIC_FVR */
 360   i = *AT91C_AIC_IVR;
 361   i = *AT91C_AIC_FVR;
 362
 363   /* write the end of interrupt control register */
 364   *AT91C_AIC_EOICR      = 0;
 365
 366   AT91F_SetPLL();
 367 }
 368
 369 void AT91F_StartUboot(unsigned int dummy, void *pvoid)
 370 {
 371   printf("Load U-BOOT from dataflash[%x] to SDRAM[%x]\n\r", AT91C_UBOOT_DATAFLASH_ADDR, AT91C_UBOOT_ADDR);
 372   read_dataflash(AT91C_UBOOT_DATAFLASH_ADDR, AT91C_UBOOT_SIZE, (char *)(AT91C_UBOOT_ADDR));
 373   printf("Set PLLA to 180Mhz and Master clock to 60Mhz and start U-BOOT\n\r");
 374   //* Reset registers
 375   AT91F_ResetRegisters();
 376   Jump(AT91C_UBOOT_ADDR);
 377   while(1);
 378 }
 379
 380 void turn_led_off()
 381 {
 382    *AT91C_PIOD_ODR = AT91C_PIO_PD6;     /* Manage PD6 io */
 383    *AT91C_PIOD_PER = AT91C_PIO_PD6;     /* Enable PD6 PIO */
 384    *AT91C_PIOD_OER = AT91C_PIO_PD6;     /* Enables PD6 ouput */
 385    *AT91C_PIOD_SODR = AT91C_PIO_PD6;    /* Set PD6 */
 386 }
 387
 388 void turn_led_on()
 389 {
 390    *AT91C_PIOD_ODR = AT91C_PIO_PD6;
 391    *AT91C_PIOD_PER = AT91C_PIO_PD6;
 392    *AT91C_PIOD_OER = AT91C_PIO_PD6;
 393    *AT91C_PIOD_CODR = AT91C_PIO_PD6;
 394 }
 395
 396
 397 void reset_flash_lock()
 398 {
 399    *AT91C_PIOB_ODR = AT91C_PIO_PC15;    /* Manage PB15 io */
 400    *AT91C_PIOB_PER = AT91C_PIO_PC15;    /* Enables PB15 io */
 401    *AT91C_PIOB_OER = AT91C_PIO_PC15;    /* Enables PB15 ouput */
 402    *AT91C_PIOB_CODR = AT91C_PIO_PC15;   /* Clear PB 15 */
 403 }
 404
 405 #define ILIM0 0
 406 #define ILIM1 100
 407 #define ILIM2 200
 408 int cnt=0;
 409 void switch_led()
 410 {
 411    switch ( cnt)
 412    {
 413       case ILIM0 :
 414       {
 415          turn_led_on();
 416          cnt++;
 417          break;
 418       }
 419
 420       case ILIM1:
 421       {
 422          turn_led_off();
 423          cnt++;
 424          break;
 425       }
 426       case ILIM2:
 427       {
 428          cnt = ILIM0;
 429          break;
 430       }
 431       default:
 432       {
 433          cnt++;
 434          break;
 435       }
 436    }
 437 }
 438
 439
 440 //*----------------------------------------------------------------------------
 441 //* Function Name       : main
 442 //* Object              : Main function
 443 //* Input Parameters    : none
 444 //* Output Parameters   : True
 445 //*----------------------------------------------------------------------------
 446
 447 void flash_lock_reg(void);
 448 void flash_unlock_reg(void);
 449
 450
 451 int main(void)
 452 {
 453   AT91PS_Buffer                 pXmBuffer;
 454   AT91PS_SvcComm                pSvcXmodem;
 455   AT91S_SvcTempo                svcUbootTempo;   // Link to a AT91S_Tempo object
 456
 457   unsigned int AddressToDownload, SizeToDownload;
 458   unsigned int DeviceAddress = 0;
 459   volatile int i = 0;
 460   char command = 0;
 461   unsigned int crc1 = 0, crc2 = 0;
 462   volatile int device;
 463   int NbPage;
 464
 465   reset_flash_lock();
 466
 467   stdin = fopen(0, at91_dbgu_getc);
 468   stdout = fopen(at91_dbgu_putc, 0);
 469
 470   pAT91 = AT91C_ROM_BOOT_ADDRESS;
 471
 472   // Tempo Initialisation
 473   pAT91->OpenCtlTempo(&ctlTempo, (void *) &(pAT91->SYSTIMER_DESC));
 474   ctlTempo.CtlTempoStart((void *) &(pAT91->SYSTIMER_DESC));
 475
 476   // Attach the tempo to a tempo controler
 477   ctlTempo.CtlTempoCreate(&ctlTempo, &svcUbootTempo);
 478
 479   //    Xmodem Initialisation
 480   pXmBuffer     = pAT91->OpenSBuffer(&sXmBuffer);
 481   pSvcXmodem = pAT91->OpenSvcXmodem(&svcXmodem, (AT91PS_USART)AT91C_BASE_DBGU, &ctlTempo);
 482   pAT91->OpenPipe(&xmodemPipe, pSvcXmodem, pXmBuffer);
 483
 484   //* System Timer initialization
 485   AT91F_AIC_ConfigureIt (
 486                          AT91C_BASE_AIC,                        // AIC base address
 487                          AT91C_ID_SYS,                          // System peripheral ID
 488                          AT91C_AIC_PRIOR_HIGHEST,               // Max priority
 489                          AT91C_AIC_SRCTYPE_INT_LEVEL_SENSITIVE, // Level sensitive
 490                          AT91F_ST_ASM_Handler );
 491   //* Enable ST interrupt
 492   AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_SYS);
 493
 494   //    DataFlash on SPI Configuration
 495   AT91F_DataflashInit ();
 496
 497   // start tempo to start Uboot in a delay of 1 sec if no key pressed
 498   svcUbootTempo.Start(&svcUbootTempo, 2000, 0, AT91F_StartUboot, (void *)0);
 499
 500
 501   printf("press any key to enter bootloader\n\r");
 502   getc();
 503
 504   // stop tempo
 505   svcUbootTempo.Stop(&svcUbootTempo);
 506
 507   while(1)
 508     {
 509       while(command == 0)
 510         {
 511           AddressToDownload = AT91C_DOWNLOAD_BASE_ADDRESS;
 512           SizeToDownload = AT91C_DOWNLOAD_MAX_SIZE;
 513           DeviceAddress = 0;
 514
 515           AT91F_DisplayMenu();
 516           message[0] = 0;
 517           message[2] = 0;
 518           AT91F_ReadLine("Enter: ", message);
 519
 520           command = message[0];
 521           if(command == '1' || command == '2' ||
 522              command == '3' ||
 523              command == '6' ||
 524              command == '7' ||
 525              command == 'b'
 526              )
 527             if(AsciiToHex(&message[2], &DeviceAddress) == 0)
 528               command = 0;
 529
 530           switch(command)
 531             {
 532             case '1':
 533               printf("Download Dataflash [0x%x]\n\r", DeviceAddress);
 534
 535               switch(DeviceAddress & 0xFF000000)
 536                 {
 537                 case CFG_DATAFLASH_LOGIC_ADDR_CS0:
 538                   device = 0;
 539                   break;
 540
 541                 case CFG_DATAFLASH_LOGIC_ADDR_CS1:
 542                   device = 1;
 543                   break;
 544
 545                 case CFG_DATAFLASH_LOGIC_ADDR_CS2:
 546                   device = 2;
 547                   break;
 548
 549                 case CFG_DATAFLASH_LOGIC_ADDR_CS3:
 550                   device = 3;
 551                   break;
 552
 553                 default:
 554                   command = 0;
 555                   break;
 556                 }
 557               break;
 558
 559             case '2':
 560               do
 561                 {
 562                   AT91F_MemoryDisplay(DeviceAddress, 4, 64);
 563                   AT91F_ReadLine ((char *)0, message);
 564                   DeviceAddress += 0x100;
 565                 }
 566               while(message[0] == '\0');
 567               command = 0;
 568               break;
 569
 570             case '3':
 571               erase_dataflash(DeviceAddress);
 572               command = 0;
 573               break;
 574
 575             case '4':
 576               {
 577                 int *i;
 578                 for(i = (int *)0x20000000; i < (int *)0x20004000; i++)
 579                   *i = 0;
 580               }
 581               write_dataflash(0xc0000000, 0x20000000, 0x4000);
 582               printf("Bootsection cleared\r\n");
 583               command = 0;
 584               break;
 585
 586             case '5':
 587               {
 588                 AT91F_StartUboot(0, (void *)0);
 589                 command = 0;
 590                 break;
 591               }
 592
 593             case '6':
 594               {
 595                 flash_enable_lock(DeviceAddress);
 596                 command = 0;
 597                 break;
 598               }
 599
 600             case '7':
 601               {
 602                 flash_disable_lock(DeviceAddress);
 603                 command = 0;
 604                 break;
 605               }
 606
 607             case '8':
 608               {
 609                 read_lock_reg(&read_lock_cmd_sector);
 610                 command = 0;
 611                 break;
 612               }
 613             case '9':
 614               {
 615                 read_lock_reg(&read_lock_cmd_lockdown);
 616                 command = 0;
 617                 break;
 618               }
 619             case 'a':
 620               {
 621                 read_lock_reg(&read_lock_cmd_security);
 622                 command = 0;
 623                 break;
 624               }
 625             case 'b':
 626               {
 627                 erase_lock_reg(DeviceAddress);
 628                 command = 0;
 629                 break;
 630               }
 631             case 'c':
 632               {
 633                 program_lock_reg(DeviceAddress);
 634                 command = 0;
 635                 break;
 636               }
 637
 638             case 'd':
 639             default:
 640               AT91F_DataflashPrintInfo();
 641               command = 0;
 642               break;
 643             }
 644         }
 645
 646       xmodemPipe.Read(&xmodemPipe, (char *)AddressToDownload, SizeToDownload, XmodemProtocol, 0);
 647       while(XmodemComplete !=1);
 648       SizeToDownload = (unsigned int)(svcXmodem.pData) - (unsigned int)AddressToDownload;
 649
 650       // Modification of vector 6
 651       NbPage = 0;
 652       i = dataflash_info[device].Device.pages_number;
 653       while(i >>= 1)
 654         NbPage++;
 655       i = (SizeToDownload / 512) + 1 + (NbPage << 13) + (dataflash_info[device].Device.pages_size << 17);
 656       *(int *)(AddressToDownload + AT91C_OFFSET_VECT6) = i;
 657
 658       printf("\n\rModification of Arm Vector 6 :%x\n\r", i);
 659
 660       printf("\n\rWrite %d bytes in DataFlash [0x%x]\n\r",SizeToDownload, DeviceAddress);
 661       crc1 = 0;
 662       pAT91->CRC32((const unsigned char *)AddressToDownload, SizeToDownload , &crc1);
 663
 664       // write the dataflash
 665       write_dataflash (DeviceAddress, AddressToDownload, SizeToDownload);
 666       // clear the buffer before read
 667       for(i=0; i < SizeToDownload; i++)
 668         *(unsigned char *)(AddressToDownload + i) = 0;
 669
 670       //* Read dataflash page in TestBuffer
 671       read_dataflash (DeviceAddress, SizeToDownload, (char *)(AddressToDownload));
 672
 673       printf("Verify Dataflash: ");
 674       crc2 = 0;
 675
 676       pAT91->CRC32((const unsigned char *)AddressToDownload, SizeToDownload , &crc2);
 677       if (crc1 != crc2)
 678         printf("Failed\r\n");
 679       else
 680         printf("OK\r\n");
 681
 682       command = 0;
 683       XmodemComplete = 0;
 684       AT91F_WaitKeyPressed();
 685     }
 686 }