From 4ce00daa904a40701ab6bed44506fe97b8f1da47 Mon Sep 17 00:00:00 2001 From: Yuichi Kusakabe Date: Fri, 19 May 2017 14:48:38 +0900 Subject: [PATCH 3/4] Add Hibernation swsuspmem command support Signed-off-by: Yuichi Kusakabe --- 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 +#include +#include +#include + +#include +#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h" +#include + +/* 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: \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", + "
]" +); + +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: \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", + "
]" +); 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