[PATCH] LG G2 (D802): Implement Kexec Hardboot Patch

g2-implement-kexec-hardboot.patch

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 0eb3c92..4397925 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -2306,6 +2306,32 @@ config ATAGS_PROC
 	  Should the atags used to boot the kernel be exported in an "atags"
 	  file in procfs. Useful with kexec.
 
+config KEXEC_HARDBOOT
+	bool "Support hard booting to a kexec kernel"
+	depends on KEXEC
+	help
+	  Allows hard booting (i.e., with a full hardware reboot) to a kernel
+	  previously loaded in memory by kexec.  This works around the problem of
+	  soft-booted kernel hangs due to improper device shutdown and/or
+	  reinitialization.  Support is comprised of two components:
+
+	  First, a "hardboot" flag is added to the kexec syscall to force a hard
+	  reboot in relocate_new_kernel() (which requires machine-specific assembly
+	  code).  This also requires the kexec userspace tool to load the kexec'd
+	  kernel in memory region left untouched by the bootloader (i.e., not
+	  explicitly cleared and not overwritten by the boot kernel).  Just prior
+	  to reboot, the kexec kernel arguments are stashed in a machine-specific
+	  memory page that must also be preserved.  Note that this hardboot page
+	  need not be reserved during regular kernel execution.
+
+	  Second, the zImage decompresor of the boot (bootloader-loaded) kernel is
+	  modified to check the hardboot page for fresh kexec arguments, and if
+	  present, attempts to jump to the kexec'd kernel preserved in memory.
+
+	  Note that hardboot support is only required in the boot kernel and any
+	  kernel capable of performing a hardboot kexec.  It is _not_ required by a
+	  kexec'd kernel.
+
 config CRASH_DUMP
 	bool "Build kdump crash kernel (EXPERIMENTAL)"
 	depends on EXPERIMENTAL
diff --git a/arch/arm/boot/compressed/head.S b/arch/arm/boot/compressed/head.S
index db99b53..a47adeb 100644
--- a/arch/arm/boot/compressed/head.S
+++ b/arch/arm/boot/compressed/head.S
@@ -11,6 +11,11 @@
 #include <linux/linkage.h>
 #include <asm/assembler.h>
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+  #include <asm/kexec.h>
+  #include <asm/memory.h>
+#endif
+
 	.arch	armv7-a
 /*
  * Debugging stuff
@@ -142,6 +147,64 @@ start:
 		mov	r7, r1			@ save architecture ID
 		mov	r8, r2			@ save atags pointer
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+		/* Check hardboot page for a kexec kernel. */
+		ldr	r3, =KEXEC_HB_PAGE_ADDR
+		ldr	r0, [r3]
+		ldr	r1, =KEXEC_HB_PAGE_MAGIC
+		teq	r0, r1
+		bne	not_booting_other
+
+		/* Clear hardboot page magic to avoid boot loop. */
+		mov	r0, #0
+		str	r0, [r3]
+
+		/*
+		 * Copy dtb from location up high in memory to default location.
+		 * Kernel freezes if this is not done.
+		 */
+		ldr	r1, [r3, #12]			@ kexec_boot_atags
+		ldr	r2, [r3, #16]			@ kexec_boot_atags_len
+		mov	r5, #0					@ iterator
+catags_cpy:
+		ldr	r0, [r1, r5]			@ from kexec_boot_atags
+		str	r0, [r8, r5]			@ to atags_pointer
+		add	r5, r5, #4
+		cmp	r5, r2
+		blo	catags_cpy
+
+#ifdef KEXEC_HB_KERNEL_LOC
+		/*
+		 * Copy kernel from location up high in memory to location in first 128MB.
+		 * Bootloader on hammerhead erases first 128MB of ram on reboot, so it can't
+		 * be in there before reboot, but decompressing in location above 128MB takes
+		 * a long time. This memcpy is much quicker, for some reason.
+		 */
+		ldr	r2, [r3, #4]				@ kexec_start_address
+		ldr	r4, [r3, #20]				@ kexec_kernel_len
+		ldr	r6, =KEXEC_HB_KERNEL_LOC    @ target
+		mov	r5, #0						@ iterator
+kernel_cpy:
+		ldr	r0, [r2, r5]				@ from kexec_start_address
+		str	r0, [r6, r5]				@ to KEXEC_HB_KERNEL_LOC
+		add	r5, r5, #4
+		cmp	r5, r4
+		blo	kernel_cpy
+#else
+		ldr	r6, [r3, #4]				@ kexec_start_address
+#endif
+
+		/* set registers and boot kexecd' kernel */
+		mov	r0, #0
+		ldr	r1, [r3, #8]				@ kexec_mach_type
+		mov	r2, r8						@ atags pointer
+		mov	pc, r6
+
+		.ltorg
+
+not_booting_other:
+#endif
+
 #ifndef __ARM_ARCH_2__
 		/*
 		 * Booting from Angel - need to enter SVC mode and disable
diff --git a/arch/arm/include/asm/kexec.h b/arch/arm/include/asm/kexec.h
index c2b9b4b..564c55b 100644
--- a/arch/arm/include/asm/kexec.h
+++ b/arch/arm/include/asm/kexec.h
@@ -17,6 +17,10 @@
 #define KEXEC_ARM_ATAGS_OFFSET  0x1000
 #define KEXEC_ARM_ZIMAGE_OFFSET 0x8000
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+  #define KEXEC_HB_PAGE_MAGIC 0x4a5db007
+#endif
+
 #ifndef __ASSEMBLY__
 
 /**
@@ -53,6 +57,10 @@ static inline void crash_setup_regs(struct pt_regs *newregs,
 /* Function pointer to optional machine-specific reinitialization */
 extern void (*kexec_reinit)(void);
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+extern void (*kexec_hardboot_hook)(void);
+#endif
+
 #endif /* __ASSEMBLY__ */
 
 #endif /* CONFIG_KEXEC */
diff --git a/arch/arm/kernel/machine_kexec.c b/arch/arm/kernel/machine_kexec.c
index 2daf338..ba11024 100644
--- a/arch/arm/kernel/machine_kexec.c
+++ b/arch/arm/kernel/machine_kexec.c
@@ -14,6 +14,9 @@
 #include <asm/cacheflush.h>
 #include <asm/mach-types.h>
 #include <asm/system_misc.h>
+#include <linux/memblock.h>
+#include <linux/of_fdt.h>
+#include <asm/mmu_writeable.h>
 
 extern const unsigned char relocate_new_kernel[];
 extern const unsigned int relocate_new_kernel_size;
@@ -22,6 +25,12 @@ extern unsigned long kexec_start_address;
 extern unsigned long kexec_indirection_page;
 extern unsigned long kexec_mach_type;
 extern unsigned long kexec_boot_atags;
+#ifdef CONFIG_KEXEC_HARDBOOT
+extern unsigned long kexec_hardboot;
+extern unsigned long kexec_boot_atags_len;
+extern unsigned long kexec_kernel_len;
+void (*kexec_hardboot_hook)(void);
+#endif
 
 static atomic_t waiting_for_crash_ipi;
 
@@ -32,6 +41,37 @@ static atomic_t waiting_for_crash_ipi;
 
 int machine_kexec_prepare(struct kimage *image)
 {
+	struct kexec_segment *current_segment;
+	__be32 header;
+	int i, err;
+
+	/* No segment at default ATAGs address. try to locate
+	 * a dtb using magic */
+	for (i = 0; i < image->nr_segments; i++) {
+		current_segment = &image->segment[i];
+
+		err = memblock_is_region_memory(current_segment->mem,
+						current_segment->memsz);
+		if (!err)
+			return - EINVAL;
+
+#ifdef CONFIG_KEXEC_HARDBOOT
+		if(current_segment->mem == image->start)
+			mem_text_write_kernel_word(&kexec_kernel_len, current_segment->memsz);
+#endif
+
+		err = get_user(header, (__be32*)current_segment->buf);
+		if (err)
+			return err;
+
+		if (be32_to_cpu(header) == OF_DT_HEADER)
+		{
+			mem_text_write_kernel_word(&kexec_boot_atags, current_segment->mem);
+#ifdef CONFIG_KEXEC_HARDBOOT
+			mem_text_write_kernel_word(&kexec_boot_atags_len, current_segment->memsz);
+#endif
+		}
+	}
 	return 0;
 }
 
@@ -120,10 +160,14 @@ void machine_kexec(struct kimage *image)
 	reboot_code_buffer = page_address(image->control_code_page);
 
 	/* Prepare parameters for reboot_code_buffer*/
-	kexec_start_address = image->start;
-	kexec_indirection_page = page_list;
-	kexec_mach_type = machine_arch_type;
-	kexec_boot_atags = image->start - KEXEC_ARM_ZIMAGE_OFFSET + KEXEC_ARM_ATAGS_OFFSET;
+	mem_text_write_kernel_word(&kexec_start_address, image->start);
+	mem_text_write_kernel_word(&kexec_indirection_page, page_list);
+	mem_text_write_kernel_word(&kexec_mach_type, machine_arch_type);
+	if (!kexec_boot_atags)
+		mem_text_write_kernel_word(&kexec_boot_atags, image->start - KEXEC_ARM_ZIMAGE_OFFSET + KEXEC_ARM_ATAGS_OFFSET);
+#ifdef CONFIG_KEXEC_HARDBOOT
+	mem_text_write_kernel_word(&kexec_hardboot, image->hardboot);
+#endif
 
 	/* copy our kernel relocation code to the control code page */
 	memcpy(reboot_code_buffer,
@@ -137,6 +181,12 @@ void machine_kexec(struct kimage *image)
 	if (kexec_reinit)
 		kexec_reinit();
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+	/* Run any final machine-specific shutdown code. */
+	if (image->hardboot && kexec_hardboot_hook)
+		kexec_hardboot_hook();
+#endif
+
 	soft_restart(reboot_code_buffer_phys);
 }
 
diff --git a/arch/arm/kernel/relocate_kernel.S b/arch/arm/kernel/relocate_kernel.S
index d0cdedf..0e45ffc 100644
--- a/arch/arm/kernel/relocate_kernel.S
+++ b/arch/arm/kernel/relocate_kernel.S
@@ -4,6 +4,15 @@
 
 #include <asm/kexec.h>
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+#include <asm/memory.h>
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
+  #include <mach/iomap.h>
+#elif defined(CONFIG_ARCH_APQ8064) || defined(CONFIG_ARCH_MSM8974)
+  #include <mach/msm_iomap.h>
+#endif
+#endif
+
 	.globl relocate_new_kernel
 relocate_new_kernel:
 
@@ -52,6 +61,12 @@ relocate_new_kernel:
 	b 0b
 
 2:
+#ifdef CONFIG_KEXEC_HARDBOOT
+	ldr	r0, kexec_hardboot
+	teq	r0, #0
+	bne	hardboot
+#endif
+
 	/* Jump to relocated kernel */
 	mov lr,r1
 	mov r0,#0
@@ -60,6 +75,52 @@ relocate_new_kernel:
  ARM(	mov pc, lr	)
  THUMB(	bx lr		)
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+hardboot:
+	/* Stash boot arguments in hardboot page:
+	 *  0: KEXEC_HB_PAGE_MAGIC
+	 *  4: kexec_start_address
+	 *  8: kexec_mach_type
+	 * 12: kexec_boot_atags
+	 * 16: kexec_boot_atags_len
+	 * 20: kexec_kernel_len */
+	ldr	r0, =KEXEC_HB_PAGE_ADDR
+	str	r1, [r0, #4]
+	ldr	r1, kexec_mach_type
+	str	r1, [r0, #8]
+	ldr	r1, kexec_boot_atags
+	str	r1, [r0, #12]
+	ldr	r1, kexec_boot_atags_len
+	str	r1, [r0, #16]
+	ldr	r1, kexec_kernel_len
+	str	r1, [r0, #20]
+	ldr	r1, =KEXEC_HB_PAGE_MAGIC
+	str	r1, [r0]
+
+#if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
+	ldr     r0, =TEGRA_PMC_BASE
+	ldr	r1, [r0]
+	orr	r1, r1, #0x10
+	str	r1, [r0]
+loop:	b	loop
+#elif defined(CONFIG_ARCH_APQ8064)
+	/* Restart using the PMIC chip, see mach-msm/restart.c */
+	ldr	r0, =APQ8064_TLMM_PHYS
+	mov	r1, #0
+	str	r1, [r0, #0x820]  @ PSHOLD_CTL_SU
+loop:	b	loop
+#elif defined(CONFIG_ARCH_MSM8974)
+	/* Restart using the PMIC chip, see mach-msm/restart.c */
+	ldr	r0, =MSM8974_MPM2_PSHOLD_PHYS
+	mov	r1, #0
+	str	r1, [r0, #0]
+loop:	b	loop
+#else
+#error "No reboot method defined for hardboot."
+#endif
+
+	.ltorg
+#endif
 	.align
 
 	.globl kexec_start_address
@@ -79,6 +140,20 @@ kexec_mach_type:
 kexec_boot_atags:
 	.long	0x0
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+	.globl kexec_boot_atags_len
+kexec_boot_atags_len:
+	.long	0x0
+
+	.globl kexec_kernel_len
+kexec_kernel_len:
+	.long	0x0
+
+	.globl kexec_hardboot
+kexec_hardboot:
+	.long	0x0
+#endif
+
 relocate_new_kernel_end:
 
 	.globl relocate_new_kernel_size
diff --git a/arch/arm/mach-msm/include/mach/memory.h b/arch/arm/mach-msm/include/mach/memory.h
index 6119a3c..3304172 100644
--- a/arch/arm/mach-msm/include/mach/memory.h
+++ b/arch/arm/mach-msm/include/mach/memory.h
@@ -20,6 +20,22 @@
 /* physical offset of RAM */
 #define PLAT_PHYS_OFFSET UL(CONFIG_PHYS_OFFSET)
 
+#if defined(CONFIG_KEXEC_HARDBOOT)
+#if defined(CONFIG_MACH_APQ8064_FLO)
+#define KEXEC_HB_PAGE_ADDR		UL(0x88C00000)
+#elif defined(CONFIG_MACH_APQ8064_MAKO)
+#define KEXEC_HB_PAGE_ADDR		UL(0x88600000)
+#elif defined(CONFIG_MACH_MSM8974_HAMMERHEAD)
+#define KEXEC_HB_PAGE_ADDR		UL(0x10100000)
+#define KEXEC_HB_KERNEL_LOC		UL(0x3208000)
+#elif defined (CONFIG_MACH_MSM8974_G2_OPEN_COM || CONFIG_MACH_MSM8974_G2_ATT || CONFIG_MACH_MSM8974_G2_TMO_US || CONFIG_MACH_MSM8974_G2_CA || CONFIG_MACH_MSM8974_G2_TLS || CONFIG_MACH_MSM8974_G2_KR || CONFIG_MACH_MSM8974_G2_KT || CONFIG_MACH_MSM8974_G2_SPR || CONFIG_MACH_MSM8974_G2_VZW)
+#define KEXEC_HB_PAGE_ADDR		UL(0x07b00000)
+#else
+#error "Adress for kexec hardboot page not defined"
+#endif
+#endif
+
 #define MAX_PHYSMEM_BITS 32
 #define SECTION_SIZE_BITS 28
 
diff --git a/arch/arm/mach-msm/lge/devices_lge.c b/arch/arm/mach-msm/lge/devices_lge.c
index 91293a7..5dbcd0a 100644
--- a/arch/arm/mach-msm/lge/devices_lge.c
+++ b/arch/arm/mach-msm/lge/devices_lge.c
@@ -40,6 +40,10 @@
 /* in drivers/staging/android */
 #include "ram_console.h"
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+#include <linux/memblock.h>
+#endif
+
 static int cn_arr_len = 3;
 /*                                                              */
 #ifdef CONFIG_LGE_PM
@@ -226,6 +230,27 @@ void __init lge_add_persist_ram_devices(void)
 
 void __init lge_reserve(void)
 {
+#ifdef CONFIG_KEXEC_HARDBOOT
+	// Reserve space for hardboot page - just after ram_console,
+	// at the start of second memory bank
+	int ret;
+	phys_addr_t start;
+	struct membank* bank;
+
+	if (meminfo.nr_banks < 2) {
+		pr_err("%s: not enough membank\n", __func__);
+		return;
+	}
+
+	bank = &meminfo.bank[1];
+	start = bank->start + SZ_1M + LGE_PERSISTENT_RAM_SIZE;
+	ret = memblock_remove(start, SZ_1M);
+	if(!ret)
+		pr_info("Hardboot page reserved at 0x%X\n", start);
+	else
+		pr_err("Failed to reserve space for hardboot page at 0x%X!\n", start);
+#endif
+
 #if defined(CONFIG_ANDROID_PERSISTENT_RAM)
 	lge_add_persist_ram_devices();
 #endif
diff --git a/arch/arm/mach-msm/restart.c b/arch/arm/mach-msm/restart.c
index a299918..1d7e461 100644
--- a/arch/arm/mach-msm/restart.c
+++ b/arch/arm/mach-msm/restart.c
@@ -42,6 +42,10 @@
 #include <mach/board_lge.h>
 #endif
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+#include <asm/kexec.h>
+#endif
+
 #define WDT0_RST	0x38
 #define WDT0_EN		0x40
 #define WDT0_BARK_TIME	0x4C
@@ -377,6 +381,26 @@ static int __init msm_pmic_restart_init(void)
 
 late_initcall(msm_pmic_restart_init);
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+static void msm_kexec_hardboot_hook(void)
+{
+	set_dload_mode(0);
+
+	// Set PMIC to restart-on-poweroff
+	pm8xxx_reset_pwr_off(1);
+
+	// These are executed on normal reboot, but with kexec-hardboot,
+	// they reboot/panic the system immediately.
+#if 0
+	qpnp_pon_system_pwr_off(PON_POWER_OFF_WARM_RESET);
+
+	/* Needed to bypass debug image on some chips */
+	msm_disable_wdog_debug();
+	halt_spmi_pmic_arbiter();
+#endif
+}
+#endif
+
 static int __init msm_restart_init(void)
 {
 #ifdef CONFIG_LGE_HANDLE_PANIC
@@ -400,6 +424,10 @@ static int __init msm_restart_init(void)
 	if (scm_is_call_available(SCM_SVC_PWR, SCM_IO_DISABLE_PMIC_ARBITER) > 0)
 		scm_pmic_arbiter_disable_supported = true;
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+	kexec_hardboot_hook = msm_kexec_hardboot_hook;
+#endif
+
 	return 0;
 }
 early_initcall(msm_restart_init);
diff --git a/include/linux/kexec.h b/include/linux/kexec.h
index af84a25..a4509ad 100644
--- a/include/linux/kexec.h
+++ b/include/linux/kexec.h
@@ -111,6 +111,10 @@ struct kimage {
 #define KEXEC_TYPE_CRASH   1
 	unsigned int preserve_context : 1;
 
+#ifdef CONFIG_KEXEC_HARDBOOT
+	unsigned int hardboot : 1;
+#endif
+
 #ifdef ARCH_HAS_KIMAGE_ARCH
 	struct kimage_arch arch;
 #endif
@@ -178,6 +182,11 @@ extern struct kimage *kexec_crash_image;
 
 #define KEXEC_ON_CRASH		0x00000001
 #define KEXEC_PRESERVE_CONTEXT	0x00000002
+
+#ifdef CONFIG_KEXEC_HARDBOOT
+#define KEXEC_HARDBOOT		0x00000004
+#endif
+
 #define KEXEC_ARCH_MASK		0xffff0000
 
 /* These values match the ELF architecture values.
@@ -196,10 +205,14 @@ extern struct kimage *kexec_crash_image;
 #define KEXEC_ARCH_MIPS    ( 8 << 16)
 
 /* List of defined/legal kexec flags */
-#ifndef CONFIG_KEXEC_JUMP
-#define KEXEC_FLAGS    KEXEC_ON_CRASH
-#else
+#if defined(CONFIG_KEXEC_JUMP) && defined(CONFIG_KEXEC_HARDBOOT)
+#define KEXEC_FLAGS    (KEXEC_ON_CRASH | KEXEC_PRESERVE_CONTEXT | KEXEC_HARDBOOT)
+#elif defined(CONFIG_KEXEC_JUMP)
 #define KEXEC_FLAGS    (KEXEC_ON_CRASH | KEXEC_PRESERVE_CONTEXT)
+#elif defined(CONFIG_KEXEC_HARDBOOT)
+#define KEXEC_FLAGS    (KEXEC_ON_CRASH | KEXEC_HARDBOOT)
+#else
+#define KEXEC_FLAGS    (KEXEC_ON_CRASH)
 #endif
 
 #define VMCOREINFO_BYTES           (4096)
diff --git a/kernel/kexec.c b/kernel/kexec.c
index 8fc6272..da6c0d6 100644
--- a/kernel/kexec.c
+++ b/kernel/kexec.c
@@ -1007,6 +1007,10 @@ SYSCALL_DEFINE4(kexec_load, unsigned long, entry, unsigned long, nr_segments,
 
 		if (flags & KEXEC_PRESERVE_CONTEXT)
 			image->preserve_context = 1;
+#ifdef CONFIG_KEXEC_HARDBOOT
+		if (flags & KEXEC_HARDBOOT)
+			image->hardboot = 1;
+#endif
 		result = machine_kexec_prepare(image);
 		if (result)
 			goto out;