URL https://opencores.org/ocsvn/openrisc/openrisc/trunk

Subversion Repositories openrisc

[/] [openrisc/] [trunk/] [rtos/] [ucos-ii/] [2.91/] [ucos-port/] [os_cpu_a.S] - Blame information for rev 523

Go to most recent revision | Details | Compare with Previous | View Log


/*
----------------------------------------------------------------------
    CHiPES Embedded RTR Systems Copyright (c) Tim Oliver 2002-2004
----------------------------------------------------------------------
 File   : os_cpu_a.S
 Author(s) :    Tim Oliver, timtimoliver@yahoo.co.uk
                Julius Baxter, julius@opencores.org
---------------------------[Description]------------------------------
        Start up file for OpenRISC Reference Platform
        Assembly code required for ORP port of MicroC/OS-II
 
        Macros :
         exception_vector name org
                load32i reg const
                store_context
                restore_context
 
        Internal Routines :
                _reset          Boot code installed at interrupt vector 0x100
                _start          Start up sequence
 
                _OSTickISR      Operating system timer tick interrupt service routine
                _OSStartHighRdy Starts the highest priority task that is available to run
                _OSCtxSwBP
                _OSCtxSw        Task switch
                _OSIntCtxSw     Task switch after interrupt
                _UserISR        Interrupt service routine template - requires code
                _align          Attempts to recover from memory alignment errors
                nest_not_one
 
        External Routines       :
                _OSIntExit
                _OSTimeTick
                _OSTaskSwHook
 
        External variables      :
                _OSIntNesting
                _OSRunning
                _OSPrioHighRdy
                _OSPrioCur
                _OSTCBCur
                _OSTCBHighRdy
 
        Interrupt Vectors Installed:
                0x100   _reset
                0x500   _OSTickISR
                0x600   _align
 
                0x800   _UserISR
 
                0xc00   _OSCtxSw
*/
 
/*
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
 
#include "spr-defs.h"
#include "board.h"
 
#define RED_ZONE_SIZE 128
#define STK_FRAME_SIZE (128+RED_ZONE_SIZE)
#define SPR_TTMR_LOAD  (SPR_TTMR_IE | SPR_TTMR_RT | ((IN_CLK/TICKS_PER_SEC) & SPR_TTMR_PERIOD))
 
/* function prototypes */
 
        .global  OSTickISR      /* internal functions */
        .global  OSStartHighRdy
        .global  OSCtxSwBP
        .global  OSCtxSw
        .global  OSIntCtxSw
        .global  UserISR
        .global  _align
        .global  nest_not_one
 
        .extern  OSIntExit      /* external functions */
        .extern  OSTimeTick
        .extern  OSTaskSwHook
 
        .extern  OSIntNesting   /* values */
        .extern  OSRunning
        .extern  OSPrioHighRdy
        .extern  OSPrioCur
        .extern  OSTCBCur
        .extern  OSTCBHighRdy
 
/* Macro Definitions */
/* Utility macro: Load a 32-bit constant into a register */
        .macro  load32i reg const
        l.movhi \reg,hi(\const)
        l.ori   \reg,\reg,lo(\const)
        .endm
 
/* Utility macro: Start code for an exception handler */
        .macro exception_vector name
        l.addi  r1,r1,-STK_FRAME_SIZE
        l.sw    0xc(r1),r3
        l.movhi r3,hi(\name)
        l.ori   r3,r3,lo(\name)
        l.jr    r3
        l.nop
 
        .endm
 
        .macro unhandled_exception
        l.ori r3, r0, 1
        l.j 0
        l.nop NOP_EXIT
 
        .endm
 
/* Utility macro: store the cpu context on the stack */
        .macro store_context
        l.sw    0x8(r1),r2
        l.sw    0x10(r1),r4
        l.sw    0x14(r1),r5
        l.sw    0x18(r1),r6
        l.sw    0x1c(r1),r7
        l.sw    0x20(r1),r8
        l.sw    0x24(r1),r9
        l.sw    0x28(r1),r10
        l.sw    0x2c(r1),r11
        l.sw    0x30(r1),r12
        l.sw    0x34(r1),r13
        l.sw    0x38(r1),r14
        l.sw    0x3c(r1),r15
        l.sw    0x40(r1),r16
        l.sw    0x44(r1),r17
        l.sw    0x48(r1),r18
        l.sw    0x4c(r1),r19
        l.sw    0x50(r1),r20
        l.sw    0x54(r1),r21
        l.sw    0x58(r1),r22
        l.sw    0x5c(r1),r23
        l.sw    0x60(r1),r24
        l.sw    0x64(r1),r25
        l.sw    0x68(r1),r26
        l.sw    0x6c(r1),r27
        l.sw    0x70(r1),r28
        l.sw    0x74(r1),r29
        l.sw    0x78(r1),r30
        l.sw    0x7c(r1),r31
        .endm
 
/* Utility macro: restore the cpu context from the stack */
        .macro restore_context
        l.lwz   r2,0x8(r1)
        l.lwz   r3,0xc(r1)
        l.lwz   r4,0x10(r1)
        l.lwz   r5,0x14(r1)
        l.lwz   r6,0x18(r1)
        l.lwz   r7,0x1c(r1)
        l.lwz   r8,0x20(r1)
        l.lwz   r9,0x24(r1)
        l.lwz   r10,0x28(r1)
        l.lwz   r11,0x2c(r1)
        l.lwz   r12,0x30(r1)
        l.lwz   r13,0x34(r1)
        l.lwz   r14,0x38(r1)
        l.lwz   r15,0x3c(r1)
        l.lwz   r16,0x40(r1)
        l.lwz   r17,0x44(r1)
        l.lwz   r18,0x48(r1)
        l.lwz   r19,0x4c(r1)
        l.lwz   r20,0x50(r1)
        l.lwz   r21,0x54(r1)
        l.lwz   r22,0x58(r1)
        l.lwz   r23,0x5c(r1)
        l.lwz   r24,0x60(r1)
        l.lwz   r25,0x64(r1)
        l.lwz   r26,0x68(r1)
        l.lwz   r27,0x6c(r1)
        l.lwz   r28,0x70(r1)
        l.lwz   r29,0x74(r1)
        l.lwz   r30,0x78(r1)
        l.lwz   r31,0x7c(r1)
        .endm
 
/* actual code */
 
        .section .stack, "aw", @nobits
 
.space  STACK_SIZE
_stack:
 
/* Exception vectors */
 
        .section .vectors, "ax"
 
        .org 0x100
_reset:
        l.movhi r0,0
        l.addi  r3,r0,SPR_SR_SM
        l.mtspr r0,r3,SPR_SR
        load32i r3,_start
        l.jr    r3
        l.nop
 
        .org 0x200
        unhandled_exception
 
        .org 0x300
        unhandled_exception
 
        .org 0x400
        unhandled_exception
 
        .org 0x500
        exception_vector OSTickISR
 
        .org 0x600
        exception_vector _align
 
        .org 0x700
        unhandled_exception
 
        .org 0x800
        exception_vector UserISR
 
        .org 0x900
        unhandled_exception
 
        .org 0xa00
        unhandled_exception
 
        .org 0xb00
        unhandled_exception
 
        .org 0xc00
        exception_vector OSCtxSw
 
 
        /* Start of text section */
 
        .section .text
 
        /* _start function - called immediately after reset */
_start:
 
        /* Instruction cache enable */
        /* Check if IC present and skip enabling otherwise */
        l.mfspr r24,r0,SPR_UPR
        l.andi  r26,r24,SPR_UPR_ICP
        l.sfeq  r26,r0
        l.bf    .L8
        l.nop
 
        /* Disable IC */
        l.mfspr r6,r0,SPR_SR
        l.addi  r5,r0,-1
        l.xori  r5,r5,SPR_SR_ICE
        l.and   r5,r6,r5
        l.mtspr r0,r5,SPR_SR
 
        /* Establish cache block size
        If BS=0, 16;
        If BS=1, 32;
        r14 contain block size
        */
        l.mfspr r24,r0,SPR_ICCFGR
        l.andi  r26,r24,SPR_ICCFGR_CBS
        l.srli  r28,r26,7
        l.ori   r30,r0,16
        l.sll   r14,r30,r28
 
        /* Establish number of cache sets
        r16 contains number of cache sets
        r28 contains log(# of cache sets)
        */
        l.andi  r26,r24,SPR_ICCFGR_NCS
        l.srli  r28,r26,3
        l.ori   r30,r0,1
        l.sll   r16,r30,r28
 
        /* Invalidate IC */
        l.addi  r6,r0,0
        l.sll   r5,r14,r28
 
.L7:
        l.mtspr r0,r6,SPR_ICBIR
        l.sfne  r6,r5
        l.bf    .L7
        l.add   r6,r6,r14
 
        /* Enable IC */
        l.mfspr r6,r0,SPR_SR
        l.ori   r6,r6,SPR_SR_ICE
        l.mtspr r0,r6,SPR_SR
        l.nop
        l.nop
        l.nop
        l.nop
        l.nop
        l.nop
        l.nop
        l.nop
 
.L8:
        /* Data cache enable */
        /* Check if DC present and skip enabling otherwise */
        l.mfspr r24,r0,SPR_UPR
        l.andi  r26,r24,SPR_UPR_DCP
        l.sfeq  r26,r0
        l.bf    .L10
        l.nop
        /* Disable DC */
        l.mfspr r6,r0,SPR_SR
        l.addi  r5,r0,-1
        l.xori  r5,r5,SPR_SR_DCE
        l.and   r5,r6,r5
        l.mtspr r0,r5,SPR_SR
        /* Establish cache block size
           If BS=0, 16;
           If BS=1, 32;
           r14 contain block size
        */
        l.mfspr r24,r0,SPR_DCCFGR
        l.andi  r26,r24,SPR_DCCFGR_CBS
        l.srli  r28,r26,7
        l.ori   r30,r0,16
        l.sll   r14,r30,r28
        /* Establish number of cache sets
           r16 contains number of cache sets
           r28 contains log(# of cache sets)
        */
        l.andi  r26,r24,SPR_DCCFGR_NCS
        l.srli  r28,r26,3
        l.ori   r30,r0,1
        l.sll   r16,r30,r28
        /* Invalidate DC */
        l.addi  r6,r0,0
        l.sll   r5,r14,r28
.L9:
        l.mtspr r0,r6,SPR_DCBIR
        l.sfne  r6,r5
        l.bf    .L9
        l.add   r6,r6,r14
        /* Enable DC */
        l.mfspr r6,r0,SPR_SR
        l.ori   r6,r6,SPR_SR_DCE
        l.mtspr r0,r6,SPR_SR
 
.L10:
 
        /* Clear BSS */
        load32i r28, ___bss_start
        load32i r30, __end
1:
        l.sw    (0)(r28), r0
        l.sfltu r28, r30
        l.bf    1b
        l.addi  r28, r28, 4
 
        /* Initialise stack pointer */
        load32i r1,_stack-4
 
        l.addi  r2,r0,-3
        l.and   r1,r1,r2
        l.ori   r2,r1,0
 
        load32i r3,main
 
        l.jr    r3
        l.addi  r3,r0,0
 
 
 
_align:
        l.ori   r3,r0,0x600
        l.nop   NOP_REPORT
        l.mfspr r3,r0,SPR_EPCR_BASE
        l.nop   NOP_REPORT
        l.lwz   r3,0(r3)
        l.nop   NOP_REPORT
        l.mfspr r3,r0,SPR_EEAR_BASE
        l.nop   NOP_REPORT
        /* Loop in place, cause simulator to exit */
        l.ori   r3,r0,1
        l.j     0
        l.nop   NOP_EXIT
 
/*
------------------------------
            OSCtxSw
------------------------------
 Description :
 This routine switches between two different tasks.
 The task state of one is saved on its kernel stack.
 Then the state of the other is restored from its kernel stack.
 
 There maybe memory management hardware issues
 
 Finally, we can return to the second task, via the 'return'.
 
 Includes OSIntCtxSw
------------------------------
 Uses:
------------------------------
*/
 
OSCtxSw:
        /* l.sys exception for now so we are in supervisor mode */
        /* exception  - recover pc from epcr */
 
        l.mfspr r3,r0,SPR_EPCR_BASE     /* save program counter that was put in exception register */
        l.sw    0(r1),r3
        l.mfspr r3,r0,SPR_ESR_BASE      /* save status register that was put in exception register */
        l.sw    4(r1),r3
 
        store_context
                                        /* Store current stack pointer   */
        load32i r3,OSTCBCur            /* r3= &OSTCBCur */
 
        l.lwz   r3,0(r3)                /* r3 = &CurrentTask.OSTCBStkPtr  */
        l.sw    0(r3),r1                /* CurrentTask.OSTCBStkPtr = SP */
 
OSIntCtxSw:
        l.jal   OSTaskSwHook           /* call OSTaskSwHook */
        l.nop
 
        load32i r2,OSTCBHighRdy        /* r2= &OSTCBHighRdy */
        l.lwz   r2,0(r2)
        load32i r3,OSTCBCur            /* r3= &OSTCBCur */
        l.sw    0(r3),r2                /* OSTCBCur = OSTCBHighRdy */
 
        load32i r3,OSPrioHighRdy       /* r3= &OSPrioHighRdy */
        l.lbz   r3, 0(r3)
        load32i r4,OSPrioCur           /* r4= &OSPrioCur */
        l.sb    0(r4), r3               /* OSPrioCur = OSPrioHighRdy */
 
        l.lwz   r1, 0(r2)               /* sp = OSTCBHighRdy */
 
 
        l.lwz   r2,0(r1)                /* load context for task to be resumed */
        l.mtspr r0,r2,SPR_EPCR_BASE
        l.lwz   r2,4(r1)
        l.mtspr r0,r2,SPR_ESR_BASE
 
        restore_context
 
OSCtxSwBP:
        l.addi  r1,r1,STK_FRAME_SIZE
        l.rfe
        l.nop
 
/*
------------------------------
        OSStartHighRdy
------------------------------
 Description :
   Starts the highest priority task that is available to run
 
 OSStartHighRdy() MUST:
  a) Call OSTaskSwHook() then,
  b) Set OSRunning to TRUE,
  c) Switch to the HPT
 
------------------------------
 Uses :
------------------------------
*/
OSStartHighRdy:
 
        l.jal   OSTaskSwHook           /* call OSTaskSwHook */
        l.nop
 
        load32i r3,OSRunning           /* r3= &OSRunning */
 
        l.ori   r4,r0, 0x01             /* set OSRunning == TRUE */
        l.sb    0(r3), r4
                                        /* load stack pointer from next task's TCB area  */
        load32i r3,OSTCBHighRdy        /* r3 = &OSTCBHighRdy */
        l.lwz   r3,0(r3)                /* r3 = &OS_TCB */
        l.lwz   r1, 0(r3)               /* stack is the first element */
 
        l.lwz   r2,0(r1)
        l.mtspr r0,r2,SPR_EPCR_BASE
        l.lwz   r2,4(r1)
        l.mtspr r0,r2,SPR_ESR_BASE
 
        restore_context
 
        l.addi  r1,r1,STK_FRAME_SIZE
        l.rfe
        l.nop
 
/*
------------------------------
        OSTickISR
------------------------------
 Description :
 
------------------------------
 Uses :
------------------------------
*/
OSTickISR:
        l.mfspr r3,r0,SPR_EPCR_BASE     /* save program counter that was put in exception register */
        l.sw    0(r1),r3
        l.mfspr r3,r0,SPR_ESR_BASE      /* save status register that was put in exception register */
        l.sw    4(r1),r3
 
        store_context
/* either call OSIntEnter or Increment OSIntNesting */
 
/*
        l.jal   OSIntEnter
        l.nop
        load32i r2,OSIntNesting
        l.lbz   r3,0(r2)
*/
        load32i r2,OSIntNesting     /* r2 &OSIntNesting */
        l.lbz   r3,0(r2)             /* r3 OSIntNesting  */
        l.addi  r3,r3,1
        l.sb    0(r2),r3
 
   /* if (OSIntNesting == 1) OSTCBCur->OSTCBStkPtr = sp */
        l.sfeqi r3,1
        l.bnf   nest_not_one
        l.nop
 
        load32i r4,OSTCBCur            /* set pointer to Current TCB pointer */
                                        /* r4= &OSTCBCur */
 
                                        /* Store current stack pointer   */
        l.lwz   r5,0(r4)                /* r5 = &CurrentTask.OSTCBStkPtr  */
        l.sw    0(r5),r1                /* CurrentTask.OSTCBStkPtr = SP */
 
nest_not_one:
        /* clear interrupt */
        load32i r3,SPR_TTMR_LOAD
        l.mtspr r0,r3,SPR_TTMR
 
        /* optionally re enable interrupt */
 
        /*Call OSTimeTick()*/
        l.jal   OSTimeTick
        l.nop
        /*Call OSIntExit()*/
        l.jal   OSIntExit
        l.nop
                                        /* load stack pointer from next task's TCB area  */
        load32i r3,OSTCBHighRdy        /* r3 = &OSTCBHighRdy */
        l.lwz   r3,0(r3)                /* r3 = &OS_TCB */
        l.lwz   r1, 0(r3)               /* stack is the first element */
 
        l.lwz   r2,0(r1)
        l.mtspr r0,r2,SPR_EPCR_BASE
        l.lwz   r2,4(r1)
        l.mtspr r0,r2,SPR_ESR_BASE
 
        restore_context
 
        l.addi  r1,r1,STK_FRAME_SIZE
        l.rfe
        l.nop
 
 
        /*
------------------------------
        UserISR
------------------------------
 Description :
 
------------------------------
 Uses :
------------------------------
*/
UserISR:
        l.mfspr r2,r0,SPR_EPCR_BASE     /* save program counter that was put in exception register */
        l.sw    0(r1),r2
        l.mfspr r2,r0,SPR_ESR_BASE      /* save status register that was put in exception register */
        l.sw    4(r1),r2
 
        store_context
/* either call OSIntEnter or Increment OSIntNesting */
 
/*
        l.jal   _OSIntEnter
        l.nop
        load32i r2,_OSIntNesting
        l.lbz   r3,0(r2)
*/
        load32i r2,OSIntNesting     /* r2 &OSIntNesting */
 
        l.lbz   r3,0(r2)             /* r3 OSIntNesting  */
        l.addi  r3,r3,1
        l.sb    0(r2),r3
 
   /* if (OSIntNesting == 1) OSTCBCur->OSTCBStkPtr = sp */
        l.sfeqi r3,1
        l.bnf   Unest_not_one
        l.nop
 
        load32i r4,OSTCBCur            /* set pointer to Current TCB pointer */
                                        /* r4= &OSTCBCur */
 
                                        /* Store current stack pointer   */
        l.lwz   r5,0(r4)                /* r5 = &CurrentTask.OSTCBStkPtr  */
        l.sw    0(r5),r1                /* CurrentTask.OSTCBStkPtr = SP */
 
Unest_not_one:
        /* clear interrupt */
        l.mtspr r0,r0,SPR_PICSR
 
        /* optionally re enable interrupt */
 
        /*Call interrupt service routine */
 
        /*Call OSIntExit()*/
        l.jal   OSIntExit
        l.nop
 
                                        /* load stack pointer from next task's TCB area  */
        load32i r3,OSTCBHighRdy        /* r3 = &OSTCBHighRdy */
        l.lwz   r3,0(r3)                /* r3 = &OS_TCB */
        l.lwz   r1, 0(r3)               /* stack is the first element */
 
        l.lwz   r2,0(r1)
        l.mtspr r0,r2,SPR_EPCR_BASE
        l.lwz   r2,4(r1)
        l.mtspr r0,r2,SPR_ESR_BASE
 
        restore_context
 
        l.addi  r1,r1,STK_FRAME_SIZE
        l.rfe
        l.nop

Browse

Tools

Subversion Repositories openrisc

[/] [openrisc/] [trunk/] [rtos/] [ucos-ii/] [2.91/] [ucos-port/] [os_cpu_a.S] - Blame information for rev 523

Line No.	Rev	Author	Line
1	471	julius	`/*`
2			`----------------------------------------------------------------------`
3			`CHiPES Embedded RTR Systems Copyright (c) Tim Oliver 2002-2004`
4			`----------------------------------------------------------------------`
5			`File : os_cpu_a.S`
6			`Author(s) : Tim Oliver, timtimoliver@yahoo.co.uk`
7			`Julius Baxter, julius@opencores.org`
8			`---------------------------[Description]------------------------------`
9			`Start up file for OpenRISC Reference Platform`
10			`Assembly code required for ORP port of MicroC/OS-II`
11
12			`Macros :`
13			`exception_vector name org`
14			`load32i reg const`
15			`store_context`
16			`restore_context`
17
18			`Internal Routines :`
19			`_reset Boot code installed at interrupt vector 0x100`
20			`_start Start up sequence`
21
22			`_OSTickISR Operating system timer tick interrupt service routine`
23			`_OSStartHighRdy Starts the highest priority task that is available to run`
24			`_OSCtxSwBP`
25			`_OSCtxSw Task switch`
26			`_OSIntCtxSw Task switch after interrupt`
27			`_UserISR Interrupt service routine template - requires code`
28			`_align Attempts to recover from memory alignment errors`
29			`nest_not_one`
30
31			`External Routines :`
32			`_OSIntExit`
33			`_OSTimeTick`
34			`_OSTaskSwHook`
35
36			`External variables :`
37			`_OSIntNesting`
38			`_OSRunning`
39			`_OSPrioHighRdy`
40			`_OSPrioCur`
41			`_OSTCBCur`
42			`_OSTCBHighRdy`
43
44			`Interrupt Vectors Installed:`
45			`0x100 _reset`
46			`0x500 _OSTickISR`
47			`0x600 _align`
48
49			`0x800 _UserISR`
50
51			`0xc00 _OSCtxSw`
52			`*/`
53
54			`/*`
55			`This program is free software; you can redistribute it and/or modify`
56			`it under the terms of the GNU General Public License as published by`
57			`the Free Software Foundation; either version 2 of the License, or`
58			`(at your option) any later version.`
59
60			`This program is distributed in the hope that it will be useful,`
61			`but WITHOUT ANY WARRANTY; without even the implied warranty of`
62			`MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the`
63			`GNU General Public License for more details.`
64
65			`You should have received a copy of the GNU General Public License`
66			`along with this program; if not, write to the Free Software`
67			`Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.`
68			`*/`
69
70			`#include "spr-defs.h"`
71			`#include "board.h"`
72
73			`#define RED_ZONE_SIZE 128`
74			`#define STK_FRAME_SIZE (128+RED_ZONE_SIZE)`
75			`#define SPR_TTMR_LOAD (SPR_TTMR_IE \| SPR_TTMR_RT \| ((IN_CLK/TICKS_PER_SEC) & SPR_TTMR_PERIOD))`
76
77			`/* function prototypes */`
78
79			`.global OSTickISR /* internal functions */`
80			`.global OSStartHighRdy`
81			`.global OSCtxSwBP`
82			`.global OSCtxSw`
83			`.global OSIntCtxSw`
84			`.global UserISR`
85			`.global _align`
86			`.global nest_not_one`
87
88			`.extern OSIntExit /* external functions */`
89			`.extern OSTimeTick`
90			`.extern OSTaskSwHook`
91
92			`.extern OSIntNesting /* values */`
93			`.extern OSRunning`
94			`.extern OSPrioHighRdy`
95			`.extern OSPrioCur`
96			`.extern OSTCBCur`
97			`.extern OSTCBHighRdy`
98
99			`/* Macro Definitions */`
100			`/* Utility macro: Load a 32-bit constant into a register */`
101			`.macro load32i reg const`
102			`l.movhi \reg,hi(\const)`
103			`l.ori \reg,\reg,lo(\const)`
104			`.endm`
105
106			`/* Utility macro: Start code for an exception handler */`
107			`.macro exception_vector name`
108			`l.addi r1,r1,-STK_FRAME_SIZE`
109			`l.sw 0xc(r1),r3`
110			`l.movhi r3,hi(\name)`
111			`l.ori r3,r3,lo(\name)`
112			`l.jr r3`
113			`l.nop`
114
115			`.endm`
116
117			`.macro unhandled_exception`
118			`l.ori r3, r0, 1`
119			`l.j 0`
120			`l.nop NOP_EXIT`
121
122			`.endm`
123
124			`/* Utility macro: store the cpu context on the stack */`
125			`.macro store_context`
126			`l.sw 0x8(r1),r2`
127			`l.sw 0x10(r1),r4`
128			`l.sw 0x14(r1),r5`
129			`l.sw 0x18(r1),r6`
130			`l.sw 0x1c(r1),r7`
131			`l.sw 0x20(r1),r8`
132			`l.sw 0x24(r1),r9`
133			`l.sw 0x28(r1),r10`
134			`l.sw 0x2c(r1),r11`
135			`l.sw 0x30(r1),r12`
136			`l.sw 0x34(r1),r13`
137			`l.sw 0x38(r1),r14`
138			`l.sw 0x3c(r1),r15`
139			`l.sw 0x40(r1),r16`
140			`l.sw 0x44(r1),r17`
141			`l.sw 0x48(r1),r18`
142			`l.sw 0x4c(r1),r19`
143			`l.sw 0x50(r1),r20`
144			`l.sw 0x54(r1),r21`
145			`l.sw 0x58(r1),r22`
146			`l.sw 0x5c(r1),r23`
147			`l.sw 0x60(r1),r24`
148			`l.sw 0x64(r1),r25`
149			`l.sw 0x68(r1),r26`
150			`l.sw 0x6c(r1),r27`
151			`l.sw 0x70(r1),r28`
152			`l.sw 0x74(r1),r29`
153			`l.sw 0x78(r1),r30`
154			`l.sw 0x7c(r1),r31`
155			`.endm`
156
157			`/* Utility macro: restore the cpu context from the stack */`
158			`.macro restore_context`
159			`l.lwz r2,0x8(r1)`
160			`l.lwz r3,0xc(r1)`
161			`l.lwz r4,0x10(r1)`
162			`l.lwz r5,0x14(r1)`
163			`l.lwz r6,0x18(r1)`
164			`l.lwz r7,0x1c(r1)`
165			`l.lwz r8,0x20(r1)`
166			`l.lwz r9,0x24(r1)`
167			`l.lwz r10,0x28(r1)`
168			`l.lwz r11,0x2c(r1)`
169			`l.lwz r12,0x30(r1)`
170			`l.lwz r13,0x34(r1)`
171			`l.lwz r14,0x38(r1)`
172			`l.lwz r15,0x3c(r1)`
173			`l.lwz r16,0x40(r1)`
174			`l.lwz r17,0x44(r1)`
175			`l.lwz r18,0x48(r1)`
176			`l.lwz r19,0x4c(r1)`
177			`l.lwz r20,0x50(r1)`
178			`l.lwz r21,0x54(r1)`
179			`l.lwz r22,0x58(r1)`
180			`l.lwz r23,0x5c(r1)`
181			`l.lwz r24,0x60(r1)`
182			`l.lwz r25,0x64(r1)`
183			`l.lwz r26,0x68(r1)`
184			`l.lwz r27,0x6c(r1)`
185			`l.lwz r28,0x70(r1)`
186			`l.lwz r29,0x74(r1)`
187			`l.lwz r30,0x78(r1)`
188			`l.lwz r31,0x7c(r1)`
189			`.endm`
190
191			`/* actual code */`
192
193			`.section .stack, "aw", @nobits`
194
195			`.space STACK_SIZE`
196			`_stack:`
197
198			`/* Exception vectors */`
199
200			`.section .vectors, "ax"`
201
202			`.org 0x100`
203			`_reset:`
204			`l.movhi r0,0`
205			`l.addi r3,r0,SPR_SR_SM`
206			`l.mtspr r0,r3,SPR_SR`
207			`load32i r3,_start`
208			`l.jr r3`
209			`l.nop`
210
211			`.org 0x200`
212			`unhandled_exception`
213
214			`.org 0x300`
215			`unhandled_exception`
216
217			`.org 0x400`
218			`unhandled_exception`
219
220			`.org 0x500`
221			`exception_vector OSTickISR`
222
223			`.org 0x600`
224			`exception_vector _align`
225
226			`.org 0x700`
227			`unhandled_exception`
228
229			`.org 0x800`
230			`exception_vector UserISR`
231
232			`.org 0x900`
233			`unhandled_exception`
234
235			`.org 0xa00`
236			`unhandled_exception`
237
238			`.org 0xb00`
239			`unhandled_exception`
240
241			`.org 0xc00`
242			`exception_vector OSCtxSw`
243
244
245			`/* Start of text section */`
246
247			`.section .text`
248
249			`/* _start function - called immediately after reset */`
250			`_start:`
251
252			`/* Instruction cache enable */`
253			`/* Check if IC present and skip enabling otherwise */`
254			`l.mfspr r24,r0,SPR_UPR`
255			`l.andi r26,r24,SPR_UPR_ICP`
256			`l.sfeq r26,r0`
257			`l.bf .L8`
258			`l.nop`
259
260			`/* Disable IC */`
261			`l.mfspr r6,r0,SPR_SR`
262			`l.addi r5,r0,-1`
263			`l.xori r5,r5,SPR_SR_ICE`
264			`l.and r5,r6,r5`
265			`l.mtspr r0,r5,SPR_SR`
266
267			`/* Establish cache block size`
268			`If BS=0, 16;`
269			`If BS=1, 32;`
270			`r14 contain block size`
271			`*/`
272			`l.mfspr r24,r0,SPR_ICCFGR`
273			`l.andi r26,r24,SPR_ICCFGR_CBS`
274			`l.srli r28,r26,7`
275			`l.ori r30,r0,16`
276			`l.sll r14,r30,r28`
277
278			`/* Establish number of cache sets`
279			`r16 contains number of cache sets`
280			`r28 contains log(# of cache sets)`
281			`*/`
282			`l.andi r26,r24,SPR_ICCFGR_NCS`
283			`l.srli r28,r26,3`
284			`l.ori r30,r0,1`
285			`l.sll r16,r30,r28`
286
287			`/* Invalidate IC */`
288			`l.addi r6,r0,0`
289			`l.sll r5,r14,r28`
290
291			`.L7:`
292			`l.mtspr r0,r6,SPR_ICBIR`
293			`l.sfne r6,r5`
294			`l.bf .L7`
295			`l.add r6,r6,r14`
296
297			`/* Enable IC */`
298			`l.mfspr r6,r0,SPR_SR`
299			`l.ori r6,r6,SPR_SR_ICE`
300			`l.mtspr r0,r6,SPR_SR`
301			`l.nop`
302			`l.nop`
303			`l.nop`
304			`l.nop`
305			`l.nop`
306			`l.nop`
307			`l.nop`
308			`l.nop`
309
310			`.L8:`
311			`/* Data cache enable */`
312			`/* Check if DC present and skip enabling otherwise */`
313			`l.mfspr r24,r0,SPR_UPR`
314			`l.andi r26,r24,SPR_UPR_DCP`
315			`l.sfeq r26,r0`
316			`l.bf .L10`
317			`l.nop`
318			`/* Disable DC */`
319			`l.mfspr r6,r0,SPR_SR`
320			`l.addi r5,r0,-1`
321			`l.xori r5,r5,SPR_SR_DCE`
322			`l.and r5,r6,r5`
323			`l.mtspr r0,r5,SPR_SR`
324			`/* Establish cache block size`
325			`If BS=0, 16;`
326			`If BS=1, 32;`
327			`r14 contain block size`
328			`*/`
329			`l.mfspr r24,r0,SPR_DCCFGR`
330			`l.andi r26,r24,SPR_DCCFGR_CBS`
331			`l.srli r28,r26,7`
332			`l.ori r30,r0,16`
333			`l.sll r14,r30,r28`
334			`/* Establish number of cache sets`
335			`r16 contains number of cache sets`
336			`r28 contains log(# of cache sets)`
337			`*/`
338			`l.andi r26,r24,SPR_DCCFGR_NCS`
339			`l.srli r28,r26,3`
340			`l.ori r30,r0,1`
341			`l.sll r16,r30,r28`
342			`/* Invalidate DC */`
343			`l.addi r6,r0,0`
344			`l.sll r5,r14,r28`
345			`.L9:`
346			`l.mtspr r0,r6,SPR_DCBIR`
347			`l.sfne r6,r5`
348			`l.bf .L9`
349			`l.add r6,r6,r14`
350			`/* Enable DC */`
351			`l.mfspr r6,r0,SPR_SR`
352			`l.ori r6,r6,SPR_SR_DCE`
353			`l.mtspr r0,r6,SPR_SR`
354
355			`.L10:`
356
357			`/* Clear BSS */`
358			`load32i r28, ___bss_start`
359			`load32i r30, __end`
360			`1:`
361			`l.sw (0)(r28), r0`
362			`l.sfltu r28, r30`
363			`l.bf 1b`
364			`l.addi r28, r28, 4`
365
366			`/* Initialise stack pointer */`
367			`load32i r1,_stack-4`
368
369			`l.addi r2,r0,-3`
370			`l.and r1,r1,r2`
371			`l.ori r2,r1,0`
372
373			`load32i r3,main`
374
375			`l.jr r3`
376			`l.addi r3,r0,0`
377
378
379
380			`_align:`
381			`l.ori r3,r0,0x600`
382			`l.nop NOP_REPORT`
383			`l.mfspr r3,r0,SPR_EPCR_BASE`
384			`l.nop NOP_REPORT`
385			`l.lwz r3,0(r3)`
386			`l.nop NOP_REPORT`
387			`l.mfspr r3,r0,SPR_EEAR_BASE`
388			`l.nop NOP_REPORT`
389			`/* Loop in place, cause simulator to exit */`
390			`l.ori r3,r0,1`
391			`l.j 0`
392			`l.nop NOP_EXIT`
393
394			`/*`
395			`------------------------------`
396			`OSCtxSw`
397			`------------------------------`
398			`Description :`
399			`This routine switches between two different tasks.`
400			`The task state of one is saved on its kernel stack.`
401			`Then the state of the other is restored from its kernel stack.`
402
403			`There maybe memory management hardware issues`
404
405			`Finally, we can return to the second task, via the 'return'.`
406
407			`Includes OSIntCtxSw`
408			`------------------------------`
409			`Uses:`
410			`------------------------------`
411			`*/`
412
413			`OSCtxSw:`
414			`/* l.sys exception for now so we are in supervisor mode */`
415			`/* exception - recover pc from epcr */`
416
417			`l.mfspr r3,r0,SPR_EPCR_BASE /* save program counter that was put in exception register */`
418			`l.sw 0(r1),r3`
419			`l.mfspr r3,r0,SPR_ESR_BASE /* save status register that was put in exception register */`
420			`l.sw 4(r1),r3`
421
422			`store_context`
423			`/* Store current stack pointer */`
424			`load32i r3,OSTCBCur /* r3= &OSTCBCur */`
425
426			`l.lwz r3,0(r3) /* r3 = &CurrentTask.OSTCBStkPtr */`
427			`l.sw 0(r3),r1 /* CurrentTask.OSTCBStkPtr = SP */`
428
429			`OSIntCtxSw:`
430			`l.jal OSTaskSwHook /* call OSTaskSwHook */`
431			`l.nop`
432
433			`load32i r2,OSTCBHighRdy /* r2= &OSTCBHighRdy */`
434			`l.lwz r2,0(r2)`
435			`load32i r3,OSTCBCur /* r3= &OSTCBCur */`
436			`l.sw 0(r3),r2 /* OSTCBCur = OSTCBHighRdy */`
437
438			`load32i r3,OSPrioHighRdy /* r3= &OSPrioHighRdy */`
439			`l.lbz r3, 0(r3)`
440			`load32i r4,OSPrioCur /* r4= &OSPrioCur */`
441			`l.sb 0(r4), r3 /* OSPrioCur = OSPrioHighRdy */`
442
443			`l.lwz r1, 0(r2) /* sp = OSTCBHighRdy */`
444
445
446			`l.lwz r2,0(r1) /* load context for task to be resumed */`
447			`l.mtspr r0,r2,SPR_EPCR_BASE`
448			`l.lwz r2,4(r1)`
449			`l.mtspr r0,r2,SPR_ESR_BASE`
450
451			`restore_context`
452
453			`OSCtxSwBP:`
454			`l.addi r1,r1,STK_FRAME_SIZE`
455			`l.rfe`
456			`l.nop`
457
458			`/*`
459			`------------------------------`
460			`OSStartHighRdy`
461			`------------------------------`
462			`Description :`
463			`Starts the highest priority task that is available to run`
464
465			`OSStartHighRdy() MUST:`
466			`a) Call OSTaskSwHook() then,`
467			`b) Set OSRunning to TRUE,`
468			`c) Switch to the HPT`
469
470			`------------------------------`
471			`Uses :`
472			`------------------------------`
473			`*/`
474			`OSStartHighRdy:`
475
476			`l.jal OSTaskSwHook /* call OSTaskSwHook */`
477			`l.nop`
478
479			`load32i r3,OSRunning /* r3= &OSRunning */`
480
481			`l.ori r4,r0, 0x01 /* set OSRunning == TRUE */`
482			`l.sb 0(r3), r4`
483			`/* load stack pointer from next task's TCB area */`
484			`load32i r3,OSTCBHighRdy /* r3 = &OSTCBHighRdy */`
485			`l.lwz r3,0(r3) /* r3 = &OS_TCB */`
486			`l.lwz r1, 0(r3) /* stack is the first element */`
487
488			`l.lwz r2,0(r1)`
489			`l.mtspr r0,r2,SPR_EPCR_BASE`
490			`l.lwz r2,4(r1)`
491			`l.mtspr r0,r2,SPR_ESR_BASE`
492
493			`restore_context`
494
495			`l.addi r1,r1,STK_FRAME_SIZE`
496			`l.rfe`
497			`l.nop`
498
499			`/*`
500			`------------------------------`
501			`OSTickISR`
502			`------------------------------`
503			`Description :`
504
505			`------------------------------`
506			`Uses :`
507			`------------------------------`
508			`*/`
509			`OSTickISR:`
510			`l.mfspr r3,r0,SPR_EPCR_BASE /* save program counter that was put in exception register */`
511			`l.sw 0(r1),r3`
512			`l.mfspr r3,r0,SPR_ESR_BASE /* save status register that was put in exception register */`
513			`l.sw 4(r1),r3`
514
515			`store_context`
516			`/* either call OSIntEnter or Increment OSIntNesting */`
517
518			`/*`
519			`l.jal OSIntEnter`
520			`l.nop`
521			`load32i r2,OSIntNesting`
522			`l.lbz r3,0(r2)`
523			`*/`
524			`load32i r2,OSIntNesting /* r2 &OSIntNesting */`
525			`l.lbz r3,0(r2) /* r3 OSIntNesting */`
526			`l.addi r3,r3,1`
527			`l.sb 0(r2),r3`
528
529			`/* if (OSIntNesting == 1) OSTCBCur->OSTCBStkPtr = sp */`
530			`l.sfeqi r3,1`
531			`l.bnf nest_not_one`
532			`l.nop`
533
534			`load32i r4,OSTCBCur /* set pointer to Current TCB pointer */`
535			`/* r4= &OSTCBCur */`
536
537			`/* Store current stack pointer */`
538			`l.lwz r5,0(r4) /* r5 = &CurrentTask.OSTCBStkPtr */`
539			`l.sw 0(r5),r1 /* CurrentTask.OSTCBStkPtr = SP */`
540
541			`nest_not_one:`
542			`/* clear interrupt */`
543			`load32i r3,SPR_TTMR_LOAD`
544			`l.mtspr r0,r3,SPR_TTMR`
545
546			`/* optionally re enable interrupt */`
547
548			`/Call OSTimeTick()/`
549			`l.jal OSTimeTick`
550			`l.nop`
551			`/Call OSIntExit()/`
552			`l.jal OSIntExit`
553			`l.nop`
554			`/* load stack pointer from next task's TCB area */`
555			`load32i r3,OSTCBHighRdy /* r3 = &OSTCBHighRdy */`
556			`l.lwz r3,0(r3) /* r3 = &OS_TCB */`
557			`l.lwz r1, 0(r3) /* stack is the first element */`
558
559			`l.lwz r2,0(r1)`
560			`l.mtspr r0,r2,SPR_EPCR_BASE`
561			`l.lwz r2,4(r1)`
562			`l.mtspr r0,r2,SPR_ESR_BASE`
563
564			`restore_context`
565
566			`l.addi r1,r1,STK_FRAME_SIZE`
567			`l.rfe`
568			`l.nop`
569
570
571			`/*`
572			`------------------------------`
573			`UserISR`
574			`------------------------------`
575			`Description :`
576
577			`------------------------------`
578			`Uses :`
579			`------------------------------`
580			`*/`
581			`UserISR:`
582			`l.mfspr r2,r0,SPR_EPCR_BASE /* save program counter that was put in exception register */`
583			`l.sw 0(r1),r2`
584			`l.mfspr r2,r0,SPR_ESR_BASE /* save status register that was put in exception register */`
585			`l.sw 4(r1),r2`
586
587			`store_context`
588			`/* either call OSIntEnter or Increment OSIntNesting */`
589
590			`/*`
591			`l.jal _OSIntEnter`
592			`l.nop`
593			`load32i r2,_OSIntNesting`
594			`l.lbz r3,0(r2)`
595			`*/`
596			`load32i r2,OSIntNesting /* r2 &OSIntNesting */`
597
598			`l.lbz r3,0(r2) /* r3 OSIntNesting */`
599			`l.addi r3,r3,1`
600			`l.sb 0(r2),r3`
601
602			`/* if (OSIntNesting == 1) OSTCBCur->OSTCBStkPtr = sp */`
603			`l.sfeqi r3,1`
604			`l.bnf Unest_not_one`
605			`l.nop`
606
607			`load32i r4,OSTCBCur /* set pointer to Current TCB pointer */`
608			`/* r4= &OSTCBCur */`
609
610			`/* Store current stack pointer */`
611			`l.lwz r5,0(r4) /* r5 = &CurrentTask.OSTCBStkPtr */`
612			`l.sw 0(r5),r1 /* CurrentTask.OSTCBStkPtr = SP */`
613
614			`Unest_not_one:`
615			`/* clear interrupt */`
616			`l.mtspr r0,r0,SPR_PICSR`
617
618			`/* optionally re enable interrupt */`
619
620			`/Call interrupt service routine /`
621
622			`/Call OSIntExit()/`
623			`l.jal OSIntExit`
624			`l.nop`
625
626			`/* load stack pointer from next task's TCB area */`
627			`load32i r3,OSTCBHighRdy /* r3 = &OSTCBHighRdy */`
628			`l.lwz r3,0(r3) /* r3 = &OS_TCB */`
629			`l.lwz r1, 0(r3) /* stack is the first element */`
630
631			`l.lwz r2,0(r1)`
632			`l.mtspr r0,r2,SPR_EPCR_BASE`
633			`l.lwz r2,4(r1)`
634			`l.mtspr r0,r2,SPR_ESR_BASE`
635
636			`restore_context`
637
638			`l.addi r1,r1,STK_FRAME_SIZE`
639			`l.rfe`
640			`l.nop`