1/*
   2 * This file contains miscellaneous low-level functions.
   3 *    Copyright (C) 1995-1996 Gary Thomas (gdt@linuxppc.org)
   4 *
   5 * Largely rewritten by Cort Dougan (cort@cs.nmt.edu)
   6 * and Paul Mackerras.
   7 *
   8 * kexec bits:
   9 * Copyright (C) 2002-2003 Eric Biederman  <ebiederm@xmission.com>
  10 * GameCube/ppc32 port Copyright (C) 2004 Albert Herranz
  11 * PPC44x port. Copyright (C) 2011,  IBM Corporation
  12 * 		Author: Suzuki Poulose <suzuki@in.ibm.com>
  13 *
  14 * This program is free software; you can redistribute it and/or
  15 * modify it under the terms of the GNU General Public License
  16 * as published by the Free Software Foundation; either version
  17 * 2 of the License, or (at your option) any later version.
  18 *
  19 */
  20
 
  21#include <linux/sys.h>
  22#include <asm/unistd.h>
  23#include <asm/errno.h>
  24#include <asm/reg.h>
  25#include <asm/page.h>
  26#include <asm/cache.h>
  27#include <asm/cputable.h>
  28#include <asm/mmu.h>
  29#include <asm/ppc_asm.h>
  30#include <asm/thread_info.h>
  31#include <asm/asm-offsets.h>
  32#include <asm/processor.h>
  33#include <asm/kexec.h>
  34#include <asm/bug.h>
  35#include <asm/ptrace.h>
  36#include <asm/export.h>
  37
  38	.text
  39
  40/*
  41 * We store the saved ksp_limit in the unused part
  42 * of the STACK_FRAME_OVERHEAD
  43 */
  44_GLOBAL(call_do_softirq)
  45	mflr	r0
  46	stw	r0,4(r1)
  47	lwz	r10,THREAD+KSP_LIMIT(r2)
  48	addi	r11,r3,THREAD_INFO_GAP
  49	stwu	r1,THREAD_SIZE-STACK_FRAME_OVERHEAD(r3)
  50	mr	r1,r3
  51	stw	r10,8(r1)
  52	stw	r11,THREAD+KSP_LIMIT(r2)
  53	bl	__do_softirq
  54	lwz	r10,8(r1)
  55	lwz	r1,0(r1)
  56	lwz	r0,4(r1)
  57	stw	r10,THREAD+KSP_LIMIT(r2)
  58	mtlr	r0
  59	blr
  60
  61/*
  62 * void call_do_irq(struct pt_regs *regs, struct thread_info *irqtp);
  63 */
  64_GLOBAL(call_do_irq)
  65	mflr	r0
  66	stw	r0,4(r1)
  67	lwz	r10,THREAD+KSP_LIMIT(r2)
  68	addi	r11,r4,THREAD_INFO_GAP
  69	stwu	r1,THREAD_SIZE-STACK_FRAME_OVERHEAD(r4)
  70	mr	r1,r4
  71	stw	r10,8(r1)
  72	stw	r11,THREAD+KSP_LIMIT(r2)
  73	bl	__do_irq
  74	lwz	r10,8(r1)
  75	lwz	r1,0(r1)
  76	lwz	r0,4(r1)
  77	stw	r10,THREAD+KSP_LIMIT(r2)
  78	mtlr	r0
  79	blr
  80
  81/*
  82 * This returns the high 64 bits of the product of two 64-bit numbers.
  83 */
  84_GLOBAL(mulhdu)
  85	cmpwi	r6,0
  86	cmpwi	cr1,r3,0
  87	mr	r10,r4
  88	mulhwu	r4,r4,r5
  89	beq	1f
  90	mulhwu	r0,r10,r6
  91	mullw	r7,r10,r5
  92	addc	r7,r0,r7
  93	addze	r4,r4
  941:	beqlr	cr1		/* all done if high part of A is 0 */
  95	mullw	r9,r3,r5
  96	mulhwu	r10,r3,r5
  97	beq	2f
  98	mullw	r0,r3,r6
  99	mulhwu	r8,r3,r6
 100	addc	r7,r0,r7
 101	adde	r4,r4,r8
 102	addze	r10,r10
 1032:	addc	r4,r4,r9
 104	addze	r3,r10
 105	blr
 106
 107/*
 108 * reloc_got2 runs through the .got2 section adding an offset
 109 * to each entry.
 110 */
 111_GLOBAL(reloc_got2)
 112	mflr	r11
 113	lis	r7,__got2_start@ha
 114	addi	r7,r7,__got2_start@l
 115	lis	r8,__got2_end@ha
 116	addi	r8,r8,__got2_end@l
 117	subf	r8,r7,r8
 118	srwi.	r8,r8,2
 119	beqlr
 120	mtctr	r8
 121	bl	1f
 1221:	mflr	r0
 123	lis	r4,1b@ha
 124	addi	r4,r4,1b@l
 125	subf	r0,r4,r0
 126	add	r7,r0,r7
 1272:	lwz	r0,0(r7)
 128	add	r0,r0,r3
 129	stw	r0,0(r7)
 130	addi	r7,r7,4
 131	bdnz	2b
 132	mtlr	r11
 133	blr
 134
 135/*
 136 * call_setup_cpu - call the setup_cpu function for this cpu
 137 * r3 = data offset, r24 = cpu number
 138 *
 139 * Setup function is called with:
 140 *   r3 = data offset
 141 *   r4 = ptr to CPU spec (relocated)
 142 */
 143_GLOBAL(call_setup_cpu)
 144	addis	r4,r3,cur_cpu_spec@ha
 145	addi	r4,r4,cur_cpu_spec@l
 146	lwz	r4,0(r4)
 147	add	r4,r4,r3
 148	lwz	r5,CPU_SPEC_SETUP(r4)
 149	cmpwi	0,r5,0
 150	add	r5,r5,r3
 151	beqlr
 152	mtctr	r5
 153	bctr
 154
 155#if defined(CONFIG_CPU_FREQ_PMAC) && defined(CONFIG_6xx)
 156
 157/* This gets called by via-pmu.c to switch the PLL selection
 158 * on 750fx CPU. This function should really be moved to some
 159 * other place (as most of the cpufreq code in via-pmu
 160 */
 161_GLOBAL(low_choose_750fx_pll)
 162	/* Clear MSR:EE */
 163	mfmsr	r7
 164	rlwinm	r0,r7,0,17,15
 165	mtmsr	r0
 166
 167	/* If switching to PLL1, disable HID0:BTIC */
 168	cmplwi	cr0,r3,0
 169	beq	1f
 170	mfspr	r5,SPRN_HID0
 171	rlwinm	r5,r5,0,27,25
 172	sync
 173	mtspr	SPRN_HID0,r5
 174	isync
 175	sync
 176
 1771:
 178	/* Calc new HID1 value */
 179	mfspr	r4,SPRN_HID1	/* Build a HID1:PS bit from parameter */
 180	rlwinm	r5,r3,16,15,15	/* Clear out HID1:PS from value read */
 181	rlwinm	r4,r4,0,16,14	/* Could have I used rlwimi here ? */
 182	or	r4,r4,r5
 183	mtspr	SPRN_HID1,r4
 184
 
 185	/* Store new HID1 image */
 186	CURRENT_THREAD_INFO(r6, r1)
 187	lwz	r6,TI_CPU(r6)
 188	slwi	r6,r6,2
 
 
 
 189	addis	r6,r6,nap_save_hid1@ha
 190	stw	r4,nap_save_hid1@l(r6)
 191
 192	/* If switching to PLL0, enable HID0:BTIC */
 193	cmplwi	cr0,r3,0
 194	bne	1f
 195	mfspr	r5,SPRN_HID0
 196	ori	r5,r5,HID0_BTIC
 197	sync
 198	mtspr	SPRN_HID0,r5
 199	isync
 200	sync
 201
 2021:
 203	/* Return */
 204	mtmsr	r7
 205	blr
 206
 207_GLOBAL(low_choose_7447a_dfs)
 208	/* Clear MSR:EE */
 209	mfmsr	r7
 210	rlwinm	r0,r7,0,17,15
 211	mtmsr	r0
 212	
 213	/* Calc new HID1 value */
 214	mfspr	r4,SPRN_HID1
 215	insrwi	r4,r3,1,9	/* insert parameter into bit 9 */
 216	sync
 217	mtspr	SPRN_HID1,r4
 218	sync
 219	isync
 220
 221	/* Return */
 222	mtmsr	r7
 223	blr
 224
 225#endif /* CONFIG_CPU_FREQ_PMAC && CONFIG_6xx */
 226
 227/*
 228 * complement mask on the msr then "or" some values on.
 229 *     _nmask_and_or_msr(nmask, value_to_or)
 230 */
 231_GLOBAL(_nmask_and_or_msr)
 232	mfmsr	r0		/* Get current msr */
 233	andc	r0,r0,r3	/* And off the bits set in r3 (first parm) */
 234	or	r0,r0,r4	/* Or on the bits in r4 (second parm) */
 235	SYNC			/* Some chip revs have problems here... */
 236	mtmsr	r0		/* Update machine state */
 237	isync
 238	blr			/* Done */
 239
 240#ifdef CONFIG_40x
 241
 242/*
 243 * Do an IO access in real mode
 244 */
 245_GLOBAL(real_readb)
 246	mfmsr	r7
 247	rlwinm	r0,r7,0,~MSR_DR
 248	sync
 249	mtmsr	r0
 250	sync
 251	isync
 252	lbz	r3,0(r3)
 253	sync
 254	mtmsr	r7
 255	sync
 256	isync
 257	blr
 
 258
 259	/*
 260 * Do an IO access in real mode
 261 */
 262_GLOBAL(real_writeb)
 263	mfmsr	r7
 264	rlwinm	r0,r7,0,~MSR_DR
 265	sync
 266	mtmsr	r0
 267	sync
 268	isync
 269	stb	r3,0(r4)
 270	sync
 271	mtmsr	r7
 272	sync
 273	isync
 274	blr
 
 275
 276#endif /* CONFIG_40x */
 277
 278
 279/*
 280 * Flush instruction cache.
 281 * This is a no-op on the 601.
 282 */
 283#ifndef CONFIG_PPC_8xx
 284_GLOBAL(flush_instruction_cache)
 285#if defined(CONFIG_4xx)
 286#ifdef CONFIG_403GCX
 287	li      r3, 512
 288	mtctr   r3
 289	lis     r4, KERNELBASE@h
 2901:	iccci   0, r4
 291	addi    r4, r4, 16
 292	bdnz    1b
 293#else
 294	lis	r3, KERNELBASE@h
 295	iccci	0,r3
 296#endif
 297#elif defined(CONFIG_FSL_BOOKE)
 298BEGIN_FTR_SECTION
 299	mfspr   r3,SPRN_L1CSR0
 300	ori     r3,r3,L1CSR0_CFI|L1CSR0_CLFC
 301	/* msync; isync recommended here */
 302	mtspr   SPRN_L1CSR0,r3
 303	isync
 304	blr
 305END_FTR_SECTION_IFSET(CPU_FTR_UNIFIED_ID_CACHE)
 306	mfspr	r3,SPRN_L1CSR1
 307	ori	r3,r3,L1CSR1_ICFI|L1CSR1_ICLFR
 308	mtspr	SPRN_L1CSR1,r3
 309#else
 310	mfspr	r3,SPRN_PVR
 311	rlwinm	r3,r3,16,16,31
 312	cmpwi	0,r3,1
 313	beqlr			/* for 601, do nothing */
 314	/* 603/604 processor - use invalidate-all bit in HID0 */
 315	mfspr	r3,SPRN_HID0
 316	ori	r3,r3,HID0_ICFI
 317	mtspr	SPRN_HID0,r3
 318#endif /* CONFIG_4xx */
 319	isync
 320	blr
 321EXPORT_SYMBOL(flush_instruction_cache)
 322#endif /* CONFIG_PPC_8xx */
 323
 324/*
 325 * Write any modified data cache blocks out to memory
 326 * and invalidate the corresponding instruction cache blocks.
 327 * This is a no-op on the 601.
 328 *
 329 * flush_icache_range(unsigned long start, unsigned long stop)
 330 */
 331_GLOBAL(flush_icache_range)
 332BEGIN_FTR_SECTION
 333	PURGE_PREFETCHED_INS
 334	blr				/* for 601, do nothing */
 335END_FTR_SECTION_IFSET(CPU_FTR_COHERENT_ICACHE)
 336	rlwinm	r3,r3,0,0,31 - L1_CACHE_SHIFT
 337	subf	r4,r3,r4
 338	addi	r4,r4,L1_CACHE_BYTES - 1
 339	srwi.	r4,r4,L1_CACHE_SHIFT
 340	beqlr
 341	mtctr	r4
 342	mr	r6,r3
 3431:	dcbst	0,r3
 344	addi	r3,r3,L1_CACHE_BYTES
 345	bdnz	1b
 346	sync				/* wait for dcbst's to get to ram */
 347#ifndef CONFIG_44x
 348	mtctr	r4
 3492:	icbi	0,r6
 350	addi	r6,r6,L1_CACHE_BYTES
 351	bdnz	2b
 352#else
 353	/* Flash invalidate on 44x because we are passed kmapped addresses and
 354	   this doesn't work for userspace pages due to the virtually tagged
 355	   icache.  Sigh. */
 356	iccci	0, r0
 357#endif
 358	sync				/* additional sync needed on g4 */
 359	isync
 360	blr
 361_ASM_NOKPROBE_SYMBOL(flush_icache_range)
 362EXPORT_SYMBOL(flush_icache_range)
 363
 364/*
 365 * Flush a particular page from the data cache to RAM.
 366 * Note: this is necessary because the instruction cache does *not*
 367 * snoop from the data cache.
 368 * This is a no-op on the 601 which has a unified cache.
 369 *
 370 *	void __flush_dcache_icache(void *page)
 371 */
 372_GLOBAL(__flush_dcache_icache)
 373BEGIN_FTR_SECTION
 374	PURGE_PREFETCHED_INS
 375	blr
 376END_FTR_SECTION_IFSET(CPU_FTR_COHERENT_ICACHE)
 377	rlwinm	r3,r3,0,0,31-PAGE_SHIFT		/* Get page base address */
 378	li	r4,PAGE_SIZE/L1_CACHE_BYTES	/* Number of lines in a page */
 379	mtctr	r4
 380	mr	r6,r3
 3810:	dcbst	0,r3				/* Write line to ram */
 382	addi	r3,r3,L1_CACHE_BYTES
 383	bdnz	0b
 384	sync
 385#ifdef CONFIG_44x
 386	/* We don't flush the icache on 44x. Those have a virtual icache
 387	 * and we don't have access to the virtual address here (it's
 388	 * not the page vaddr but where it's mapped in user space). The
 389	 * flushing of the icache on these is handled elsewhere, when
 390	 * a change in the address space occurs, before returning to
 391	 * user space
 392	 */
 393BEGIN_MMU_FTR_SECTION
 394	blr
 395END_MMU_FTR_SECTION_IFSET(MMU_FTR_TYPE_44x)
 396#endif /* CONFIG_44x */
 397	mtctr	r4
 3981:	icbi	0,r6
 399	addi	r6,r6,L1_CACHE_BYTES
 400	bdnz	1b
 401	sync
 402	isync
 403	blr
 404
 405#ifndef CONFIG_BOOKE
 406/*
 407 * Flush a particular page from the data cache to RAM, identified
 408 * by its physical address.  We turn off the MMU so we can just use
 409 * the physical address (this may be a highmem page without a kernel
 410 * mapping).
 411 *
 412 *	void __flush_dcache_icache_phys(unsigned long physaddr)
 413 */
 414_GLOBAL(__flush_dcache_icache_phys)
 415BEGIN_FTR_SECTION
 416	PURGE_PREFETCHED_INS
 417	blr					/* for 601, do nothing */
 418END_FTR_SECTION_IFSET(CPU_FTR_COHERENT_ICACHE)
 419	mfmsr	r10
 420	rlwinm	r0,r10,0,28,26			/* clear DR */
 421	mtmsr	r0
 422	isync
 423	rlwinm	r3,r3,0,0,31-PAGE_SHIFT		/* Get page base address */
 424	li	r4,PAGE_SIZE/L1_CACHE_BYTES	/* Number of lines in a page */
 425	mtctr	r4
 426	mr	r6,r3
 4270:	dcbst	0,r3				/* Write line to ram */
 428	addi	r3,r3,L1_CACHE_BYTES
 429	bdnz	0b
 430	sync
 431	mtctr	r4
 4321:	icbi	0,r6
 433	addi	r6,r6,L1_CACHE_BYTES
 434	bdnz	1b
 435	sync
 436	mtmsr	r10				/* restore DR */
 437	isync
 438	blr
 439#endif /* CONFIG_BOOKE */
 440
 441/*
 442 * Copy a whole page.  We use the dcbz instruction on the destination
 443 * to reduce memory traffic (it eliminates the unnecessary reads of
 444 * the destination into cache).  This requires that the destination
 445 * is cacheable.
 446 */
 447#define COPY_16_BYTES		\
 448	lwz	r6,4(r4);	\
 449	lwz	r7,8(r4);	\
 450	lwz	r8,12(r4);	\
 451	lwzu	r9,16(r4);	\
 452	stw	r6,4(r3);	\
 453	stw	r7,8(r3);	\
 454	stw	r8,12(r3);	\
 455	stwu	r9,16(r3)
 456
 457_GLOBAL(copy_page)
 
 458	addi	r3,r3,-4
 
 
 
 
 459	addi	r4,r4,-4
 460
 461	li	r5,4
 462
 463#if MAX_COPY_PREFETCH > 1
 464	li	r0,MAX_COPY_PREFETCH
 465	li	r11,4
 466	mtctr	r0
 46711:	dcbt	r11,r4
 468	addi	r11,r11,L1_CACHE_BYTES
 469	bdnz	11b
 470#else /* MAX_COPY_PREFETCH == 1 */
 471	dcbt	r5,r4
 472	li	r11,L1_CACHE_BYTES+4
 473#endif /* MAX_COPY_PREFETCH */
 474	li	r0,PAGE_SIZE/L1_CACHE_BYTES - MAX_COPY_PREFETCH
 475	crclr	4*cr0+eq
 4762:
 477	mtctr	r0
 4781:
 479	dcbt	r11,r4
 480	dcbz	r5,r3
 481	COPY_16_BYTES
 482#if L1_CACHE_BYTES >= 32
 483	COPY_16_BYTES
 484#if L1_CACHE_BYTES >= 64
 485	COPY_16_BYTES
 486	COPY_16_BYTES
 487#if L1_CACHE_BYTES >= 128
 488	COPY_16_BYTES
 489	COPY_16_BYTES
 490	COPY_16_BYTES
 491	COPY_16_BYTES
 492#endif
 493#endif
 494#endif
 495	bdnz	1b
 496	beqlr
 497	crnot	4*cr0+eq,4*cr0+eq
 498	li	r0,MAX_COPY_PREFETCH
 499	li	r11,4
 500	b	2b
 501EXPORT_SYMBOL(copy_page)
 502
 503/*
 504 * Extended precision shifts.
 505 *
 506 * Updated to be valid for shift counts from 0 to 63 inclusive.
 507 * -- Gabriel
 508 *
 509 * R3/R4 has 64 bit value
 510 * R5    has shift count
 511 * result in R3/R4
 512 *
 513 *  ashrdi3: arithmetic right shift (sign propagation)	
 514 *  lshrdi3: logical right shift
 515 *  ashldi3: left shift
 516 */
 517_GLOBAL(__ashrdi3)
 518	subfic	r6,r5,32
 519	srw	r4,r4,r5	# LSW = count > 31 ? 0 : LSW >> count
 520	addi	r7,r5,32	# could be xori, or addi with -32
 521	slw	r6,r3,r6	# t1 = count > 31 ? 0 : MSW << (32-count)
 522	rlwinm	r8,r7,0,32	# t3 = (count < 32) ? 32 : 0
 523	sraw	r7,r3,r7	# t2 = MSW >> (count-32)
 524	or	r4,r4,r6	# LSW |= t1
 525	slw	r7,r7,r8	# t2 = (count < 32) ? 0 : t2
 526	sraw	r3,r3,r5	# MSW = MSW >> count
 527	or	r4,r4,r7	# LSW |= t2
 528	blr
 529EXPORT_SYMBOL(__ashrdi3)
 530
 531_GLOBAL(__ashldi3)
 532	subfic	r6,r5,32
 533	slw	r3,r3,r5	# MSW = count > 31 ? 0 : MSW << count
 534	addi	r7,r5,32	# could be xori, or addi with -32
 535	srw	r6,r4,r6	# t1 = count > 31 ? 0 : LSW >> (32-count)
 536	slw	r7,r4,r7	# t2 = count < 32 ? 0 : LSW << (count-32)
 537	or	r3,r3,r6	# MSW |= t1
 538	slw	r4,r4,r5	# LSW = LSW << count
 539	or	r3,r3,r7	# MSW |= t2
 540	blr
 541EXPORT_SYMBOL(__ashldi3)
 542
 543_GLOBAL(__lshrdi3)
 544	subfic	r6,r5,32
 545	srw	r4,r4,r5	# LSW = count > 31 ? 0 : LSW >> count
 546	addi	r7,r5,32	# could be xori, or addi with -32
 547	slw	r6,r3,r6	# t1 = count > 31 ? 0 : MSW << (32-count)
 548	srw	r7,r3,r7	# t2 = count < 32 ? 0 : MSW >> (count-32)
 549	or	r4,r4,r6	# LSW |= t1
 550	srw	r3,r3,r5	# MSW = MSW >> count
 551	or	r4,r4,r7	# LSW |= t2
 552	blr
 553EXPORT_SYMBOL(__lshrdi3)
 554
 555/*
 556 * 64-bit comparison: __cmpdi2(s64 a, s64 b)
 557 * Returns 0 if a < b, 1 if a == b, 2 if a > b.
 558 */
 559_GLOBAL(__cmpdi2)
 560	cmpw	r3,r5
 561	li	r3,1
 562	bne	1f
 563	cmplw	r4,r6
 564	beqlr
 5651:	li	r3,0
 566	bltlr
 567	li	r3,2
 568	blr
 569EXPORT_SYMBOL(__cmpdi2)
 570/*
 571 * 64-bit comparison: __ucmpdi2(u64 a, u64 b)
 572 * Returns 0 if a < b, 1 if a == b, 2 if a > b.
 573 */
 574_GLOBAL(__ucmpdi2)
 575	cmplw	r3,r5
 576	li	r3,1
 577	bne	1f
 578	cmplw	r4,r6
 579	beqlr
 5801:	li	r3,0
 581	bltlr
 582	li	r3,2
 583	blr
 584EXPORT_SYMBOL(__ucmpdi2)
 585
 586_GLOBAL(__bswapdi2)
 587	rotlwi  r9,r4,8
 588	rotlwi  r10,r3,8
 589	rlwimi  r9,r4,24,0,7
 590	rlwimi  r10,r3,24,0,7
 591	rlwimi  r9,r4,24,16,23
 592	rlwimi  r10,r3,24,16,23
 593	mr      r3,r9
 594	mr      r4,r10
 595	blr
 596EXPORT_SYMBOL(__bswapdi2)
 597
 598#ifdef CONFIG_SMP
 599_GLOBAL(start_secondary_resume)
 600	/* Reset stack */
 601	CURRENT_THREAD_INFO(r1, r1)
 602	addi	r1,r1,THREAD_SIZE-STACK_FRAME_OVERHEAD
 603	li	r3,0
 604	stw	r3,0(r1)		/* Zero the stack frame pointer	*/
 605	bl	start_secondary
 606	b	.
 607#endif /* CONFIG_SMP */
 608	
 609/*
 610 * This routine is just here to keep GCC happy - sigh...
 611 */
 612_GLOBAL(__main)
 613	blr
 614
 615#ifdef CONFIG_KEXEC_CORE
 616	/*
 617	 * Must be relocatable PIC code callable as a C function.
 618	 */
 619	.globl relocate_new_kernel
 620relocate_new_kernel:
 621	/* r3 = page_list   */
 622	/* r4 = reboot_code_buffer */
 623	/* r5 = start_address      */
 624
 625#ifdef CONFIG_FSL_BOOKE
 626
 627	mr	r29, r3
 628	mr	r30, r4
 629	mr	r31, r5
 630
 631#define ENTRY_MAPPING_KEXEC_SETUP
 632#include "fsl_booke_entry_mapping.S"
 633#undef ENTRY_MAPPING_KEXEC_SETUP
 634
 635	mr      r3, r29
 636	mr      r4, r30
 637	mr      r5, r31
 638
 639	li	r0, 0
 640#elif defined(CONFIG_44x)
 641
 642	/* Save our parameters */
 643	mr	r29, r3
 644	mr	r30, r4
 645	mr	r31, r5
 646
 647#ifdef CONFIG_PPC_47x
 648	/* Check for 47x cores */
 649	mfspr	r3,SPRN_PVR
 650	srwi	r3,r3,16
 651	cmplwi	cr0,r3,PVR_476FPE@h
 652	beq	setup_map_47x
 653	cmplwi	cr0,r3,PVR_476@h
 654	beq	setup_map_47x
 655	cmplwi	cr0,r3,PVR_476_ISS@h
 656	beq	setup_map_47x
 657#endif /* CONFIG_PPC_47x */
 658	
 659/*
 660 * Code for setting up 1:1 mapping for PPC440x for KEXEC
 661 *
 662 * We cannot switch off the MMU on PPC44x.
 663 * So we:
 664 * 1) Invalidate all the mappings except the one we are running from.
 665 * 2) Create a tmp mapping for our code in the other address space(TS) and
 666 *    jump to it. Invalidate the entry we started in.
 667 * 3) Create a 1:1 mapping for 0-2GiB in chunks of 256M in original TS.
 668 * 4) Jump to the 1:1 mapping in original TS.
 669 * 5) Invalidate the tmp mapping.
 670 *
 671 * - Based on the kexec support code for FSL BookE
 672 *
 673 */
 674
 675	/* 
 676	 * Load the PID with kernel PID (0).
 677	 * Also load our MSR_IS and TID to MMUCR for TLB search.
 678	 */
 679	li	r3, 0
 680	mtspr	SPRN_PID, r3
 681	mfmsr	r4
 682	andi.	r4,r4,MSR_IS@l
 683	beq	wmmucr
 684	oris	r3,r3,PPC44x_MMUCR_STS@h
 685wmmucr:
 686	mtspr	SPRN_MMUCR,r3
 687	sync
 688
 689	/*
 690	 * Invalidate all the TLB entries except the current entry
 691	 * where we are running from
 692	 */
 693	bl	0f				/* Find our address */
 6940:	mflr	r5				/* Make it accessible */
 695	tlbsx	r23,0,r5			/* Find entry we are in */
 696	li	r4,0				/* Start at TLB entry 0 */
 697	li	r3,0				/* Set PAGEID inval value */
 6981:	cmpw	r23,r4				/* Is this our entry? */
 699	beq	skip				/* If so, skip the inval */
 700	tlbwe	r3,r4,PPC44x_TLB_PAGEID		/* If not, inval the entry */
 701skip:
 702	addi	r4,r4,1				/* Increment */
 703	cmpwi	r4,64				/* Are we done?	*/
 704	bne	1b				/* If not, repeat */
 705	isync
 706
 707	/* Create a temp mapping and jump to it */
 708	andi.	r6, r23, 1		/* Find the index to use */
 709	addi	r24, r6, 1		/* r24 will contain 1 or 2 */
 710
 711	mfmsr	r9			/* get the MSR */
 712	rlwinm	r5, r9, 27, 31, 31	/* Extract the MSR[IS] */
 713	xori	r7, r5, 1		/* Use the other address space */
 714
 715	/* Read the current mapping entries */
 716	tlbre	r3, r23, PPC44x_TLB_PAGEID
 717	tlbre	r4, r23, PPC44x_TLB_XLAT
 718	tlbre	r5, r23, PPC44x_TLB_ATTRIB
 719
 720	/* Save our current XLAT entry */
 721	mr	r25, r4
 722
 723	/* Extract the TLB PageSize */
 724	li	r10, 1 			/* r10 will hold PageSize */
 725	rlwinm	r11, r3, 0, 24, 27	/* bits 24-27 */
 726
 727	/* XXX: As of now we use 256M, 4K pages */
 728	cmpwi	r11, PPC44x_TLB_256M
 729	bne	tlb_4k
 730	rotlwi	r10, r10, 28		/* r10 = 256M */
 731	b	write_out
 732tlb_4k:
 733	cmpwi	r11, PPC44x_TLB_4K
 734	bne	default
 735	rotlwi	r10, r10, 12		/* r10 = 4K */
 736	b	write_out
 737default:
 738	rotlwi	r10, r10, 10		/* r10 = 1K */
 739
 740write_out:
 741	/*
 742	 * Write out the tmp 1:1 mapping for this code in other address space
 743	 * Fixup  EPN = RPN , TS=other address space
 744	 */
 745	insrwi	r3, r7, 1, 23		/* Bit 23 is TS for PAGEID field */
 746
 747	/* Write out the tmp mapping entries */
 748	tlbwe	r3, r24, PPC44x_TLB_PAGEID
 749	tlbwe	r4, r24, PPC44x_TLB_XLAT
 750	tlbwe	r5, r24, PPC44x_TLB_ATTRIB
 751
 752	subi	r11, r10, 1		/* PageOffset Mask = PageSize - 1 */
 753	not	r10, r11		/* Mask for PageNum */
 754
 755	/* Switch to other address space in MSR */
 756	insrwi	r9, r7, 1, 26		/* Set MSR[IS] = r7 */
 757
 758	bl	1f
 7591:	mflr	r8
 760	addi	r8, r8, (2f-1b)		/* Find the target offset */
 761
 762	/* Jump to the tmp mapping */
 763	mtspr	SPRN_SRR0, r8
 764	mtspr	SPRN_SRR1, r9
 765	rfi
 766
 7672:
 768	/* Invalidate the entry we were executing from */
 769	li	r3, 0
 770	tlbwe	r3, r23, PPC44x_TLB_PAGEID
 771
 772	/* attribute fields. rwx for SUPERVISOR mode */
 773	li	r5, 0
 774	ori	r5, r5, (PPC44x_TLB_SW | PPC44x_TLB_SR | PPC44x_TLB_SX | PPC44x_TLB_G)
 775
 776	/* Create 1:1 mapping in 256M pages */
 777	xori	r7, r7, 1			/* Revert back to Original TS */
 778
 779	li	r8, 0				/* PageNumber */
 780	li	r6, 3				/* TLB Index, start at 3  */
 781
 782next_tlb:
 783	rotlwi	r3, r8, 28			/* Create EPN (bits 0-3) */
 784	mr	r4, r3				/* RPN = EPN  */
 785	ori	r3, r3, (PPC44x_TLB_VALID | PPC44x_TLB_256M) /* SIZE = 256M, Valid */
 786	insrwi	r3, r7, 1, 23			/* Set TS from r7 */
 787
 788	tlbwe	r3, r6, PPC44x_TLB_PAGEID	/* PageID field : EPN, V, SIZE */
 789	tlbwe	r4, r6, PPC44x_TLB_XLAT		/* Address translation : RPN   */
 790	tlbwe	r5, r6, PPC44x_TLB_ATTRIB	/* Attributes */
 791
 792	addi	r8, r8, 1			/* Increment PN */
 793	addi	r6, r6, 1			/* Increment TLB Index */
 794	cmpwi	r8, 8				/* Are we done ? */
 795	bne	next_tlb
 796	isync
 797
 798	/* Jump to the new mapping 1:1 */
 799	li	r9,0
 800	insrwi	r9, r7, 1, 26			/* Set MSR[IS] = r7 */
 801
 802	bl	1f
 8031:	mflr	r8
 804	and	r8, r8, r11			/* Get our offset within page */
 805	addi	r8, r8, (2f-1b)
 806
 807	and	r5, r25, r10			/* Get our target PageNum */
 808	or	r8, r8, r5			/* Target jump address */
 809
 810	mtspr	SPRN_SRR0, r8
 811	mtspr	SPRN_SRR1, r9
 812	rfi
 8132:
 814	/* Invalidate the tmp entry we used */
 815	li	r3, 0
 816	tlbwe	r3, r24, PPC44x_TLB_PAGEID
 817	sync
 818	b	ppc44x_map_done
 819
 820#ifdef CONFIG_PPC_47x
 821
 822	/* 1:1 mapping for 47x */
 823
 824setup_map_47x:
 825
 826	/*
 827	 * Load the kernel pid (0) to PID and also to MMUCR[TID].
 828	 * Also set the MSR IS->MMUCR STS
 829	 */
 830	li	r3, 0
 831	mtspr	SPRN_PID, r3			/* Set PID */
 832	mfmsr	r4				/* Get MSR */
 833	andi.	r4, r4, MSR_IS@l		/* TS=1? */
 834	beq	1f				/* If not, leave STS=0 */
 835	oris	r3, r3, PPC47x_MMUCR_STS@h	/* Set STS=1 */
 8361:	mtspr	SPRN_MMUCR, r3			/* Put MMUCR */
 837	sync
 838
 839	/* Find the entry we are running from */
 840	bl	2f
 8412:	mflr	r23
 842	tlbsx	r23, 0, r23
 843	tlbre	r24, r23, 0			/* TLB Word 0 */
 844	tlbre	r25, r23, 1			/* TLB Word 1 */
 845	tlbre	r26, r23, 2			/* TLB Word 2 */
 846
 847
 848	/*
 849	 * Invalidates all the tlb entries by writing to 256 RPNs(r4)
 850	 * of 4k page size in all  4 ways (0-3 in r3).
 851	 * This would invalidate the entire UTLB including the one we are
 852	 * running from. However the shadow TLB entries would help us 
 853	 * to continue the execution, until we flush them (rfi/isync).
 854	 */
 855	addis	r3, 0, 0x8000			/* specify the way */
 856	addi	r4, 0, 0			/* TLB Word0 = (EPN=0, VALID = 0) */
 857	addi	r5, 0, 0
 858	b	clear_utlb_entry
 859
 860	/* Align the loop to speed things up. from head_44x.S */
 861	.align	6
 862
 863clear_utlb_entry:
 864
 865	tlbwe	r4, r3, 0
 866	tlbwe	r5, r3, 1
 867	tlbwe	r5, r3, 2
 868	addis	r3, r3, 0x2000			/* Increment the way */
 869	cmpwi	r3, 0
 870	bne	clear_utlb_entry
 871	addis	r3, 0, 0x8000
 872	addis	r4, r4, 0x100			/* Increment the EPN */
 873	cmpwi	r4, 0
 874	bne	clear_utlb_entry
 875
 876	/* Create the entries in the other address space */
 877	mfmsr	r5
 878	rlwinm	r7, r5, 27, 31, 31		/* Get the TS (Bit 26) from MSR */
 879	xori	r7, r7, 1			/* r7 = !TS */
 880
 881	insrwi	r24, r7, 1, 21			/* Change the TS in the saved TLB word 0 */
 882
 883	/* 
 884	 * write out the TLB entries for the tmp mapping
 885	 * Use way '0' so that we could easily invalidate it later.
 886	 */
 887	lis	r3, 0x8000			/* Way '0' */ 
 888
 889	tlbwe	r24, r3, 0
 890	tlbwe	r25, r3, 1
 891	tlbwe	r26, r3, 2
 892
 893	/* Update the msr to the new TS */
 894	insrwi	r5, r7, 1, 26
 895
 896	bl	1f
 8971:	mflr	r6
 898	addi	r6, r6, (2f-1b)
 899
 900	mtspr	SPRN_SRR0, r6
 901	mtspr	SPRN_SRR1, r5
 902	rfi
 903
 904	/* 
 905	 * Now we are in the tmp address space.
 906	 * Create a 1:1 mapping for 0-2GiB in the original TS.
 907	 */
 9082:
 909	li	r3, 0
 910	li	r4, 0				/* TLB Word 0 */
 911	li	r5, 0				/* TLB Word 1 */
 912	li	r6, 0
 913	ori	r6, r6, PPC47x_TLB2_S_RWX	/* TLB word 2 */
 914
 915	li	r8, 0				/* PageIndex */
 916
 917	xori	r7, r7, 1			/* revert back to original TS */
 918
 919write_utlb:
 920	rotlwi	r5, r8, 28			/* RPN = PageIndex * 256M */
 921						/* ERPN = 0 as we don't use memory above 2G */
 922
 923	mr	r4, r5				/* EPN = RPN */
 924	ori	r4, r4, (PPC47x_TLB0_VALID | PPC47x_TLB0_256M)
 925	insrwi	r4, r7, 1, 21			/* Insert the TS to Word 0 */
 926
 927	tlbwe	r4, r3, 0			/* Write out the entries */
 928	tlbwe	r5, r3, 1
 929	tlbwe	r6, r3, 2
 930	addi	r8, r8, 1
 931	cmpwi	r8, 8				/* Have we completed ? */
 932	bne	write_utlb
 933
 934	/* make sure we complete the TLB write up */
 935	isync
 936
 937	/* 
 938	 * Prepare to jump to the 1:1 mapping.
 939	 * 1) Extract page size of the tmp mapping
 940	 *    DSIZ = TLB_Word0[22:27]
 941	 * 2) Calculate the physical address of the address
 942	 *    to jump to.
 943	 */
 944	rlwinm	r10, r24, 0, 22, 27
 945
 946	cmpwi	r10, PPC47x_TLB0_4K
 947	bne	0f
 948	li	r10, 0x1000			/* r10 = 4k */
 949	bl	1f
 950
 9510:
 952	/* Defaults to 256M */
 953	lis	r10, 0x1000
 954	
 955	bl	1f
 9561:	mflr	r4
 957	addi	r4, r4, (2f-1b)			/* virtual address  of 2f */
 958
 959	subi	r11, r10, 1			/* offsetmask = Pagesize - 1 */
 960	not	r10, r11			/* Pagemask = ~(offsetmask) */
 961
 962	and	r5, r25, r10			/* Physical page */
 963	and	r6, r4, r11			/* offset within the current page */
 964
 965	or	r5, r5, r6			/* Physical address for 2f */
 966
 967	/* Switch the TS in MSR to the original one */
 968	mfmsr	r8
 969	insrwi	r8, r7, 1, 26
 970
 971	mtspr	SPRN_SRR1, r8
 972	mtspr	SPRN_SRR0, r5
 973	rfi
 974
 9752:
 976	/* Invalidate the tmp mapping */
 977	lis	r3, 0x8000			/* Way '0' */
 978
 979	clrrwi	r24, r24, 12			/* Clear the valid bit */
 980	tlbwe	r24, r3, 0
 981	tlbwe	r25, r3, 1
 982	tlbwe	r26, r3, 2
 983
 984	/* Make sure we complete the TLB write and flush the shadow TLB */
 985	isync
 986
 987#endif
 988
 989ppc44x_map_done:
 990
 991
 992	/* Restore the parameters */
 993	mr	r3, r29
 994	mr	r4, r30
 995	mr	r5, r31
 996
 997	li	r0, 0
 998#else
 999	li	r0, 0
1000
1001	/*
1002	 * Set Machine Status Register to a known status,
1003	 * switch the MMU off and jump to 1: in a single step.
1004	 */
1005
1006	mr	r8, r0
1007	ori     r8, r8, MSR_RI|MSR_ME
1008	mtspr	SPRN_SRR1, r8
1009	addi	r8, r4, 1f - relocate_new_kernel
1010	mtspr	SPRN_SRR0, r8
1011	sync
1012	rfi
1013
10141:
1015#endif
1016	/* from this point address translation is turned off */
1017	/* and interrupts are disabled */
1018
1019	/* set a new stack at the bottom of our page... */
1020	/* (not really needed now) */
1021	addi	r1, r4, KEXEC_CONTROL_PAGE_SIZE - 8 /* for LR Save+Back Chain */
1022	stw	r0, 0(r1)
1023
1024	/* Do the copies */
1025	li	r6, 0 /* checksum */
1026	mr	r0, r3
1027	b	1f
1028
10290:	/* top, read another word for the indirection page */
1030	lwzu	r0, 4(r3)
1031
10321:
1033	/* is it a destination page? (r8) */
1034	rlwinm.	r7, r0, 0, 31, 31 /* IND_DESTINATION (1<<0) */
1035	beq	2f
1036
1037	rlwinm	r8, r0, 0, 0, 19 /* clear kexec flags, page align */
1038	b	0b
1039
10402:	/* is it an indirection page? (r3) */
1041	rlwinm.	r7, r0, 0, 30, 30 /* IND_INDIRECTION (1<<1) */
1042	beq	2f
1043
1044	rlwinm	r3, r0, 0, 0, 19 /* clear kexec flags, page align */
1045	subi	r3, r3, 4
1046	b	0b
1047
10482:	/* are we done? */
1049	rlwinm.	r7, r0, 0, 29, 29 /* IND_DONE (1<<2) */
1050	beq	2f
1051	b	3f
1052
10532:	/* is it a source page? (r9) */
1054	rlwinm.	r7, r0, 0, 28, 28 /* IND_SOURCE (1<<3) */
1055	beq	0b
1056
1057	rlwinm	r9, r0, 0, 0, 19 /* clear kexec flags, page align */
1058
1059	li	r7, PAGE_SIZE / 4
1060	mtctr   r7
1061	subi    r9, r9, 4
1062	subi    r8, r8, 4
10639:
1064	lwzu    r0, 4(r9)  /* do the copy */
1065	xor	r6, r6, r0
1066	stwu    r0, 4(r8)
1067	dcbst	0, r8
1068	sync
1069	icbi	0, r8
1070	bdnz    9b
1071
1072	addi    r9, r9, 4
1073	addi    r8, r8, 4
1074	b	0b
1075
10763:
1077
1078	/* To be certain of avoiding problems with self-modifying code
1079	 * execute a serializing instruction here.
1080	 */
1081	isync
1082	sync
1083
1084	mfspr	r3, SPRN_PIR /* current core we are running on */
1085	mr	r4, r5 /* load physical address of chunk called */
1086
1087	/* jump to the entry point, usually the setup routine */
1088	mtlr	r5
1089	blrl
1090
10911:	b	1b
1092
1093relocate_new_kernel_end:
1094
1095	.globl relocate_new_kernel_size
1096relocate_new_kernel_size:
1097	.long relocate_new_kernel_end - relocate_new_kernel
1098#endif