meta-agl-bsp/meta-renesas/recipes-bsp/u-boot/u-boot/hibernation/0002-Add-Hibernation-swsusp-command-support.patch

   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
   5
   6 Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
   7 ---
   8  common/cmd_swsusp.c | 889 ++++++++++++++++++++++++++++++++++++++++++++++++++++
   9  1 file changed, 889 insertions(+)
  10  create mode 100644 common/cmd_swsusp.c
  11
  12 diff --git a/common/cmd_swsusp.c b/common/cmd_swsusp.c
  13 new file mode 100644
  14 index 0000000..ba05aa4
  15 --- /dev/null
  16 +++ b/common/cmd_swsusp.c
  17 @@ -0,0 +1,889 @@
  18 +/*
  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.
  23 + *
  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.
  28 + *
  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.
  32 + */
  33 +
  34 +#include <common.h>
  35 +#include <command.h>
  36 +#include <part.h>
  37 +#include <malloc.h>
  38 +
  39 +#include <linux/lzo.h>
  40 +#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
  41 +#include <swsuspmem.h>
  42 +
  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
  51 + * is an error.
  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. */
  58 +
  59 +DECLARE_GLOBAL_DATA_PTR;
  60 +
  61 +/* #define PAGEMAP_DEBUG */
  62 +
  63 +#ifdef PAGEMAP_DEBUG
  64 +#define SWSUSP_DEBUG_INFO
  65 +#endif
  66 +
  67 +#define SWSUSP_KEEP_IMAGE
  68 +
  69 +#ifndef likely
  70 +# define likely(x)     __builtin_expect(!!(x), 1)
  71 +# define unlikely(x)   __builtin_expect(!!(x), 0)
  72 +#endif
  73 +
  74 +#define HIBERNATE_SIG "S1SUSPEND"
  75 +#define PAGE_SIZE 4096
  76 +
  77 +/* Define depending on CONFIG_LBDAF in kernel */
  78 +typedef u64 sector_t;
  79 +
  80 +
  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 */
  87 +       u32     crc32;
  88 +       sector_t        image;
  89 +       unsigned int flags;
  90 +       char    orig_sig[10];
  91 +       char    sig[10];
  92 +} __packed;
  93 +
  94 +#define __NEW_UTS_LEN 64
  95 +
  96 +struct new_utsname {
  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];
 103 +};
 104 +
 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);
 110 +};
 111 +
 112 +struct swsusp_info {
 113 +       struct new_utsname      uts;
 114 +       u32                     version_code;
 115 +       unsigned long           num_physpages;
 116 +       int                     cpus;
 117 +       unsigned long           image_pages;
 118 +       unsigned long           pages;
 119 +       unsigned long           size;
 120 +       char                    archdata[1024];
 121 +};
 122 +
 123 +struct swap_map_page {
 124 +       u64 entries[PAGE_SIZE / sizeof(u64) - 1];
 125 +       u64 next_swap;
 126 +};
 127 +
 128 +struct swsusp_finish_context {
 129 +       void *remap_orig_page;
 130 +       void *remap_temp_page;
 131 +       struct swsusp_archdata archdata;
 132 +};
 133 +
 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)
 137 +
 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
 148 +
 149 +#define LZO_HEADER      sizeof(size_t)
 150 +
 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)
 154 +
 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)
 159 +
 160 +static block_dev_desc_t *swap_dev;
 161 +static disk_partition_t swap_info;
 162 +
 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;
 168 +
 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)
 174 +#endif
 175 +
 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
 184 +
 185 +static int get_meta(void);
 186 +
 187 +static inline int pfn_is_low(u32 pfn)
 188 +{
 189 +       return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
 190 +}
 191 +
 192 +static inline int pfn_is_high(u32 pfn)
 193 +{
 194 +       return (pfn >= HIGHMEM_PFN);
 195 +}
 196 +
 197 +#define pfn_is_valid(p)                (pfn_is_low(p) || pfn_is_high(p))
 198 +
 199 +static inline int pfn_is_excluded(u32 pfn)
 200 +{
 201 +       /* Allow bottom 2 pages for exception vectors */
 202 +       if (pfn < (LOW_BOTTOM_PFN + 2))
 203 +               return 0;
 204 +       else if (exclude_min_page >= exclude_max_page)
 205 +               return 0;
 206 +       else
 207 +               return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
 208 +}
 209 +
 210 +/* PFN to zero-counted page */
 211 +static inline u32 pg_ub2zero(u32 pg)
 212 +{
 213 +       return pg - LOW_BOTTOM_PFN;
 214 +}
 215 +
 216 +/* zero-counted page to PFN */
 217 +static inline u32 pg_zero2ub(u32 pg)
 218 +{
 219 +       return pg + LOW_BOTTOM_PFN;
 220 +}
 221 +
 222 +/* PFN to physical address (64-bit (40-bit)) */
 223 +static inline u64 pg2phys(u32 page)
 224 +{
 225 +       return (u64) page * PAGE_SIZE;
 226 +}
 227 +
 228 +/* PFN to virtual address */
 229 +static inline void *pg2addr(u32 page)
 230 +{
 231 +       void *addr;
 232 +       if (page >= HIGHMEM_PFN)
 233 +               addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
 234 +       else
 235 +               addr = (void *) (u32)pg2phys(page);
 236 +
 237 +       return addr;
 238 +}
 239 +
 240 +#ifdef CONFIG_SH_DMA
 241 +static inline void *malloc_aligned(u32 size, u32 align)
 242 +{
 243 +       return (void *)(((u32)malloc(size + align) + align - 1) & ~(align - 1));
 244 +}
 245 +
 246 +#endif
 247 +
 248 +static int page_read(u32 page, void *addr)
 249 +{
 250 +       __u32 cnt;
 251 +       int blk_per_page;
 252 +
 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;
 258 +}
 259 +
 260 +#ifndef SWSUSP_KEEP_IMAGE
 261 +static int page_write(u32 page, void *addr)
 262 +{
 263 +       __u32 cnt;
 264 +       int blk_per_page;
 265 +
 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;
 271 +}
 272 +#endif
 273 +
 274 +#define FAST_COPY
 275 +void __attribute__((section(".rodata")))
 276 +       __attribute__((optimize("O6", "unroll-loops")))
 277 +swsusp_finish(void *userdata)
 278 +{
 279 +       struct swsusp_finish_context *context = userdata;
 280 +       u32 **remap_orig;
 281 +       u32 **remap_temp;
 282 +       int idx = 0;
 283 +       const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
 284 +
 285 +       remap_orig = context->remap_orig_page;
 286 +       remap_temp = context->remap_temp_page;
 287 +
 288 +       __asm__ volatile ("" : : : "memory");
 289 +       for (;;) {
 290 +               u32 *orig, *temp;
 291 +               int count;
 292 +
 293 +               /* Linked list to next page */
 294 +               if (idx == lastidx) {
 295 +                       remap_orig = (u32 **)remap_orig[idx];
 296 +                       remap_temp = (u32 **)remap_temp[idx];
 297 +                       idx = 0;
 298 +               }
 299 +               if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
 300 +                       break;
 301 +               orig = remap_orig[idx];
 302 +               temp = remap_temp[idx];
 303 +#ifdef FAST_COPY
 304 +               count = PAGE_SIZE / sizeof(u32) / 32;
 305 +               __asm__ volatile (
 306 +                       "1:\n"
 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"
 316 +                       "bgt 1b\n"
 317 +                       : /* No outputs */
 318 +                       :
 319 +                         [rorig]"h" (orig),
 320 +                         [rtemp]"h" (temp),
 321 +                         [count]"h" (count)
 322 +                       : "r0", "r1", "r2",
 323 +                         "r3", "r4", "r5",
 324 +                         "r6", "r7", "cc", "memory"
 325 +               );
 326 +#else
 327 +               count = PAGE_SIZE / sizeof(u32);
 328 +               while (count--)
 329 +                       *orig++ = *temp++;
 330 +#endif
 331 +#ifdef SWSUSP_CHECK_COPY_RESULT
 332 +               count = PAGE_SIZE / sizeof(u32);
 333 +               orig = remap_orig[idx];
 334 +               temp = remap_temp[idx];
 335 +               __asm__ volatile (
 336 +                       "1:\n"
 337 +                       "ldr r3, [%[rorig]]\n"
 338 +                       "ldr r4, [%[rtemp]]\n"
 339 +                       "cmp r3, r4\n"
 340 +                       "bne 2f\n"
 341 +                       "add %[rorig], %[rorig], #4\n"
 342 +                       "add %[rtemp], %[rtemp], #4\n"
 343 +                       "subs %[count], %[count], #1\n"
 344 +                       "bgt 1b\n"
 345 +                       "b 3f\n"
 346 +                       "2:b 2b\n"
 347 +                       "3:\n"
 348 +                       :
 349 +                        [rorig]"+r" (orig),
 350 +                        [rtemp]"+r" (temp),
 351 +                        [count]"+r" (count)
 352 +                       :
 353 +                       : "r3", "r4", "cc", "memory"
 354 +               );
 355 +#endif
 356 +               idx++;
 357 +       }
 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);
 362 +}
 363 +
 364 +static int raw_page_init(u64 start)
 365 +{
 366 +#ifdef CONFIG_SH_DMA
 367 +       meta_map = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
 368 +#else
 369 +       meta_map = malloc(PAGE_SIZE);
 370 +#endif
 371 +       if (!meta_map)
 372 +               return -1;
 373 +       meta_map_next = 0;
 374 +       meta_map_curr = 0;
 375 +       meta_map_start = start;
 376 +       return 0;
 377 +}
 378 +
 379 +static void raw_page_start(void)
 380 +{
 381 +       meta_idx = ARRAY_SIZE(meta_map->entries);
 382 +       meta_map_next = meta_map_start;
 383 +}
 384 +
 385 +static int get_meta(void)
 386 +{
 387 +       if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
 388 +               if (!meta_map_next)
 389 +                       return 0;
 390 +               if (meta_map_curr != meta_map_next) {
 391 +#ifdef PAGEMAP_DEBUG
 392 +                       PAGEMAP_INFO("META: %d (%08x)\n",
 393 +                               (int)meta_map_next,
 394 +                               (unsigned int)
 395 +                                       (meta_map_next * PAGE_SIZE));
 396 +#endif
 397 +                       if (page_read(meta_map_next, meta_map))
 398 +                               return -1;
 399 +                       meta_map_curr = meta_map_next;
 400 +                       meta_map_next = meta_map->next_swap;
 401 +               }
 402 +               meta_idx = 0;
 403 +       }
 404 +#ifdef PAGEMAP_DEBUG
 405 +       {
 406 +               static unsigned int pre;
 407 +               if ((pre+1) != meta_map->entries[meta_idx]) {
 408 +                       PAGEMAP_INFO("DATA-Skipped: %d->%d (%08x->%08x)\n",
 409 +                               pre,
 410 +                               (unsigned int)meta_map->entries[meta_idx],
 411 +                               pre*PAGE_SIZE,
 412 +                               (unsigned int)
 413 +                                       (meta_map->entries[meta_idx] *
 414 +                                               PAGE_SIZE));
 415 +               }
 416 +               pre = meta_map->entries[meta_idx];
 417 +               _last_read_pages = pre;
 418 +       }
 419 +#endif
 420 +       return 1;
 421 +}
 422 +
 423 +static int raw_page_get_next(void *buffer)
 424 +{
 425 +       if (!get_meta())
 426 +               return 0;
 427 +
 428 +       if (page_read(meta_map->entries[meta_idx++], buffer))
 429 +               return -1;
 430 +
 431 +       return 1;
 432 +}
 433 +
 434 +static void raw_page_exit(void)
 435 +{
 436 +       free(meta_map);
 437 +       meta_map = NULL;
 438 +}
 439 +
 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;
 445 +
 446 +static int image_page_init(int compressed)
 447 +{
 448 +       if (!compressed)
 449 +               return 1;
 450 +
 451 +#ifdef CONFIG_SH_DMA
 452 +       cmp_buf = malloc_aligned(LZO_CMP_SIZE, ARCH_DMA_MINALIGN);
 453 +#else
 454 +       cmp_buf = malloc(LZO_CMP_SIZE);
 455 +#endif
 456 +       unc_buf = malloc(LZO_UNC_SIZE);
 457 +       if (!unc_buf || !cmp_buf) {
 458 +               printf("not enogh memory\n");
 459 +               return 1;
 460 +       }
 461 +       image_compressed = compressed;
 462 +       return 0;
 463 +}
 464 +
 465 +static void image_page_start(void)
 466 +{
 467 +       image_pages_avail = 0;
 468 +}
 469 +
 470 +static int image_page_get_next(void *buffer)
 471 +{
 472 +       if (image_compressed) {
 473 +#ifdef CONFIG_LZO
 474 +               if (!image_pages_avail) {
 475 +                       int ret;
 476 +                       size_t unc_len, cmp_len, cmp_avail;
 477 +
 478 +                       ret = raw_page_get_next(cmp_buf);
 479 +                       if (ret <= 0)
 480 +                               return ret;
 481 +
 482 +                       cmp_len = *(size_t *) cmp_buf;
 483 +                       cmp_avail = PAGE_SIZE;
 484 +
 485 +                       while (cmp_avail < cmp_len + LZO_HEADER) {
 486 +                               ret = raw_page_get_next(cmp_buf + cmp_avail);
 487 +                               if (unlikely(ret <= 0))
 488 +                                       return ret;
 489 +                               cmp_avail += PAGE_SIZE;
 490 +                       }
 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);
 500 +                               return ret;
 501 +                       }
 502 +                       image_pages_avail = unc_len / PAGE_SIZE;
 503 +                       unc_offset = 0;
 504 +               }
 505 +
 506 +               memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
 507 +               unc_offset += PAGE_SIZE;
 508 +               image_pages_avail--;
 509 +               return 1;
 510 +#else
 511 +               printf("No LZO support in u-boot.\n");
 512 +               return -1;
 513 +#endif
 514 +       } else {
 515 +               return raw_page_get_next(buffer);
 516 +       }
 517 +}
 518 +
 519 +static void image_page_exit(void)
 520 +{
 521 +       free(unc_buf);
 522 +       free(cmp_buf);
 523 +       unc_buf = cmp_buf = NULL;
 524 +}
 525 +
 526 +static void bitmap_set(u32 *bm, unsigned int bit)
 527 +{
 528 +       bm[bit >> 5] |= (1 << (bit & 0x1f));
 529 +}
 530 +
 531 +static int bitmap_is_set(u32 *bm, unsigned int bit)
 532 +{
 533 +       return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
 534 +}
 535 +
 536 +static u32 *used_bitmap;
 537 +static u32 next_free_page;
 538 +static u32 total_pages;
 539 +
 540 +static int free_page_init(void)
 541 +{
 542 +       total_pages = (u32)((TOP_ADDRESS -
 543 +                       LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
 544 +       used_bitmap = malloc(total_pages * sizeof(u32) / 32);
 545 +       if (!used_bitmap)
 546 +               return -1;
 547 +       return 0;
 548 +}
 549 +
 550 +static void free_page_start(int offset)
 551 +{
 552 +       memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
 553 +       next_free_page = pg_ub2zero(offset);
 554 +}
 555 +
 556 +static void free_page_mark_used(u32 page);
 557 +/* Returns full-address based pages */
 558 +static int free_page_get_next(void)
 559 +{
 560 +       while (bitmap_is_set(used_bitmap, next_free_page))
 561 +               next_free_page++;
 562 +       free_page_mark_used(next_free_page);
 563 +       return pg_zero2ub(next_free_page++);
 564 +}
 565 +
 566 +static void free_page_mark_used(u32 page)
 567 +{
 568 +       bitmap_set(used_bitmap, page);
 569 +}
 570 +
 571 +static void free_page_exit(void)
 572 +{
 573 +       free(used_bitmap);
 574 +       used_bitmap = NULL;
 575 +}
 576 +
 577 +int do_swsusp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 578 +{
 579 +       int ret;
 580 +       __u32 offset = 0;
 581 +       void *spare_page = NULL;
 582 +       struct swsusp_header *swsusp_header;
 583 +       struct swsusp_info *swsusp_info;
 584 +       struct swsusp_finish_context *context;
 585 +       int max_page;
 586 +       int i;
 587 +       u32 nr_pfn_pages;
 588 +       u32 **pfn_pages = NULL;
 589 +       u32 *remap_orig_page;
 590 +       u32 *remap_temp_page;
 591 +       u32 **remap_orig;
 592 +       u32 **remap_temp;
 593 +       int remap_idx;
 594 +       int m;
 595 +       void (*swsusp_finish_copy)(void *);
 596 +       char *data_page;
 597 +       char *stack_addr;
 598 +       int high_page;
 599 +#ifdef USE_CRC_32x4
 600 +       CRC32_WORD4_t calc_crc;
 601 +#endif
 602 +#ifdef PAGEMAP_DEBUG
 603 +       int high_page_images = 0;
 604 +       int total_remap = 0;
 605 +       if (getenv("hybdebug") != NULL)
 606 +               debugout = 1;
 607 +#endif
 608 +       /* Address hack */
 609 +       void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
 610 +
 611 +       if (argc < 2) {
 612 +               printf("usage: swsusp <interface> "
 613 +                      "[<dev[:part]>] [<offset>]\n");
 614 +               return 0;
 615 +       }
 616 +
 617 +       if (argc == 4) {
 618 +               char *ep;
 619 +               offset = simple_strtoul(argv[3], &ep, 16);
 620 +               if (*ep) {
 621 +                       printf("Invalid block offset\n");
 622 +                       return 1;
 623 +               }
 624 +       }
 625 +
 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));
 638 +#endif
 639 +
 640 +       if (free_page_init())
 641 +               goto mem_err;
 642 +       free_page_start(exclude_max_page + 1);
 643 +
 644 +#ifdef CONFIG_SH_DMA
 645 +       spare_page = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
 646 +#else
 647 +       spare_page = malloc(PAGE_SIZE);
 648 +#endif
 649 +       if (!spare_page)
 650 +               goto mem_err;
 651 +
 652 +       ret = get_device_and_partition(argv[1], argv[2], &swap_dev, &swap_info,
 653 +                                                               1);
 654 +       if (ret < 0)
 655 +               goto err;
 656 +
 657 +       swsusp_header = spare_page;
 658 +       if (page_read(offset, swsusp_header))
 659 +               goto read_err;
 660 +
 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 */
 671 +
 672 +       if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)) {
 673 +               printf("No hibernation image present\n");
 674 +               return 0;
 675 +       }
 676 +
 677 +#ifdef USE_CRC_32x4
 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);
 681 +
 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]);
 691 +               return 0;
 692 +       }
 693 +#endif
 694 +
 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");
 701 +       }
 702 +#endif
 703 +
 704 +       if (raw_page_init(swsusp_header->image))
 705 +               goto mem_err;
 706 +       raw_page_start();
 707 +
 708 +       if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
 709 +               goto mem_err;
 710 +       image_page_start();
 711 +
 712 +       swsusp_info = spare_page;
 713 +       if (raw_page_get_next(swsusp_info) <= 0)
 714 +               goto read_err;
 715 +
 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);
 731 +#endif
 732 +
 733 +       nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
 734 +                                                               PAGE_SIZE;
 735 +       pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
 736 +       if (!pfn_pages)
 737 +               goto mem_err;
 738 +       memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
 739 +
 740 +       /* UBOOT using memory */
 741 +       for (i = max_page; i <= high_page; i++)
 742 +               free_page_mark_used(pg_ub2zero(i));
 743 +
 744 +       printf("Allocating %u bytes (nr_pfn_pages %u)\n",
 745 +                       nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
 746 +
 747 +       for (i = 0; i < nr_pfn_pages; i++) {
 748 +               u32 idx;
 749 +#ifdef CONFIG_SH_DMA
 750 +               pfn_pages[i] = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
 751 +#else
 752 +               pfn_pages[i] = malloc(PAGE_SIZE);
 753 +#endif
 754 +               if (unlikely(!pfn_pages[i]))
 755 +                       goto mem_err;
 756 +               if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
 757 +                       goto read_err;
 758 +               for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
 759 +                       u32 page = pfn_pages[i][idx];
 760 +                       if (page == ~0UL)
 761 +                               break;
 762 +                       free_page_mark_used(pg_ub2zero(page));
 763 +               }
 764 +       }
 765 +
 766 +       printf("Loading image data pages (%lu pages)\n",
 767 +                                               swsusp_info->image_pages);
 768 +
 769 +       remap_orig_page = pg2addr(free_page_get_next());
 770 +       remap_temp_page = pg2addr(free_page_get_next());
 771 +
 772 +       remap_orig = (u32 **)remap_orig_page;
 773 +       remap_temp = (u32 **)remap_temp_page;
 774 +       remap_idx = 0;
 775 +
 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",
 781 +                                       pg2phys(page));
 782 +                       continue;
 783 +               } else if (unlikely(pfn_is_excluded(page))) {
 784 +                       printf("Attempt to restore excluded address %llx\n",
 785 +                                       pg2phys(page));
 786 +                       continue;
 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);
 792 +                       remap_idx++;
 793 +#ifdef PAGEMAP_DEBUG
 794 +                       ++total_remap;
 795 +#endif
 796 +                       /* If we fill our current page, allocate a new one */
 797 +                       if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
 798 +                               u32 *next;
 799 +
 800 +                               next = pg2addr(free_page_get_next());
 801 +                               remap_orig[remap_idx] = next;
 802 +                               remap_orig = (u32 **)next;
 803 +
 804 +                               next = pg2addr(free_page_get_next());
 805 +                               remap_temp[remap_idx] = next;
 806 +                               remap_temp = (u32 **)next;
 807 +
 808 +                               remap_idx = 0;
 809 +                       }
 810 +               }
 811 +               if (image_page_get_next(pg2addr(page)) <= 0)
 812 +                       goto read_err;
 813 +               if (!(i % m))
 814 +                       printf("Image loading progress: %3d%%\n", 10 * i / m);
 815 +       }
 816 +
 817 +       printf("Image loading done.\n");
 818 +       invalidate_icache_all();
 819 +
 820 +       /* put end markers on the remap list */
 821 +       remap_orig[remap_idx] = (void *) ~0UL;
 822 +       remap_temp[remap_idx] = (void *) ~0UL;
 823 +
 824 +#ifdef PAGEMAP_DEBUG
 825 +       PAGEMAP_INFO("Number of remap pages:%d\n",
 826 +                       total_remap);
 827 +       PAGEMAP_INFO("Number of high pages:%d\n",
 828 +                       high_page_images);
 829 +       PAGEMAP_INFO("Last read page %d (%08x)\n",
 830 +                       _last_read_pages,
 831 +                       _last_read_pages * PAGE_SIZE);
 832 +#endif
 833 +       remap_orig = (u32 **)remap_orig_page;
 834 +       remap_temp = (u32 **)remap_temp_page;
 835 +
 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;
 840 +
 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));
 848 +
 849 +       /* Get a stack pointer for swsusp_finish, growing down from context */
 850 +       stack_addr = (char *) context;
 851 +
 852 +#ifdef CONFIG_NETCONSOLE
 853 +       /*
 854 +        * Stop the ethernet stack if NetConsole could have
 855 +        * left it up
 856 +        */
 857 +       eth_halt();
 858 +#endif
 859 +
 860 +#ifdef CONFIG_USB_DEVICE
 861 +       udc_disconnect();
 862 +#endif
 863 +#ifdef PAGEMAP_DEBUG
 864 +       PAGEMAP_INFO("Execution routine: %08x\n",
 865 +               (u32)context->archdata.cpu_resume_restore_nosave);
 866 +#endif
 867 +       arch_preboot_os();
 868 +       cleanup_before_linux();
 869 +
 870 +       /* Copy the final data from a safe place */
 871 +       call_with_stack(swsusp_finish_copy, context, stack_addr);
 872 +
 873 +       return 0;
 874 +
 875 +mem_err:
 876 +       printf("Not enough memory.\n");
 877 +       goto err;
 878 +
 879 +read_err:
 880 +       printf("Read error while restoring image.\n");
 881 +
 882 +err:
 883 +       __asm__ volatile (
 884 +       "mov    r0, #0\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");
 889 +
 890 +       raw_page_exit();
 891 +       image_page_exit();
 892 +       free_page_exit();
 893 +       if (pfn_pages) {
 894 +               for (i = 0; i < nr_pfn_pages; i++)
 895 +                       free(pfn_pages[i]);
 896 +               free(pfn_pages);
 897 +       }
 898 +       free(spare_page);
 899 +
 900 +       return 1;
 901 +}
 902 +
 903 +U_BOOT_CMD(swsusp, 4, 0, do_swsusp,
 904 +       "Restore SWSUSP hibernation image",
 905 +       "<interface> [<dev[:part]>] [<offset>]"
 906 +);
 907 --
 908 1.8.3.1
 909