diff options
author | Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com> | 2017-05-22 00:11:15 +0900 |
---|---|---|
committer | Jan-Simon Moeller <jsmoeller@linuxfoundation.org> | 2017-05-25 13:47:36 +0000 |
commit | 3bc8101a0775d37da28672b2dbdfe806c961a52c (patch) | |
tree | d5a7793037e00c8c78409c654dc9cc8ab804a94e /meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch | |
parent | a14e289caaae4c342c2bc686bd5d327ed612b0fc (diff) |
Add u-boot Hibernation code for porter board.
This patch set is a support to Hibernation for a porter board.
I've commit with Hibernation Off patch, because it depends strongly on user land.
If you can use Hibernation, Please add local.conf agl-porter-hibernate.
OVERRIDES .= ":agl-porter-hibernate"
DISTRO_FEATURES_append = " agl-porter-hibernate"
Change-Id: I3f0560074b710c27f49a73ca871038246d222b73
Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
Reviewed-on: https://gerrit.automotivelinux.org/gerrit/9449
Tested-by: Jenkins Job builder account <agl-jobbuilder@automotivelinux.org>
ci-image-build: Jenkins Job builder account <agl-jobbuilder@automotivelinux.org>
ci-image-boot-test: Jenkins Job builder account <agl-jobbuilder@automotivelinux.org>
Reviewed-by: Jan-Simon Moeller <jsmoeller@linuxfoundation.org>
Diffstat (limited to 'meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch')
-rwxr-xr-x | meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch | 1058 |
1 files changed, 1058 insertions, 0 deletions
diff --git a/meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch b/meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch new file mode 100755 index 000000000..8bfcccbbf --- /dev/null +++ b/meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch @@ -0,0 +1,1058 @@ +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 + |