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.

176 lines
5.4 KiB

13 years ago
12 years ago
13 years ago
13 years ago
13 years ago
13 years ago
13 years ago
13 years ago
13 years ago
  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. }