[OpenSPARC-T2-DV] / verif / diag / assembly / include / kaos_handlers.s

/*
* ========== Copyright Header Begin ==========================================
* 
* OpenSPARC T2 Processor File: kaos_handlers.s
* Copyright (C) 1995-2007 Sun Microsystems, Inc. All Rights Reserved
* 4150 Network Circle, Santa Clara, California 95054, U.S.A.
*
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER. 
*
* 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; version 2 of the License.
*
* 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*
* For the avoidance of doubt, and except that if any non-GPL license 
* choice is available it will apply instead, Sun elects to use only 
* the General Public License version 2 (GPLv2) at this time for any 
* software where a choice of GPL license versions is made 
* available with the language indicating that GPLv2 or any later version 
* may be used, or where a choice of which version of the GPL is applied is 
* otherwise unspecified. 
*
* Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara, 
* CA 95054 USA or visit www.sun.com if you need additional information or 
* have any questions. 
*
* 
* ========== Copyright Header End ============================================
*/
attr_text {
        Name = local_hypervisor_section_text,	
	VA=LOCAL_SUPERVISOR_SECTION_TEXT_VA, 
        hypervisor
}
	
	
SECTION local_hypervisor_section_text TEXT_VA=LOCAL_SUPERVISOR_SECTION_TEXT_VA
.seg "text"
.global	toggle_tte_w_handler
toggle_tte_w_handler:
 	setx    0xfffff000, %g2, %g3
	and	%g6, %g3, %g6
	or	%g6, 0x44, %g6	
#ifdef CHECK_SFSR_SFAR
        mov     0x18, %g7
        ldxa    [%g7] ASI_PARTITION_ID, %g2         ! get sfsr
        mov     0x20, %g7
        ldxa    [%g7] ASI_PARTITION_ID, %g3         ! get sfar
#endif
toggle_tte_w_conf_0:
	! Load TSB_CONFIG address in %g7
        mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_0, %g7
	
toggle_tte_w_tsbptr_calc:	
!Uses g1-g5, Expects TSB_CONFIG address in %g7, va in %g6, REsults in %g1
!Leave %g6 intact
#include "mmu_ptr_calc.s"	
toggle_w_load_entry:
	setx	0x1fff, %g4, %g2			! 13 context-bits
	and	%g6, %g2, %g2
	sllx	%g2, 48, %g2
	srl	%g6, 22, %g4
	or	%g2, %g4, %g2			
	ldda	[%g1] ASI_NUCLEUS_QUAD_LDD, %g4		! load tte from ps0 tsb
	!ldxa	[%g0] ASI_DMMU_TAG_REG, %g2		! get va/context from tag-target
	cmp	%g2, %g4
	bne	%xcc, toggle_tte_w_check_for_ptr_chase
	nop
	ba	toggle_tte_w_trap_done
	mov	0x80, %g1
	
toggle_tte_w_check_for_ptr_chase:
    cmp %g4, -1         ! if all 1's, follow link
    be,a    %xcc, toggle_tte_w_ptr_chase
    nop

    !! Look up all config registers (1-3)
    !! Gotta do SW table walk through the 3 remaing configs ..
    ! Expect %g7 to stll have addr of last ctx0 config register
    ! and %g6 should have va/context from tag access register

    ! Normalize %g7
    sllx %g6, 51, %g5
    brnz,a %g5, 1f
    sub %g7, 0x20, %g7     ! this executes only if branch taken

1:

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1
    bl,a toggle_tte_w_tsbptr_calc
toggle_tte_w_conf_1:
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1, %g7

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2
    bl,a toggle_tte_w_tsbptr_calc
toggle_tte_w_conf_2:
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2, %g7

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3
    bl,a toggle_tte_w_tsbptr_calc
toggle_tte_w_conf_3:
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g7

#ifndef SUN4V
	sllx	%g5, 15, %g3		! extract size[2]
	srlx	%g3, 61, %g3
	sllx	%g5, 1, %g1		! extract size[1:0]
	srlx	%g1, 62, %g1
	or	%g3, %g1, %g1		! %g1 = size[2:0]
#else
	sllx	%g5, 61, %g1
	srlx	%g1, 61, %g1            ! %g1 = size[2:0]
#endif
	mulx	%g1, 3, %g1
	sub	%g0, 1, %g3
	sllx	%g3, 13, %g3
	sllx	%g3, %g1, %g3
        sethi   %hi(0x00001fff), %g1
        or      %g1, 0xfff, %g1
	or	%g3, %g1, %g3		! %g3 = va/ctxt mask based on size[2:0]

	and	%g2, %g3, %g3		! apply mask
	cmp	%g3, %g4		! check if va/ctxt match
	be	%xcc, toggle_tte_w_trap_done
	nop
	
toggle_tte_w_ptr_chase:
    or  %g5, %g0, %g3           ! %g3 is link-reg
toggle_tte_w_ptr_chase_loop:
    ldda    [%g3] ASI_NUCLEUS_QUAD_LDD, %g4     ! load tte from tsb
    ldxa    [%g0] ASI_DMMU_TAG_REG, %g2     ! get va/context from tag-target
    cmp %g2, %g4
    bne %xcc, toggle_tte_w_follow_link
    nop
    ba  toggle_tte_w_trap_done
    mov ASI_PARTITION_ID_VAL, %g1       ! offset (VA) for patrition id

toggle_tte_w_follow_link:
    ldx [%g3+16], %g3
    cmp %g3, -1
    bne %xcc, toggle_tte_w_ptr_chase_loop ! keep chasing pointer
    nop

toggle_tte_w_next_tsb:
    ! Look up the Next TSB, until done with all TSBs ?
    ! Compare with TSB_CONFIG_3 for ctx0 and TSB_CONFIG_3+0x20 for ctx!0
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g4
    sllx %g6, 51, %g5
    brnz,a %g5, 2f
    sub %g7, 0x20, %g7
2:  
    cmp %g7, %g4
    bl 1b
    nop		

toggle_tte_w_trap_done:
	! check to see if RA[39] is set.
	! RA[39] = 0 means accessing memory space
	! RA[39] = 1 means accessing I/O space
	mov	%g5, %g3
	sllx	%g3, 24, %g3
	srlx	%g3, 63, %g3
	brnz	%g3, toggle_tte_w_skip_part_base	
	! add partition base to data-in
	setx	partition_base_list, %g3, %g2	! for partition base
	ldxa	[%g1] ASI_PARTITION_ID, %g3		! partition id
	sllx	%g3, 3, %g3		! offset - partition list
	ldx	[%g2 + %g3], %g1
	add	%g5, %g1, %g5
toggle_tte_w_skip_part_base:	
	mov	0x30, %g7

	mov	0x000, %g6		
	mov	0x40, %g1		! set W-bit
	
	xor	%g1, %g5, %g5
	!stxa	%g4, [ %g7 ] ASI_PARTITION_ID	! {tag-access, data-in}
	stxa	%g5, [ %g6 ] ASI_DTLB_DATA_IN
	retl
	ta	 T_CHANGE_NONHPRIV
!!	

	
toggle_tte_w_handler_ext:
	cmp	%g4, -1			! if all 1's, follow link
	be,a	%xcc, toggle_tte_w_ptr_chase
	mov	0, %g7			! remember ptr chase from ps0

    !! Look up all config registers (1-3)
    !! Gotta do SW table walk through the 3 remaing configs ..
    ! Expect %g7 to stll have addr of last ctx0 config register
    ! and %g6 should have va/context from tag access register

    ! Normalize %g7
    sllx %g6, 51, %g5
    brnz,a %g5, 1f
    sub %g7, 0x20, %g7     ! this executes only if branch taken

1:

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1
    bl,a toggle_tte_w_tsbptr_calc
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1, %g7

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2
    bl,a toggle_tte_w_tsbptr_calc
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2, %g7

    cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3
    bl,a toggle_tte_w_tsbptr_calc
    mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g7

    ! Now try other crazy stuff, which has not been debugged for N2
    ! TBD TBD TBD TBD TBD

#ifndef SUN4V
	sllx	%g5, 15, %g3		! extract size[2]
	srlx	%g3, 61, %g3
	sllx	%g5, 1, %g1		! extract size[1:0]
	srlx	%g1, 62, %g1
	or	%g3, %g1, %g1		! %g1 = size[2:0]
#else
	sllx	%g5, 61, %g1
	srlx	%g1, 61, %g1            ! %g1 = size[2:0]
#endif
	mulx	%g1, 3, %g1
	sub	%g0, 1, %g3
	sllx	%g3, 13, %g3
	sllx	%g3, %g1, %g3
    sethi   %hi(0x00001fff), %g1
    or      %g1, 0xfff, %g1
	or	%g3, %g1, %g3		! %g3 = va/ctxt mask based on size[2:0]

	and	%g2, %g3, %g3		! apply mask
	cmp	%g3, %g4		! check if va/ctxt match
	be,a	%xcc, toggle_tte_w_trap_done
	mov	0x80, %g1		! offset (VA) for patrition id


.global toggle_ic_handler	
toggle_ic_handler:	
	ldxa	[%g0]ASI_LSU_CONTROL, %g1
	membar  #LoadStore
	xor	%g1, 0x1, %g1
	stxa	%g1, [%g0]ASI_LSU_CONTROL
	membar	#Sync
	retl
	ta	 T_CHANGE_NONHPRIV
	
.global toggle_im_handler	
toggle_im_handler:	
	ldxa	[%g0]ASI_LSU_CONTROL, %g1
	membar  #LoadStore
	xor	%g1, 0x4, %g1
	stxa	%g1, [%g0]ASI_LSU_CONTROL
	membar	#Sync
	retl
	ta	 T_CHANGE_NONHPRIV

.global toggle_dc_handler
toggle_dc_handler:
        ldxa    [%g0]ASI_LSU_CONTROL, %g1
        membar  #LoadStore
        xor     %g1, 0x2, %g1
        stxa    %g1, [%g0]ASI_LSU_CONTROL
        membar  #Sync
        retl
        ta       T_CHANGE_NONHPRIV

.global itlb_demap_subr	
itlb_demap_random:
        membar  #LoadStore
        stxa    %g0, [%g1]ASI_IMMU_DEMAP
        membar  #Sync
        retl
        ta       T_CHANGE_NONHPRIV
	
.global itlb_demap_all
itlb_demap_all:
        membar  #LoadStore
	mov	0x80, %g3
	stxa	%g3, [%g3]ASI_IMMU_DEMAP
	membar  #Sync
        retl
        ta       T_CHANGE_NONHPRIV
	
.global dtlb_demap_all	
dtlb_demap_all:       
	membar  #LoadStore
	mov	0x80, %g3
	stxa	%g3, [%g3]ASI_DMMU_DEMAP
	membar  #Sync
        retl
        ta	T_CHANGE_NONHPRIV

.global dtlb_demap_random
dtlb_demap_random:
	membar  #LoadStore
	stxa	%g0, [%l7]ASI_DMMU_DEMAP
	membar  #Sync
        retl
        ta	T_CHANGE_NONHPRIV
			
.global inject_pe_handler
inject_pe_handler:	
	membar  #LoadStore
	ldxa	[%i0]ASI_ICACHE_INSTR, %g3
	xor	%g3, 0x1, %g3
	stxa	%g3, [%i0]ASI_ICACHE_INSTR
	membar  #Sync
        retl
        ta	T_CHANGE_NONHPRIV

.global itlb_demap
itlb_demap:
	membar  #LoadStore
	stxa    %g0, [%g3]ASI_IMMU_DEMAP
	membar  #Sync
	retl
        ta       T_CHANGE_NONHPRIV

.global dtlb_demap
dtlb_demap:
	membar  #LoadStore
	stxa    %g0, [%g3]ASI_DMMU_DEMAP
	membar  #Sync
	retl
        ta       T_CHANGE_NONHPRIV

.global park_thread
park_thread:
	set	ASI_CMP_CORE_RUNNING_STATUS, %g1
	ldxa	[%g1]ASI_CMP_CORE, %g2
	and	%g2, 0xff, %g2
	sub	%g2, 0xff, %g2
	brnz	%g2, 1f
	set	ASI_HYP_SCRATCHPAD_0, %g3
	ldxa	[%g3]ASI_HYP_SCRATCHPAD, %g1
	brnz	%g1, 1f
	set	ASI_CMP_CORE_RUNNING_W1C, %g1
	ta	0x2e
	cmp	%o1, 0
	bne	1f
	set	0xaa, %g2	
	stxa	%g2, [%g1]ASI_CMP_CORE
	set	0x1, %g2
	stxa	%g2, [%g3]ASI_HYP_SCRATCHPAD
	membar	#Sync
1:	retl
	ta       T_CHANGE_NONHPRIV
	
.global unpark_thread
unpark_thread:
	set	ASI_CMP_CORE_RUNNING_STATUS, %g1
	ldxa	[%g1]ASI_CMP_CORE, %g2
	and	%g2, 0xaa, %g2
	brnz	%g2, 1f	
	set	ASI_CMP_CORE_RUNNING_W1S, %g1
	set	0xaa, %g2
	stxa	%g2, [%g1]ASI_CMP_CORE
	set	ASI_HYP_SCRATCHPAD_0, %g3
	stxa	%g0, [%g3]ASI_HYP_SCRATCHPAD
	membar	#Sync
1:	retl
	ta       T_CHANGE_NONHPRIV

.global toggle_ic_direct_mode
toggle_ic_direct_mode:	
   	mov     ASI_LSU_DIAG, %g5 
    setx    sync_thread_data_start, %g2, %g1
    add     %g1, 0x8, %g1
2:  ! acquire sync lock    
    ldstub  [%g1], %g2
    tst     %g2
    bne     2b
    nop
    ! lock acquired
	ldxa	[%g5]ASI_DIAG, %g6 
	xor     %g6, 0x1, %g6	
	stxa	%g6, [%g5]ASI_DIAG 
	membar	#Sync
    ! release lock
    stb     %g0, [%g1]		
	retl
	ta       T_CHANGE_NONHPRIV
Commit	Line	Data
86530b38 AT	1	/*
	2	* ========== Copyright Header Begin ==========================================
	3	*
	4	* OpenSPARC T2 Processor File: kaos_handlers.s
	5	* Copyright (C) 1995-2007 Sun Microsystems, Inc. All Rights Reserved
	6	* 4150 Network Circle, Santa Clara, California 95054, U.S.A.
	7	*
	8	* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
	9	*
	10	* This program is free software; you can redistribute it and/or modify
	11	* it under the terms of the GNU General Public License as published by
	12	* the Free Software Foundation; version 2 of the License.
	13	*
	14	* This program is distributed in the hope that it will be useful,
	15	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	16	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	17	* GNU General Public License for more details.
	18	*
	19	* You should have received a copy of the GNU General Public License
	20	* along with this program; if not, write to the Free Software
	21	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
	22	*
	23	* For the avoidance of doubt, and except that if any non-GPL license
	24	* choice is available it will apply instead, Sun elects to use only
	25	* the General Public License version 2 (GPLv2) at this time for any
	26	* software where a choice of GPL license versions is made
	27	* available with the language indicating that GPLv2 or any later version
	28	* may be used, or where a choice of which version of the GPL is applied is
	29	* otherwise unspecified.
	30	*
	31	* Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
	32	* CA 95054 USA or visit www.sun.com if you need additional information or
	33	* have any questions.
	34	*
	35	*
	36	* ========== Copyright Header End ============================================
	37	*/
	38	attr_text {
	39	Name = local_hypervisor_section_text,
	40	VA=LOCAL_SUPERVISOR_SECTION_TEXT_VA,
	41	hypervisor
	42	}
	43
	44
	45	SECTION local_hypervisor_section_text TEXT_VA=LOCAL_SUPERVISOR_SECTION_TEXT_VA
	46	.seg "text"
	47	.global toggle_tte_w_handler
	48	toggle_tte_w_handler:
	49	setx 0xfffff000, %g2, %g3
	50	and %g6, %g3, %g6
	51	or %g6, 0x44, %g6
	52	#ifdef CHECK_SFSR_SFAR
	53	mov 0x18, %g7
	54	ldxa [%g7] ASI_PARTITION_ID, %g2 ! get sfsr
	55	mov 0x20, %g7
	56	ldxa [%g7] ASI_PARTITION_ID, %g3 ! get sfar
	57	#endif
	58	toggle_tte_w_conf_0:
	59	! Load TSB_CONFIG address in %g7
	60	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_0, %g7
	61
	62	toggle_tte_w_tsbptr_calc:
	63	!Uses g1-g5, Expects TSB_CONFIG address in %g7, va in %g6, REsults in %g1
	64	!Leave %g6 intact
65	#include "mmu_ptr_calc.s"
66	toggle_w_load_entry:
67	setx 0x1fff, %g4, %g2 ! 13 context-bits
68	and %g6, %g2, %g2
69	sllx %g2, 48, %g2
70	srl %g6, 22, %g4
71	or %g2, %g4, %g2
72	ldda [%g1] ASI_NUCLEUS_QUAD_LDD, %g4 ! load tte from ps0 tsb
73	!ldxa [%g0] ASI_DMMU_TAG_REG, %g2 ! get va/context from tag-target
74	cmp %g2, %g4
75	bne %xcc, toggle_tte_w_check_for_ptr_chase
76	nop
77	ba toggle_tte_w_trap_done
78	mov 0x80, %g1
79
80	toggle_tte_w_check_for_ptr_chase:
81	cmp %g4, -1 ! if all 1's, follow link
82	be,a %xcc, toggle_tte_w_ptr_chase
83	nop
84
85	!! Look up all config registers (1-3)
86	!! Gotta do SW table walk through the 3 remaing configs ..
87	! Expect %g7 to stll have addr of last ctx0 config register
88	! and %g6 should have va/context from tag access register
89
90	! Normalize %g7
91	sllx %g6, 51, %g5
92	brnz,a %g5, 1f
93	sub %g7, 0x20, %g7 ! this executes only if branch taken
94
95	1:
96
97	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1
98	bl,a toggle_tte_w_tsbptr_calc
99	toggle_tte_w_conf_1:
100	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1, %g7
101
102	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2
103	bl,a toggle_tte_w_tsbptr_calc
104	toggle_tte_w_conf_2:
105	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2, %g7
106
107	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3
108	bl,a toggle_tte_w_tsbptr_calc
109	toggle_tte_w_conf_3:
110	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g7
111
112	#ifndef SUN4V
113	sllx %g5, 15, %g3 ! extract size[2]
114	srlx %g3, 61, %g3
115	sllx %g5, 1, %g1 ! extract size[1:0]
116	srlx %g1, 62, %g1
117	or %g3, %g1, %g1 ! %g1 = size[2:0]
118	#else
119	sllx %g5, 61, %g1
120	srlx %g1, 61, %g1 ! %g1 = size[2:0]
121	#endif
122	mulx %g1, 3, %g1
123	sub %g0, 1, %g3
124	sllx %g3, 13, %g3
125	sllx %g3, %g1, %g3
126	sethi %hi(0x00001fff), %g1
127	or %g1, 0xfff, %g1
128	or %g3, %g1, %g3 ! %g3 = va/ctxt mask based on size[2:0]
129
130	and %g2, %g3, %g3 ! apply mask
131	cmp %g3, %g4 ! check if va/ctxt match
132	be %xcc, toggle_tte_w_trap_done
133	nop
134
135	toggle_tte_w_ptr_chase:
136	or %g5, %g0, %g3 ! %g3 is link-reg
137	toggle_tte_w_ptr_chase_loop:
138	ldda [%g3] ASI_NUCLEUS_QUAD_LDD, %g4 ! load tte from tsb
139	ldxa [%g0] ASI_DMMU_TAG_REG, %g2 ! get va/context from tag-target
140	cmp %g2, %g4
141	bne %xcc, toggle_tte_w_follow_link
142	nop
143	ba toggle_tte_w_trap_done
144	mov ASI_PARTITION_ID_VAL, %g1 ! offset (VA) for patrition id
145
146	toggle_tte_w_follow_link:
147	ldx [%g3+16], %g3
148	cmp %g3, -1
149	bne %xcc, toggle_tte_w_ptr_chase_loop ! keep chasing pointer
150	nop
151
152	toggle_tte_w_next_tsb:
153	! Look up the Next TSB, until done with all TSBs ?
154	! Compare with TSB_CONFIG_3 for ctx0 and TSB_CONFIG_3+0x20 for ctx!0
155	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g4
156	sllx %g6, 51, %g5
157	brnz,a %g5, 2f
158	sub %g7, 0x20, %g7
159	2:
160	cmp %g7, %g4
161	bl 1b
162	nop
163
164	toggle_tte_w_trap_done:
165	! check to see if RA[39] is set.
166	! RA[39] = 0 means accessing memory space
167	! RA[39] = 1 means accessing I/O space
168	mov %g5, %g3
169	sllx %g3, 24, %g3
170	srlx %g3, 63, %g3
171	brnz %g3, toggle_tte_w_skip_part_base
172	! add partition base to data-in
173	setx partition_base_list, %g3, %g2 ! for partition base
174	ldxa [%g1] ASI_PARTITION_ID, %g3 ! partition id
175	sllx %g3, 3, %g3 ! offset - partition list
176	ldx [%g2 + %g3], %g1
177	add %g5, %g1, %g5
178	toggle_tte_w_skip_part_base:
179	mov 0x30, %g7
180
181	mov 0x000, %g6
182	mov 0x40, %g1 ! set W-bit
183
184	xor %g1, %g5, %g5
185	!stxa %g4, [ %g7 ] ASI_PARTITION_ID ! {tag-access, data-in}
186	stxa %g5, [ %g6 ] ASI_DTLB_DATA_IN
187	retl
188	ta T_CHANGE_NONHPRIV
189	!!
190
191
192	toggle_tte_w_handler_ext:
193	cmp %g4, -1 ! if all 1's, follow link
194	be,a %xcc, toggle_tte_w_ptr_chase
195	mov 0, %g7 ! remember ptr chase from ps0
196
197	!! Look up all config registers (1-3)
198	!! Gotta do SW table walk through the 3 remaing configs ..
199	! Expect %g7 to stll have addr of last ctx0 config register
200	! and %g6 should have va/context from tag access register
201
202	! Normalize %g7
203	sllx %g6, 51, %g5
204	brnz,a %g5, 1f
205	sub %g7, 0x20, %g7 ! this executes only if branch taken
206
207	1:
208
209	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1
210	bl,a toggle_tte_w_tsbptr_calc
211	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_1, %g7
212
213	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2
214	bl,a toggle_tte_w_tsbptr_calc
215	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_2, %g7
216
217	cmp %g7, ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3
218	bl,a toggle_tte_w_tsbptr_calc
219	mov ASI_MMU_ZERO_CONTEXT_TSB_CONFIG_3, %g7
220
221	! Now try other crazy stuff, which has not been debugged for N2
222	! TBD TBD TBD TBD TBD
223
224	#ifndef SUN4V
225	sllx %g5, 15, %g3 ! extract size[2]
226	srlx %g3, 61, %g3
227	sllx %g5, 1, %g1 ! extract size[1:0]
228	srlx %g1, 62, %g1
229	or %g3, %g1, %g1 ! %g1 = size[2:0]
230	#else
231	sllx %g5, 61, %g1
232	srlx %g1, 61, %g1 ! %g1 = size[2:0]
233	#endif
234	mulx %g1, 3, %g1
235	sub %g0, 1, %g3
236	sllx %g3, 13, %g3
237	sllx %g3, %g1, %g3
238	sethi %hi(0x00001fff), %g1
239	or %g1, 0xfff, %g1
240	or %g3, %g1, %g3 ! %g3 = va/ctxt mask based on size[2:0]
241
242	and %g2, %g3, %g3 ! apply mask
243	cmp %g3, %g4 ! check if va/ctxt match
244	be,a %xcc, toggle_tte_w_trap_done
245	mov 0x80, %g1 ! offset (VA) for patrition id
246
247
248	.global toggle_ic_handler
249	toggle_ic_handler:
250	ldxa [%g0]ASI_LSU_CONTROL, %g1
251	membar #LoadStore
252	xor %g1, 0x1, %g1
253	stxa %g1, [%g0]ASI_LSU_CONTROL
254	membar #Sync
255	retl
256	ta T_CHANGE_NONHPRIV
257
258	.global toggle_im_handler
259	toggle_im_handler:
260	ldxa [%g0]ASI_LSU_CONTROL, %g1
261	membar #LoadStore
262	xor %g1, 0x4, %g1
263	stxa %g1, [%g0]ASI_LSU_CONTROL
264	membar #Sync
265	retl
266	ta T_CHANGE_NONHPRIV
267
268	.global toggle_dc_handler
269	toggle_dc_handler:
270	ldxa [%g0]ASI_LSU_CONTROL, %g1
271	membar #LoadStore
272	xor %g1, 0x2, %g1
273	stxa %g1, [%g0]ASI_LSU_CONTROL
274	membar #Sync
275	retl
276	ta T_CHANGE_NONHPRIV
277
278	.global itlb_demap_subr
279	itlb_demap_random:
280	membar #LoadStore
281	stxa %g0, [%g1]ASI_IMMU_DEMAP
282	membar #Sync
283	retl
284	ta T_CHANGE_NONHPRIV
285
286	.global itlb_demap_all
287	itlb_demap_all:
288	membar #LoadStore
289	mov 0x80, %g3
290	stxa %g3, [%g3]ASI_IMMU_DEMAP
291	membar #Sync
292	retl
293	ta T_CHANGE_NONHPRIV
294
295	.global dtlb_demap_all
296	dtlb_demap_all:
297	membar #LoadStore
298	mov 0x80, %g3
299	stxa %g3, [%g3]ASI_DMMU_DEMAP
300	membar #Sync
301	retl
302	ta T_CHANGE_NONHPRIV
303
304	.global dtlb_demap_random
305	dtlb_demap_random:
306	membar #LoadStore
307	stxa %g0, [%l7]ASI_DMMU_DEMAP
308	membar #Sync
309	retl
310	ta T_CHANGE_NONHPRIV
311
312	.global inject_pe_handler
313	inject_pe_handler:
314	membar #LoadStore
315	ldxa [%i0]ASI_ICACHE_INSTR, %g3
316	xor %g3, 0x1, %g3
317	stxa %g3, [%i0]ASI_ICACHE_INSTR
318	membar #Sync
319	retl
320	ta T_CHANGE_NONHPRIV
321
322	.global itlb_demap
323	itlb_demap:
324	membar #LoadStore
325	stxa %g0, [%g3]ASI_IMMU_DEMAP
326	membar #Sync
327	retl
328	ta T_CHANGE_NONHPRIV
329
330	.global dtlb_demap
331	dtlb_demap:
332	membar #LoadStore
333	stxa %g0, [%g3]ASI_DMMU_DEMAP
334	membar #Sync
335	retl
336	ta T_CHANGE_NONHPRIV
337
338	.global park_thread
339	park_thread:
340	set ASI_CMP_CORE_RUNNING_STATUS, %g1
341	ldxa [%g1]ASI_CMP_CORE, %g2
342	and %g2, 0xff, %g2
343	sub %g2, 0xff, %g2
344	brnz %g2, 1f
345	set ASI_HYP_SCRATCHPAD_0, %g3
346	ldxa [%g3]ASI_HYP_SCRATCHPAD, %g1
347	brnz %g1, 1f
348	set ASI_CMP_CORE_RUNNING_W1C, %g1
349	ta 0x2e
350	cmp %o1, 0
351	bne 1f
352	set 0xaa, %g2
353	stxa %g2, [%g1]ASI_CMP_CORE
354	set 0x1, %g2
355	stxa %g2, [%g3]ASI_HYP_SCRATCHPAD
356	membar #Sync
357	1: retl
358	ta T_CHANGE_NONHPRIV
359
360	.global unpark_thread
361	unpark_thread:
362	set ASI_CMP_CORE_RUNNING_STATUS, %g1
363	ldxa [%g1]ASI_CMP_CORE, %g2
364	and %g2, 0xaa, %g2
365	brnz %g2, 1f
366	set ASI_CMP_CORE_RUNNING_W1S, %g1
367	set 0xaa, %g2
368	stxa %g2, [%g1]ASI_CMP_CORE
369	set ASI_HYP_SCRATCHPAD_0, %g3
370	stxa %g0, [%g3]ASI_HYP_SCRATCHPAD
371	membar #Sync
372	1: retl
373	ta T_CHANGE_NONHPRIV
374
375	.global toggle_ic_direct_mode
376	toggle_ic_direct_mode:
377	mov ASI_LSU_DIAG, %g5
378	setx sync_thread_data_start, %g2, %g1
379	add %g1, 0x8, %g1
380	2: ! acquire sync lock
381	ldstub [%g1], %g2
382	tst %g2
383	bne 2b
384	nop
385	! lock acquired
386	ldxa [%g5]ASI_DIAG, %g6
387	xor %g6, 0x1, %g6
388	stxa %g6, [%g5]ASI_DIAG
389	membar #Sync
390	! release lock
391	stb %g0, [%g1]
392	retl
393	ta T_CHANGE_NONHPRIV
394