/*
 * 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 VPE_1 1

    .set    noat
/**************************************************************************************
**************************************************************************************/
LEAF(init_vpe1)

   // Each vpe will need to set up additional TC bound to it. (No rebinding.)

    beqz    r21_more_tcs, done_init_vpe1    // If there is no more TCs then return
    beqz    r20_more_vpes, done_init_vpe1   // If there is no vpe1 then return

    // 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
    li      target_TC, 1

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
    mttgpr    $2, $1
    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 TC1 and all other remaining TCs to VPE1 context
    // All mftc0 and mttc0 instructions
    // will then operate on the VPE1 CP0 registers
    li      a0, VPE_1
    mftc0   a1, C0_TCBIND    // Read C0_TCBind
    ins     a1, a0, 0, 4     // insert vpe 1 into CurVPE field
    mttc0   a1, C0_TCBIND    // write C0_TCBind

// Must only do next part up to check_for_more_TC label once
    bne     a0, target_TC, check_for_more_TC // branch if not TC1 NOTE: a0 set to 1 above

    // 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 (only tc1 can execute code)
    mttc0    a0, C0_VPECONTROL    // write C0_VPECTL

    // for VPE1  (Just Clear VPA to prevent any TC bound to it  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

    // vpe1 of each core can execute cached as it's L1 I$ has already been initialized.
    // and the L2$ has been initialized or "disabled" via CCA override.
    la       a1, __reset_vector // load boot code starting address
    #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

    // 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_TC:    // Done initializing VPE 1 if there was one

    addu    target_TC, 1                    // advance TC number
    sltu    a1, r21_more_tcs, target_TC     // set a1 if TC number is less than total # of TC in system
    beqz    a1, nexttc                      // go back and initialize another TC

    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

    // 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_vpe1:

    jr   ra
END(init_vpe1)

#undef target_TC
#undef VPE_1
