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