s390/early: rewrite program parameter setup in C

And move it earlier in the decompressor.
Reviewed-by: Heiko Carstens <hca@linux.ibm.com>
Signed-off-by: Vasily Gorbik <gor@linux.ibm.com>
Signed-off-by: Heiko Carstens <hca@linux.ibm.com>

arch/s390/boot/startup.c

@@ -2,6 +2,7 @@
 #include <linux/string.h>
 #include <linux/elf.h>
 #include <asm/sections.h>
+#include <asm/cpu_mf.h>
 #include <asm/setup.h>
 #include <asm/kexec.h>
 #include <asm/sclp.h>
@@ -58,6 +59,14 @@ void error(char *x)
 	disabled_wait();
 }
 
+static void setup_lpp(void)
+{
+	S390_lowcore.current_pid = 0;
+	S390_lowcore.lpp = LPP_MAGIC;
+	if (test_facility(40))
+		lpp(&S390_lowcore.lpp);
+}
+
 #ifdef CONFIG_KERNEL_UNCOMPRESSED
 unsigned long mem_safe_offset(void)
 {
@@ -147,6 +156,7 @@ void startup_kernel(void)
 	unsigned long safe_addr;
 	void *img;
 
+	setup_lpp();
 	store_ipl_parmblock();
 	safe_addr = mem_safe_offset();
 	safe_addr = read_ipl_report(safe_addr);

arch/s390/kernel/head64.S

@@ -18,12 +18,7 @@
 
 __HEAD
 ENTRY(startup_continue)
-	tm	__LC_STFLE_FAC_LIST+5,0x80	# LPP available ?
-	jz	0f
-	xc	__LC_LPP+1(7,0),__LC_LPP+1	# clear lpp and current_pid
-	mvi	__LC_LPP,0x80			#   and set LPP_MAGIC
-	.insn	s,0xb2800000,__LC_LPP		# load program parameter
-0:	larl	%r1,tod_clock_base
+	larl	%r1,tod_clock_base
 	mvc	0(16,%r1),__LC_BOOT_CLOCK
 	larl	%r13,.LPG1		# get base
 #