Fixed the issue of the data/bss section cannot be read from ARM FVP debug tool in cortex-A5 GNU port (#306)

https://msazure.visualstudio.com/One/_workitems/edit/25153813/

7 files changed, 861 insertions, 27 deletions

diff --git a/ports/cortex_a5/gnu/example_build/MP_GIC.S b/ports/cortex_a5/gnu/example_build/MP_GIC.S
new file mode 100644
index 00000000..2ff179fb
--- /dev/null
+++ b/ports/cortex_a5/gnu/example_build/MP_GIC.S
@@ -0,0 +1,516 @@
+// ------------------------------------------------------------
+// Cortex-A MPCore - Interrupt Controller functions
+//
+// Copyright (c) 2011-2018 Arm Limited (or its affiliates). All rights reserved.
+// Use, modification and redistribution of this file is subject to your possession of a
+// valid End User License Agreement for the Arm Product of which these examples are part of 
+// and your compliance with all applicable terms and conditions of such licence agreement.
+// ------------------------------------------------------------
+
+
+  .text
+  .cfi_sections .debug_frame  // put stack frame info into .debug_frame instead of .eh_frame
+
+
+// ------------------------------------------------------------
+// GIC
+// ------------------------------------------------------------
+
+  // CPU Interface offset from base of private peripheral space --> 0x0100
+  // Interrupt Distributor offset from base of private peripheral space --> 0x1000
+
+  // Typical calls to enable interrupt ID X:
+  // disableIntID(X)               <-- Disable that ID
+  // setIntPriority(X, 0)          <-- Set the priority of X to 0 (the max priority)
+  // setPriorityMask(0x1F)         <-- Set CPU's priority mask to 0x1F (the lowest priority)
+  // enableGIC()                   <-- Enable the GIC (global)
+  // enableGICProcessorInterface() <-- Enable the CPU interface (local to the CPU)
+
+
+  .global enableGIC
+  // void enableGIC(void)
+  // Global enable of the Interrupt Distributor
+  .type enableGIC, "function"
+  .cfi_startproc
+enableGIC:
+
+  // Get base address of private peripheral space
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+  ADD     r0, r0, #0x1000         // Add the GIC offset
+
+  LDR     r1, [r0]                // Read the GIC Enable Register  (ICDDCR)
+  ORR     r1, r1, #0x01           // Set bit 0, the enable bit
+  STR     r1, [r0]                // Write the GIC Enable Register  (ICDDCR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global disableGIC
+  // void disableGIC(void)
+  // Global disable of the Interrupt Distributor
+  .type disableGIC, "function"
+  .cfi_startproc
+disableGIC:
+
+  // Get base address of private peripheral space
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+  ADD     r0, r0, #0x1000         // Add the GIC offset
+
+  LDR     r1, [r0]                // Read the GIC Enable Register  (ICDDCR)
+  BIC     r1, r1, #0x01           // Clear bit 0, the enable bit
+  STR     r1, [r0]                // Write the GIC Enable Register  (ICDDCR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global enableIntID
+  // void enableIntID(uint32_t ID)
+  // Enables the interrupt source number ID
+  .type enableIntID, "function"
+  .cfi_startproc
+enableIntID:
+
+  // Get base address of private peripheral space
+  MOV     r1, r0                  // Back up passed in ID value
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  // Each interrupt source has an enable bit in the GIC.  These
+  // are grouped into registers, with 32 sources per register
+  // First, we need to identify which 32-bit block the interrupt lives in
+  MOV     r2, r1                  // Make working copy of ID in r2
+  MOV     r2, r2, LSR #5          // LSR by 5 places, affective divide by 32
+                                  // r2 now contains the 32-bit block this ID lives in
+  MOV     r2, r2, LSL #2          // Now multiply by 4, to convert offset into an address offset (four bytes per reg)
+
+  // Now work out which bit within the 32-bit block the ID is
+  AND     r1, r1, #0x1F           // Mask off to give offset within 32-bit block
+  MOV     r3, #1                  // Move enable value into r3
+  MOV     r3, r3, LSL r1          // Shift it left to position of ID
+
+  ADD     r2, r2, #0x1100         // Add the base offset of the Enable Set registers to the offset for the ID
+  STR     r3, [r0, r2]            // Store out (ICDISER)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global disableIntID
+  // void disableIntID(uint32_t ID)
+  // Disables the interrupt source number ID
+  .type disableIntID, "function"
+  .cfi_startproc
+disableIntID:
+
+  // Get base address of private peripheral space
+  MOV     r1, r0                  // Back up passed in ID value
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  // First, we need to identify which 32-bit block the interrupt lives in
+  MOV     r2, r1                  // Make working copy of ID in r2
+  MOV     r2, r2, LSR #5          // LSR by 5 places, affective divide by 32
+                                  // r2 now contains the 32-bit block this ID lives in
+  MOV     r2, r2, LSL #2          // Now multiply by 4, to convert offset into an address offset (four bytes per reg)
+
+  // Now work out which bit within the 32-bit block the ID is
+  AND     r1, r1, #0x1F           // Mask off to give offset within 32-bit block
+  MOV     r3, #1                  // Move enable value into r3
+  MOV     r3, r3, LSL r1          // Shift it left to position of ID in 32-bit block
+
+  ADD     r2, r2, #0x1180         // Add the base offset of the Enable Clear registers to the offset for the ID
+  STR     r3, [r0, r2]            // Store out (ICDICER)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global setIntPriority
+  // void setIntPriority(uint32_t ID, uint32_t priority)
+  // Sets the priority of the specified ID
+  // r0 = ID
+  // r1 = priority
+  .type setIntPriority, "function"
+  .cfi_startproc
+setIntPriority:
+
+  // Get base address of private peripheral space
+  MOV     r2, r0                  // Back up passed in ID value
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  // r0 = base addr
+  // r1 = priority
+  // r2 = ID
+
+  // Make sure that priority value is only 5 bits, and convert to expected format
+  AND     r1, r1, #0x1F
+  MOV     r1, r1, LSL #3
+
+  // Find which register this ID lives in
+  BIC     r3, r2, #0x03           // Make a copy of the ID, clearing off the bottom two bits
+                                  // There are four IDs per reg, by clearing the bottom two bits we get an address offset
+  ADD     r3, r3, #0x1400         // Now add the offset of the Priority Level registers from the base of the private peripheral space
+  ADD     r0, r0, r3              // Now add in the base address of the private peripheral space, giving us the absolute address
+
+
+  // Now work out which ID in the register it is
+  AND     r2, r2, #0x03           // Clear all but the bottom two bits, leaves which ID in the reg it is (which byte)
+  MOV     r2, r2, LSL #3          // Multiply by 8, this gives a bit offset
+
+  // Read -> Modify -> Write
+  MOV     r12, #0xFF              // 8 bit field mask
+  MOV     r12, r12, LSL r2        // Move mask into correct bit position
+  MOV     r1, r1, LSL r2          // Also, move passed in priority value into correct bit position
+
+
+  LDR     r3, [r0]                // Read current value of the Priority Level register
+  BIC     r3, r3, r12             // Clear appropriate field
+  ORR     r3, r3, r1              // Now OR in the priority value
+  STR     r3, [r0]                // And store it back again (ICDIPR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global getIntPriority
+  // uint32_t getIntPriority(void)
+  // Returns the priority of the specified ID
+  .type getIntPriority, "function"
+  .cfi_startproc
+getIntPriority:
+
+  // TBD
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global setIntTarget
+  // void setIntTarget(uint32_t ID, uint32_t target)
+  // Sets the target CPUs of the specified ID
+  .type setIntTarget, "function"
+  .cfi_startproc
+setIntTarget:
+
+  // Get base address of private peripheral space
+  MRC     p15, 4, r2, c15, c0, 0  // Read periph base address
+
+  // r0 = ID
+  // r1 = target
+  // r2 = base addr
+
+  // Clear unused bits
+  AND     r1, r1, #0xF
+
+  // Find which register this ID lives in
+  BIC     r3, r0, #0x03           // Make a copy of the ID, clearing the bottom 2 bits
+                                  // There are four IDs per reg, by clearing the bottom two bits we get an address offset
+  ADD     r3, r3, #0x1800         // Now add the offset of the Target registers from the base of the private peripheral space
+  ADD     r2, r2, r3              // Now add in the base address of the private peripheral space, giving us the absolute address
+
+  // Now work out which ID in the register it is
+  AND     r0, r0, #0x03           // Clear all but the bottom two bits, leaves which ID in the reg it is (which byte)
+  MOV     r0, r0, LSL #3          // Multiply by 8, this gives a bit offset
+
+  // Read -> Modify -> Write
+  MOV     r12, #0xFF              // 8 bit field mask
+  MOV     r12, r12, LSL r0        // Move mask into correct bit position
+  MOV     r1, r1, LSL r0          // Also, move passed in target value into correct bit position
+
+  LDR     r3, [r2]                // Read current value of the Target register
+  BIC     r3, r3, r12             // Clear appropriate field
+  ORR     r3, r3, r1              // Now OR in the target value
+  STR     r3, [r2]                // And store it back again
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global getIntTarget
+  // uint32_t getIntTarget(uint32_t ID)
+  // Returns the target CPUs of the specified ID
+  .type getIntTarget, "function"
+  .cfi_startproc
+getIntTarget:
+
+  // TBD
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global enableGICProcessorInterface
+  // void enableGICProcessorInterface(void)
+  // Enables the processor interface
+  // Must be done on each core separately
+  .type enableGICProcessorInterface, "function"
+  .cfi_startproc
+enableGICProcessorInterface:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x100]        // Read the Processor Interface Control register (ICCICR/ICPICR)
+  ORR     r1, r1, #0x03           // Bit 0: Enables secure interrupts, Bit 1: Enables Non-Secure interrupts
+  BIC     r1, r1, #0x08           // Bit 3: Ensure Group 0 interrupts are signalled using IRQ, not FIQ
+  STR     r1, [r0, #0x100]        // Write the Processor Interface Control register (ICCICR/ICPICR)
+
+  BX      lr
+  .cfi_endproc
+
+
+
+// ------------------------------------------------------------
+
+  .global disableGICProcessorInterface
+  // void disableGICProcessorInterface(void)
+  // Disables the processor interface
+  // Must be done on each core separately
+  .type disableGICProcessorInterface, "function"
+  .cfi_startproc
+disableGICProcessorInterface:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x100]        // Read the Processor Interface Control register (ICCICR/ICPICR)
+  BIC     r1, r1, #0x03           // Bit 0: Enables secure interrupts, Bit 1: Enables Non-Secure interrupts
+  STR     r1, [r0, #0x100]        // Write the Processor Interface Control register (ICCICR/ICPICR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global setPriorityMask
+  // void setPriorityMask(uint32_t priority)
+  // Sets the Priority mask register for the CPU run on
+  // The reset value masks ALL interrupts!
+  .type setPriorityMask, "function"
+  .cfi_startproc
+setPriorityMask:
+  MRC     p15, 4, r1, c15, c0, 0  // Read periph base address
+
+  STR     r0, [r1, #0x0104]       // Write the Priority Mask register
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global setBinaryPoint
+  // void setBinaryPoint(uint32_t priority)
+  // Sets the Binary Point Register for the CPU run on
+  .type setBinaryPoint, "function"
+  .cfi_startproc
+setBinaryPoint:
+  MRC     p15, 4, r1, c15, c0, 0  // Read periph base address
+
+  STR     r0, [r1, #0x0108]       // Write the Priority Mask register (ICCPMR/ICCIPMR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global readIntAck
+  // uint32_t readIntAck(void)
+  // Returns the value of the Interrupt Acknowledge Register
+  .type readIntAck, "function"
+  .cfi_startproc
+readIntAck:
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+  LDR     r0, [r0, #0x010C]       // Read the Interrupt Acknowledge Register
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global writeEOI
+  // void writeEOI(uint32_t ID)
+  // Writes ID to the End Of Interrupt register
+  .type writeEOI, "function"
+  .cfi_startproc
+writeEOI:
+  MRC     p15, 4, r1, c15, c0, 0  // Read periph base address
+
+  STR     r0, [r1, #0x0110]       // Write ID to the End of Interrupt register
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+// SGI
+// ------------------------------------------------------------
+
+  .global sendSGI
+  // void sendSGI(uint32_t ID, uint32_t target_list, uint32_t filter_list)
+  // Send a software generate interrupt
+  .type sendSGI, "function"
+  .cfi_startproc
+sendSGI:
+
+  AND     r3, r0, #0x0F           // Mask off unused bits of ID, and move to r3
+  AND     r1, r1, #0x0F           // Mask off unused bits of target_filter
+  AND     r2, r2, #0x0F           // Mask off unused bits of filter_list
+
+  ORR     r3, r3, r1, LSL #16     // Combine ID and target_filter
+  ORR     r3, r3, r2, LSL #24     // and now the filter list
+
+  // Get the address of the GIC
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+  ADD     r0, r0, #0x1F00         // Add offset of the sgi_trigger reg
+
+  STR     r3, [r0]                // Write to the Software Generated Interrupt Register (ICDSGIR)
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+// TrustZone
+// ------------------------------------------------------------
+
+  .global enableSecureFIQs
+  // void enableSecureFIQs(void)
+  // Enables the sending of secure interrupts as FIQs
+  .type enableSecureFIQs, "function"
+  .cfi_startproc
+enableSecureFIQs:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x100]        // Read the Processor Interface Control register
+  ORR     r1, r1, #0x08           // Bit 3: Controls whether secure interrupts are signalled as IRQs or FIQs
+  STR     r1, [r0, #0x100]        // Write the Processor Interface Control register
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global disableSecureFIQs
+  // void disableSecureFIQs(void)
+  // Disables the sending of secure interrupts as FIQs
+  .type disableSecureFIQs, "function"
+  .cfi_startproc
+disableSecureFIQs:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x100]        // Read the Processor Interface Control register
+  BIC     r1, r1, #0x08           // Bit 3: Controls whether secure interrupts are signalled as IRQs or FIQs
+  STR     r1, [r0, #0x100]        // Write the Processor Interface Control register
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global makeIntSecure
+  // void makeIntSecure(uint32_t ID)
+  // Sets the specified ID as being Secure
+  // r0 - ID
+  .type makeIntSecure, "function"
+  .cfi_startproc
+makeIntSecure:
+
+  MRC     p15, 4, r1, c15, c0, 0  // Read periph base address
+
+  // Each interrupt source has a secutiy bit in the GIC.  These
+  // are grouped into registers, with 32 sources per register
+  // First, we need to identify which 32-bit block the interrupt lives in
+  MOV     r2, r0                  // Make working copy of ID in r2
+  MOV     r2, r2, LSR #5          // LSR by 5 places, affective divide by 32
+                                  // r2 now contains the 32-bit block this ID lives in
+  MOV     r2, r2, LSL #2          // Now multiply by 4, to convert offset into an address offset (four bytes per reg)
+
+  // Now work out which bit within the 32-bit block the ID is
+  AND     r0, r0, #0x1F           // Mask off to give offset within 32-bit block
+  MOV     r3, #1                  // Move enable value into r3
+  MOV     r3, r3, LSL r0          // Shift it left to position of ID
+
+  ADD     r2, r2, #0x1080         // Add the base offset of the Interrupt Configuration registers to the offset for the ID
+
+  LDR     r0, [r1, r2]            // Read appropriate Interrupt Configuration
+  BIC     r0, r0, r3              // Clear bit (0 = secure)
+  STR     r0, [r1, r2]            // Store out
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global makeIntNonSecure
+  // void makeIntNonSecure(uint32_t ID)
+  // Sets the specified ID as being non-secure
+  // r0 - ID
+  .type makeIntNonSecure, "function"
+  .cfi_startproc
+makeIntNonSecure:
+
+  MRC     p15, 4, r1, c15, c0, 0  // Read periph base address
+
+  // Each interrupt source has a secutiy bit in the GIC.  These
+  // are grouped into registers, with 32 sources per register
+  // First, we need to identify which 32-bit block the interrupt lives in
+  MOV     r2, r0                  // Make working copy of ID in r2
+  MOV     r2, r2, LSR #5          // LSR by 5 places, affective divide by 32
+                                  // r2 now contains the 32-bit block this ID lives in
+  MOV     r2, r2, LSL #2          // Now multiply by 4, to convert offset into an address offset (four bytes per reg)
+
+  // Now work out which bit within the 32-bit block the ID is
+  AND     r0, r0, #0x1F           // Mask off to give offset within 32-bit block
+  MOV     r3, #1                  // Move enable value into r3
+  MOV     r3, r3, LSL r0          // Shift it left to position of ID
+
+  ADD     r2, r2, #0x1080         // Add the base offset of the Interrupt Configuration registers to the offset for the ID
+
+  LDR     r0, [r1, r2]            // Read appropriate Interrupt Configuration
+  ORR     r0, r0, r3              // Set bit (1 = secure)
+  STR     r0, [r1, r2]            // Store out
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+
+  .global getIntSecurity
+  // uint32_t getIntSecurity(uint32_t ID, uint32_t security)
+  // Returns the security of the specified ID
+  .type getIntSecurity, "function"
+  .cfi_startproc
+getIntSecurity:
+
+  // TBD
+
+  BX      lr
+  .cfi_endproc
+
+
+// ------------------------------------------------------------
+// End of MP_GIC.s
+// ------------------------------------------------------------
diff --git a/ports/cortex_a5/gnu/example_build/MP_GIC.h b/ports/cortex_a5/gnu/example_build/MP_GIC.h
new file mode 100644
index 00000000..1d047611
--- /dev/null
+++ b/ports/cortex_a5/gnu/example_build/MP_GIC.h
@@ -0,0 +1,120 @@
+// ------------------------------------------------------------
+// Cortex-A MPCore - Interrupt Controller functions
+// Header File
+//
+// Copyright (c) 2011-2018 Arm Limited (or its affiliates). All rights reserved.
+// Use, modification and redistribution of this file is subject to your possession of a
+// valid End User License Agreement for the Arm Product of which these examples are part of 
+// and your compliance with all applicable terms and conditions of such licence agreement.
+// ------------------------------------------------------------
+
+#ifndef _CORTEXA_GIC_H
+#define _CORTEXA_GIC_H
+
+#define SPURIOUS                   (255)
+
+// PPI IDs:
+#define   MPCORE_PPI_PRIVATE_TIMER (29)
+#define   MPCORE_PPI_PRIVATE_WD    (30)
+#define   MPCORE_PPI_GLOBAL_TIMER  (27)
+#define   MPCORE_PPI_LEGACY_IRQ    (31)
+#define   MPCORE_PPI_LEGACY_FIQ    (28)
+
+// ------------------------------------------------------------
+// GIC
+// ------------------------------------------------------------
+
+// Typical calls to enable interrupt ID X:
+// enableIntID(X)                <-- Enable that ID
+// setIntPriority(X, 0)          <-- Set the priority of X to 0 (the max priority)
+// setPriorityMask(0x1F)         <-- Set Core's priority mask to 0x1F (the lowest priority)
+// enableGIC()                   <-- Enable the GIC (global)
+// enableGICProcessorInterface() <-- Enable the CPU interface (local to the core)
+//
+
+
+//  Global enable of the Interrupt Distributor
+void enableGIC(void);
+
+// Global disable of the Interrupt Distributor
+void disableGIC(void);
+
+// Enables the interrupt source number ID
+void enableIntID(unsigned int ID);
+
+// Disables the interrupt source number ID
+void disableIntID(unsigned int ID);
+
+// Enables the processor interface
+// Must be done on each core separately
+void enableGICProcessorInterface(void);
+
+// Disables the processor interface
+// Must be done on each core separately
+void disableGICProcessorInterface(void);
+
+// Sets the Priority mask register for the core run on
+// The reset value masks ALL interrupts!
+//
+// NOTE: Bits 2:0 of this register are SBZ, the function does perform any shifting!
+void setPriorityMask(unsigned int priority);
+
+// Sets the Binary Point Register for the core run on
+void setBinaryPoint(unsigned int priority);
+
+// Sets the priority of the specified ID
+void setIntPriority(unsigned int ID, unsigned int priority);
+
+// Returns the priority of the specified ID
+unsigned int getIntPriority(unsigned int ID, unsigned int priority);
+
+#define MPCORE_IC_TARGET_NONE      (0x0)
+#define MPCORE_IC_TARGET_CPU0      (0x1)
+#define MPCORE_IC_TARGET_CPU1      (0x2)
+#define MPCORE_IC_TARGET_CPU2      (0x4)
+#define MPCORE_IC_TARGET_CPU3      (0x8)
+
+// Sets the target CPUs of the specified ID
+// For 'target' use one of the above defines
+void setIntTarget(unsigned int ID, unsigned int target);
+
+// Returns the target CPUs of the specified ID
+unsigned int getIntTarget(unsigned int ID);
+
+// Returns the value of the Interrupt Acknowledge Register
+unsigned int readIntAck(void);
+
+// Writes ID to the End Of Interrupt register
+void writeEOI(unsigned int ID);
+
+// ------------------------------------------------------------
+// SGI
+// ------------------------------------------------------------
+
+// Send a software generate interrupt
+void sendSGI(unsigned int ID, unsigned int core_list, unsigned int filter_list);
+
+// ------------------------------------------------------------
+// TrustZone
+// ------------------------------------------------------------
+
+// Enables the sending of secure interrupts as FIQs
+void enableSecureFIQs(void);
+
+// Disables the sending of secure interrupts as FIQs
+void disableSecureFIQs(void);
+
+// Sets the specified ID as secure
+void makeIntSecure(unsigned int ID);
+
+// Set the specified ID as non-secure
+void makeIntNonSecure(unsigned int ID);
+
+// Returns the security of the specified ID
+unsigned int getIntSecurity(unsigned int ID);
+
+#endif
+
+// ------------------------------------------------------------
+// End of MP_GIC.h
+// ------------------------------------------------------------
diff --git a/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.S b/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.S
new file mode 100644
index 00000000..2077d917
--- /dev/null
+++ b/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.S
@@ -0,0 +1,118 @@
+// ------------------------------------------------------------
+// Cortex-A MPCore - Private timer functions
+//
+// Copyright ARM Ltd 2009. All rights reserved.
+// ------------------------------------------------------------
+
+  .text
+  .align 3
+
+  // PPI ID 29
+
+
+  // Typical set of calls to enable Timer:
+  // init_private_timer(0xXXXX, 0)   <-- Counter down value of 0xXXXX, with auto-reload
+  // start_private_timer()
+
+  // Timer offset from base of private peripheral space --> 0x600
+
+// ------------------------------------------------------------
+
+  .global init_private_timer
+  .type init_private_timer,function
+  // void init_private_timer(unsigned int load_value, unsigned int auto_reload)
+  // Sets up the private timer
+  // r0: initial load value
+  // r1:  IF 0 (AutoReload) ELSE (SingleShot)
+init_private_timer:
+
+  // Get base address of private perpherial space
+  MOV     r2, r0                  // Make a copy of r0 before corrupting
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  // Set the load value
+  STR     r2, [r0, #0x600]
+
+  // Control register bit layout
+  // Bit 0 - Enable
+  // Bit 1 - Auto-Reload           // see DE681117
+  // Bit 2 - IRQ Generation
+
+  // Form control reg value
+  CMP     r1, #0                  // Check whether to enable auto-reload
+  MOVNE   r2, #0x04               // No auto-reload
+  MOVEQ   r2, #0x06               // With auto-reload
+
+  // Store to control register
+  STR     r2, [r0, #0x608]
+
+  BX      lr
+
+// ------------------------------------------------------------
+
+  // void start_private_timer(void)
+  // Starts the private timer
+  .global start_private_timer
+  .type start_private_timer,function
+start_private_timer:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x608]        // Read control reg
+  ORR     r1, r1, #0x01           // Set enable bit
+  STR     r1, [r0, #0x608]        // Write modified value back
+
+  BX      lr
+
+// ------------------------------------------------------------
+
+  // void stop_private_timer(void)
+  // Stops the private timer
+  .global stop_private_timer
+  .type stop_private_timer,function
+stop_private_timer:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r1, [r0, #0x608]        // Read control reg
+  BIC     r1, r1, #0x01           // Clear enable bit
+  STR     r1, [r0, #0x608]        // Write modified value back
+
+  BX      lr
+
+// ------------------------------------------------------------
+
+  // unsigned int read_private_timer(void)
+  // Reads the current value of the timer count register
+  .global get_private_timer_count
+  .type get_private_timer_count,function
+get_private_timer_count:
+
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  LDR     r0, [r0, #0x604]        // Read count register
+
+  BX      lr
+  
+// ------------------------------------------------------------
+
+  // void clear_private_timer_irq(void)
+  // Clears the private timer interrupt
+  .global clear_private_timer_irq
+  .type clear_private_timer_irq,function
+clear_private_timer_irq:
+  MRC     p15, 4, r0, c15, c0, 0  // Read periph base address
+
+  // Clear the interrupt by writing 0x1 to the Timer's Interrupt Status register
+  MOV     r1, #1
+  STR     r1, [r0, #0x60C]
+
+  BX      lr
+
+// ------------------------------------------------------------
+// End of code
+// ------------------------------------------------------------
+
+// ------------------------------------------------------------
+// End of MP_PrivateTimer.s
+// ------------------------------------------------------------
diff --git a/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.h b/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.h
new file mode 100644
index 00000000..b0ab212a
--- /dev/null
+++ b/ports/cortex_a5/gnu/example_build/MP_PrivateTimer.h
@@ -0,0 +1,36 @@
+// ------------------------------------------------------------
+// Cortex-A MPCore - Private timer functions
+// Header Filer
+//
+// Copyright ARM Ltd 2009. All rights reserved.
+// ------------------------------------------------------------
+
+#ifndef _CORTEXA_PRIVATE_TIMER_
+#define _CORTEXA_PRIVATE_TIMER_
+
+// Typical set of calls to enable Timer:
+// init_private_timer(0xXXXX, 0)   <-- Counter down value of 0xXXXX, with auto-reload
+// start_private_timer()
+
+// Sets up the private timer
+// r0: initial load value
+// r1:  IF 0 (AutoReload) ELSE (SingleShot)
+void init_private_timer(unsigned int load_value, unsigned int auto_reload);
+
+// Starts the private timer
+void start_private_timer(void);
+
+// Stops the private timer
+void stop_private_timer(void);
+
+// Reads the current value of the timer count register
+unsigned int get_private_timer_count(void);
+
+// Clears the private timer interrupt
+void clear_private_timer_irq(void);
+
+#endif
+
+// ------------------------------------------------------------
+// End of MP_PrivateTimer.h
+// ------------------------------------------------------------
diff --git a/ports/cortex_a5/gnu/example_build/build_threadx_sample.bat b/ports/cortex_a5/gnu/example_build/build_threadx_sample.bat
index 54aa192c..faf8aede 100644
--- a/ports/cortex_a5/gnu/example_build/build_threadx_sample.bat
+++ b/ports/cortex_a5/gnu/example_build/build_threadx_sample.bat
@@ -1,6 +1,8 @@
 arm-none-eabi-gcc -c -g -mcpu=cortex-a5 reset.S
 arm-none-eabi-gcc -c -g -mcpu=cortex-a5 crt0.S
 arm-none-eabi-gcc -c -g -mcpu=cortex-a5 tx_initialize_low_level.S
+arm-none-eabi-gcc -c -g -mcpu=cortex-a5 MP_GIC.s
+arm-none-eabi-gcc -c -g -mcpu=cortex-a5 MP_PrivateTimer.s
+arm-none-eabi-gcc -c -g -mcpu=cortex-a5 V7.s
 arm-none-eabi-gcc -c -g -mcpu=cortex-a5 -I../../../../common/inc -I../inc sample_threadx.c
-arm-none-eabi-gcc -g -mcpu=cortex-a5 -T sample_threadx.ld --specs=nosys.specs -o sample_threadx.out -Wl,-Map=sample_threadx.map tx_initialize_low_level.o sample_threadx.o tx.a
-
+arm-none-eabi-gcc -g -nostartfiles -mcpu=cortex-a5 -T sample_threadx.ld --specs=nosys.specs -o sample_threadx.out -Wl,-Map=sample_threadx.map MP_GIC.o MP_PrivateTimer.o V7.o crt0.o reset.o tx_initialize_low_level.o sample_threadx.o tx.a
diff --git a/ports/cortex_a5/gnu/example_build/tx_initialize_low_level.S b/ports/cortex_a5/gnu/example_build/tx_initialize_low_level.S
index 4b324e0a..e81cb2ca 100644
--- a/ports/cortex_a5/gnu/example_build/tx_initialize_low_level.S
+++ b/ports/cortex_a5/gnu/example_build/tx_initialize_low_level.S
@@ -41,8 +41,13 @@ SYS_STACK_SIZE  =       1024                    // System stack size
     .global     _end
     .global     _sp
     .global     _stack_bottom
-
-
+    .global     __vectors
+    .global     disableHighVecs
+    .global     enableGIC
+    .global     enableGICProcessorInterface
+    .global     enableCaches
+    .global     init_private_timer
+    .global     start_private_timer
 /* Define the 16-bit Thumb mode veneer for _tx_initialize_low_level for
    applications calling this function from to 16-bit Thumb mode.  */
 
@@ -160,6 +165,46 @@ _stack_error_loop:
     ADD     r1, r1, #8                          // Increment to next free word
     STR     r1, [r2]                            // Save first free memory address
 
+    PUSH    {lr}
+
+    /* Setup the vector table.  */
+    LDR     r0, =__vectors                      // Get address of vector table
+    MCR     p15, 0, r0, c12, c0, 0              // Write vector table address to CP15
+    BL      disableHighVecs                     // Disable high vectors
+
+    //
+    // GIC Init
+    // ---------
+    BL      enableGIC
+    BL      enableGICProcessorInterface
+
+    //
+    // Enable Private Timer for periodic IRQ
+    // --------------------------------------
+    MOV     r0, #0x1F
+    BL      setPriorityMask               // Set priority mask (local)
+
+    // [EL] Change start - don't enable interrupts here!  
+    //CPSIE   i                            // Clear CPSR I bit
+    // [EL] Change end
+
+    // Enable the Private Timer Interrupt Source
+    MOV     r0, #29
+    MOV     r1, #0
+    BL      enableIntID
+
+    // Set the priority
+    MOV     r0, #29
+    MOV     r1, #0
+    BL      setIntPriority
+
+    // Configure Timer
+    MOV     r0, #0xF0000
+    MOV     r1, #0x0
+    BL      init_private_timer
+    BL      start_private_timer
+
+    POP    {lr}
 #ifdef __THUMB_INTERWORK
     BX      lr                                  // Return to caller
 #else
@@ -202,16 +247,16 @@ __tx_irq_processing_return:
        if nested IRQ interrupts are desired.  Interrupts may be re-enabled over
        small code sequences where lr is saved before enabling interrupts and
        restored after interrupts are again disabled.  */
+    
+    PUSH    {r4, r5}            // Save some preserved registers (r5 is saved just for 8-byte alignment)
+    BL      readIntAck
+    MOV     r4, r0
 
-    /* Interrupt nesting is allowed after calling _tx_thread_irq_nesting_start
-       from IRQ mode with interrupts disabled.  This routine switches to the
-       system mode and returns with IRQ interrupts enabled.
+    CMP     r0, #29             // If not Private Timer interrupt (ID 29), by pass
+    BNE     by_pass_timer_interrupt
 
-       NOTE:  It is very important to ensure all IRQ interrupts are cleared
-       prior to enabling nested IRQ interrupts.  */
-#ifdef TX_ENABLE_IRQ_NESTING
-    BL      _tx_thread_irq_nesting_start
-#endif
+    BL      clear_private_timer_irq
+    DSB
 
     /* For debug purpose, execute the timer interrupt processing here.  In
        a real system, some kind of status indication would have to be checked
@@ -219,13 +264,10 @@ __tx_irq_processing_return:
 
     BL     _tx_timer_interrupt                  // Timer interrupt handler
 
-
-    /* If interrupt nesting was started earlier, the end of interrupt nesting
-       service must be called before returning to _tx_thread_context_restore.
-       This routine returns in processing in IRQ mode with interrupts disabled.  */
-#ifdef TX_ENABLE_IRQ_NESTING
-    BL      _tx_thread_irq_nesting_end
-#endif
+by_pass_timer_interrupt:
+    MOV     r0, r4
+    BL      writeEOI
+    POP     {r4, r5}                            // Recover preserved registers
 
     /* Jump to context restore to restore system context.  */
     B       _tx_thread_context_restore
diff --git a/test/ports/azrtos_test_tx_gnu_cortex_a5.log.expected b/test/ports/azrtos_test_tx_gnu_cortex_a5.log.expected
index 3d35b442..6bbb7579 100644
--- a/test/ports/azrtos_test_tx_gnu_cortex_a5.log.expected
+++ b/test/ports/azrtos_test_tx_gnu_cortex_a5.log.expected
@@ -1,18 +1,18 @@
 >output thread_0_counter
-2913840557
+10
 >output thread_1_counter
-2913840557
+599068
 >output thread_2_counter
-2913840557
+599046
 >output thread_3_counter
-2913840557
+23
 >output thread_4_counter
-2913840557
+23
 >output thread_5_counter
-2913840557
+9
 >output thread_6_counter
-2913840557
+23
 >output thread_7_counter
-2913840557
+23
 >log file
 Stopped duplicating logging output