cps-vec.S 9.29 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
/*
 * Copyright (C) 2013 Imagination Technologies
 * Author: Paul Burton <paul.burton@imgtec.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 <asm/addrspace.h>
#include <asm/asm.h>
#include <asm/asm-offsets.h>
#include <asm/asmmacro.h>
#include <asm/cacheops.h>
16
#include <asm/eva.h>
17
#include <asm/mipsregs.h>
18
#include <asm/mipsmtregs.h>
19
#include <asm/pm.h>
20

21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
#define GCR_CL_COHERENCE_OFS	0x2008
#define GCR_CL_ID_OFS		0x2028

.extern mips_cm_base

.set noreorder

	/*
	 * Set dest to non-zero if the core supports the MT ASE, else zero. If
	 * MT is not supported then branch to nomt.
	 */
	.macro	has_mt	dest, nomt
	mfc0	\dest, CP0_CONFIG
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 1
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 2
	bgez	\dest, \nomt
	 mfc0	\dest, CP0_CONFIG, 3
	andi	\dest, \dest, MIPS_CONF3_MT
	beqz	\dest, \nomt
42
	 nop
43
	.endm
44 45 46 47 48 49

.section .text.cps-vec
.balign 0x1000

LEAF(mips_cps_core_entry)
	/*
50 51 52
	 * These first 12 bytes will be patched by cps_smp_setup to load the
	 * base address of the CM GCRs into register v1 and the CCA to use into
	 * register s0.
53 54
	 */
	.quad	0
55
	.word	0
56 57 58 59 60 61 62 63

	/* Check whether we're here due to an NMI */
	mfc0	k0, CP0_STATUS
	and	k0, k0, ST0_NMI
	beqz	k0, not_nmi
	 nop

	/* This is an NMI */
64
	PTR_LA	k0, nmi_handler
65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102
	jr	k0
	 nop

not_nmi:
	/* Setup Cause */
	li	t0, CAUSEF_IV
	mtc0	t0, CP0_CAUSE

	/* Setup Status */
	li	t0, ST0_CU1 | ST0_CU0
	mtc0	t0, CP0_STATUS

	/*
	 * Clear the bits used to index the caches. Note that the architecture
	 * dictates that writing to any of TagLo or TagHi selects 0 or 2 should
	 * be valid for all MIPS32 CPUs, even those for which said writes are
	 * unnecessary.
	 */
	mtc0	zero, CP0_TAGLO, 0
	mtc0	zero, CP0_TAGHI, 0
	mtc0	zero, CP0_TAGLO, 2
	mtc0	zero, CP0_TAGHI, 2
	ehb

	/* Primary cache configuration is indicated by Config1 */
	mfc0	v0, CP0_CONFIG, 1

	/* Detect I-cache line size */
	_EXT	t0, v0, MIPS_CONF1_IL_SHF, MIPS_CONF1_IL_SZ
	beqz	t0, icache_done
	 li	t1, 2
	sllv	t0, t1, t0

	/* Detect I-cache size */
	_EXT	t1, v0, MIPS_CONF1_IS_SHF, MIPS_CONF1_IS_SZ
	xori	t2, t1, 0x7
	beqz	t2, 1f
	 li	t3, 32
103
	addiu	t1, t1, 1
104 105 106
	sllv	t1, t3, t1
1:	/* At this point t1 == I-cache sets per way */
	_EXT	t2, v0, MIPS_CONF1_IA_SHF, MIPS_CONF1_IA_SZ
107
	addiu	t2, t2, 1
108 109 110
	mul	t1, t1, t0
	mul	t1, t1, t2

111
	li	a0, CKSEG0
112
	PTR_ADD	a1, a0, t1
113
1:	cache	Index_Store_Tag_I, 0(a0)
114
	PTR_ADD	a0, a0, t0
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129
	bne	a0, a1, 1b
	 nop
icache_done:

	/* Detect D-cache line size */
	_EXT	t0, v0, MIPS_CONF1_DL_SHF, MIPS_CONF1_DL_SZ
	beqz	t0, dcache_done
	 li	t1, 2
	sllv	t0, t1, t0

	/* Detect D-cache size */
	_EXT	t1, v0, MIPS_CONF1_DS_SHF, MIPS_CONF1_DS_SZ
	xori	t2, t1, 0x7
	beqz	t2, 1f
	 li	t3, 32
130
	addiu	t1, t1, 1
131 132 133
	sllv	t1, t3, t1
1:	/* At this point t1 == D-cache sets per way */
	_EXT	t2, v0, MIPS_CONF1_DA_SHF, MIPS_CONF1_DA_SZ
134
	addiu	t2, t2, 1
135 136 137
	mul	t1, t1, t0
	mul	t1, t1, t2

138
	li	a0, CKSEG0
139 140
	PTR_ADDU a1, a0, t1
	PTR_SUBU a1, a1, t0
141 142
1:	cache	Index_Store_Tag_D, 0(a0)
	bne	a0, a1, 1b
143
	 PTR_ADD a0, a0, t0
144 145
dcache_done:

146
	/* Set Kseg0 CCA to that in s0 */
147 148
	mfc0	t0, CP0_CONFIG
	ori	t0, 0x7
149 150
	xori	t0, 0x7
	or	t0, t0, s0
151 152 153 154 155
	mtc0	t0, CP0_CONFIG
	ehb

	/* Enter the coherent domain */
	li	t0, 0xff
156
	sw	t0, GCR_CL_COHERENCE_OFS(v1)
157 158 159
	ehb

	/* Jump to kseg0 */
160
	PTR_LA	t0, 1f
161 162 163
	jr	t0
	 nop

164 165 166 167 168 169
	/*
	 * We're up, cached & coherent. Perform any further required core-level
	 * initialisation.
	 */
1:	jal	mips_cps_core_init
	 nop
170

171 172 173
	/* Do any EVA initialization if necessary */
	eva_init

174
	/*
175 176
	 * Boot any other VPEs within this core that should be online, and
	 * deactivate this VPE if it should be offline.
177
	 */
178 179
	jal	mips_cps_boot_vpes
	 nop
180 181

	/* Off we go! */
182 183 184
	PTR_L	t1, VPEBOOTCFG_PC(v0)
	PTR_L	gp, VPEBOOTCFG_GP(v0)
	PTR_L	sp, VPEBOOTCFG_SP(v0)
185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220
	jr	t1
	 nop
	END(mips_cps_core_entry)

.org 0x200
LEAF(excep_tlbfill)
	b	.
	 nop
	END(excep_tlbfill)

.org 0x280
LEAF(excep_xtlbfill)
	b	.
	 nop
	END(excep_xtlbfill)

.org 0x300
LEAF(excep_cache)
	b	.
	 nop
	END(excep_cache)

.org 0x380
LEAF(excep_genex)
	b	.
	 nop
	END(excep_genex)

.org 0x400
LEAF(excep_intex)
	b	.
	 nop
	END(excep_intex)

.org 0x480
LEAF(excep_ejtag)
221
	PTR_LA	k0, ejtag_debug_handler
222 223 224
	jr	k0
	 nop
	END(excep_ejtag)
225 226

LEAF(mips_cps_core_init)
227
#ifdef CONFIG_MIPS_MT_SMP
228 229 230 231
	/* Check that the core implements the MT ASE */
	has_mt	t0, 3f

	.set	push
232
	.set	mips64r2
233 234 235 236 237 238 239
	.set	mt

	/* Only allow 1 TC per VPE to execute... */
	dmt

	/* ...and for the moment only 1 VPE */
	dvpe
240
	PTR_LA	t1, 1f
241 242 243 244 245 246 247 248 249 250 251 252
	jr.hb	t1
	 nop

	/* Enter VPE configuration state */
1:	mfc0	t0, CP0_MVPCONTROL
	ori	t0, t0, MVPCONTROL_VPC
	mtc0	t0, CP0_MVPCONTROL

	/* Retrieve the number of VPEs within the core */
	mfc0	t0, CP0_MVPCONF0
	srl	t0, t0, MVPCONF0_PVPE_SHIFT
	andi	t0, t0, (MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT)
253
	addiu	ta3, t0, 1
254 255 256 257 258 259

	/* If there's only 1, we're done */
	beqz	t0, 2f
	 nop

	/* Loop through each VPE within this core */
260
	li	ta1, 1
261 262

1:	/* Operate on the appropriate TC */
263
	mtc0	ta1, CP0_VPECONTROL
264 265 266
	ehb

	/* Bind TC to VPE (1:1 TC:VPE mapping) */
267
	mttc0	ta1, CP0_TCBIND
268 269 270

	/* Set exclusive TC, non-active, master */
	li	t0, VPECONF0_MVP
271
	sll	t1, ta1, VPECONF0_XTC_SHIFT
272 273 274 275 276 277 278 279 280 281 282
	or	t0, t0, t1
	mttc0	t0, CP0_VPECONF0

	/* Set TC non-active, non-allocatable */
	mttc0	zero, CP0_TCSTATUS

	/* Set TC halted */
	li	t0, TCHALT_H
	mttc0	t0, CP0_TCHALT

	/* Next VPE */
283 284
	addiu	ta1, ta1, 1
	slt	t0, ta1, ta3
285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
	bnez	t0, 1b
	 nop

	/* Leave VPE configuration state */
2:	mfc0	t0, CP0_MVPCONTROL
	xori	t0, t0, MVPCONTROL_VPC
	mtc0	t0, CP0_MVPCONTROL

3:	.set	pop
#endif
	jr	ra
	 nop
	END(mips_cps_core_init)

LEAF(mips_cps_boot_vpes)
	/* Retrieve CM base address */
301
	PTR_LA	t0, mips_cm_base
302
	PTR_L	t0, 0(t0)
303 304

	/* Calculate a pointer to this cores struct core_boot_config */
305
	lw	t0, GCR_CL_ID_OFS(t0)
306 307
	li	t1, COREBOOTCFG_SIZE
	mul	t0, t0, t1
308
	PTR_LA	t1, mips_cps_core_bootcfg
309 310
	PTR_L	t1, 0(t1)
	PTR_ADDU t0, t0, t1
311 312

	/* Calculate this VPEs ID. If the core doesn't support MT use 0 */
313
	li	t9, 0
314
#ifdef CONFIG_MIPS_MT_SMP
315
	has_mt	ta2, 1f
316 317 318 319 320

	/* Find the number of VPEs present in the core */
	mfc0	t1, CP0_MVPCONF0
	srl	t1, t1, MVPCONF0_PVPE_SHIFT
	andi	t1, t1, MVPCONF0_PVPE >> MVPCONF0_PVPE_SHIFT
321
	addiu	t1, t1, 1
322 323 324 325 326 327 328 329 330 331 332 333

	/* Calculate a mask for the VPE ID from EBase.CPUNum */
	clz	t1, t1
	li	t2, 31
	subu	t1, t2, t1
	li	t2, 1
	sll	t1, t2, t1
	addiu	t1, t1, -1

	/* Retrieve the VPE ID from EBase.CPUNum */
	mfc0	t9, $15, 1
	and	t9, t9, t1
334
#endif
335 336 337 338

1:	/* Calculate a pointer to this VPEs struct vpe_boot_config */
	li	t1, VPEBOOTCFG_SIZE
	mul	v0, t9, t1
339 340
	PTR_L	ta3, COREBOOTCFG_VPECONFIG(t0)
	PTR_ADDU v0, v0, ta3
341

342
#ifdef CONFIG_MIPS_MT_SMP
343 344

	/* If the core doesn't support MT then return */
345
	bnez	ta2, 1f
346 347 348 349 350
	 nop
	jr	ra
	 nop

	.set	push
351
	.set	mips64r2
352 353 354 355
	.set	mt

1:	/* Enter VPE configuration state */
	dvpe
356
	PTR_LA	t1, 1f
357 358 359 360 361 362 363 364
	jr.hb	t1
	 nop
1:	mfc0	t1, CP0_MVPCONTROL
	ori	t1, t1, MVPCONTROL_VPC
	mtc0	t1, CP0_MVPCONTROL
	ehb

	/* Loop through each VPE */
365
	PTR_L	ta2, COREBOOTCFG_VPEMASK(t0)
366 367
	move	t8, ta2
	li	ta1, 0
368 369

	/* Check whether the VPE should be running. If not, skip it */
370
1:	andi	t0, ta2, 1
371 372 373 374 375 376 377
	beqz	t0, 2f
	 nop

	/* Operate on the appropriate TC */
	mfc0	t0, CP0_VPECONTROL
	ori	t0, t0, VPECONTROL_TARGTC
	xori	t0, t0, VPECONTROL_TARGTC
378
	or	t0, t0, ta1
379 380 381 382 383 384 385 386 387 388
	mtc0	t0, CP0_VPECONTROL
	ehb

	/* Skip the VPE if its TC is not halted */
	mftc0	t0, CP0_TCHALT
	beqz	t0, 2f
	 nop

	/* Calculate a pointer to the VPEs struct vpe_boot_config */
	li	t0, VPEBOOTCFG_SIZE
389 390
	mul	t0, t0, ta1
	addu	t0, t0, ta3
391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427

	/* Set the TC restart PC */
	lw	t1, VPEBOOTCFG_PC(t0)
	mttc0	t1, CP0_TCRESTART

	/* Set the TC stack pointer */
	lw	t1, VPEBOOTCFG_SP(t0)
	mttgpr	t1, sp

	/* Set the TC global pointer */
	lw	t1, VPEBOOTCFG_GP(t0)
	mttgpr	t1, gp

	/* Copy config from this VPE */
	mfc0	t0, CP0_CONFIG
	mttc0	t0, CP0_CONFIG

	/* Ensure no software interrupts are pending */
	mttc0	zero, CP0_CAUSE
	mttc0	zero, CP0_STATUS

	/* Set TC active, not interrupt exempt */
	mftc0	t0, CP0_TCSTATUS
	li	t1, ~TCSTATUS_IXMT
	and	t0, t0, t1
	ori	t0, t0, TCSTATUS_A
	mttc0	t0, CP0_TCSTATUS

	/* Clear the TC halt bit */
	mttc0	zero, CP0_TCHALT

	/* Set VPE active */
	mftc0	t0, CP0_VPECONF0
	ori	t0, t0, VPECONF0_VPA
	mttc0	t0, CP0_VPECONF0

	/* Next VPE */
428 429 430
2:	srl	ta2, ta2, 1
	addiu	ta1, ta1, 1
	bnez	ta2, 1b
431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449
	 nop

	/* Leave VPE configuration state */
	mfc0	t1, CP0_MVPCONTROL
	xori	t1, t1, MVPCONTROL_VPC
	mtc0	t1, CP0_MVPCONTROL
	ehb
	evpe

	/* Check whether this VPE is meant to be running */
	li	t0, 1
	sll	t0, t0, t9
	and	t0, t0, t8
	bnez	t0, 2f
	 nop

	/* This VPE should be offline, halt the TC */
	li	t0, TCHALT_H
	mtc0	t0, CP0_TCHALT
450
	PTR_LA	t0, 1f
451 452 453 454 455
1:	jr.hb	t0
	 nop

2:	.set	pop

456
#endif /* CONFIG_MIPS_MT_SMP */
457 458 459 460 461

	/* Return */
	jr	ra
	 nop
	END(mips_cps_boot_vpes)
462 463 464 465 466 467 468 469 470

#if defined(CONFIG_MIPS_CPS_PM) && defined(CONFIG_CPU_PM)

	/* Calculate a pointer to this CPUs struct mips_static_suspend_state */
	.macro	psstate	dest
	.set	push
	.set	noat
	lw	$1, TI_CPU(gp)
	sll	$1, $1, LONGLOG
471
	PTR_LA	\dest, __per_cpu_offset
472 473
	addu	$1, $1, \dest
	lw	$1, 0($1)
474
	PTR_LA	\dest, cps_cpu_state
475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495
	addu	\dest, \dest, $1
	.set	pop
	.endm

LEAF(mips_cps_pm_save)
	/* Save CPU state */
	SUSPEND_SAVE_REGS
	psstate	t1
	SUSPEND_SAVE_STATIC
	jr	v0
	 nop
	END(mips_cps_pm_save)

LEAF(mips_cps_pm_restore)
	/* Restore CPU state */
	psstate	t1
	RESUME_RESTORE_STATIC
	RESUME_RESTORE_REGS_RETURN
	END(mips_cps_pm_restore)

#endif /* CONFIG_MIPS_CPS_PM && CONFIG_CPU_PM */