path: root/meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch

From 4ce00daa904a40701ab6bed44506fe97b8f1da47 Mon Sep 17 00:00:00 2001
From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
Date: Fri, 19 May 2017 14:48:38 +0900
Subject: [PATCH 3/4] Add Hibernation swsuspmem command support

Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
---
 common/Makefile            |   2 +
 common/cmd_swsuspmem.c     | 944 +++++++++++++++++++++++++++++++++++++++++++++
 include/swsuspmem.h        |  24 ++
 lib/lzo/lzo1x_decompress.c |  12 +-
 4 files changed, 980 insertions(+), 2 deletions(-)
 create mode 100644 common/cmd_swsuspmem.c
 create mode 100644 include/swsuspmem.h

diff --git a/common/Makefile b/common/Makefile
index 54fcc81..7a18486 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -160,6 +160,8 @@ COBJS-$(CONFIG_CMD_SETEXPR) += cmd_setexpr.o
 COBJS-$(CONFIG_CMD_SPI) += cmd_spi.o
 COBJS-$(CONFIG_CMD_SPIBOOTLDR) += cmd_spibootldr.o
 COBJS-$(CONFIG_CMD_STRINGS) += cmd_strings.o
+COBJS-$(CONFIG_CMD_SWSUSP) += cmd_swsusp.o
+COBJS-$(CONFIG_CMD_SWSUSPMEM) += cmd_swsuspmem.o
 COBJS-$(CONFIG_CMD_TERMINAL) += cmd_terminal.o
 COBJS-$(CONFIG_CMD_TIME) += cmd_time.o
 COBJS-$(CONFIG_SYS_HUSH_PARSER) += cmd_test.o
diff --git a/common/cmd_swsuspmem.c b/common/cmd_swsuspmem.c
new file mode 100644
index 0000000..6980aaf
--- /dev/null
+++ b/common/cmd_swsuspmem.c
@@ -0,0 +1,944 @@
+/*
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <common.h>
+#include <command.h>
+#include <part.h>
+#include <malloc.h>
+
+#include <linux/lzo.h>
+#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
+#include <swsuspmem.h>
+
+/* Note for Renesas--based boards:
+ * We have the following memory split here:
+ * 0x40000000 - u_boot_lowest - used to store pfns at physical addresses
+ * u_boot_lowest - 0x8000000 - pfns are relocated, and then later put
+ * on physical addresses (swsusp_finish)
+ * 0x8000000 - 0xc0000000 - used to store pfns with physical address
+ * of 0x200000000 (long address), we have to change offset for them.
+ * Any pfn with address > 0x8000000 but less than 0x200000000
+ * is an error.
+ * For boards which do not have memory above first GB, that will
+ * still work, as they won't have anything above 0x80000000
+ * in their image, so for standard 2GB setup ou should put
+ * your secong GB in 0x80000000-0xC0000000 range, you can
+ * use MMU for that or if your RAM is continous, it will
+ * naturally be there. */
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/* #define PAGEMAP_DEBUG */
+
+#ifdef PAGEMAP_DEBUG
+#define SWSUSP_DEBUG_INFO
+#endif
+
+#define SWSUSP_KEEP_IMAGE
+
+#ifndef likely
+# define likely(x)	__builtin_expect(!!(x), 1)
+# define unlikely(x)	__builtin_expect(!!(x), 0)
+#endif
+
+#define HIBERNATE_SIG "S1SUSPEND"
+#define PAGE_SIZE (4096)
+/* Define depending on CONFIG_LBDAF in kernel */
+
+typedef u64 sector_t;
+
+struct swsusp_header {
+	char reserved[PAGE_SIZE - 20
+		- sizeof(sector_t) - sizeof(int) - sizeof(u32)
+		- sizeof(CRC32_WORD4_t) - sizeof(u32)];
+	CRC32_WORD4_t comp_crc32;
+	u32 img_size; /* append */
+	u32	crc32;
+	sector_t	image;
+	unsigned int flags;
+	char	orig_sig[10];
+	char	sig[10];
+} __packed;
+
+#define __NEW_UTS_LEN 64
+
+struct new_utsname {
+	char sysname[__NEW_UTS_LEN + 1];
+	char nodename[__NEW_UTS_LEN + 1];
+	char release[__NEW_UTS_LEN + 1];
+	char version[__NEW_UTS_LEN + 1];
+	char machine[__NEW_UTS_LEN + 1];
+	char domainname[__NEW_UTS_LEN + 1];
+};
+
+struct swsusp_archdata {
+	u32	nosave_backup_phys;
+	u32	nosave_begin_phys;
+	u32	nosave_end_phys;
+	void	(*cpu_resume_restore_nosave)(u32, u32, u32);
+};
+
+struct swsusp_info {
+	struct new_utsname	uts;
+	u32			version_code;
+	unsigned long		num_physpages;
+	int			cpus;
+	unsigned long		image_pages;
+	unsigned long		pages;
+	unsigned long		size;
+	char			archdata[1024];
+};
+
+struct swap_map_page {
+	u64 entries[PAGE_SIZE / sizeof(u64) - 1];
+	u64 next_swap;
+};
+
+struct swsusp_finish_context {
+	void *remap_orig_page;
+	void *remap_temp_page;
+	struct swsusp_archdata archdata;
+};
+#ifdef FTEN_SPF_SDRAM_BASE
+#define USED_ADDRESS_TOP	(CONFIG_SYS_SDRAM_BASE)
+#define USED_ADDRESS_END	(CONFIG_SYS_SDRAM_BASE + CONFIG_SYS_LOAD_OFFSET)
+#else
+#define USED_ADDRESS_TOP	(0x40000000)
+#define USED_ADDRESS_END	(0x48000000)
+#endif
+#define PG_UB2ZERO(pg) ((pg) - CONFIG_SYS_SDRAM_BASE / PAGE_SIZE)
+static u32 const exclude_min_page =
+	(USED_ADDRESS_TOP) / PAGE_SIZE;
+static u32 const exclude_max_page =
+	(USED_ADDRESS_END - 1) / PAGE_SIZE;
+static u32 const exclude_min_page_ub =
+	PG_UB2ZERO((USED_ADDRESS_TOP) / PAGE_SIZE);
+static u32 const exclude_max_page_ub =
+	PG_UB2ZERO((USED_ADDRESS_END-1) / PAGE_SIZE);
+
+/*
+ #define SD_PLATFORM_MODE  1
+ #define SD_CRC32_MODE     4
+ */
+#define SF_NOCOMPRESS_MODE 2
+
+#define LZO_HEADER      sizeof(size_t)
+
+/* Number of pages/bytes we'll compress at one time. */
+#define LZO_UNC_PAGES	32
+#define LZO_UNC_SIZE	(LZO_UNC_PAGES * PAGE_SIZE)
+
+/* Number of pages/bytes we need for compressed data (worst case). */
+#define LZO_CMP_PAGES	DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
+				LZO_HEADER, PAGE_SIZE)
+#define LZO_CMP_SIZE	(LZO_CMP_PAGES * PAGE_SIZE)
+
+static struct swsuspmem_hook *_hook;
+
+#define CALL_HOOK(f, param) \
+	do {						\
+		if (_hook != NULL) {			\
+			if (_hook->f != NULL)		\
+				_hook->f(param);	\
+		}					\
+	} while (0)
+
+#ifdef PAGEMAP_DEBUG
+static int debugout;
+static int _last_read_pages;
+#define PAGEMAP_INFO(_msg, ...)				\
+	do {						\
+		if (debugout == 1)			\
+			printf(_msg, ## __VA_ARGS__);	\
+	} while (0)
+#endif
+
+#define HIGHMEM_PHYS_ADDR	0x200000000ULL
+#define HIGHMEM_VA		0x80000000UL
+#define HIGHMEM_PFN		(HIGHMEM_PHYS_ADDR / PAGE_SIZE)
+#define LOW_TOP			0x80000000
+#define LOW_TOP_PFN		(LOW_TOP / PAGE_SIZE)
+#define LOW_BOTTOM		CONFIG_SYS_SDRAM_BASE
+#define LOW_BOTTOM_PFN		(LOW_BOTTOM / PAGE_SIZE)
+#define TOP_ADDRESS		0x240000000ULL
+
+static inline int pfn_is_low(u32 pfn)
+{
+	return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
+}
+
+static inline int pfn_is_high(u32 pfn)
+{
+	return (pfn >= HIGHMEM_PFN);
+}
+
+#define pfn_is_valid(p)		(pfn_is_low(p) || pfn_is_high(p))
+
+static inline int pfn_is_excluded(u32 pfn)
+{
+	/* Allow bottom 2 pages for exception vectors */
+	if (pfn < (LOW_BOTTOM_PFN + 2))
+		return 0;
+	else if (exclude_min_page >= exclude_max_page)
+		return 0;
+	else
+		return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
+}
+/* PFN to zero-counted page */
+static inline u32 pg_ub2zero(u32 pg)
+{
+	return pg - LOW_BOTTOM_PFN;
+}
+
+/* zero-counted page to PFN */
+static inline u32 pg_zero2ub(u32 pg)
+{
+	return pg + LOW_BOTTOM_PFN;
+}
+
+/* PFN to physical address (64-bit (40-bit)) */
+static inline u64 pg2phys(u32 page)
+{
+	return (u64) page * PAGE_SIZE;
+}
+
+/* PFN to virtual address */
+static inline void *pg2addr(u32 page)
+{
+	void *addr;
+	if (page >= HIGHMEM_PFN)
+		addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
+	else
+		addr = (void *) (u32)pg2phys(page);
+
+	return addr;
+}
+/* Virtual address to PFN */
+static inline u32 addr2pg(void *addr)
+{
+	return ((u32)(addr)) / PAGE_SIZE;
+}
+static void *offt_addr = (void *)0x44000000;
+static int page_read_mem(u64 page, void *addr)
+{
+	memcpy(addr, (u8 *)offt_addr + page * PAGE_SIZE, PAGE_SIZE);
+	return 0;
+}
+
+#ifndef SWSUSP_KEEP_IMAGE
+static int page_write_mem(u32 page, void *addr)
+{
+	memcpy((u8 *)offt_addr + page * PAGE_SIZE, addr, PAGE_SIZE);
+	return 0;
+}
+#endif
+
+#define FAST_COPY
+static void __attribute__((section(".rodata")))
+	__attribute__((optimize("O6", "unroll-loops")))
+swsusp_finish(void *userdata)
+{
+	struct swsusp_finish_context *context = userdata;
+	u32 **remap_orig;
+	u32 **remap_temp;
+	int idx = 0;
+	const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
+
+	remap_orig = context->remap_orig_page;
+	remap_temp = context->remap_temp_page;
+
+	__asm__ volatile ("" : : : "memory");
+	for (;;) {
+		u32 *orig, *temp;
+		int count;
+
+		/* Linked list to next page */
+		if (idx == lastidx) {
+			remap_orig = (u32 **)remap_orig[idx];
+			remap_temp = (u32 **)remap_temp[idx];
+			idx = 0;
+		}
+		if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
+			break;
+		orig = remap_orig[idx];
+		temp = remap_temp[idx];
+#ifdef FAST_COPY
+		count = PAGE_SIZE / sizeof(u32) / 32;
+		__asm__ volatile (
+			"1:\n"
+			"ldmia %[rtemp]!, {r0-r7}\n"
+			"stmia %[rorig]!, {r0-r7}\n"
+			"ldmia %[rtemp]!, {r0-r7}\n"
+			"stmia %[rorig]!, {r0-r7}\n"
+			"ldmia %[rtemp]!, {r0-r7}\n"
+			"stmia %[rorig]!, {r0-r7}\n"
+			"ldmia %[rtemp]!, {r0-r7}\n"
+			"subs %[count], %[count], #1\n"
+			"stmia %[rorig]!, {r0-r7}\n"
+			"bgt 1b\n"
+			: /* No outputs */
+			:
+			  [rorig]"h" (orig),
+			  [rtemp]"h" (temp),
+			  [count]"h" (count)
+			: "r0", "r1", "r2", "r3",
+			  "r4", "r5", "r6", "r7",
+			  "cc", "memory"
+		);
+#else
+		count = PAGE_SIZE / sizeof(u32);
+		while (count--)
+			*orig++ = *temp++;
+#endif
+#ifdef SWSUSP_CHECK_COPY_RESULT
+		count = PAGE_SIZE / sizeof(u32);
+		orig = remap_orig[idx];
+		temp = remap_temp[idx];
+		__asm__ volatile (
+			"1:\n"
+			"ldr r3, [%[rorig]]\n"
+			"ldr r4, [%[rtemp]]\n"
+			"cmp r3, r4\n"
+			"bne 2f\n"
+			"add %[rorig], %[rorig], #4\n"
+			"add %[rtemp], %[rtemp], #4\n"
+			"subs %[count], %[count], #1\n"
+			"bgt 1b\n"
+			"b 3f\n"
+			"2:b 2b\n"
+			"3:\n"
+			:
+			  [rorig]"+r" (orig),
+			  [rtemp]"+r" (temp),
+			  [count]"+r" (count)
+			:
+			: "r3", "r4", "cc", "memory"
+		);
+#endif
+		idx++;
+	}
+	context->archdata.cpu_resume_restore_nosave(
+			context->archdata.nosave_backup_phys,
+			context->archdata.nosave_begin_phys,
+			context->archdata.nosave_end_phys);
+}
+
+static struct swap_map_page *meta_map;
+static u64 meta_map_next;
+static u64 meta_map_curr;
+static u64 meta_map_start;
+static int meta_idx;
+
+static int raw_page_init(u64 start)
+{
+	meta_map = malloc(PAGE_SIZE);
+	if (!meta_map)
+		return -1;
+	meta_map_next = 0;
+	meta_map_curr = 0;
+	meta_map_start = start;
+	return 0;
+}
+
+static void raw_page_start(void)
+{
+	meta_idx = ARRAY_SIZE(meta_map->entries);
+	meta_map_next = meta_map_start;
+}
+
+static int raw_page_get_next(void *buffer)
+{
+	if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
+		if (!meta_map_next)
+			return 0;
+		if (meta_map_curr != meta_map_next) {
+#ifdef PAGEMAP_DEBUG
+			PAGEMAP_INFO("META: %d (%08x)\n", (int)meta_map_next,
+				(unsigned int)(meta_map_next
+					* PAGE_SIZE));
+#endif
+			if (page_read_mem(meta_map_next, meta_map))
+				return -1;
+			meta_map_curr = meta_map_next;
+			meta_map_next = meta_map->next_swap;
+		}
+		meta_idx = 0;
+	}
+#ifdef PAGEMAP_DEBUG
+	{
+		static unsigned int pre;
+		if ((pre + 1) != meta_map->entries[meta_idx]) {
+			PAGEMAP_INFO("DATA-Skiped: %d->%d (%08x->%08x)\n",
+			pre,  (unsigned int)meta_map->entries[meta_idx],
+			pre * PAGE_SIZE,
+			(unsigned int)(meta_map->entries[meta_idx]
+				* PAGE_SIZE));
+	}
+	pre = meta_map->entries[meta_idx];
+	_last_read_pages = pre;
+	}
+#endif
+	if (page_read_mem(meta_map->entries[meta_idx++], buffer))
+		return -1;
+
+	return 1;
+}
+
+static void raw_page_exit(void)
+{
+	free(meta_map);
+	meta_map = NULL;
+}
+
+static int image_pages_avail;
+static unsigned char *unc_buf, *cmp_buf;
+static int unc_offset;
+
+static int image_page_init(int compressed)
+{
+	if (!compressed)
+		return 1;
+
+	unc_buf = malloc(LZO_UNC_SIZE);
+	cmp_buf = malloc(LZO_CMP_SIZE);
+	if (!unc_buf || !cmp_buf) {
+		printf("not enogh memory\n");
+		return 1;
+	}
+	return 0;
+}
+
+static void image_page_start(void)
+{
+	image_pages_avail = 0;
+}
+
+static int image_page_get_next(void *buffer)
+{
+#ifdef CONFIG_LZO
+		if (!image_pages_avail) {
+			int ret;
+			size_t unc_len, cmp_len, cmp_avail;
+
+			ret = raw_page_get_next(cmp_buf);
+			if (ret <= 0)
+				return ret;
+
+			cmp_len = *(size_t *) cmp_buf;
+			cmp_avail = PAGE_SIZE;
+
+			while (cmp_avail < cmp_len + LZO_HEADER) {
+				ret = raw_page_get_next(cmp_buf + cmp_avail);
+				if (unlikely(ret <= 0))
+					return ret;
+				cmp_avail += PAGE_SIZE;
+			}
+
+			unc_len = LZO_UNC_SIZE;
+			ret = lzo1x_decompress_safe(cmp_buf + LZO_HEADER,
+						cmp_len, unc_buf, &unc_len);
+			if (unlikely(ret != LZO_E_OK)) {
+				printf("Decompression failure: %d,"
+				       " cmp_buf = %p,"
+				       " cmp_len = %d, unc_len = %d\n",
+				       ret, cmp_buf + LZO_HEADER, cmp_len,
+				       unc_len);
+				return ret;
+			}
+			image_pages_avail = unc_len / PAGE_SIZE;
+			unc_offset = 0;
+		}
+
+		memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
+		unc_offset += PAGE_SIZE;
+		image_pages_avail--;
+		return 1;
+#else
+		printf("No LZO support in u-boot.\n");
+		return -1;
+#endif
+}
+
+static void image_page_exit(void)
+{
+	free(unc_buf);
+	free(cmp_buf);
+	unc_buf = cmp_buf = NULL;
+}
+
+static void bitmap_set(u32 *bm, unsigned int bit)
+{
+	bm[bit >> 5] |= (1 << (bit & 0x1f));
+}
+
+static int bitmap_is_set(u32 *bm, unsigned int bit)
+{
+	return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
+}
+
+static u32 *used_bitmap;
+static u32 next_free_page;
+static u32 total_pages;
+
+static int free_page_init(void)
+{
+	total_pages = (u32)((TOP_ADDRESS -
+			LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
+	used_bitmap = malloc(total_pages * sizeof(u32) / 32);
+	if (!used_bitmap)
+		return -1;
+	return 0;
+}
+
+static void free_page_start(int offset)
+{
+	memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
+	next_free_page = pg_ub2zero(offset);
+}
+
+static void free_page_mark_used(u32 page);
+/* Returns full-address based pages */
+static int free_page_get_next(void)
+{
+	while (bitmap_is_set(used_bitmap, next_free_page))
+		next_free_page++;
+	free_page_mark_used(next_free_page);
+	return pg_zero2ub(next_free_page++);
+}
+
+static void free_page_mark_used(u32 page)
+{
+	bitmap_set(used_bitmap, page);
+}
+
+static void free_page_exit(void)
+{
+	free(used_bitmap);
+	used_bitmap = NULL;
+}
+
+void set_swsuspmem_hook(struct swsuspmem_hook *hook)
+{
+	_hook = hook;
+}
+
+/* 
+ * rtn = 1 : Hibernation image OK.
+ * rtn = 0 : Hibernation image NG.
+ * */
+int do_checksnapimage(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	__u32 offset = 0;
+	void *spare_page = NULL;
+	struct swsusp_header *swsusp_header;
+	CRC32_WORD4_t calc_crc;
+
+	/* Address hack */
+	if (argc > 1) {
+		char *ep;
+		offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
+		if (*ep) {
+			printf("Invalid address\n");
+			return 0;
+		}
+	}
+
+	spare_page = malloc(PAGE_SIZE);
+	if (!spare_page)
+		goto mem_err;
+
+	swsusp_header = spare_page;
+	if (page_read_mem(offset, swsusp_header))
+		goto read_err;
+
+#ifdef SWSUSP_DEBUG_INFO
+	PAGEMAP_INFO("swssp_header:%x\n", swsusp_header);
+	PAGEMAP_INFO("    comp_crc: <snip>\n");
+	PAGEMAP_INFO("    img_size: %d\n", swsusp_header->img_size);
+	PAGEMAP_INFO("    image(swap firest sector): %08x\n",
+			(unsigned int)swsusp_header->image);
+	PAGEMAP_INFO("    flags: %08x\n", swsusp_header->flags);
+	PAGEMAP_INFO("    orig_sig:%s\n", swsusp_header->orig_sig);
+	PAGEMAP_INFO("    sig:%s\n",      swsusp_header->sig);
+#endif /* SWSUSP_DEBUG_INFO */
+
+	if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
+			|| (swsusp_header->img_size == 0)
+			|| (swsusp_header->img_size > 0x03fff000)) {
+		printf("No hibernation image present\n");
+		CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
+		return 0;
+	}
+	memset(&calc_crc, 0, sizeof(calc_crc));
+
+	calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
+			swsusp_header->img_size, &calc_crc);
+
+	if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
+				sizeof(CRC32_WORD4_t))) {
+		printf("Bad CRC for image, image: %08x:%08x:"
+			"%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
+			swsusp_header->comp_crc32.crc_w[0],
+			swsusp_header->comp_crc32.crc_w[1],
+			swsusp_header->comp_crc32.crc_w[2],
+			swsusp_header->comp_crc32.crc_w[3],
+			calc_crc.crc_w[0], calc_crc.crc_w[1],
+			calc_crc.crc_w[2], calc_crc.crc_w[3]);
+		CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
+		return 0;
+	}
+	free(spare_page);
+	printf("Hibernation image OK!.\n");
+
+	return 1;
+
+mem_err:
+	printf("Not enough memory.\n");
+	CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
+	goto err;
+
+read_err:
+	printf("Read error while restoring image.\n");
+
+err:
+	__asm__ volatile (
+	"mov	r0, #0\n"
+	"mcr	p15, 0, r0, c7, c5, 0   @ invalidate icache\n"
+	"mcr	p15, 0, r0, c7, c10, 4  @ DSB\n"
+	"mcr	p15, 0, r0, c7, c5, 4   @ ISB\n"
+	: : : "r0", "memory");
+
+	free(spare_page);
+
+	CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
+	return 0;
+}
+
+U_BOOT_CMD(checksnapimage, 2, 2, do_checksnapimage,
+	"Check hibernation image data from memory",
+	"<address>]"
+);
+
+int do_swsuspmem(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	__u32 offset = 0;
+	void *spare_page = NULL;
+	struct swsusp_header *swsusp_header;
+	struct swsusp_info *swsusp_info;
+	struct swsusp_finish_context *context;
+	int max_page;
+	int i;
+	u32 nr_pfn_pages;
+	u32 **pfn_pages = NULL;
+	u32 *remap_orig_page;
+	u32 *remap_temp_page;
+	u32 **remap_orig;
+	u32 **remap_temp;
+	int remap_idx;
+	void (*swsusp_finish_copy)(void *);
+	char *data_page;
+	char *stack_addr;
+	CRC32_WORD4_t calc_crc;
+	int high_page;
+
+#ifdef PAGEMAP_DEBUG
+	int high_page_images = 0;
+	int total_remap = 0;
+	if (getenv("hybdebug") != NULL)
+		debugout = 1;
+#endif
+	/* Address hack */
+	void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
+	if (argc > 1) {
+		char *ep;
+		offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
+		if (*ep) {
+			printf("Invalid address\n");
+			return 1;
+		}
+	}
+
+	/* Allow for 16 pages of stack */
+	max_page = gd->start_addr_sp / PAGE_SIZE - 32;
+	high_page = (((gd->relocaddr + _bss_end_ofs)
+				+ (PAGE_SIZE - 1)) / PAGE_SIZE) + 1;
+#define pfn_is_occupied(pfn) (page > max_page && page <= high_page)
+#ifdef PAGEMAP_DEBUG
+	PAGEMAP_INFO(" *gd->start_addr_sp:%p\n",
+			(void *)gd->start_addr_sp);
+	PAGEMAP_INFO(" *gd->relocaddr:%p\n",
+			(void *)gd->relocaddr);
+	PAGEMAP_INFO(" *bss_start_offset:%d bss_end_offset:%d\n",
+			(int)_bss_start_ofs, (int)_bss_end_ofs);
+	PAGEMAP_INFO(" UBOOT own memory [%p-%p]\n",
+			pg2addr(max_page), pg2addr(high_page));
+#endif
+	if (free_page_init())
+		goto mem_err;
+	free_page_start(exclude_max_page + 1);
+
+	spare_page = malloc(PAGE_SIZE);
+	if (!spare_page)
+		goto mem_err;
+
+	swsusp_header = spare_page;
+	if (page_read_mem(offset, swsusp_header))
+		goto read_err;
+
+#ifdef SWSUSP_DEBUG_INFO
+	PAGEMAP_INFO("swssp_header:\n");
+	PAGEMAP_INFO("    comp_crc: <snip>\n");
+	PAGEMAP_INFO("    img_size: %d\n", swsusp_header->img_size);
+	PAGEMAP_INFO("    image(swap firest sector): %08x\n",
+			(unsigned int)swsusp_header->image);
+	PAGEMAP_INFO("    flags: %08x\n", swsusp_header->flags);
+	PAGEMAP_INFO("    orig_sig:%s\n", swsusp_header->orig_sig);
+	PAGEMAP_INFO("    sig:%s\n",      swsusp_header->sig);
+#endif /* SWSUSP_DEBUG_INFO */
+
+	if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
+			|| (swsusp_header->img_size == 0)
+			|| (swsusp_header->img_size > 0x03fff000)) {
+		printf("No hibernation image present\n");
+		CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
+		return 0;
+	}
+	memset(&calc_crc, 0, sizeof(calc_crc));
+
+	calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
+			swsusp_header->img_size, &calc_crc);
+
+	if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
+				sizeof(CRC32_WORD4_t))) {
+		printf("Bad CRC for image, image: %08x:%08x:"
+			"%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
+			swsusp_header->comp_crc32.crc_w[0],
+			swsusp_header->comp_crc32.crc_w[1],
+			swsusp_header->comp_crc32.crc_w[2],
+			swsusp_header->comp_crc32.crc_w[3],
+			calc_crc.crc_w[0], calc_crc.crc_w[1],
+			calc_crc.crc_w[2], calc_crc.crc_w[3]);
+		CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
+		return 0;
+	}
+
+	/* Overwrite header if necessary */
+#ifndef SWSUSP_KEEP_IMAGE
+	if (memcmp(swsusp_header->sig, swsusp_header->orig_sig, 10)) {
+		memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
+		if (page_write_mem(offset, swsusp_header))
+			printf("Write error resetting header\n");
+	}
+#endif
+
+	if (raw_page_init(swsusp_header->image))
+		goto mem_err;
+	raw_page_start();
+
+	if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
+		goto mem_err;
+	image_page_start();
+
+	swsusp_info = spare_page;
+	if (raw_page_get_next(swsusp_info) <= 0)
+		goto read_err;
+
+#ifdef SWSUSP_DEBUG_INFO
+	PAGEMAP_INFO("swsup_info:\n");
+	PAGEMAP_INFO("	utsname.sysname:%s\n",
+			swsusp_info->uts.sysname);
+	PAGEMAP_INFO("            nodename:%s\n",
+			swsusp_info->uts.nodename);
+	PAGEMAP_INFO("            release:%s\n",
+			swsusp_info->uts.release);
+	PAGEMAP_INFO("            version:%s\n",
+			swsusp_info->uts.version);
+	PAGEMAP_INFO("            machine:%s\n",
+			swsusp_info->uts.machine);
+	PAGEMAP_INFO("    vesion_code:%#08x\n",
+			(unsigned int)swsusp_info->version_code);
+	PAGEMAP_INFO("    num_physpages:%d\n",
+			(unsigned int)swsusp_info->num_physpages);
+	PAGEMAP_INFO("    pages        :%d\n",
+			(unsigned int)swsusp_info->pages);
+	PAGEMAP_INFO("    size         :%d\n",
+			(unsigned int)swsusp_info->size);
+#endif
+
+	nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
+								PAGE_SIZE;
+	pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
+	if (!pfn_pages)
+		goto mem_err;
+	memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
+
+	/* UBOOT using memory */
+	for (i = max_page; i <= high_page; i++)
+		free_page_mark_used(pg_ub2zero(i));
+
+	printf("Allocating %u bytes (nr_pfn_pages %u)\n",
+			nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
+
+	for (i = 0; i < nr_pfn_pages; i++) {
+		u32 idx;
+		pfn_pages[i] = malloc(PAGE_SIZE);
+		memset(pfn_pages[i], 0xff, PAGE_SIZE);
+		if (unlikely(!pfn_pages[i]))
+			goto mem_err;
+		if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
+			goto read_err;
+		for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
+			u32 page = pfn_pages[i][idx];
+			if (page == ~0UL) /* End of list of pages */
+				break;
+			free_page_mark_used(pg_ub2zero(page));
+		}
+	}
+
+	printf("Loading image data pages (%lu pages)\n",
+						swsusp_info->image_pages);
+
+	remap_orig_page = pg2addr(free_page_get_next());
+	remap_temp_page = pg2addr(free_page_get_next());
+
+	remap_orig = (u32 **)remap_orig_page;
+	remap_temp = (u32 **)remap_temp_page;
+	remap_idx = 0;
+
+	for (i = 0; i < swsusp_info->image_pages; i++) {
+		u32 page = pfn_pages[i >> 10][i & 0x3ff];
+		if (unlikely(!pfn_is_valid(page))) {
+			printf("Attempt to restore invalid address %llx\n",
+					pg2phys(page));
+			continue;
+		} else if (unlikely(pfn_is_excluded(page))) {
+			printf("Attempt to restore excluded address %llx\n",
+					pg2phys(page));
+			continue;
+		} else if (unlikely(pfn_is_low(page) &&
+					pfn_is_occupied(page))) {
+			remap_orig[remap_idx] = pg2addr(page);
+			page = free_page_get_next();
+			remap_temp[remap_idx] = pg2addr(page);
+			remap_idx++;
+#ifdef PAGEMAP_DEBUG
+			++total_remap;
+#endif
+			/* If we fill our current page, allocate a new one */
+			if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
+				u32 *next;
+
+				next = pg2addr(free_page_get_next());
+				remap_orig[remap_idx] = next;
+				remap_orig = (u32 **)next;
+
+				next = pg2addr(free_page_get_next());
+				remap_temp[remap_idx] = next;
+				remap_temp = (u32 **)next;
+
+				remap_idx = 0;
+			}
+		}
+		if (image_page_get_next(pg2addr(page)) <= 0)
+			goto read_err;
+	}
+
+	printf("Image loading done.\n");
+	invalidate_icache_all();
+
+	CALL_HOOK(resume_boot, SWSUSPMEM_IMAGEDONE);
+	/* put end markers on the remap list */
+	remap_orig[remap_idx] = (void *) ~0UL;
+	remap_temp[remap_idx] = (void *) ~0UL;
+
+#ifdef PAGEMAP_DEBUG
+	PAGEMAP_INFO("Number of remap pages:%d\n", total_remap);
+	PAGEMAP_INFO("Number of high pages:%d\n", high_page_images);
+	PAGEMAP_INFO("Last read page %d (%08x)\n",
+		     _last_read_pages, _last_read_pages * PAGE_SIZE);
+#endif
+	remap_orig = (u32 **)remap_orig_page;
+	remap_temp = (u32 **)remap_temp_page;
+
+	/* Make a copy of swsusp_finish in a free data page */
+	data_page = pg2addr(free_page_get_next());
+	memcpy(data_page, swsusp_finish_p, PAGE_SIZE);
+	swsusp_finish_copy = (void *) data_page;
+
+	/* Setup context for swsusp_finish at the end of the data_page */
+	context = (struct swsusp_finish_context *) (data_page + PAGE_SIZE -
+					sizeof(struct swsusp_finish_context));
+	context->remap_orig_page = remap_orig_page;
+	context->remap_temp_page = remap_temp_page;
+	memcpy((void *)&context->archdata, (void *)swsusp_info->archdata,
+			sizeof(struct swsusp_archdata));
+
+	/* Get a stack pointer for swsusp_finish, growing down from context */
+	stack_addr = (char *) context;
+
+#ifdef CONFIG_NETCONSOLE
+	/*
+	 * Stop the ethernet stack if NetConsole could have
+	 * left it up
+	 */
+	eth_halt();
+#endif
+#ifdef CONFIG_USB_DEVICE
+	udc_disconnect();
+#endif
+	arch_preboot_os();
+	cleanup_before_linux();
+
+	CALL_HOOK(resume_boot, SWSUSPMEM_RESUME);
+	/* Copy the final data from a safe place */
+	call_with_stack(swsusp_finish_copy, context, stack_addr);
+
+	return 0;
+
+mem_err:
+	printf("Not enough memory.\n");
+	CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
+	goto err;
+
+read_err:
+	printf("Read error while restoring image.\n");
+
+err:
+	__asm__ volatile (
+	"mov	r0, #0\n"
+	"mcr	p15, 0, r0, c7, c5, 0   @ invalidate icache\n"
+	"mcr	p15, 0, r0, c7, c10, 4  @ DSB\n"
+	"mcr	p15, 0, r0, c7, c5, 4   @ ISB\n"
+	: : : "r0", "memory");
+
+	raw_page_exit();
+	image_page_exit();
+	free_page_exit();
+	if (pfn_pages) {
+		for (i = 0; i < nr_pfn_pages; i++)
+			free(pfn_pages[i]);
+		free(pfn_pages);
+	}
+	free(spare_page);
+
+	CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
+	return 1;
+}
+
+U_BOOT_CMD(swsuspmem, 2, 2, do_swsuspmem,
+	"Restore SWSUSP hibernation image from memory",
+	"<address>]"
+);
diff --git a/include/swsuspmem.h b/include/swsuspmem.h
new file mode 100644
index 0000000..3b353ea
--- /dev/null
+++ b/include/swsuspmem.h
@@ -0,0 +1,24 @@
+#ifndef _SWSUSPMEM_H_
+#define _SWSUSPMEM_H_
+
+enum { SWSUSPMEM_NORM = 0,
+	SWSUSPMEM_NOIMAGE = 0x01,
+	SWSUSPMEM_BROKENIMAGE = 0x02,
+	SWSUSPMEM_ENOMEM  = 0x80,
+	SWSUSPMEM_RESTOREFAIL = 0x81,
+};
+
+enum { SWSUSPMEM_IMAGEDONE = 0x01,
+	   SWSUSPMEM_RESUME = 0x02
+};
+
+struct swsuspmem_hook {
+	void (*err_hook)(int errcode);
+	void (*resume_boot)(int  param);
+};
+
+void set_swsuspmem_hook(struct swsuspmem_hook *hook);
+void arch_preboot_os(void);
+void call_with_stack(void (*fn)(void *),
+		void *userdata, void *stack);
+#endif
diff --git a/lib/lzo/lzo1x_decompress.c b/lib/lzo/lzo1x_decompress.c
index e6ff708..ebdf10b 100644
--- a/lib/lzo/lzo1x_decompress.c
+++ b/lib/lzo/lzo1x_decompress.c
@@ -68,13 +68,14 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
 	unsigned char *start = dst;
 	const unsigned char *send = src + src_len;
 	u32 slen, dlen;
-	size_t tmp;
+	size_t tmp, remaining;
 	int r;
 
 	src = parse_header(src);
 	if (!src)
 		return LZO_E_ERROR;
 
+	remaining = *dst_len;
 	while (src < send) {
 		/* read uncompressed block size */
 		dlen = get_unaligned_be32(src);
@@ -93,18 +94,25 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
 		if (slen <= 0 || slen > dlen)
 			return LZO_E_ERROR;
 
+		/* abort if buffer ran out of room */
+		if (dlen > remaining)
+			return LZO_E_OUTPUT_OVERRUN;
+
 		/* decompress */
 		tmp = dlen;
 		r = lzo1x_decompress_safe((u8 *) src, slen, dst, &tmp);
 
-		if (r != LZO_E_OK)
+		if (r != LZO_E_OK) {
+			*dst_len = dst - start;
 			return r;
+		}
 
 		if (dlen != tmp)
 			return LZO_E_ERROR;
 
 		src += slen;
 		dst += dlen;
+		remaining -= dlen;
 	}
 
 	return LZO_E_INPUT_OVERRUN;
-- 
1.8.3.1