ARM7 based quadrocopter
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

at91_init1.c 5.4KB

12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176
  1. /***************************************************************************
  2. * sam7fc - PLL Init, IRQ/FIQ Vectors *
  3. * *
  4. * Copyright (C) 01/2008 by Olaf Rempel *
  5. * razzor@kopf-tisch.de *
  6. * *
  7. * This program is free software; you can redistribute it and/or modify *
  8. * it under the terms of the GNU General Public License as published by *
  9. * the Free Software Foundation; version 2 of the License *
  10. * *
  11. * This program is distributed in the hope that it will be useful, *
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  14. * GNU General Public License for more details. *
  15. * *
  16. * You should have received a copy of the GNU General Public License *
  17. * along with this program; if not, write to the *
  18. * Free Software Foundation, Inc., *
  19. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  20. ***************************************************************************/
  21. #include <stdint.h>
  22. #include "AT91SAM7S256.h"
  23. static void empty_isr(void) {}
  24. /*
  25. * Init critical onchip hardware:
  26. * - disable Watchdog
  27. * - enable Oscillator and PLL, switch to 48MHz MCK
  28. * - set empty Interrupt Handlers
  29. */
  30. void at91_init1(void)
  31. {
  32. /* disable watchdog */
  33. *AT91C_WDTC_WDMR = AT91C_WDTC_WDDIS;
  34. /* enable user reset */
  35. *AT91C_RSTC_RMR = (AT91C_RSTC_KEY & 0xA5 << 24) | AT91C_RSTC_URSTEN;
  36. /* Set Flash Waitstates */
  37. *AT91C_MC_FMR = AT91C_MC_FWS_1FWS;
  38. /*
  39. * Enable main oscillator (MAINCK)
  40. * startup time: 8*6/32768 -> 1.46ms
  41. */
  42. AT91S_PMC *pmc = AT91C_BASE_PMC;
  43. pmc->PMC_MOR = (AT91C_CKGR_OSCOUNT & (6<<8)) |
  44. AT91C_CKGR_MOSCEN;
  45. while (!(pmc->PMC_SR & AT91C_PMC_MOSCS));
  46. /*
  47. * PLLCK = 18.432MHz / 24 * 125 = 96MHz -> div:24, mul:124
  48. * startup time: 32/32768 -> 976us
  49. */
  50. pmc->PMC_PLLR = (AT91C_CKGR_DIV & 24) |
  51. (AT91C_CKGR_MUL & (124<<16)) |
  52. (AT91C_CKGR_PLLCOUNT & (32<<8)) ;
  53. while (!(pmc->PMC_SR & AT91C_PMC_LOCK));
  54. /* MCK = PLLCK / 2 = 48MHz */
  55. pmc->PMC_MCKR = AT91C_PMC_CSS_PLL_CLK |
  56. AT91C_PMC_PRES_CLK_2;
  57. while (!(pmc->PMC_SR & AT91C_PMC_MCKRDY));
  58. /* enable protected mode (let AIC work with debugger) */
  59. AT91S_AIC *aic = AT91C_BASE_AIC;
  60. aic->AIC_DCR = AT91C_AIC_DCR_PROT;
  61. /* Disable & clear all Interrupts */
  62. aic->AIC_IDCR = ~0;
  63. aic->AIC_ICCR = ~0;
  64. /* default Interrupt Handlers just return */
  65. aic->AIC_FVR = (uint32_t)empty_isr;
  66. aic->AIC_IVR = (uint32_t)empty_isr;
  67. uint32_t i;
  68. for (i = 0; i < 32; i++) {
  69. aic->AIC_SMR[i] = 0;
  70. aic->AIC_SVR[i] = (uint32_t)empty_isr;
  71. }
  72. aic->AIC_SPU = (uint32_t)empty_isr;
  73. }
  74. __attribute__((naked)) void IRQ_Handler(void)
  75. {
  76. asm volatile (
  77. ".equ ARM_MODE_IRQ, 0x12 \n\t"
  78. ".equ ARM_MODE_SVC, 0x13 \n\t"
  79. ".equ I_BIT, 0x80 \n\t"
  80. ".equ AIC_IVR, (256) \n\t"
  81. ".equ AIC_EOICR, (304) \n\t"
  82. ".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t"
  83. /* Adjust and save lr_irq on IRQ stack */
  84. "sub lr, lr, #4 \n\t"
  85. "stmfd sp!, { lr } \n\t"
  86. /* Save SPSR (for nested interrupts) */
  87. "mrs r14, SPSR \n\t"
  88. "stmfd sp!, { r14 } \n\t"
  89. /* Save r0 on IRQ stack */
  90. "stmfd sp!, { r0 } \n\t"
  91. /* Write in the IVR to support Protect Mode */
  92. "ldr r14, =AT91C_BASE_AIC \n\t"
  93. "ldr r0, [r14, #AIC_IVR] \n\t"
  94. "str r14, [r14, #AIC_IVR] \n\t"
  95. /* Enable Interrupt and switch to SVC mode */
  96. "msr CPSR_c, #ARM_MODE_SVC \n\t"
  97. /* Save scratch/used registers and lr on SVC Stack */
  98. "stmfd sp!, { r1-r3, r12, r14 } \n\t"
  99. /* Branch to the routine pointed by the AIC_IVR */
  100. "mov r14, pc \n\t"
  101. "bx r0 \n\t"
  102. /* Restore scratch/used registers and lr from SVC Stack */
  103. "ldmia sp!, { r1-r3, r12, r14 } \n\t"
  104. /* Disable Interrupt and switch back to IRQ mode */
  105. "msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t"
  106. /* Mark the End of Interrupt on the AIC */
  107. "ldr r14, =AT91C_BASE_AIC \n\t"
  108. "str r14, [r14, #AIC_EOICR] \n\t"
  109. /* Restore SPSR_irq and r0 from IRQ stack */
  110. "ldmia sp!, { r0 } \n\t"
  111. "ldmia sp!, { r14 } \n\t"
  112. "msr SPSR_cxsf, r14 \n\t"
  113. /* Restore adjusted lr_irq from IRQ stack */
  114. "ldmia sp!, { pc }^ \n\t"
  115. );
  116. }
  117. __attribute__((naked)) void FIQ_Handler(void)
  118. {
  119. asm volatile (
  120. ".equ ARM_MODE_FIQ, 0x11 \n\t"
  121. ".equ ARM_MODE_SVC, 0x13 \n\t"
  122. ".equ I_BIT, 0x80 \n\t"
  123. ".equ F_BIT, 0x40 \n\t"
  124. ".equ AIC_FVR, (260) \n\t"
  125. /* Save r0 to r9_fiq */
  126. "mov r9, r0 \n\t"
  127. /* get FIQ Vector from AIC and thus clear FIQ */
  128. "ldr r0, [r8, #AIC_FVR] \n\t"
  129. /* Switch to SVC and save registers there */
  130. "msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT \n\t"
  131. "stmfd sp!, { r1-r3, r12, lr } \n\t"
  132. /* execute FIQ in SVC_MODE */
  133. "mov r14, pc \n\t"
  134. "bx r0 \n\t"
  135. /* restore registers and switch back to FIQ */
  136. "ldmia sp!, { r1-r3, r12, lr } \n\t"
  137. "msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT \n\t"
  138. /* restore the r0 from r9_fiq */
  139. "mov r0, r9 \n\t"
  140. /* restore PC using the lr_fiq directly */
  141. "subs pc, lr, #4 \n\t"
  142. );
  143. }