1 From 45b3abc592bd685726a6b55693ab95e4cb6065fc Mon Sep 17 00:00:00 2001
2 From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
3 Date: Fri, 19 May 2017 14:27:46 +0900
4 Subject: [PATCH 2/4] Add Hibernation swsusp command support
6 Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
8 common/cmd_swsusp.c | 889 ++++++++++++++++++++++++++++++++++++++++++++++++++++
9 1 file changed, 889 insertions(+)
10 create mode 100644 common/cmd_swsusp.c
12 diff --git a/common/cmd_swsusp.c b/common/cmd_swsusp.c
14 index 0000000..ba05aa4
16 +++ b/common/cmd_swsusp.c
19 + * This program is free software; you can redistribute it and/or modify
20 + * it under the terms of the GNU General Public License as published by
21 + * the Free Software Foundation; either version 2 of the License, or
22 + * (at your option) any later version.
24 + * This program is distributed in the hope that it will be useful,
25 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
26 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
27 + * GNU General Public License for more details.
29 + * You should have received a copy of the GNU General Public License
30 + * along with this program; if not, write to the Free Software
31 + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
39 +#include <linux/lzo.h>
40 +#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
41 +#include <swsuspmem.h>
43 +/* Note for Renesas--based boards:
44 + * We have the following memory split here:
45 + * 0x40000000 - u_boot_lowest - used to store pfns at physical addresses
46 + * u_boot_lowest - 0x8000000 - pfns are relocated, and then later put
47 + * on physical addresses (swsusp_finish)
48 + * 0x8000000 - 0xc0000000 - used to store pfns with physical address
49 + * of 0x200000000 (long address), we have to change offset for them.
50 + * Any pfn with address > 0x8000000 but less than 0x200000000
52 + * For boards which do not have memory above first GB, that will
53 + * still work, as they won't have anything above 0x80000000
54 + * in their image, so for standard 2GB setup ou should put
55 + * your secong GB in 0x80000000-0xC0000000 range, you can
56 + * use MMU for that or if your RAM is continous, it will
57 + * naturally be there. */
59 +DECLARE_GLOBAL_DATA_PTR;
61 +/* #define PAGEMAP_DEBUG */
64 +#define SWSUSP_DEBUG_INFO
67 +#define SWSUSP_KEEP_IMAGE
70 +# define likely(x) __builtin_expect(!!(x), 1)
71 +# define unlikely(x) __builtin_expect(!!(x), 0)
74 +#define HIBERNATE_SIG "S1SUSPEND"
75 +#define PAGE_SIZE 4096
77 +/* Define depending on CONFIG_LBDAF in kernel */
78 +typedef u64 sector_t;
81 +struct swsusp_header {
82 + char reserved[PAGE_SIZE - 20 - sizeof(sector_t) -
83 + sizeof(int) - sizeof(u32) -
84 + sizeof(CRC32_WORD4_t) - sizeof(u32)];
85 + CRC32_WORD4_t comp_crc32;
86 + u32 img_size; /* append */
94 +#define __NEW_UTS_LEN 64
97 + char sysname[__NEW_UTS_LEN + 1];
98 + char nodename[__NEW_UTS_LEN + 1];
99 + char release[__NEW_UTS_LEN + 1];
100 + char version[__NEW_UTS_LEN + 1];
101 + char machine[__NEW_UTS_LEN + 1];
102 + char domainname[__NEW_UTS_LEN + 1];
105 +struct swsusp_archdata {
106 + u32 nosave_backup_phys;
107 + u32 nosave_begin_phys;
108 + u32 nosave_end_phys;
109 + void (*cpu_resume_restore_nosave)(u32, u32, u32);
112 +struct swsusp_info {
113 + struct new_utsname uts;
115 + unsigned long num_physpages;
117 + unsigned long image_pages;
118 + unsigned long pages;
119 + unsigned long size;
120 + char archdata[1024];
123 +struct swap_map_page {
124 + u64 entries[PAGE_SIZE / sizeof(u64) - 1];
128 +struct swsusp_finish_context {
129 + void *remap_orig_page;
130 + void *remap_temp_page;
131 + struct swsusp_archdata archdata;
134 +/* Do not specially exclude any bottom area */
135 +#define USED_ADDRESS_TOP (CONFIG_SYS_SDRAM_BASE)
136 +#define USED_ADDRESS_END (CONFIG_SYS_SDRAM_BASE)
138 +#define PG_UB2ZERO(pg) (pg - CONFIG_SYS_SDRAM_BASE / PAGE_SIZE)
139 +static u32 const exclude_min_page =
140 + (USED_ADDRESS_TOP) / PAGE_SIZE;
141 +static u32 const exclude_max_page =
142 + (USED_ADDRESS_END - 1) / PAGE_SIZE;
143 +static u32 const exclude_min_page_ub =
144 + PG_UB2ZERO((USED_ADDRESS_TOP) / PAGE_SIZE);
145 +static u32 const exclude_max_page_ub =
146 + PG_UB2ZERO((USED_ADDRESS_END-1) / PAGE_SIZE);
147 +#define SF_NOCOMPRESS_MODE 2
149 +#define LZO_HEADER sizeof(size_t)
151 +/* Number of pages/bytes we'll compress at one time. */
152 +#define LZO_UNC_PAGES 32
153 +#define LZO_UNC_SIZE (LZO_UNC_PAGES * PAGE_SIZE)
155 +/* Number of pages/bytes we need for compressed data (worst case). */
156 +#define LZO_CMP_PAGES DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
157 + LZO_HEADER, PAGE_SIZE)
158 +#define LZO_CMP_SIZE (LZO_CMP_PAGES * PAGE_SIZE)
160 +static block_dev_desc_t *swap_dev;
161 +static disk_partition_t swap_info;
163 +static struct swap_map_page *meta_map;
164 +static u64 meta_map_next;
165 +static u64 meta_map_curr;
166 +static u64 meta_map_start;
167 +static int meta_idx;
169 +#ifdef PAGEMAP_DEBUG
170 +static int debugout;
171 +static int _last_read_pages;
172 +#define PAGEMAP_INFO(_msg, ...) do { if (debugout == 1) \
173 + printf(_msg, ## __VA_ARGS__); } while (0)
176 +#define HIGHMEM_PHYS_ADDR 0x200000000ULL
177 +#define HIGHMEM_VA 0x80000000UL
178 +#define HIGHMEM_PFN (HIGHMEM_PHYS_ADDR / PAGE_SIZE)
179 +#define LOW_TOP 0x80000000
180 +#define LOW_TOP_PFN (LOW_TOP / PAGE_SIZE)
181 +#define LOW_BOTTOM CONFIG_SYS_SDRAM_BASE
182 +#define LOW_BOTTOM_PFN (LOW_BOTTOM / PAGE_SIZE)
183 +#define TOP_ADDRESS 0x240000000ULL
185 +static int get_meta(void);
187 +static inline int pfn_is_low(u32 pfn)
189 + return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
192 +static inline int pfn_is_high(u32 pfn)
194 + return (pfn >= HIGHMEM_PFN);
197 +#define pfn_is_valid(p) (pfn_is_low(p) || pfn_is_high(p))
199 +static inline int pfn_is_excluded(u32 pfn)
201 + /* Allow bottom 2 pages for exception vectors */
202 + if (pfn < (LOW_BOTTOM_PFN + 2))
204 + else if (exclude_min_page >= exclude_max_page)
207 + return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
210 +/* PFN to zero-counted page */
211 +static inline u32 pg_ub2zero(u32 pg)
213 + return pg - LOW_BOTTOM_PFN;
216 +/* zero-counted page to PFN */
217 +static inline u32 pg_zero2ub(u32 pg)
219 + return pg + LOW_BOTTOM_PFN;
222 +/* PFN to physical address (64-bit (40-bit)) */
223 +static inline u64 pg2phys(u32 page)
225 + return (u64) page * PAGE_SIZE;
228 +/* PFN to virtual address */
229 +static inline void *pg2addr(u32 page)
232 + if (page >= HIGHMEM_PFN)
233 + addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
235 + addr = (void *) (u32)pg2phys(page);
240 +#ifdef CONFIG_SH_DMA
241 +static inline void *malloc_aligned(u32 size, u32 align)
243 + return (void *)(((u32)malloc(size + align) + align - 1) & ~(align - 1));
248 +static int page_read(u32 page, void *addr)
253 + blk_per_page = PAGE_SIZE / swap_dev->blksz;
254 + cnt = swap_dev->block_read(swap_dev->dev,
255 + swap_info.start + (page * blk_per_page),
256 + blk_per_page, addr);
257 + return cnt != blk_per_page;
260 +#ifndef SWSUSP_KEEP_IMAGE
261 +static int page_write(u32 page, void *addr)
266 + blk_per_page = PAGE_SIZE / swap_dev->blksz;
267 + cnt = swap_dev->block_write(swap_dev->dev,
268 + swap_info.start + (page * blk_per_page),
269 + blk_per_page, addr);
270 + return cnt != blk_per_page;
275 +void __attribute__((section(".rodata")))
276 + __attribute__((optimize("O6", "unroll-loops")))
277 +swsusp_finish(void *userdata)
279 + struct swsusp_finish_context *context = userdata;
283 + const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
285 + remap_orig = context->remap_orig_page;
286 + remap_temp = context->remap_temp_page;
288 + __asm__ volatile ("" : : : "memory");
293 + /* Linked list to next page */
294 + if (idx == lastidx) {
295 + remap_orig = (u32 **)remap_orig[idx];
296 + remap_temp = (u32 **)remap_temp[idx];
299 + if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
301 + orig = remap_orig[idx];
302 + temp = remap_temp[idx];
304 + count = PAGE_SIZE / sizeof(u32) / 32;
307 + "ldmia %[rtemp]!, {r0-r7}\n"
308 + "stmia %[rorig]!, {r0-r7}\n"
309 + "ldmia %[rtemp]!, {r0-r7}\n"
310 + "stmia %[rorig]!, {r0-r7}\n"
311 + "ldmia %[rtemp]!, {r0-r7}\n"
312 + "stmia %[rorig]!, {r0-r7}\n"
313 + "ldmia %[rtemp]!, {r0-r7}\n"
314 + "subs %[count], %[count], #1\n"
315 + "stmia %[rorig]!, {r0-r7}\n"
322 + : "r0", "r1", "r2",
324 + "r6", "r7", "cc", "memory"
327 + count = PAGE_SIZE / sizeof(u32);
331 +#ifdef SWSUSP_CHECK_COPY_RESULT
332 + count = PAGE_SIZE / sizeof(u32);
333 + orig = remap_orig[idx];
334 + temp = remap_temp[idx];
337 + "ldr r3, [%[rorig]]\n"
338 + "ldr r4, [%[rtemp]]\n"
341 + "add %[rorig], %[rorig], #4\n"
342 + "add %[rtemp], %[rtemp], #4\n"
343 + "subs %[count], %[count], #1\n"
349 + [rorig]"+r" (orig),
350 + [rtemp]"+r" (temp),
351 + [count]"+r" (count)
353 + : "r3", "r4", "cc", "memory"
358 + context->archdata.cpu_resume_restore_nosave(
359 + context->archdata.nosave_backup_phys,
360 + context->archdata.nosave_begin_phys,
361 + context->archdata.nosave_end_phys);
364 +static int raw_page_init(u64 start)
366 +#ifdef CONFIG_SH_DMA
367 + meta_map = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
369 + meta_map = malloc(PAGE_SIZE);
375 + meta_map_start = start;
379 +static void raw_page_start(void)
381 + meta_idx = ARRAY_SIZE(meta_map->entries);
382 + meta_map_next = meta_map_start;
385 +static int get_meta(void)
387 + if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
388 + if (!meta_map_next)
390 + if (meta_map_curr != meta_map_next) {
391 +#ifdef PAGEMAP_DEBUG
392 + PAGEMAP_INFO("META: %d (%08x)\n",
393 + (int)meta_map_next,
395 + (meta_map_next * PAGE_SIZE));
397 + if (page_read(meta_map_next, meta_map))
399 + meta_map_curr = meta_map_next;
400 + meta_map_next = meta_map->next_swap;
404 +#ifdef PAGEMAP_DEBUG
406 + static unsigned int pre;
407 + if ((pre+1) != meta_map->entries[meta_idx]) {
408 + PAGEMAP_INFO("DATA-Skipped: %d->%d (%08x->%08x)\n",
410 + (unsigned int)meta_map->entries[meta_idx],
413 + (meta_map->entries[meta_idx] *
416 + pre = meta_map->entries[meta_idx];
417 + _last_read_pages = pre;
423 +static int raw_page_get_next(void *buffer)
428 + if (page_read(meta_map->entries[meta_idx++], buffer))
434 +static void raw_page_exit(void)
440 +static int image_compressed;
441 +static int image_pages_avail;
442 +static unsigned char *unc_buf;
443 +static unsigned char *cmp_buf;
444 +static int unc_offset;
446 +static int image_page_init(int compressed)
451 +#ifdef CONFIG_SH_DMA
452 + cmp_buf = malloc_aligned(LZO_CMP_SIZE, ARCH_DMA_MINALIGN);
454 + cmp_buf = malloc(LZO_CMP_SIZE);
456 + unc_buf = malloc(LZO_UNC_SIZE);
457 + if (!unc_buf || !cmp_buf) {
458 + printf("not enogh memory\n");
461 + image_compressed = compressed;
465 +static void image_page_start(void)
467 + image_pages_avail = 0;
470 +static int image_page_get_next(void *buffer)
472 + if (image_compressed) {
474 + if (!image_pages_avail) {
476 + size_t unc_len, cmp_len, cmp_avail;
478 + ret = raw_page_get_next(cmp_buf);
482 + cmp_len = *(size_t *) cmp_buf;
483 + cmp_avail = PAGE_SIZE;
485 + while (cmp_avail < cmp_len + LZO_HEADER) {
486 + ret = raw_page_get_next(cmp_buf + cmp_avail);
487 + if (unlikely(ret <= 0))
489 + cmp_avail += PAGE_SIZE;
491 + unc_len = LZO_UNC_SIZE;
492 + ret = lzo1x_decompress_safe(cmp_buf + LZO_HEADER,
493 + cmp_len, unc_buf, &unc_len);
494 + if (unlikely(ret != LZO_E_OK)) {
495 + printf("Decompression failure:\n");
496 + printf("ret = %d\n", ret);
497 + printf("cmp_buf = %p\n", cmp_buf + LZO_HEADER);
498 + printf("cmp_len = %zu\n", cmp_len);
499 + printf("unc_len = %zu\n", unc_len);
502 + image_pages_avail = unc_len / PAGE_SIZE;
506 + memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
507 + unc_offset += PAGE_SIZE;
508 + image_pages_avail--;
511 + printf("No LZO support in u-boot.\n");
515 + return raw_page_get_next(buffer);
519 +static void image_page_exit(void)
523 + unc_buf = cmp_buf = NULL;
526 +static void bitmap_set(u32 *bm, unsigned int bit)
528 + bm[bit >> 5] |= (1 << (bit & 0x1f));
531 +static int bitmap_is_set(u32 *bm, unsigned int bit)
533 + return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
536 +static u32 *used_bitmap;
537 +static u32 next_free_page;
538 +static u32 total_pages;
540 +static int free_page_init(void)
542 + total_pages = (u32)((TOP_ADDRESS -
543 + LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
544 + used_bitmap = malloc(total_pages * sizeof(u32) / 32);
550 +static void free_page_start(int offset)
552 + memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
553 + next_free_page = pg_ub2zero(offset);
556 +static void free_page_mark_used(u32 page);
557 +/* Returns full-address based pages */
558 +static int free_page_get_next(void)
560 + while (bitmap_is_set(used_bitmap, next_free_page))
562 + free_page_mark_used(next_free_page);
563 + return pg_zero2ub(next_free_page++);
566 +static void free_page_mark_used(u32 page)
568 + bitmap_set(used_bitmap, page);
571 +static void free_page_exit(void)
574 + used_bitmap = NULL;
577 +int do_swsusp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
581 + void *spare_page = NULL;
582 + struct swsusp_header *swsusp_header;
583 + struct swsusp_info *swsusp_info;
584 + struct swsusp_finish_context *context;
588 + u32 **pfn_pages = NULL;
589 + u32 *remap_orig_page;
590 + u32 *remap_temp_page;
595 + void (*swsusp_finish_copy)(void *);
600 + CRC32_WORD4_t calc_crc;
602 +#ifdef PAGEMAP_DEBUG
603 + int high_page_images = 0;
604 + int total_remap = 0;
605 + if (getenv("hybdebug") != NULL)
609 + void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
612 + printf("usage: swsusp <interface> "
613 + "[<dev[:part]>] [<offset>]\n");
619 + offset = simple_strtoul(argv[3], &ep, 16);
621 + printf("Invalid block offset\n");
626 + /* Allow for 32 pages of stack */
627 + max_page = gd->start_addr_sp / PAGE_SIZE - 32;
628 + high_page = (((gd->relocaddr
629 + + _bss_end_ofs)+(PAGE_SIZE-1)) / PAGE_SIZE) + 1;
630 +#define pfn_is_occupied(pfn) (page > max_page && page <= high_page)
631 +#ifdef PAGEMAP_DEBUG
632 + PAGEMAP_INFO(" *gd->start_addr_sp:%p\n", (void *)gd->start_addr_sp);
633 + PAGEMAP_INFO(" *gd->relocaddr:%p\n", (void *)gd->relocaddr);
634 + PAGEMAP_INFO(" *bss_start_offset:%d bss_end_offset:%d\n",
635 + (int)_bss_start_ofs, (int)_bss_end_ofs);
636 + PAGEMAP_INFO(" UBOOT own memory [%p-%p]\n",
637 + pg2addr(max_page), pg2addr(high_page));
640 + if (free_page_init())
642 + free_page_start(exclude_max_page + 1);
644 +#ifdef CONFIG_SH_DMA
645 + spare_page = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
647 + spare_page = malloc(PAGE_SIZE);
652 + ret = get_device_and_partition(argv[1], argv[2], &swap_dev, &swap_info,
657 + swsusp_header = spare_page;
658 + if (page_read(offset, swsusp_header))
661 +#ifdef SWSUSP_DEBUG_INFO
662 + PAGEMAP_INFO("swssp_header:\n");
663 + PAGEMAP_INFO(" comp_crc: <snip>\n");
664 + PAGEMAP_INFO(" img_size: %d\n", swsusp_header->img_size);
665 + PAGEMAP_INFO(" image(swap firest sector): %08x\n",
666 + (unsigned int)swsusp_header->image);
667 + PAGEMAP_INFO(" flags: %08x\n", swsusp_header->flags);
668 + PAGEMAP_INFO(" orig_sig:%s\n", swsusp_header->orig_sig);
669 + PAGEMAP_INFO(" sig:%s\n", swsusp_header->sig);
670 +#endif /* SWSUSP_DEBUG_INFO */
672 + if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)) {
673 + printf("No hibernation image present\n");
678 + memset(&calc_crc, 0, sizeof(calc_crc));
679 + calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
680 + swsusp_header->img_size, &calc_crc);
682 + if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
683 + sizeof(CRC32_WORD4_t))) {
684 + printf("Bad CRC for image, image: %08x:%08x:%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
685 + swsusp_header->comp_crc32.crc_w[0],
686 + swsusp_header->comp_crc32.crc_w[1],
687 + swsusp_header->comp_crc32.crc_w[2],
688 + swsusp_header->comp_crc32.crc_w[3],
689 + calc_crc.crc_w[0], calc_crc.crc_w[1],
690 + calc_crc.crc_w[2], calc_crc.crc_w[3]);
695 + /* Overwrite header if necessary */
696 +#ifndef SWSUSP_KEEP_IMAGE
697 + if (memcmp(swsusp_header->sig, swsusp_header->orig_sig, 10)) {
698 + memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
699 + if (page_write(offset, swsusp_header))
700 + printf("Write error resetting header\n");
704 + if (raw_page_init(swsusp_header->image))
708 + if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
710 + image_page_start();
712 + swsusp_info = spare_page;
713 + if (raw_page_get_next(swsusp_info) <= 0)
716 +#ifdef SWSUSP_DEBUG_INFO
717 + PAGEMAP_INFO("swsup_info:\n");
718 + PAGEMAP_INFO(" utsname.sysname:%s\n", swsusp_info->uts.sysname);
719 + PAGEMAP_INFO(" nodename:%s\n", swsusp_info->uts.nodename);
720 + PAGEMAP_INFO(" release:%s\n", swsusp_info->uts.release);
721 + PAGEMAP_INFO(" version:%s\n", swsusp_info->uts.version);
722 + PAGEMAP_INFO(" machine:%s\n", swsusp_info->uts.machine);
723 + PAGEMAP_INFO(" vesion_code:%#08x\n",
724 + (unsigned int)swsusp_info->version_code);
725 + PAGEMAP_INFO(" num_physpages:%d\n",
726 + (unsigned int)swsusp_info->num_physpages);
727 + PAGEMAP_INFO(" pages :%d\n",
728 + (unsigned int)swsusp_info->pages);
729 + PAGEMAP_INFO(" size :%d\n",
730 + (unsigned int)swsusp_info->size);
733 + nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
735 + pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
738 + memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
740 + /* UBOOT using memory */
741 + for (i = max_page; i <= high_page; i++)
742 + free_page_mark_used(pg_ub2zero(i));
744 + printf("Allocating %u bytes (nr_pfn_pages %u)\n",
745 + nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
747 + for (i = 0; i < nr_pfn_pages; i++) {
749 +#ifdef CONFIG_SH_DMA
750 + pfn_pages[i] = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
752 + pfn_pages[i] = malloc(PAGE_SIZE);
754 + if (unlikely(!pfn_pages[i]))
756 + if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
758 + for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
759 + u32 page = pfn_pages[i][idx];
762 + free_page_mark_used(pg_ub2zero(page));
766 + printf("Loading image data pages (%lu pages)\n",
767 + swsusp_info->image_pages);
769 + remap_orig_page = pg2addr(free_page_get_next());
770 + remap_temp_page = pg2addr(free_page_get_next());
772 + remap_orig = (u32 **)remap_orig_page;
773 + remap_temp = (u32 **)remap_temp_page;
776 + m = (swsusp_info->image_pages / 10) ? : 1;
777 + for (i = 0; i < swsusp_info->image_pages; i++) {
778 + u32 page = pfn_pages[i >> 10][i & 0x3ff];
779 + if (unlikely(!pfn_is_valid(page))) {
780 + printf("Attempt to restore invalid address %llx\n",
783 + } else if (unlikely(pfn_is_excluded(page))) {
784 + printf("Attempt to restore excluded address %llx\n",
787 + } else if (unlikely(pfn_is_low(page) &&
788 + pfn_is_occupied(page))) {
789 + remap_orig[remap_idx] = pg2addr(page);
790 + page = free_page_get_next();
791 + remap_temp[remap_idx] = pg2addr(page);
793 +#ifdef PAGEMAP_DEBUG
796 + /* If we fill our current page, allocate a new one */
797 + if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
800 + next = pg2addr(free_page_get_next());
801 + remap_orig[remap_idx] = next;
802 + remap_orig = (u32 **)next;
804 + next = pg2addr(free_page_get_next());
805 + remap_temp[remap_idx] = next;
806 + remap_temp = (u32 **)next;
811 + if (image_page_get_next(pg2addr(page)) <= 0)
814 + printf("Image loading progress: %3d%%\n", 10 * i / m);
817 + printf("Image loading done.\n");
818 + invalidate_icache_all();
820 + /* put end markers on the remap list */
821 + remap_orig[remap_idx] = (void *) ~0UL;
822 + remap_temp[remap_idx] = (void *) ~0UL;
824 +#ifdef PAGEMAP_DEBUG
825 + PAGEMAP_INFO("Number of remap pages:%d\n",
827 + PAGEMAP_INFO("Number of high pages:%d\n",
829 + PAGEMAP_INFO("Last read page %d (%08x)\n",
831 + _last_read_pages * PAGE_SIZE);
833 + remap_orig = (u32 **)remap_orig_page;
834 + remap_temp = (u32 **)remap_temp_page;
836 + /* Make a copy of swsusp_finish in a free data page */
837 + data_page = pg2addr(free_page_get_next());
838 + memcpy(data_page, swsusp_finish_p, PAGE_SIZE);
839 + swsusp_finish_copy = (void *) data_page;
841 + /* Setup context for swsusp_finish at the end of the data_page */
842 + context = (struct swsusp_finish_context *) (data_page + PAGE_SIZE -
843 + sizeof(struct swsusp_finish_context));
844 + context->remap_orig_page = remap_orig_page;
845 + context->remap_temp_page = remap_temp_page;
846 + memcpy((void *)&context->archdata, (void *)swsusp_info->archdata,
847 + sizeof(struct swsusp_archdata));
849 + /* Get a stack pointer for swsusp_finish, growing down from context */
850 + stack_addr = (char *) context;
852 +#ifdef CONFIG_NETCONSOLE
854 + * Stop the ethernet stack if NetConsole could have
860 +#ifdef CONFIG_USB_DEVICE
863 +#ifdef PAGEMAP_DEBUG
864 + PAGEMAP_INFO("Execution routine: %08x\n",
865 + (u32)context->archdata.cpu_resume_restore_nosave);
868 + cleanup_before_linux();
870 + /* Copy the final data from a safe place */
871 + call_with_stack(swsusp_finish_copy, context, stack_addr);
876 + printf("Not enough memory.\n");
880 + printf("Read error while restoring image.\n");
885 + "mcr p15, 0, r0, c7, c5, 0 @ invalidate icache\n"
886 + "mcr p15, 0, r0, c7, c10, 4 @ DSB\n"
887 + "mcr p15, 0, r0, c7, c5, 4 @ ISB\n"
888 + : : : "r0", "memory");
894 + for (i = 0; i < nr_pfn_pages; i++)
895 + free(pfn_pages[i]);
903 +U_BOOT_CMD(swsusp, 4, 0, do_swsusp,
904 + "Restore SWSUSP hibernation image",
905 + "<interface> [<dev[:part]>] [<offset>]"