root/loader/s90/resetcode/main.c

/* [<][>][^][v][top][bottom][index][help] */

DEFINITIONS

This source file includes following definitions.
  1. copy_and_restart

   1 
   2 /*
   3  * Operating system has died. Known functions will be killed
   4  * after memmove.
   5  *
   6  * Make sure stack is not used.
   7  */
   8 // ERR99 ToDo: Fix LED
   9 #define DP (void*)0xC0220138    // direct-print (blue) (G11 FW bottom)
  10 #define LED_AF (void*)0xC0223030        //LED_AF G11
  11 #define LED_ISO (void*)0xC022012C       //LED_ISO   G11
  12 #define LED_PWR (void*)0xC0220130       //LED_PWR   (G11 VF above)
  13 #define LED_BLUE (void*)0xC0220128      //LED_BLUE (G11 EV)
  14 
  15 
  16 #define DELAY 5000000
  17 
  18 void __attribute__((noreturn)) copy_and_restart(void *dst_void, const void *src_void, long length) {
  19 
  20         volatile long *p = (void*)LED_BLUE;       // turned off later, so assumed to be power
  21         //volatile long *p2 = (void*)DP2;       // turned off later, so assumed to be power
  22 
  23         int counter;
  24 
  25         // DEBUG: blink led
  26         //counter = DELAY; *p = 0x46;  while (counter--) { asm("nop\n nop\n"); };
  27         //counter = DELAY; *p = 0x44;  while (counter--) { asm("nop\n nop\n"); };
  28 
  29         {
  30                 char *dst = dst_void;
  31                 const char *src = src_void;
  32 
  33                 if (src < dst && dst < src + length)
  34                 {
  35                         /* Have to copy backwards */
  36                         src += length;
  37                         dst += length;
  38                         while (length--)
  39                         {
  40                                 *--dst = *--src;
  41                         }
  42                 }
  43                 else
  44                 {
  45                         while (length--)
  46                         {
  47                                 *dst++ = *src++;
  48                         }
  49                 }
  50         }
  51 
  52         //DEBUG: blink again
  53         //counter = DELAY; *p = 0x46;  while (counter--) { asm("nop\n nop\n"); };
  54         //counter = DELAY; *p = 0x44;  while (counter--) { asm("nop\n nop\n"); };
  55 
  56         // DEBUG: jump to regular firmware-boot (causing a boot loop)
  57         //dst_void = (void*) 0xFF810000;
  58 
  59         // resetcode here:
  60         asm volatile (
  61                         "MRS     R0, CPSR\n"
  62                         "BIC     R0, R0, #0x3F\n"
  63                         "ORR     R0, R0, #0xD3\n"
  64                         "MSR     CPSR, R0\n"
  65                         "LDR     R1, =0xC0200000\n"
  66                         "MOV    R0, #0xFFFFFFFF\n"
  67                         "STR     R0, [R1,#0x10C]\n"
  68                         "STR     R0, [R1,#0xC]\n"
  69                         "STR     R0, [R1,#0x1C]\n"
  70                         "STR     R0, [R1,#0x2C]\n"
  71                         "STR     R0, [R1,#0x3C]\n"
  72                         "STR     R0, [R1,#0x4C]\n"
  73                         "STR     R0, [R1,#0x5C]\n"
  74                         "STR     R0, [R1,#0x6C]\n"
  75                         "STR     R0, [R1,#0x7C]\n"
  76                         "STR     R0, [R1,#0x8C]\n"
  77                         "STR     R0, [R1,#0x9C]\n"
  78                         "STR     R0, [R1,#0xAC]\n"
  79                         "STR     R0, [R1,#0xBC]\n"
  80                         "STR     R0, [R1,#0xCC]\n"
  81                         "STR     R0, [R1,#0xDC]\n"
  82                         "STR     R0, [R1,#0xEC]\n"
  83                         "CMP     R4, #7\n"
  84                         "STR     R0, [R1,#0xFC]\n"
  85                         "LDMEQFD SP!, {R4,PC}\n"
  86                         "MOV     R0, #0x78\n"
  87                         "MCR     p15, 0, R0,c1,c0\n"
  88                         "MOV     R0, #0\n"
  89                         "MCR     p15, 0, R0,c7,c10, 4\n"
  90                         "MCR     p15, 0, R0,c7,c5\n"
  91                         "MCR     p15, 0, R0,c7,c6\n"
  92                         "MOV     R0, #0x80000006\n"
  93                         "MCR     p15, 0, R0,c9,c1\n"
  94                         "MCR     p15, 0, R0,c9,c1, 1\n"
  95                         "MRC     p15, 0, R0,c1,c0\n"
  96                         "ORR     R0, R0, #0x50000\n"
  97                         "MCR     p15, 0, R0,c1,c0\n"
  98                         "LDR     R0, =0x12345678\n"
  99                         "MOV     R1, #0x80000000\n"
 100                         "STR     R0, [R1,#0xFFC]\n"
 101           //"LDR     R0, =0xFF810000\n"   // original jump-vector
 102             "MOV     R0, %0\n"              // new jump-vector
 103                         "LDMFD   SP!, {R4,LR}\n"
 104                         "BX      R0\n"
 105                         : : "r"(dst_void) : "memory","r0","r1","r2","r3","r4");
 106         while(1);
 107 }

/* [<][>][^][v][top][bottom][index][help] */