s390/smp: reallocate IPL CPU lowcore

The lowcore for IPL CPU is special. It is allocated early
in the boot process using memblock and never freed since.
The reason is pcpu_alloc_lowcore() and pcpu_free_lowcore()
routines use page allocator which is not available when
the IPL CPU is getting initialized.

Similar problem is already addressed for stacks - once the
virtual memory is available the early boot stacks get re-
allocated. Doing the same for lowcore will allow freeing
the IPL CPU lowcore and make no difference between the
boot and secondary CPUs.
Signed-off-by: Alexander Gordeev <agordeev@linux.ibm.com>
Reviewed-by: Heiko Carstens <hca@linux.ibm.com>
Signed-off-by: Vasily Gorbik <gor@linux.ibm.com>

arch/s390/kernel/setup.c

@@ -341,27 +341,6 @@ int __init arch_early_irq_init(void)
 	return 0;
 }
 
-static int __init stack_realloc(void)
-{
-	unsigned long old, new;
-
-	old = S390_lowcore.async_stack - STACK_INIT_OFFSET;
-	new = stack_alloc();
-	if (!new)
-		panic("Couldn't allocate async stack");
-	WRITE_ONCE(S390_lowcore.async_stack, new + STACK_INIT_OFFSET);
-	free_pages(old, THREAD_SIZE_ORDER);
-
-	old = S390_lowcore.mcck_stack - STACK_INIT_OFFSET;
-	new = stack_alloc();
-	if (!new)
-		panic("Couldn't allocate machine check stack");
-	WRITE_ONCE(S390_lowcore.mcck_stack, new + STACK_INIT_OFFSET);
-	memblock_free_late(old, THREAD_SIZE);
-	return 0;
-}
-early_initcall(stack_realloc);
-
 void __init arch_call_rest_init(void)
 {
 	unsigned long stack;

arch/s390/kernel/smp.c

@@ -1234,3 +1234,55 @@ static int __init s390_smp_init(void)
 	return rc;
 }
 subsys_initcall(s390_smp_init);
+
+static __always_inline void set_new_lowcore(struct lowcore *lc)
+{
+	struct lowcore *old_lc = &S390_lowcore;
+	struct lowcore *new_lc = lc;
+	u32 pfx;
+	register struct lowcore *reg2 asm ("2") = new_lc;
+	register unsigned long	 reg3 asm ("3") = sizeof(*reg2);
+	register struct lowcore *reg4 asm ("4") = old_lc;
+	register unsigned long	 reg5 asm ("5") = sizeof(*reg4);
+
+	asm volatile(
+		"	st	2,%[pfx]\n"
+		"	mvcl	2,4\n"
+		"	spx	%[pfx]\n"
+		: "+&d" (reg2), "+&d" (reg3),
+		  "+&d" (reg4), "+&d" (reg5), [pfx] "=Q" (pfx)
+		: : "memory", "cc");
+}
+
+static int __init smp_reinit_ipl_cpu(void)
+{
+	unsigned long async_stack, nodat_stack, mcck_stack;
+	struct lowcore *lc, *lc_ipl;
+	unsigned long flags;
+
+	lc_ipl = lowcore_ptr[0];
+	lc = (struct lowcore *)	__get_free_pages(GFP_KERNEL | GFP_DMA, LC_ORDER);
+	nodat_stack = __get_free_pages(GFP_KERNEL, THREAD_SIZE_ORDER);
+	async_stack = stack_alloc();
+	mcck_stack = stack_alloc();
+	if (!lc || !nodat_stack || !async_stack || !mcck_stack)
+		panic("Couldn't allocate memory");
+
+	local_irq_save(flags);
+	local_mcck_disable();
+	set_new_lowcore(lc);
+	S390_lowcore.nodat_stack = nodat_stack + STACK_INIT_OFFSET;
+	S390_lowcore.async_stack = async_stack + STACK_INIT_OFFSET;
+	S390_lowcore.mcck_stack = mcck_stack + STACK_INIT_OFFSET;
+	lowcore_ptr[0] = lc;
+	pcpu_devices[0].lowcore = lc;
+	local_mcck_enable();
+	local_irq_restore(flags);
+
+	free_pages(lc_ipl->async_stack - STACK_INIT_OFFSET, THREAD_SIZE_ORDER);
+	memblock_free_late(lc_ipl->mcck_stack - STACK_INIT_OFFSET, THREAD_SIZE);
+	memblock_free_late((unsigned long) lc_ipl, sizeof(*lc_ipl));
+
+	return 0;
+}
+early_initcall(smp_reinit_ipl_cpu);