增加所有文件

arch/armv7-r/iar/arm_atomic.S

@@ -0,0 +1,63 @@
+/*
+ * arm_atomic.s
+ *
+ * Copyright (c) 2020 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description: ARM atomic function.
+ *
+ * Revision History:
+ * -----------------
+ * 2021-09-08   Wanghao.Xu      Export function symbols
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+
+    PUBLIC arch_atomic_swap
+    PUBLIC arch_atomic_add
+    PUBLIC arch_atomic_and
+    PUBLIC arch_atomic_or
+
+    SECTION .text:CODE(2)
+
+arch_atomic_swap:
+.Lloop_atomic_swap:
+    ldrex   r12, [r0]
+    strex   r2, r1, [r0]
+    cmp     r2, #0
+    bne     .Lloop_atomic_swap
+    mov     r0, r12
+    bx      lr
+
+arch_atomic_add:
+.Lloop_atomic_add:
+    ldrex   r12, [r0]
+    add     r2, r12, r1
+    strex   r3, r2, [r0]
+    cmp     r3, #0
+    bne     .Lloop_atomic_add
+    mov     r0, r12
+    bx      lr
+
+arch_atomic_and:
+.Lloop_atomic_and:
+    ldrex   r12, [r0]
+    and     r2, r12, r1
+    strex   r3, r2, [r0]
+    cmp     r3, #0
+    bne     .Lloop_atomic_and
+    mov     r0, r12
+    bx      lr
+
+arch_atomic_or:
+.Lloop_atomic_or:
+    ldrex   r12, [r0]
+    orr     r2, r12, r1
+    strex   r3, r2, [r0]
+    cmp     r3, #0
+    bne     .Lloop_atomic_or
+    mov     r0, r12
+    bx      lr
+
+    END

arch/armv7-r/iar/arm_cache.S

@@ -0,0 +1,324 @@
+/*
+ * Copyright (c) 2008-2012 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+    INCLUDE config.h
+    INCLUDE compiler.h
+    INCLUDE armv7-r/cache.h
+
+#if CONFIG_ARCH_WITH_CACHE
+
+    PUBLIC arch_disable_cache
+    PUBLIC arch_enable_cache
+    PUBLIC arch_clean_cache_range
+    PUBLIC arch_clean_invalidate_cache_range
+    PUBLIC arch_clean_invalidate_dcache_all
+    PUBLIC arch_invalidate_cache_range
+    PUBLIC arch_sync_cache_range
+
+    SECTION .text:CODE(2)
+
+/* void arch_disable_cache(uint flags) */
+arch_disable_cache:
+    stmfd   sp!, {r4-r11, lr}
+
+    mov     r7, r0                      // save flags
+
+    mrs     r8, cpsr                    // save the old interrupt state
+    cpsid   iaf                         // interrupts disabled
+
+.Ldcache_disable:
+    tst     r7, #DCACHE
+    beq     .Licache_disable
+    mrc     p15, 0, r0, c1, c0, 0       // cr1
+    tst     r0, #(1<<2)                 // is the dcache already disabled?
+    beq     .Ldcache_already_disabled
+
+    bic     r0, #(1<<2)
+    mcr     p15, 0, r0, c1, c0, 0       // disable dcache
+
+    // flush and invalidate the dcache
+    // NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
+    bl      flush_invalidate_cache_v7
+
+    b       .Ldcache_disable_L2
+
+.Ldcache_already_disabled:
+    // make sure all of the caches are invalidated
+    // NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
+    bl      invalidate_cache_v7
+
+.Ldcache_disable_L2:
+
+#if CONFIG_ARCH_L2CACHE
+    // disable the L2, if present
+    mrc     p15, 0, r0, c1, c0, 1       // aux cr1
+    bic     r0, #(1<<1)
+    mcr     p15, 0, r0, c1, c0, 1       // disable L2 dcache
+#endif
+
+.Licache_disable:
+    tst     r7, #ICACHE
+    beq     .Ldone_disable
+
+    mrc     p15, 0, r0, c1, c0, 0       // cr1
+    bic     r0, #(1<<12)
+    mcr     p15, 0, r0, c1, c0, 0       // disable icache
+
+.Ldone_disable:
+    // make sure the icache is always invalidated
+    mov     r0, #0
+    mcr     p15, 0, r0, c7, c5, 0       // invalidate icache to PoU
+
+    msr     cpsr_cxsf, r8
+    ldmfd   sp!, {r4-r11, pc}
+
+/* void arch_enable_cache(uint flags) */
+arch_enable_cache:
+    stmfd   sp!, {r4-r12, lr}
+
+    mov     r7, r0                      // save flags
+
+    mrs     r8, cpsr                    // save the old interrupt state
+    cpsid   iaf                         // interrupts disabled
+
+.Ldcache_enable:
+    tst     r7, #DCACHE
+    beq     .Licache_enable
+    mrc     p15, 0, r0, c1, c0, 0       // cr1
+    tst     r0, #(1<<2)                 // is the dcache already enabled?
+    bne     .Licache_enable
+
+    // invalidate L1 and L2
+    // NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
+    bl      invalidate_cache_v7
+
+#if CONFIG_ARCH_L2CACHE
+    // enable the L2, if present
+    mrc     p15, 0, r0, c1, c0, 1       // aux cr1
+    orr     r0, #(1<<1)
+    mcr     p15, 0, r0, c1, c0, 1       // enable L2 dcache
+#endif
+
+    mrc     p15, 0, r0, c1, c0, 0       // cr1
+    orr     r0, #(1<<2)
+    mcr     p15, 0, r0, c1, c0, 0       // enable dcache
+
+.Licache_enable:
+    tst     r7, #ICACHE
+    beq     .Ldone_enable
+
+    mov     r0, #0
+    mcr     p15, 0, r0, c7, c5, 0       // invalidate icache to PoU
+
+    mrc     p15, 0, r0, c1, c0, 0       // cr1
+    orr     r0, #(1<<12)
+    mcr     p15, 0, r0, c1, c0, 0       // enable icache
+
+.Ldone_enable:
+    isb
+    msr     cpsr_cxsf, r8
+    ldmfd   sp!, {r4-r12, pc}
+
+// flush & invalidate cache routine, trashes r0-r6, r9-r11
+flush_invalidate_cache_v7:
+    /* from ARMv7 manual, B2-17 */
+    dmb
+    MRC     p15, 1, R0, c0, c0, 1       // Read CLIDR
+    ANDS    R3, R0, #0x7000000
+    MOV     R3, R3, LSR #23             // Cache level value (naturally aligned)
+    BEQ     .Lfinished
+    MOV     R10, #0
+.Loop1:
+    ADD     R2, R10, R10, LSR #1        // Work out 3xcachelevel
+    MOV     R1, R0, LSR R2              // bottom 3 bits are the Cache type for this level
+    AND     R1, R1, #7                  // get those 3 bits alone
+    CMP     R1, #2
+    BLT     .Lskip                      // no cache or only instruction cache at this level
+    MCR     p15, 2, R10, c0, c0, 0      // write the Cache Size selection register
+    isb                                 // ISB to sync the change to the CacheSizeID reg
+    MRC     p15, 1, R1, c0, c0, 0       // reads current Cache Size ID register
+    AND     R2, R1, #0x7                // extract the line length field
+    ADD     R2, R2, #4                  // add 4 for the line length offset (log2 16 bytes)
+    LDR     R4, =0x3FF
+    ANDS    R4, R4, R1, LSR #3          // R4 is the max number on the way size (right aligned)
+    CLZ     R5, R4                      // R5 is the bit position of the way size increment
+    LDR     R6, =0x00007FFF
+    ANDS    R6, R6, R1, LSR #13         // R6 is the max number of the index size (right aligned)
+.Loop2:
+    MOV     R9, R4                      // R9 working copy of the max way size (right aligned)
+.Loop3:
+    ORR     R11, R10, R9, LSL R5        // factor in the way number and cache number into R11
+    ORR     R11, R11, R6, LSL R2        // factor in the index number
+    MCR     p15, 0, R11, c7, c14, 2     // clean & invalidate by set/way
+    SUBS    R9, R9, #1                  // decrement the way number
+    BGE     .Loop3
+    SUBS    R6, R6, #1                  // decrement the index
+    BGE     .Loop2
+.Lskip:
+    ADD     R10, R10, #2                // increment the cache number
+    CMP     R3, R10
+    BGT     .Loop1
+
+.Lfinished:
+    mov     r10, #0
+    mcr     p15, 2, r10, c0, c0, 0      // select cache level 0
+    dsb
+    isb
+
+    bx      lr
+
+// invalidate cache routine, trashes r0-r6, r9-r11
+invalidate_cache_v7:
+    /* from ARMv7 manual, B2-17 */
+    dmb
+    MRC     p15, 1, R0, c0, c0, 1       // Read CLIDR
+    ANDS    R3, R0, #0x7000000
+    MOV     R3, R3, LSR #23             // Cache level value (naturally aligned)
+    BEQ     .Lfinished_invalidate
+    MOV     R10, #0
+.Loop1_invalidate:
+    ADD     R2, R10, R10, LSR #1        // Work out 3xcachelevel
+    MOV     R1, R0, LSR R2              // bottom 3 bits are the Cache type for this level
+    AND     R1, R1, #7                  // get those 3 bits alone
+    CMP     R1, #2
+    BLT     .Lskip_invalidate           // no cache or only instruction cache at this level
+    MCR     p15, 2, R10, c0, c0, 0      // write the Cache Size selection register
+    isb                                 // ISB to sync the change to the CacheSizeID reg
+    MRC     p15, 1, R1, c0, c0, 0       // reads current Cache Size ID register
+    AND     R2, R1, #0x7                // extract the line length field
+    ADD     R2, R2, #4                  // add 4 for the line length offset (log2 16 bytes)
+    LDR     R4, =0x3FF
+    ANDS    R4, R4, R1, LSR #3          // R4 is the max number on the way size (right aligned)
+    CLZ     R5, R4                      // R5 is the bit position of the way size increment
+    LDR     R6, =0x00007FFF
+    ANDS    R6, R6, R1, LSR #13         // R6 is the max number of the index size (right aligned)
+.Loop2_invalidate:
+    MOV     R9, R4                      // R9 working copy of the max way size (right aligned)
+.Loop3_invalidate:
+    ORR     R11, R10, R9, LSL R5        // factor in the way number and cache number into R11
+    ORR     R11, R11, R6, LSL R2        // factor in the index number
+    MCR     p15, 0, R11, c7, c6, 2      // invalidate by set/way
+    SUBS    R9, R9, #1                  // decrement the way number
+    BGE     .Loop3_invalidate
+    SUBS    R6, R6, #1                  // decrement the index
+    BGE     .Loop2_invalidate
+.Lskip_invalidate:
+    ADD     R10, R10, #2                // increment the cache number
+    CMP     R3, R10
+    BGT     .Loop1_invalidate
+
+.Lfinished_invalidate:
+    dsb
+    mov     r10, #0
+    mcr     p15, 2, r10, c0, c0, 0      // select cache level 0
+    isb
+
+    bx      lr
+
+    /* void arch_flush_cache_range(addr_t start, size_t len); */
+arch_clean_cache_range:
+#if CONFIG_ARM_WITH_CP15
+    mov     r3, r0                      // save the start address
+    add     r2, r0, r1                  // calculate the end address
+    bic     r0, #(CONFIG_ARCH_CACHE_LINE-1)    // align the start with a cache line
+.Loop_clean:
+    mcr     p15, 0, r0, c7, c10, 1      // clean cache to PoC by MVA
+    add     r0, #CONFIG_ARCH_CACHE_LINE
+    cmp     r0, r2
+    blo     .Loop_clean
+
+    dsb
+#endif
+    bx      lr
+
+    /* void arch_flush_invalidate_cache_range(addr_t start, size_t len); */
+arch_clean_invalidate_cache_range:
+#if CONFIG_ARM_WITH_CP15
+    mov     r3, r0                      // save the start address
+    add     r2, r0, r1                  // calculate the end address
+    bic     r0, #(CONFIG_ARCH_CACHE_LINE-1)    // align the start with a cache line
+.Loop_clean_invalidate:
+    mcr     p15, 0, r0, c7, c14, 1      // clean & invalidate dcache to PoC by MVA
+    add     r0, r0, #CONFIG_ARCH_CACHE_LINE
+    cmp     r0, r2
+    blo     .Loop_clean_invalidate
+
+    dsb
+#endif
+    bx      lr
+
+    /* void arch_clean_invalidate_dcache_all(void); */
+arch_clean_invalidate_dcache_all:
+#if CONFIG_ARM_WITH_CP15
+	mrc		p15, 1, r0, c0, c0, 0   /* Read the Cache Size Identification Register */
+	ldr		r3, =0x7fff			/* Isolate the NumSets field (bits 13-27) */
+	and		r0, r3, r0, lsr #13		/* r0=NumSets (number of sets - 1) */
+
+	mov		r1, #0				/* r1 = way loop counter */
+way_loop:
+
+	mov		r3, #0				/* r3 = set loop counter */
+set_loop:
+	mov		r2, r1, lsl #30			/* r2 = way loop counter << 30 */
+	orr		r2, r3, lsl #5			/* r2 = set/way cache operation format */
+	mcr		p15, 0, r2, c7, c14, 2  /* Data Cache Clean Invalidate by Set/Way */
+	add		r3, r3, #1			/* Increment set counter */
+	cmp		r0, r3				/* Last set? */
+	bge		set_loop			/* Keep looping if not */
+
+	add		r1, r1, #1			/* Increment the way counter */
+	cmp		r1, #4				/* Last way? (four ways assumed) */
+	bne		way_loop			/* Keep looping if not */
+
+	dsb
+#endif
+    bx      lr
+
+    /* void arch_invalidate_cache_range(addr_t start, size_t len); */
+arch_invalidate_cache_range:
+#if CONFIG_ARM_WITH_CP15
+    mov     r3, r0                      // save the start address
+    add     r2, r0, r1                  // calculate the end address
+    bic     r0, #(CONFIG_ARCH_CACHE_LINE-1)    // align the start with a cache line
+.Loop_invalidate:
+    mcr     p15, 0, r0, c7, c6, 1       // invalidate dcache to PoC by MVA
+    add     r0, r0, #CONFIG_ARCH_CACHE_LINE
+    cmp     r0, r2
+    blo     .Loop_invalidate
+
+    dsb
+#endif
+    bx      lr
+
+    /* void arch_sync_cache_range(addr_t start, size_t len); */
+arch_sync_cache_range:
+    push    { r14 }
+    bl      arch_clean_cache_range
+
+    mov     r0, #0
+    mcr     p15, 0, r0, c7, c5, 0       // invalidate icache to PoU
+
+    pop     { pc }
+
+#endif
+
+    END

arch/armv7-r/iar/arm_exceptions.S

@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2008-2015 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+
+    EXTERN arm_undefined_handler
+    EXTERN arm_prefetch_abort_handler
+    EXTERN arm_data_abort_handler
+    EXTERN arm_irq_handler
+    EXTERN arm_fiq_handler
+
+    PUBLIC Arm_Undefined_Handler
+    PUBLIC Arm_SWI_Handler
+    PUBLIC Arm_Prefetch_Handler
+    PUBLIC Arm_Abort_Handler
+    PUBLIC Arm_IRQ_Handler
+    PUBLIC Arm_FIQ_Handler
+
+    PUBLIC arm_save_mode_regs
+    PUBLIC arm_reserved
+
+    SECTION .text:CODE:ROOT(2)
+
+; macros to align and unalign the stack on 8 byte boundary for ABI compliance
+stack_align macro tempreg
+    ; make sure the stack is aligned
+    mov     tempreg, sp
+    tst     sp, #4
+    subeq   sp, #4
+    push    { tempreg }
+
+    ; tempreg holds the original stack
+    endm
+
+stack_restore macro tempreg
+    ; restore the potentially unaligned stack
+    pop     { tempreg }
+    mov     sp, tempreg
+    endm
+
+; save and disable the vfp unit
+vfp_save macro temp1
+    ; save old fpexc
+    vmrs    temp1, fpexc
+
+    push    { temp1 }
+
+    ; hard disable the vfp unit
+    bic     temp1, #(1<<30)
+    vmsr    fpexc, temp1
+    endm
+
+; restore the vfp enable/disable state
+vfp_restore macro temp1
+    ; restore fpexc
+    pop     { temp1 }
+
+    vmsr    fpexc, temp1
+    endm
+
+; Save callee trashed registers.
+ ; At exit r0 contains a pointer to the register frame.
+
+save macro
+    ; save spsr and r14 onto the svc stack
+    srsdb   #0x13!
+
+    ; switch to svc mode, interrupts disabled
+    cpsid   i,#0x13
+
+    ; save callee trashed regs and lr
+    push    { r0-r4, r12, lr }
+
+    ; save user space sp/lr
+    sub     sp, #8
+    stmia   sp, { r13, r14 }^
+
+#if CONFIG_ARCH_WITH_FPU
+    ; save and disable the vfp unit
+    vfp_save    r0
+#endif
+
+    ; make sure the stack is 8 byte aligned
+    stack_align r0
+
+    ; r0 now holds the pointer to the original iframe (before alignment)
+    endm
+
+save_offset macro offset
+    sub     lr, offset
+    save
+
+    endm
+
+restore macro
+    ; undo the stack alignment we did before
+    stack_restore r0
+
+#if CONFIG_ARCH_WITH_FPU
+    ; restore the old state of the vfp unit
+    vfp_restore r0
+#endif
+
+    ; restore user space sp/lr
+    ldmia   sp, { r13, r14 }^
+    add     sp, #8
+
+    pop     { r0-r4, r12, lr }
+
+    ; return to whence we came from
+    rfeia   sp!
+    endm
+
+; Save all registers.
+ ; At exit r0 contains a pointer to the register frame.
+
+saveall macro
+    ; save spsr and r14 onto the svc stack
+    srsdb   #0x13!
+
+    ; switch to svc mode, interrupts disabled
+    cpsid   i,#0x13
+
+    ; save all regs
+    push    { r0-r12, lr }
+
+    ; save user space sp/lr
+    sub     sp, #8
+    stmia   sp, { r13, r14 }^
+
+#if CONFIG_ARCH_WITH_FPU
+    ; save and disable the vfp unit
+    vfp_save    r0
+#endif
+
+    ; make sure the stack is 8 byte aligned
+    stack_align r0
+
+    ; r0 now holds the pointer to the original iframe (before alignment)
+    endm
+
+saveall_offset macro offset
+    sub     lr, offset
+    saveall
+    endm
+
+restoreall macro
+    ; undo the stack alignment we did before
+    stack_restore r0
+
+#if CONFIG_ARCH_WITH_FPU
+    ; restore the old state of the vfp unit
+    vfp_restore r0
+#endif
+
+    ; restore user space sp/lr
+    ldmia   sp, { r13, r14 }^
+    add     sp, #8
+
+    pop     { r0-r12, r14 }
+
+    ; return to whence we came from
+    rfeia   sp!
+    endm
+
+arm_save_mode_regs:
+    mrs     r1, cpsr
+
+    stmia   r0, { r13, r14 }^ /* usr */
+    add     r0, #8
+
+    cps     #0x11   /* fiq */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    cps     #0x12   /* irq */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    cps     #0x13   /* svc */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    cps     #0x17   /* abt */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    cps     #0x1b   /* und */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    cps     #0x1f   /* sys */
+    str     r13, [r0], #4
+    str     r14, [r0], #4
+
+    msr     cpsr_c, r1
+
+    bx      lr
+
+Arm_Undefined_Handler:
+    save
+    ; r0 now holds pointer to iframe
+
+    bl      arm_undefined_handler
+
+    restore
+
+Arm_Prefetch_Handler:
+    saveall_offset #4
+    ; r0 now holds pointer to iframe
+
+    bl      arm_prefetch_abort_handler
+
+    restoreall
+
+Arm_Abort_Handler:
+    saveall_offset #8
+    ; r0 now holds pointer to iframe
+
+    bl      arm_data_abort_handler
+
+    restoreall
+
+Arm_SWI_Handler:
+    /* not support */
+    ldr     r0,=Arm_SWI_Handler
+    bx      r0
+
+Arm_IRQ_Handler:
+    sub     lr, lr, #4
+
+    srsdb   #0x13!
+
+    cpsid   i,#0x13
+
+    push    {r0-r4, r12, lr}
+
+    sub     sp, #8
+    stmia   sp, {r13, r14}^
+
+    mov     r2, sp
+    tst     sp, #4
+    subeq   sp, #4
+    push    {r2}
+
+    push    {r0, lr}
+
+    bl      arm_irq_handler
+
+    pop     {r0, lr}
+
+    pop     {r2}
+    mov     sp, r2
+
+    ldmia   sp, {r13, r14}^
+    add     sp, #8
+
+    pop     {r0-r4, r12, lr}
+
+    rfeia   sp!
+
+Arm_FIQ_Handler:
+    push    {r0-r3, r12, lr}
+    blx     arm_fiq_handler
+    pop     {r0-r3, r12, lr}
+    subs    pc, lr, #4
+
+arm_reserved:
+    b   .
+
+    END

arch/armv7-r/iar/arm_fullcontextrestore.S

@@ -0,0 +1,43 @@
+/*
+ * arm_fullcontextrestore.S
+ *
+ * Copyright (c) 2022 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description: ARM full context restore.
+ *
+ * Revision History:
+ * -----------------
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+    INCLUDE armv7-r/svcall.h
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+    PUBLIC arm_fullcontextrestore
+
+    SECTION .text:CODE(2)
+
+/****************************************************************************
+ * Name: arm_fullcontextrestore
+ ****************************************************************************/
+
+arm_fullcontextrestore:
+
+	cps		#0x1F						/* Enter sys mode */
+
+	/* Perform the System call with R0=1 and R1=regs */
+
+	mov		r1, r0						/* R1: regs */
+	mov		r0, #SYS_restore_context	/* R0: restore context */
+	svc		0							/* Force synchronous SVCall (or Hard Fault) */
+
+	/* This call should not return */
+
+	bx		lr							/* Unnecessary ... will not return */
+
+	END

arch/armv7-r/iar/arm_saveusercontext.S

@@ -0,0 +1,39 @@
+/*
+ * arm_saveusercontext.S
+ *
+ * Copyright (c) 2022 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description: ARM save user context.
+ *
+ * Revision History:
+ * -----------------
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+    INCLUDE armv7-r/svcall.h
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+    PUBLIC arm_saveusercontext
+
+    SECTION .text:CODE(2)
+
+/****************************************************************************
+ * Name: arm_saveusercontext
+ ****************************************************************************/
+
+arm_saveusercontext:
+
+	/* Perform the System call with R0=0 and R1=regs */
+
+	mov		r1, r0					/* R1: regs */
+	mov		r0, #SYS_save_context	/* R0: save context (also return value) */
+	svc		0						/* Force synchronous SVCall (or Hard Fault) */
+
+	bx		lr
+
+	END

arch/armv7-r/iar/arm_switchcontext.S

@@ -0,0 +1,40 @@
+/*
+ * arm_switchcontext.S
+ *
+ * Copyright (c) 2022 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description: ARM switch context.
+ *
+ * Revision History:
+ * -----------------
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+    INCLUDE armv7-r/svcall.h
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+    PUBLIC arm_switchcontext
+
+    SECTION .text:CODE(2)
+
+/****************************************************************************
+ * Name: arm_switchcontext
+ ****************************************************************************/
+
+arm_switchcontext:
+
+	/* Perform the System call with R0=2 */
+
+	mov		r0, #SYS_switch_context	/* R0: context switch */
+	svc		0						/* Force synchronous SVCall (or Hard Fault) */
+
+	/* We will get here only after the rerturn from the context switch */
+
+	bx		lr
+
+	END

arch/armv7-r/iar/arm_tcm_asm.S

@@ -0,0 +1,127 @@
+/*
+ * arm_tcm.S
+ *
+ * Copyright (c) 2020 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description: Cortex V7R TCM driver.
+ *
+ * Revision History:
+ * -----------------
+ */
+
+    INCLUDE config.h
+    INCLUDE compiler.h
+    INCLUDE armv7-r/tcm.h
+
+#if CONFIG_ARM_WITH_TCM
+
+    PUBLIC tcma_enable_early
+    PUBLIC tcmb_enable_early
+
+    SECTION .text:CODE(2)
+
+tcm_enable MACRO idx addr_reg temp_reg
+    orr     temp_reg, addr_reg, #1
+    mcr     p15, 0, temp_reg, c9, c1, idx
+    ENDM
+
+tcm_enable_ecc MACRO temp_reg val
+    mrc     p15, 0, temp_reg, c1, c0, 1
+    orr     temp_reg, temp_reg, #(val)
+    mcr     p15, 0, temp_reg, c1, c0, 1
+    ENDM
+
+tcm_disable_ecc MACRO temp_reg val
+    mrc     p15, 0, temp_reg, c1, c0, 1
+    bic     temp_reg, temp_reg, #(val)
+    mcr     p15, 0, temp_reg, c1, c0, 1
+    ENDM
+
+tcm_get_size MACRO idx size_reg temp_reg
+    mrc     p15, 0, size_reg, c9, c1, idx
+    ubfx    size_reg, size_reg, #2, #5
+    adds    size_reg, #9
+    movs    temp_reg, #1
+    lsls    size_reg, temp_reg, size_reg
+    ENDM
+
+tcm_clear MACRO addr_reg size_reg temp_reg1 temp_reg2
+    local   .Ltcm_loop
+    local   .Ltcm_loop_end
+    mov     temp_reg1, #0
+    mov     temp_reg2, #0
+    add     size_reg, addr_reg, size_reg
+.Ltcm_loop:
+    cmp     addr_reg, size_reg
+    bhs     .Ltcm_loop_end
+    stmia   addr_reg!, {temp_reg1-temp_reg2}
+    b       .Ltcm_loop
+.Ltcm_loop_end:
+    ENDM
+
+/* This function not store registers,
+ * only used before stack setup.
+ */
+tcma_enable_early:
+
+    /* disable ecc */
+    tcm_disable_ecc r4, ATCMPCEN
+
+    /* read tcma size, r2 save size */
+    tcm_get_size 1, r2, r4
+
+    /* enable tcma, r0 save addr */
+    tcm_enable 1, r0, r4
+
+    /* check enable ecc */
+    cmp     r1, #1
+    bne     .Lout
+
+    /* clear tcma */
+    tcm_clear r0, r2, r6, r7
+
+    /* enalbe ecc */
+    tcm_enable_ecc r4, ATCMPCEN
+
+    b       .Lout
+
+/* This function not store registers,
+ * only used before stack setup.
+ */
+tcmb_enable_early:
+
+    /* disable ecc */
+    tcm_disable_ecc r4, (B0TCMPCEN | B1TCMPCEN)
+
+    /* read tcma size, r2 save size */
+    tcm_get_size 0, r2, r4
+
+    /* enable tcma, r0 save addr */
+    tcm_enable 0, r0, r4
+
+    /* check enable ecc */
+    cmp     r1, #1
+    bne     .Lout
+
+    /* clear tcma */
+    tcm_clear r0, r2, r6, r7
+
+    /* enalbe ecc */
+    tcm_enable_ecc r4, (B0TCMPCEN | B1TCMPCEN)
+
+    b       .Lout
+
+/* Clear the TCM, each store must be 64 bits aligned. */
+tcm_clear_64bit:
+
+    push {r6, r7}
+    tcm_clear r0, r1, r6, r7
+    pop {r6, r7}
+
+.Lout:
+    bx      lr
+
+#endif
+
+    END

arch/armv7-r/iar/context_saverestore.s

@@ -0,0 +1,134 @@
+/*
+ * context_saverestore.S
+ *
+ * Copyright (c) 2022 Semidrive Semiconductor.
+ * All rights reserved.
+ *
+ * Description:
+ *
+ * Revision History:
+ * -----------------
+ */
+
+portMPU_RegisterSave MACRO
+
+	MOV		r0, #0
+mpu_save_loop:
+	MCR		p15, 0, r0, c6, c2, 0
+	MRC		p15, 0, r2, c6, c1, 0
+	STR		r2, [r1], #4
+	MRC		p15, 0, r2, c6, c1, 4
+	STR		r2, [r1], #4
+	MRC		p15, 0, r2, c6, c1, 2
+	STR		r2, [r1], #4
+	ADD		r0, r0, #1
+	CMP		r0, #16
+	BNE		mpu_save_loop
+	MRC		p15, 0, r2, c1, c0, 0
+	STR		r2, [r1], #4
+
+	ENDM
+
+; /**********************************************************************/
+
+portMPU_RegisterRestore MACRO
+
+	MRC		p15, 0, r2, c1, c0, 0
+	LDR		r0, =(SCTLR_BR | SCTLR_M)
+	BIC		r2, r2, r0
+	MCR		p15, 0, r2, c1, c0, 0
+
+	MOV		r0, #0
+mpu_restore_loop:
+	MCR		p15, 0, r0, c6, c2, 0
+	LDR		r2, [r1], #4
+	MCR		p15, 0, r2, c6, c1, 0
+	LDR		r2, [r1], #4
+	MCR		p15, 0, r2, c6, c1, 4
+	LDR		r2, [r1], #4
+	MCR		p15, 0, r2, c6, c1, 2
+	ADD		r0, r0, #1
+	CMP		r0, #16
+	BNE		mpu_restore_loop
+
+	MRC		p15, 0, r0, c1, c0, 0
+	LDR		r2, =(SCTLR_BR | SCTLR_M)
+	ORR		r0, r0, r2
+	LDR		r2, [r1], #4
+	TST		r2, #SCTLR_M
+	MCRNE	p15, 0, r0, c1, c0, 0
+
+	ENDM
+
+; /**********************************************************************/
+
+portCPU_RegisterSave MACRO
+
+	PUSH	{r0-r3, r12}
+
+	STMIA	r1, {r0-r12}
+	STR		lr, [r1, #(4*REG_R15)]
+	MRS		r2, SPSR
+	STR		r2, [r1, #(4*REG_CPSR)]
+
+#if CONFIG_ARCH_WITH_FPU
+	ADD		r3, r1, #(4*REG_S0)
+	VSTMIA	r3!, {s0-s31}
+	VMRS 	r2, fpscr
+	STR		r2, [r3], #4
+#endif
+
+	CPS		#SYS_MODE
+	ADD		r3, r1, #(4*REG_R13)
+	STMIA	r3!, {sp, lr}
+
+	CPS		#SVC_MODE
+	ADD		r1, r1, #XCPTCONTEXT_SIZE
+	portMPU_RegisterSave
+
+	PUSH	{lr}
+	LDR		r0, arch_clean_invalidate_dcache_all_const
+	BLX		r0
+	POP		{lr}
+
+	POP		{r0-r3, r12}
+	MOVS	pc, lr
+
+	ENDM
+
+; /**********************************************************************/
+
+portCPU_RegisterRestore MACRO
+
+#if CONFIG_ARCH_WITH_FPU
+	ADD		r3, r1, #(4*REG_S0)
+	VLDMIA	r3!, {s0-s31}
+	LDR		r2, [r3], #4
+	VMSR	fpscr, r2
+#endif
+
+	ADD		r4, r1, #(4*REG_R15)
+	LDMIA	r4, {r2, r3}
+	MOV		lr, r2
+	MSR		SPSR_cxsf, r3
+
+	CPS		#SYS_MODE
+	ADD		r4, r1, #(4*REG_R13)
+	LDMIA	r4, {r2, r3}
+	MOV		sp, r2
+	MOV		lr, r3
+
+	PUSH	{lr}
+	MOV		lr, r1
+
+	ADD		r1, r1, #XCPTCONTEXT_SIZE
+	portMPU_RegisterRestore
+
+	LDMIA	lr, {r0-r12}
+	POP		{lr}
+
+	CPS		#SVC_MODE
+	MOVS	pc, lr
+
+	ENDM
+