HT7315_MonitorSOC/sdk/Boot/Mt/init_vpe_s.S

/*
 * init_vpe1.S
 *
 *  Initialize the second vpe and additional TCs
*/
/*
Copyright (c) 2007-2018, MIPS Tech, LLC and/or its affiliated group companies or licensors

All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list
of conditions and the following disclaimer in the documentation and/or other materials
provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be
used to endorse or promote products derived from this software without specific prior
written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <mips/asm.h>
#include <boot.h>
#include <mips/mt.h>
#include <mips/regdef.h>
#include <mips/m32c0.h>

#define target_TC a3            // will hold the current TC being configured
#define target_VPE t0           // will hold the current VPE

/**************************************************************************************
**************************************************************************************/
LEAF(init_vpe_s)

   // each vpe will need to set up additional TC bound to it
   // If there are more TCs than VPEs the remaining TCs will be bound to
   // the highest numbered VPE

    beqz    r21_more_tcs, done_init_vpe   // return if there is no more TCs
    li      target_TC, 1

    li      target_VPE, 0                // preset the target VPE to 0
    beqz    r20_more_vpes, just_1_VPE    // If there is no other vpes then leave target_VPE set to 0
    li      target_VPE, 1                // Since there are more than 1 VPEs start with VPE1

just_1_VPE:

    // This is executing on TC0 bound to VPE0.  Therefore VPEConf0.MVP is already set.
    // Enable Virtual Processor Configuration to enter configuration mode
    mfc0    a0, C0_MVPCONTROL    // read C0_MVPCtl
    or      a0, (1 << 1)         // set VPC to enable Virtual Processor Configuration
    mtc0    a0, C0_MVPCONTROL    // write C0_MVPCtl
    ehb

    // Initialize target_TC, target_TC will be incremented at the
    // bottom of the loop if there are more TCs



nexttc:

    // Set TargTC in the CP0 VPECONTROL register
    // TargTC  Selects the TC number of the "other thread context" for any
    // Move to Thread Context or Move from Thread Context instructions
    // (any instructions that begin with mtt or mft)

    mfc0    a0, C0_VPECONTROL        // read C0_VPECTL
    ins     a0, target_TC, 0, 8      // insert TargTC
    mtc0    a0, C0_VPECONTROL        // write C0_VPECTL
    ehb

    // Halt target_TC being configured
    li       a0, 1            // set Halt bit
    mttc0    a0, C0_TCHALT    // write C0_TCHALT
    ehb

    // Set up TCStatus register:
    // Disable Coprocessor Usable bits
    // Disable MDMX/DSP ASE
    // Clear Dirty target_TC
    // not dynamically allocatable
    // not allocated
    // Kernel mode
    // interrupt exempt
    // ASID 0
    // NOTE: Only bit that needs to be set is IXMT
    li       a0, (1 << 10)      // set IXMT
    mttc0    a0, C0_TCSTATUS    // write C0_TCSTATUS of target TC

    // Initialize the target_TC's register file (NOTE: $2 is in the gpr of the tc executing  this code)
    // NOTE: Good practice but not programmatically necessary
    li $2, 0xdeadbeef
.set noat
    mttgpr    $2, $1
.set at
    mttgpr    $2, $2
    mttgpr    $2, $3
    mttgpr    $2, $4
    mttgpr    $2, $5
    mttgpr    $2, $6
    mttgpr    $2, $7
    mttgpr    $2, $8
    mttgpr    $2, $9
    mttgpr    $2, $10
    mttgpr    $2, $11
    mttgpr    $2, $12
    mttgpr    $2, $13
    mttgpr    $2, $14
    mttgpr    $2, $15
    mttgpr    $2, $16
    mttgpr    $2, $17
    mttgpr    $2, $18
    mttgpr    $2, $19
    mttgpr    $2, $20
    mttgpr    $2, $21
    mttgpr    $2, $22
    mttgpr    $2, $23
    mttgpr    $2, $24
    mttgpr    $2, $25
    mttgpr    $2, $26
    mttgpr    $2, $27
    mttgpr    $2, $28
    mttgpr    $2, $29
    mttgpr    $2, $30
    mttgpr    $2, $31

    // Bind TC to VPE context
    // All mftc0 and mttc0 instructions
    // will then operate on the VPE's CP0 registers

    mftc0   a1, C0_TCBIND           // Read C0_TCBind
    ins     a1, target_VPE, 0, 4    // insert vpe into CurVPE field
    mttc0   a1, C0_TCBIND           // write C0_TCBind

    // Only what one TC per VPE to execute. Since each TC will be assigned
    // to a VPE in order then when the TC number exceeds the VPE number
    // Don't continue to set it up to execute the boot code
    bgt     target_TC, target_VPE, check_for_more

    // setup to execute the boot code

    // Set XTC for target_TC (sets TC1 to be the only TC runnable on the VPE1)
    mftc0    a0, C0_VPECONF0      // read C0_VPECONF0
    ins      a0, target_TC, 21, 8 // insert XTC
    mttc0    a0, C0_VPECONF0      // write C0_VPECONF0

    // Disable multi-threading for VPE1
    mftc0    a0, C0_VPECONTROL    // read C0_VPECTL
    ins      a0, zero, 15, 1      // clear TE
    mttc0    a0, C0_VPECONTROL    // write C0_VPECTL

    // Just Clear VPA to prevent any TC bound to VPE from executing
    // and set master VPE so CP0 registers are writable
    mftc0    a0, C0_VPECONF0    // read C0_VPECONF0
    ins      a0, zero, 0, 1     // clear VPA
    or       a0, (1 << 1)       // set MVP
    mttc0    a0, C0_VPECONF0    // write C0_VPECONF0

    mfc0     a0, C0_STATUS      // read C0_STATUS
    mttc0    a0, C0_STATUS      // write C0_Status

    li       a0, 0x12345678
    mttc0    a0, C0_EPC        // write C0_EPC

    mttc0    zero, C0_CAUSE    // write C0_CAUSE

    mfc0     a0, C0_CONFIG    // read VPE 0 C0_CONFIG
    mttc0    a0, C0_CONFIG    // write it to VPE 1 C0_CONFIG

    mftc0     a0, C0_EBASE     // read C0_EBASE
    ext       a0, a0, 0, 10    // extract CPUNum
    mttgpr    a0, r23_cpu_num  // write CPUNum to GPR 23 of target TC

    // Load the C0_TCRESTART with the reset vector so each VPE will execute
    // the boot code
    la        a1, __reset_vector // load boot code starting address
    // If this is not a EVA or MPU systm then the __reset_vector address to
    // cached address vpe other than 0 of each core can execute cached as
    // it's L1 I$ has already been initialized and the L2$ has been initialized or "disabled".
    #if !defined(EVA) && !defined(MPU)
        ins     a1, zero, 29, 1  // Convert to cached kseg0 address in case we linked to kseg1.
    #endif
    mttc0   a1,  C0_TCRESTART // write C0_TCRestart so TC 1 on VPE 1
                 // will execute this boot code once EVP is set in C0_MVPCtl below

    // now set TC1 to take interrupts and set TC1 to active
    mftc0    a0, C0_TCSTATUS    // read C0_TCSTATUS
    ins      a0, zero, 10, 1    // insert IXMT to enable this TC to take interrupts
    li       a1, 1
    ins      a0, a1, 13, 1      // set A to Activate this TC
    mttc0    a0, C0_TCSTATUS    // write C0_TCSTATUS

    mftc0    a0, C0_VPECONF0    // read C0_VPECONF0
    ori      a0, 1              // set VPA (Virtual Processor Activated)
    mttc0    a0, C0_VPECONF0    // write C0_VPECONF0 so this TC is able to execute once EVP is set in C0_MVPCtl below

    // For this boot code only TC1 bound to VPE1 will be needed to execute (unhalted)
    // The other TCs if any will remain in a Halted state assuming they will later be
    // started by the OS.
    // Clear H in TCHalt to unhalt this TC
    // NOTE will not execute until EVP is set in C0_MVPCtl below
    mttc0    zero, C0_TCHALT

check_for_more:    // Done initializing VPE
    // If incrementing the target_VPE would be greater than or equal to higest vpe number
    // then all VPEs have a TC bound to them to execute. If there are any remaining
    // TCs bind them to the last VPE
    addu    a0, target_VPE, 1                // Set up a0 for the check
    beq     a0, r20_more_vpes, more_vpe
    bgt     a0, r20_more_vpes, more_tc
    // NOTE Never can be less than!

more_vpe:
    move    target_VPE, a0                    // else advance target_VPE and fall through to check for more TC's

more_tc:
    addu    target_TC, 1                      // advance TC number
    ble     target_TC, r21_more_tcs, nexttc   // if no more TCs then fall through

    // No more TC's to bind

    // Exit config mode
    mfc0    a0, C0_MVPCONTROL    // read C0_MVPCtl
    ori     a0, 1                // set EVP (Enable Virtual Processing) to enable execution by vpe1
    ins     a0, zero, 1, 1       // Clear VPC (VPE Configuration State) bit
    mtc0    a0, C0_MVPCONTROL    // write C0_MVPCtl
    ehb

done_init_vpe:

    jr   ra
END(init_vpe_s)

#undef target_TC
#undef target_VPE