Subversion Repositories or1k_soc_on_altera_embedded_dev_kit

###############################################################################
#
# switch_to.S: context switch operation
#
# Copyright (C) 2003 Red Hat, Inc. All Rights Reserved.
# Written by David Howells (dhowells@redhat.com)
#
# 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.
#
###############################################################################

#include <linux/linkage.h>
#include <asm/thread_info.h>
#include <asm/processor.h>
#include <asm/registers.h>
#include <asm/spr-regs.h>

.macro LEDS val
        setlos          #~\val,gr27
        st              gr27,@(gr30,gr0)
        membar
        dcf             @(gr30,gr0)
.endm

        .section        .sdata
        .balign         8

        # address of frame 0 (userspace) on current kernel stack
        .globl          __kernel_frame0_ptr
__kernel_frame0_ptr:
        .long           init_thread_union + THREAD_SIZE - FRV_FRAME0_SIZE

        # address of current task
        .globl          __kernel_current_task
__kernel_current_task:
        .long           init_task

        .section        .text
        .balign         4

###############################################################################
#
# struct task_struct *__switch_to(struct thread_struct *prev_thread,
#                                 struct thread_struct *next_thread,
#                                 struct task_struct *prev)
#
###############################################################################
        .globl          __switch_to
__switch_to:
        # save outgoing process's context
        sethi.p         %hi(__switch_back),gr13
        setlo           %lo(__switch_back),gr13
        movsg           lr,gr12

        stdi            gr28,@(gr8,#__THREAD_FRAME)
        sti             sp  ,@(gr8,#__THREAD_SP)
        sti             fp  ,@(gr8,#__THREAD_FP)
        stdi            gr12,@(gr8,#__THREAD_LR)
        stdi            gr16,@(gr8,#__THREAD_GR(16))
        stdi            gr18,@(gr8,#__THREAD_GR(18))
        stdi            gr20,@(gr8,#__THREAD_GR(20))
        stdi            gr22,@(gr8,#__THREAD_GR(22))
        stdi            gr24,@(gr8,#__THREAD_GR(24))
        stdi.p          gr26,@(gr8,#__THREAD_GR(26))

        or              gr8,gr8,gr22
        ldi.p           @(gr8,#__THREAD_USER),gr8
        call            save_user_regs
        or              gr22,gr22,gr8
        
        # retrieve the new context
        sethi.p         %hi(__kernel_frame0_ptr),gr6
        setlo           %lo(__kernel_frame0_ptr),gr6
        movsg           psr,gr4

        lddi.p          @(gr9,#__THREAD_FRAME),gr10
        or              gr10,gr10,gr27          ; save prev for the return value

        ldi             @(gr11,#4),gr19         ; get new_current->thread_info

        lddi            @(gr9,#__THREAD_SP),gr12
        ldi             @(gr9,#__THREAD_LR),gr14
        ldi             @(gr9,#__THREAD_PC),gr18
        ldi.p           @(gr9,#__THREAD_FRAME0),gr7

        # actually switch kernel contexts with ordinary exceptions disabled
        andi            gr4,#~PSR_ET,gr5
        movgs           gr5,psr

        or.p            gr10,gr0,gr28           ; set __frame
        or              gr11,gr0,gr29           ; set __current
        or.p            gr12,gr0,sp
        or              gr13,gr0,fp
        or              gr19,gr0,gr15           ; set __current_thread_info

        sti             gr7,@(gr6,#0)           ; set __kernel_frame0_ptr
        sti             gr29,@(gr6,#4)          ; set __kernel_current_task

        movgs           gr14,lr
        bar

        srli            gr15,#28,gr5
        subicc          gr5,#0xc,gr0,icc0
        beq             icc0,#0,111f
        break
        nop
111:

        # jump to __switch_back or ret_from_fork as appropriate
        # - move prev to GR8
        movgs           gr4,psr
        jmpl.p          @(gr18,gr0)
        or              gr27,gr27,gr8

###############################################################################
#
# restore incoming process's context
# - on entry:
#   - SP, FP, LR, GR15, GR28 and GR29 will have been set up appropriately
#   - GR8 will point to the outgoing task_struct
#   - GR9 will point to the incoming thread_struct
#
###############################################################################
__switch_back:
        lddi            @(gr9,#__THREAD_GR(16)),gr16
        lddi            @(gr9,#__THREAD_GR(18)),gr18
        lddi            @(gr9,#__THREAD_GR(20)),gr20
        lddi            @(gr9,#__THREAD_GR(22)),gr22
        lddi            @(gr9,#__THREAD_GR(24)),gr24
        lddi            @(gr9,#__THREAD_GR(26)),gr26

        # fall through into restore_user_regs()
        ldi.p           @(gr9,#__THREAD_USER),gr8
        or              gr8,gr8,gr9

###############################################################################
#
# restore extra general regs and FP/Media regs
# - void *restore_user_regs(const struct user_context *target, void *retval)
# - on entry:
#   - GR8 will point to the user context to swap in
#   - GR9 will contain the value to be returned in GR8 (prev task on context switch)
#
###############################################################################
        .globl          restore_user_regs
restore_user_regs:
        movsg           hsr0,gr6
        ori             gr6,#HSR0_GRHE|HSR0_FRLE|HSR0_FRHE,gr6
        movgs           gr6,hsr0
        movsg           hsr0,gr6

        movsg           psr,gr7
        ori             gr7,#PSR_EF|PSR_EM,gr7
        movgs           gr7,psr
        movsg           psr,gr7
        srli            gr7,#24,gr7
        bar

        lddi            @(gr8,#__FPMEDIA_MSR(0)),gr4

        movgs           gr4,msr0
        movgs           gr5,msr1

        lddfi           @(gr8,#__FPMEDIA_ACC(0)),fr16
        lddfi           @(gr8,#__FPMEDIA_ACC(2)),fr18
        ldbfi           @(gr8,#__FPMEDIA_ACCG(0)),fr20
        ldbfi           @(gr8,#__FPMEDIA_ACCG(1)),fr21
        ldbfi           @(gr8,#__FPMEDIA_ACCG(2)),fr22
        ldbfi           @(gr8,#__FPMEDIA_ACCG(3)),fr23

        mwtacc          fr16,acc0
        mwtacc          fr17,acc1
        mwtacc          fr18,acc2
        mwtacc          fr19,acc3
        mwtaccg         fr20,accg0
        mwtaccg         fr21,accg1
        mwtaccg         fr22,accg2
        mwtaccg         fr23,accg3

        # some CPUs have extra ACCx and ACCGx regs and maybe FSRx regs
        subicc.p        gr7,#0x50,gr0,icc0
        subicc          gr7,#0x31,gr0,icc1
        beq             icc0,#0,__restore_acc_fr451
        beq             icc1,#0,__restore_acc_fr555
__restore_acc_cont:

        # some CPU's have GR32-GR63
        setlos          #HSR0_FRHE,gr4
        andcc           gr6,gr4,gr0,icc0
        beq             icc0,#1,__restore_skip_gr32_gr63

        lddi            @(gr8,#__INT_GR(32)),gr32
        lddi            @(gr8,#__INT_GR(34)),gr34
        lddi            @(gr8,#__INT_GR(36)),gr36
        lddi            @(gr8,#__INT_GR(38)),gr38
        lddi            @(gr8,#__INT_GR(40)),gr40
        lddi            @(gr8,#__INT_GR(42)),gr42
        lddi            @(gr8,#__INT_GR(44)),gr44
        lddi            @(gr8,#__INT_GR(46)),gr46
        lddi            @(gr8,#__INT_GR(48)),gr48
        lddi            @(gr8,#__INT_GR(50)),gr50
        lddi            @(gr8,#__INT_GR(52)),gr52
        lddi            @(gr8,#__INT_GR(54)),gr54
        lddi            @(gr8,#__INT_GR(56)),gr56
        lddi            @(gr8,#__INT_GR(58)),gr58
        lddi            @(gr8,#__INT_GR(60)),gr60
        lddi            @(gr8,#__INT_GR(62)),gr62
__restore_skip_gr32_gr63:

        # all CPU's have FR0-FR31
        lddfi           @(gr8,#__FPMEDIA_FR( 0)),fr0
        lddfi           @(gr8,#__FPMEDIA_FR( 2)),fr2
        lddfi           @(gr8,#__FPMEDIA_FR( 4)),fr4
        lddfi           @(gr8,#__FPMEDIA_FR( 6)),fr6
        lddfi           @(gr8,#__FPMEDIA_FR( 8)),fr8
        lddfi           @(gr8,#__FPMEDIA_FR(10)),fr10
        lddfi           @(gr8,#__FPMEDIA_FR(12)),fr12
        lddfi           @(gr8,#__FPMEDIA_FR(14)),fr14
        lddfi           @(gr8,#__FPMEDIA_FR(16)),fr16
        lddfi           @(gr8,#__FPMEDIA_FR(18)),fr18
        lddfi           @(gr8,#__FPMEDIA_FR(20)),fr20
        lddfi           @(gr8,#__FPMEDIA_FR(22)),fr22
        lddfi           @(gr8,#__FPMEDIA_FR(24)),fr24
        lddfi           @(gr8,#__FPMEDIA_FR(26)),fr26
        lddfi           @(gr8,#__FPMEDIA_FR(28)),fr28
        lddfi.p         @(gr8,#__FPMEDIA_FR(30)),fr30

        # some CPU's have FR32-FR63
        setlos          #HSR0_FRHE,gr4
        andcc           gr6,gr4,gr0,icc0
        beq             icc0,#1,__restore_skip_fr32_fr63

        lddfi           @(gr8,#__FPMEDIA_FR(32)),fr32
        lddfi           @(gr8,#__FPMEDIA_FR(34)),fr34
        lddfi           @(gr8,#__FPMEDIA_FR(36)),fr36
        lddfi           @(gr8,#__FPMEDIA_FR(38)),fr38
        lddfi           @(gr8,#__FPMEDIA_FR(40)),fr40
        lddfi           @(gr8,#__FPMEDIA_FR(42)),fr42
        lddfi           @(gr8,#__FPMEDIA_FR(44)),fr44
        lddfi           @(gr8,#__FPMEDIA_FR(46)),fr46
        lddfi           @(gr8,#__FPMEDIA_FR(48)),fr48
        lddfi           @(gr8,#__FPMEDIA_FR(50)),fr50
        lddfi           @(gr8,#__FPMEDIA_FR(52)),fr52
        lddfi           @(gr8,#__FPMEDIA_FR(54)),fr54
        lddfi           @(gr8,#__FPMEDIA_FR(56)),fr56
        lddfi           @(gr8,#__FPMEDIA_FR(58)),fr58
        lddfi           @(gr8,#__FPMEDIA_FR(60)),fr60
        lddfi           @(gr8,#__FPMEDIA_FR(62)),fr62
__restore_skip_fr32_fr63:

        lddi            @(gr8,#__FPMEDIA_FNER(0)),gr4
        movsg           fner0,gr4
        movsg           fner1,gr5
        or.p            gr9,gr9,gr8
        bralr

        # the FR451 also has ACC8-11/ACCG8-11 regs (but not 4-7...)
__restore_acc_fr451:
        lddfi           @(gr8,#__FPMEDIA_ACC(4)),fr16
        lddfi           @(gr8,#__FPMEDIA_ACC(6)),fr18
        ldbfi           @(gr8,#__FPMEDIA_ACCG(4)),fr20
        ldbfi           @(gr8,#__FPMEDIA_ACCG(5)),fr21
        ldbfi           @(gr8,#__FPMEDIA_ACCG(6)),fr22
        ldbfi           @(gr8,#__FPMEDIA_ACCG(7)),fr23

        mwtacc          fr16,acc8
        mwtacc          fr17,acc9
        mwtacc          fr18,acc10
        mwtacc          fr19,acc11
        mwtaccg         fr20,accg8
        mwtaccg         fr21,accg9
        mwtaccg         fr22,accg10
        mwtaccg         fr23,accg11
        bra             __restore_acc_cont

        # the FR555 also has ACC4-7/ACCG4-7 regs and an FSR0 reg
__restore_acc_fr555:
        lddfi           @(gr8,#__FPMEDIA_ACC(4)),fr16
        lddfi           @(gr8,#__FPMEDIA_ACC(6)),fr18
        ldbfi           @(gr8,#__FPMEDIA_ACCG(4)),fr20
        ldbfi           @(gr8,#__FPMEDIA_ACCG(5)),fr21
        ldbfi           @(gr8,#__FPMEDIA_ACCG(6)),fr22
        ldbfi           @(gr8,#__FPMEDIA_ACCG(7)),fr23

        mnop.p
        mwtacc          fr16,acc4
        mnop.p
        mwtacc          fr17,acc5
        mnop.p
        mwtacc          fr18,acc6
        mnop.p
        mwtacc          fr19,acc7
        mnop.p
        mwtaccg         fr20,accg4
        mnop.p
        mwtaccg         fr21,accg5
        mnop.p
        mwtaccg         fr22,accg6
        mnop.p
        mwtaccg         fr23,accg7

        ldi             @(gr8,#__FPMEDIA_FSR(0)),gr4
        movgs           gr4,fsr0

        bra             __restore_acc_cont


###############################################################################
#
# save extra general regs and FP/Media regs
# - void save_user_regs(struct user_context *target)
#
###############################################################################
        .globl          save_user_regs
save_user_regs:
        movsg           hsr0,gr6
        ori             gr6,#HSR0_GRHE|HSR0_FRLE|HSR0_FRHE,gr6
        movgs           gr6,hsr0
        movsg           hsr0,gr6

        movsg           psr,gr7
        ori             gr7,#PSR_EF|PSR_EM,gr7
        movgs           gr7,psr
        movsg           psr,gr7
        srli            gr7,#24,gr7
        bar

        movsg           fner0,gr4
        movsg           fner1,gr5
        stdi.p          gr4,@(gr8,#__FPMEDIA_FNER(0))

        # some CPU's have GR32-GR63
        setlos          #HSR0_GRHE,gr4
        andcc           gr6,gr4,gr0,icc0
        beq             icc0,#1,__save_skip_gr32_gr63

        stdi            gr32,@(gr8,#__INT_GR(32))
        stdi            gr34,@(gr8,#__INT_GR(34))
        stdi            gr36,@(gr8,#__INT_GR(36))
        stdi            gr38,@(gr8,#__INT_GR(38))
        stdi            gr40,@(gr8,#__INT_GR(40))
        stdi            gr42,@(gr8,#__INT_GR(42))
        stdi            gr44,@(gr8,#__INT_GR(44))
        stdi            gr46,@(gr8,#__INT_GR(46))
        stdi            gr48,@(gr8,#__INT_GR(48))
        stdi            gr50,@(gr8,#__INT_GR(50))
        stdi            gr52,@(gr8,#__INT_GR(52))
        stdi            gr54,@(gr8,#__INT_GR(54))
        stdi            gr56,@(gr8,#__INT_GR(56))
        stdi            gr58,@(gr8,#__INT_GR(58))
        stdi            gr60,@(gr8,#__INT_GR(60))
        stdi            gr62,@(gr8,#__INT_GR(62))
__save_skip_gr32_gr63:

        # all CPU's have FR0-FR31
        stdfi           fr0 ,@(gr8,#__FPMEDIA_FR( 0))
        stdfi           fr2 ,@(gr8,#__FPMEDIA_FR( 2))
        stdfi           fr4 ,@(gr8,#__FPMEDIA_FR( 4))
        stdfi           fr6 ,@(gr8,#__FPMEDIA_FR( 6))
        stdfi           fr8 ,@(gr8,#__FPMEDIA_FR( 8))
        stdfi           fr10,@(gr8,#__FPMEDIA_FR(10))
        stdfi           fr12,@(gr8,#__FPMEDIA_FR(12))
        stdfi           fr14,@(gr8,#__FPMEDIA_FR(14))
        stdfi           fr16,@(gr8,#__FPMEDIA_FR(16))
        stdfi           fr18,@(gr8,#__FPMEDIA_FR(18))
        stdfi           fr20,@(gr8,#__FPMEDIA_FR(20))
        stdfi           fr22,@(gr8,#__FPMEDIA_FR(22))
        stdfi           fr24,@(gr8,#__FPMEDIA_FR(24))
        stdfi           fr26,@(gr8,#__FPMEDIA_FR(26))
        stdfi           fr28,@(gr8,#__FPMEDIA_FR(28))
        stdfi.p         fr30,@(gr8,#__FPMEDIA_FR(30))

        # some CPU's have FR32-FR63
        setlos          #HSR0_FRHE,gr4
        andcc           gr6,gr4,gr0,icc0
        beq             icc0,#1,__save_skip_fr32_fr63

        stdfi           fr32,@(gr8,#__FPMEDIA_FR(32))
        stdfi           fr34,@(gr8,#__FPMEDIA_FR(34))
        stdfi           fr36,@(gr8,#__FPMEDIA_FR(36))
        stdfi           fr38,@(gr8,#__FPMEDIA_FR(38))
        stdfi           fr40,@(gr8,#__FPMEDIA_FR(40))
        stdfi           fr42,@(gr8,#__FPMEDIA_FR(42))
        stdfi           fr44,@(gr8,#__FPMEDIA_FR(44))
        stdfi           fr46,@(gr8,#__FPMEDIA_FR(46))
        stdfi           fr48,@(gr8,#__FPMEDIA_FR(48))
        stdfi           fr50,@(gr8,#__FPMEDIA_FR(50))
        stdfi           fr52,@(gr8,#__FPMEDIA_FR(52))
        stdfi           fr54,@(gr8,#__FPMEDIA_FR(54))
        stdfi           fr56,@(gr8,#__FPMEDIA_FR(56))
        stdfi           fr58,@(gr8,#__FPMEDIA_FR(58))
        stdfi           fr60,@(gr8,#__FPMEDIA_FR(60))
        stdfi           fr62,@(gr8,#__FPMEDIA_FR(62))
__save_skip_fr32_fr63:

        mrdacc          acc0 ,fr4
        mrdacc          acc1 ,fr5

        stdfi.p         fr4 ,@(gr8,#__FPMEDIA_ACC(0))

        mrdacc          acc2 ,fr6
        mrdacc          acc3 ,fr7

        stdfi.p         fr6 ,@(gr8,#__FPMEDIA_ACC(2))

        mrdaccg         accg0,fr4
        stbfi.p         fr4 ,@(gr8,#__FPMEDIA_ACCG(0))

        mrdaccg         accg1,fr5
        stbfi.p         fr5 ,@(gr8,#__FPMEDIA_ACCG(1))

        mrdaccg         accg2,fr6
        stbfi.p         fr6 ,@(gr8,#__FPMEDIA_ACCG(2))

        mrdaccg         accg3,fr7
        stbfi           fr7 ,@(gr8,#__FPMEDIA_ACCG(3))

        movsg           msr0 ,gr4
        movsg           msr1 ,gr5

        stdi            gr4 ,@(gr8,#__FPMEDIA_MSR(0))

        # some CPUs have extra ACCx and ACCGx regs and maybe FSRx regs
        subicc.p        gr7,#0x50,gr0,icc0
        subicc          gr7,#0x31,gr0,icc1
        beq             icc0,#0,__save_acc_fr451
        beq             icc1,#0,__save_acc_fr555
__save_acc_cont:

        lddfi           @(gr8,#__FPMEDIA_FR(4)),fr4
        lddfi.p         @(gr8,#__FPMEDIA_FR(6)),fr6
        bralr

        # the FR451 also has ACC8-11/ACCG8-11 regs (but not 4-7...)
__save_acc_fr451:
        mrdacc          acc8 ,fr4
        mrdacc          acc9 ,fr5

        stdfi.p         fr4 ,@(gr8,#__FPMEDIA_ACC(4))

        mrdacc          acc10,fr6
        mrdacc          acc11,fr7

        stdfi.p         fr6 ,@(gr8,#__FPMEDIA_ACC(6))

        mrdaccg         accg8,fr4
        stbfi.p         fr4 ,@(gr8,#__FPMEDIA_ACCG(4))

        mrdaccg         accg9,fr5
        stbfi.p         fr5 ,@(gr8,#__FPMEDIA_ACCG(5))

        mrdaccg         accg10,fr6
        stbfi.p         fr6 ,@(gr8,#__FPMEDIA_ACCG(6))

        mrdaccg         accg11,fr7
        stbfi           fr7 ,@(gr8,#__FPMEDIA_ACCG(7))
        bra             __save_acc_cont

        # the FR555 also has ACC4-7/ACCG4-7 regs and an FSR0 reg
__save_acc_fr555:
        mnop.p
        mrdacc          acc4 ,fr4
        mnop.p
        mrdacc          acc5 ,fr5

        stdfi           fr4 ,@(gr8,#__FPMEDIA_ACC(4))

        mnop.p
        mrdacc          acc6 ,fr6
        mnop.p
        mrdacc          acc7 ,fr7

        stdfi           fr6 ,@(gr8,#__FPMEDIA_ACC(6))

        mnop.p
        mrdaccg         accg4,fr4
        stbfi           fr4 ,@(gr8,#__FPMEDIA_ACCG(4))

        mnop.p
        mrdaccg         accg5,fr5
        stbfi           fr5 ,@(gr8,#__FPMEDIA_ACCG(5))

        mnop.p
        mrdaccg         accg6,fr6
        stbfi           fr6 ,@(gr8,#__FPMEDIA_ACCG(6))

        mnop.p
        mrdaccg         accg7,fr7
        stbfi           fr7 ,@(gr8,#__FPMEDIA_ACCG(7))

        movsg           fsr0 ,gr4
        sti             gr4 ,@(gr8,#__FPMEDIA_FSR(0))
        bra             __save_acc_cont