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

   1 From 4ce00daa904a40701ab6bed44506fe97b8f1da47 Mon Sep 17 00:00:00 2001
   2 From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
   3 Date: Fri, 19 May 2017 14:48:38 +0900
   4 Subject: [PATCH 3/4] Add Hibernation swsuspmem command support
   5
   6 Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
   7 ---
   8  common/Makefile            |   2 +
   9  common/cmd_swsuspmem.c     | 944 +++++++++++++++++++++++++++++++++++++++++++++
  10  include/swsuspmem.h        |  24 ++
  11  lib/lzo/lzo1x_decompress.c |  12 +-
  12  4 files changed, 980 insertions(+), 2 deletions(-)
  13  create mode 100644 common/cmd_swsuspmem.c
  14  create mode 100644 include/swsuspmem.h
  15
  16 diff --git a/common/Makefile b/common/Makefile
  17 index 54fcc81..7a18486 100644
  18 --- a/common/Makefile
  19 +++ b/common/Makefile
  20 @@ -160,6 +160,8 @@ COBJS-$(CONFIG_CMD_SETEXPR) += cmd_setexpr.o
  21  COBJS-$(CONFIG_CMD_SPI) += cmd_spi.o
  22  COBJS-$(CONFIG_CMD_SPIBOOTLDR) += cmd_spibootldr.o
  23  COBJS-$(CONFIG_CMD_STRINGS) += cmd_strings.o
  24 +COBJS-$(CONFIG_CMD_SWSUSP) += cmd_swsusp.o
  25 +COBJS-$(CONFIG_CMD_SWSUSPMEM) += cmd_swsuspmem.o
  26  COBJS-$(CONFIG_CMD_TERMINAL) += cmd_terminal.o
  27  COBJS-$(CONFIG_CMD_TIME) += cmd_time.o
  28  COBJS-$(CONFIG_SYS_HUSH_PARSER) += cmd_test.o
  29 diff --git a/common/cmd_swsuspmem.c b/common/cmd_swsuspmem.c
  30 new file mode 100644
  31 index 0000000..6980aaf
  32 --- /dev/null
  33 +++ b/common/cmd_swsuspmem.c
  34 @@ -0,0 +1,944 @@
  35 +/*
  36 + * This program is free software; you can redistribute it and/or modify
  37 + * it under the terms of the GNU General Public License as published by
  38 + * the Free Software Foundation; either version 2 of the License, or
  39 + * (at your option) any later version.
  40 + *
  41 + * This program is distributed in the hope that it will be useful,
  42 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  43 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  44 + * GNU General Public License for more details.
  45 + *
  46 + * You should have received a copy of the GNU General Public License
  47 + * along with this program; if not, write to the Free Software
  48 + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  49 + */
  50 +
  51 +#include <common.h>
  52 +#include <command.h>
  53 +#include <part.h>
  54 +#include <malloc.h>
  55 +
  56 +#include <linux/lzo.h>
  57 +#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
  58 +#include <swsuspmem.h>
  59 +
  60 +/* Note for Renesas--based boards:
  61 + * We have the following memory split here:
  62 + * 0x40000000 - u_boot_lowest - used to store pfns at physical addresses
  63 + * u_boot_lowest - 0x8000000 - pfns are relocated, and then later put
  64 + * on physical addresses (swsusp_finish)
  65 + * 0x8000000 - 0xc0000000 - used to store pfns with physical address
  66 + * of 0x200000000 (long address), we have to change offset for them.
  67 + * Any pfn with address > 0x8000000 but less than 0x200000000
  68 + * is an error.
  69 + * For boards which do not have memory above first GB, that will
  70 + * still work, as they won't have anything above 0x80000000
  71 + * in their image, so for standard 2GB setup ou should put
  72 + * your secong GB in 0x80000000-0xC0000000 range, you can
  73 + * use MMU for that or if your RAM is continous, it will
  74 + * naturally be there. */
  75 +
  76 +DECLARE_GLOBAL_DATA_PTR;
  77 +
  78 +/* #define PAGEMAP_DEBUG */
  79 +
  80 +#ifdef PAGEMAP_DEBUG
  81 +#define SWSUSP_DEBUG_INFO
  82 +#endif
  83 +
  84 +#define SWSUSP_KEEP_IMAGE
  85 +
  86 +#ifndef likely
  87 +# define likely(x)     __builtin_expect(!!(x), 1)
  88 +# define unlikely(x)   __builtin_expect(!!(x), 0)
  89 +#endif
  90 +
  91 +#define HIBERNATE_SIG "S1SUSPEND"
  92 +#define PAGE_SIZE (4096)
  93 +/* Define depending on CONFIG_LBDAF in kernel */
  94 +
  95 +typedef u64 sector_t;
  96 +
  97 +struct swsusp_header {
  98 +       char reserved[PAGE_SIZE - 20
  99 +               - sizeof(sector_t) - sizeof(int) - sizeof(u32)
 100 +               - sizeof(CRC32_WORD4_t) - sizeof(u32)];
 101 +       CRC32_WORD4_t comp_crc32;
 102 +       u32 img_size; /* append */
 103 +       u32     crc32;
 104 +       sector_t        image;
 105 +       unsigned int flags;
 106 +       char    orig_sig[10];
 107 +       char    sig[10];
 108 +} __packed;
 109 +
 110 +#define __NEW_UTS_LEN 64
 111 +
 112 +struct new_utsname {
 113 +       char sysname[__NEW_UTS_LEN + 1];
 114 +       char nodename[__NEW_UTS_LEN + 1];
 115 +       char release[__NEW_UTS_LEN + 1];
 116 +       char version[__NEW_UTS_LEN + 1];
 117 +       char machine[__NEW_UTS_LEN + 1];
 118 +       char domainname[__NEW_UTS_LEN + 1];
 119 +};
 120 +
 121 +struct swsusp_archdata {
 122 +       u32     nosave_backup_phys;
 123 +       u32     nosave_begin_phys;
 124 +       u32     nosave_end_phys;
 125 +       void    (*cpu_resume_restore_nosave)(u32, u32, u32);
 126 +};
 127 +
 128 +struct swsusp_info {
 129 +       struct new_utsname      uts;
 130 +       u32                     version_code;
 131 +       unsigned long           num_physpages;
 132 +       int                     cpus;
 133 +       unsigned long           image_pages;
 134 +       unsigned long           pages;
 135 +       unsigned long           size;
 136 +       char                    archdata[1024];
 137 +};
 138 +
 139 +struct swap_map_page {
 140 +       u64 entries[PAGE_SIZE / sizeof(u64) - 1];
 141 +       u64 next_swap;
 142 +};
 143 +
 144 +struct swsusp_finish_context {
 145 +       void *remap_orig_page;
 146 +       void *remap_temp_page;
 147 +       struct swsusp_archdata archdata;
 148 +};
 149 +#ifdef FTEN_SPF_SDRAM_BASE
 150 +#define USED_ADDRESS_TOP       (CONFIG_SYS_SDRAM_BASE)
 151 +#define USED_ADDRESS_END       (CONFIG_SYS_SDRAM_BASE + CONFIG_SYS_LOAD_OFFSET)
 152 +#else
 153 +#define USED_ADDRESS_TOP       (0x40000000)
 154 +#define USED_ADDRESS_END       (0x48000000)
 155 +#endif
 156 +#define PG_UB2ZERO(pg) ((pg) - CONFIG_SYS_SDRAM_BASE / PAGE_SIZE)
 157 +static u32 const exclude_min_page =
 158 +       (USED_ADDRESS_TOP) / PAGE_SIZE;
 159 +static u32 const exclude_max_page =
 160 +       (USED_ADDRESS_END - 1) / PAGE_SIZE;
 161 +static u32 const exclude_min_page_ub =
 162 +       PG_UB2ZERO((USED_ADDRESS_TOP) / PAGE_SIZE);
 163 +static u32 const exclude_max_page_ub =
 164 +       PG_UB2ZERO((USED_ADDRESS_END-1) / PAGE_SIZE);
 165 +
 166 +/*
 167 + #define SD_PLATFORM_MODE  1
 168 + #define SD_CRC32_MODE     4
 169 + */
 170 +#define SF_NOCOMPRESS_MODE 2
 171 +
 172 +#define LZO_HEADER      sizeof(size_t)
 173 +
 174 +/* Number of pages/bytes we'll compress at one time. */
 175 +#define LZO_UNC_PAGES  32
 176 +#define LZO_UNC_SIZE   (LZO_UNC_PAGES * PAGE_SIZE)
 177 +
 178 +/* Number of pages/bytes we need for compressed data (worst case). */
 179 +#define LZO_CMP_PAGES  DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
 180 +                               LZO_HEADER, PAGE_SIZE)
 181 +#define LZO_CMP_SIZE   (LZO_CMP_PAGES * PAGE_SIZE)
 182 +
 183 +static struct swsuspmem_hook *_hook;
 184 +
 185 +#define CALL_HOOK(f, param) \
 186 +       do {                                            \
 187 +               if (_hook != NULL) {                    \
 188 +                       if (_hook->f != NULL)           \
 189 +                               _hook->f(param);        \
 190 +               }                                       \
 191 +       } while (0)
 192 +
 193 +#ifdef PAGEMAP_DEBUG
 194 +static int debugout;
 195 +static int _last_read_pages;
 196 +#define PAGEMAP_INFO(_msg, ...)                                \
 197 +       do {                                            \
 198 +               if (debugout == 1)                      \
 199 +                       printf(_msg, ## __VA_ARGS__);   \
 200 +       } while (0)
 201 +#endif
 202 +
 203 +#define HIGHMEM_PHYS_ADDR      0x200000000ULL
 204 +#define HIGHMEM_VA             0x80000000UL
 205 +#define HIGHMEM_PFN            (HIGHMEM_PHYS_ADDR / PAGE_SIZE)
 206 +#define LOW_TOP                        0x80000000
 207 +#define LOW_TOP_PFN            (LOW_TOP / PAGE_SIZE)
 208 +#define LOW_BOTTOM             CONFIG_SYS_SDRAM_BASE
 209 +#define LOW_BOTTOM_PFN         (LOW_BOTTOM / PAGE_SIZE)
 210 +#define TOP_ADDRESS            0x240000000ULL
 211 +
 212 +static inline int pfn_is_low(u32 pfn)
 213 +{
 214 +       return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
 215 +}
 216 +
 217 +static inline int pfn_is_high(u32 pfn)
 218 +{
 219 +       return (pfn >= HIGHMEM_PFN);
 220 +}
 221 +
 222 +#define pfn_is_valid(p)                (pfn_is_low(p) || pfn_is_high(p))
 223 +
 224 +static inline int pfn_is_excluded(u32 pfn)
 225 +{
 226 +       /* Allow bottom 2 pages for exception vectors */
 227 +       if (pfn < (LOW_BOTTOM_PFN + 2))
 228 +               return 0;
 229 +       else if (exclude_min_page >= exclude_max_page)
 230 +               return 0;
 231 +       else
 232 +               return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
 233 +}
 234 +/* PFN to zero-counted page */
 235 +static inline u32 pg_ub2zero(u32 pg)
 236 +{
 237 +       return pg - LOW_BOTTOM_PFN;
 238 +}
 239 +
 240 +/* zero-counted page to PFN */
 241 +static inline u32 pg_zero2ub(u32 pg)
 242 +{
 243 +       return pg + LOW_BOTTOM_PFN;
 244 +}
 245 +
 246 +/* PFN to physical address (64-bit (40-bit)) */
 247 +static inline u64 pg2phys(u32 page)
 248 +{
 249 +       return (u64) page * PAGE_SIZE;
 250 +}
 251 +
 252 +/* PFN to virtual address */
 253 +static inline void *pg2addr(u32 page)
 254 +{
 255 +       void *addr;
 256 +       if (page >= HIGHMEM_PFN)
 257 +               addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
 258 +       else
 259 +               addr = (void *) (u32)pg2phys(page);
 260 +
 261 +       return addr;
 262 +}
 263 +/* Virtual address to PFN */
 264 +static inline u32 addr2pg(void *addr)
 265 +{
 266 +       return ((u32)(addr)) / PAGE_SIZE;
 267 +}
 268 +static void *offt_addr = (void *)0x44000000;
 269 +static int page_read_mem(u64 page, void *addr)
 270 +{
 271 +       memcpy(addr, (u8 *)offt_addr + page * PAGE_SIZE, PAGE_SIZE);
 272 +       return 0;
 273 +}
 274 +
 275 +#ifndef SWSUSP_KEEP_IMAGE
 276 +static int page_write_mem(u32 page, void *addr)
 277 +{
 278 +       memcpy((u8 *)offt_addr + page * PAGE_SIZE, addr, PAGE_SIZE);
 279 +       return 0;
 280 +}
 281 +#endif
 282 +
 283 +#define FAST_COPY
 284 +static void __attribute__((section(".rodata")))
 285 +       __attribute__((optimize("O6", "unroll-loops")))
 286 +swsusp_finish(void *userdata)
 287 +{
 288 +       struct swsusp_finish_context *context = userdata;
 289 +       u32 **remap_orig;
 290 +       u32 **remap_temp;
 291 +       int idx = 0;
 292 +       const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
 293 +
 294 +       remap_orig = context->remap_orig_page;
 295 +       remap_temp = context->remap_temp_page;
 296 +
 297 +       __asm__ volatile ("" : : : "memory");
 298 +       for (;;) {
 299 +               u32 *orig, *temp;
 300 +               int count;
 301 +
 302 +               /* Linked list to next page */
 303 +               if (idx == lastidx) {
 304 +                       remap_orig = (u32 **)remap_orig[idx];
 305 +                       remap_temp = (u32 **)remap_temp[idx];
 306 +                       idx = 0;
 307 +               }
 308 +               if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
 309 +                       break;
 310 +               orig = remap_orig[idx];
 311 +               temp = remap_temp[idx];
 312 +#ifdef FAST_COPY
 313 +               count = PAGE_SIZE / sizeof(u32) / 32;
 314 +               __asm__ volatile (
 315 +                       "1:\n"
 316 +                       "ldmia %[rtemp]!, {r0-r7}\n"
 317 +                       "stmia %[rorig]!, {r0-r7}\n"
 318 +                       "ldmia %[rtemp]!, {r0-r7}\n"
 319 +                       "stmia %[rorig]!, {r0-r7}\n"
 320 +                       "ldmia %[rtemp]!, {r0-r7}\n"
 321 +                       "stmia %[rorig]!, {r0-r7}\n"
 322 +                       "ldmia %[rtemp]!, {r0-r7}\n"
 323 +                       "subs %[count], %[count], #1\n"
 324 +                       "stmia %[rorig]!, {r0-r7}\n"
 325 +                       "bgt 1b\n"
 326 +                       : /* No outputs */
 327 +                       :
 328 +                         [rorig]"h" (orig),
 329 +                         [rtemp]"h" (temp),
 330 +                         [count]"h" (count)
 331 +                       : "r0", "r1", "r2", "r3",
 332 +                         "r4", "r5", "r6", "r7",
 333 +                         "cc", "memory"
 334 +               );
 335 +#else
 336 +               count = PAGE_SIZE / sizeof(u32);
 337 +               while (count--)
 338 +                       *orig++ = *temp++;
 339 +#endif
 340 +#ifdef SWSUSP_CHECK_COPY_RESULT
 341 +               count = PAGE_SIZE / sizeof(u32);
 342 +               orig = remap_orig[idx];
 343 +               temp = remap_temp[idx];
 344 +               __asm__ volatile (
 345 +                       "1:\n"
 346 +                       "ldr r3, [%[rorig]]\n"
 347 +                       "ldr r4, [%[rtemp]]\n"
 348 +                       "cmp r3, r4\n"
 349 +                       "bne 2f\n"
 350 +                       "add %[rorig], %[rorig], #4\n"
 351 +                       "add %[rtemp], %[rtemp], #4\n"
 352 +                       "subs %[count], %[count], #1\n"
 353 +                       "bgt 1b\n"
 354 +                       "b 3f\n"
 355 +                       "2:b 2b\n"
 356 +                       "3:\n"
 357 +                       :
 358 +                         [rorig]"+r" (orig),
 359 +                         [rtemp]"+r" (temp),
 360 +                         [count]"+r" (count)
 361 +                       :
 362 +                       : "r3", "r4", "cc", "memory"
 363 +               );
 364 +#endif
 365 +               idx++;
 366 +       }
 367 +       context->archdata.cpu_resume_restore_nosave(
 368 +                       context->archdata.nosave_backup_phys,
 369 +                       context->archdata.nosave_begin_phys,
 370 +                       context->archdata.nosave_end_phys);
 371 +}
 372 +
 373 +static struct swap_map_page *meta_map;
 374 +static u64 meta_map_next;
 375 +static u64 meta_map_curr;
 376 +static u64 meta_map_start;
 377 +static int meta_idx;
 378 +
 379 +static int raw_page_init(u64 start)
 380 +{
 381 +       meta_map = malloc(PAGE_SIZE);
 382 +       if (!meta_map)
 383 +               return -1;
 384 +       meta_map_next = 0;
 385 +       meta_map_curr = 0;
 386 +       meta_map_start = start;
 387 +       return 0;
 388 +}
 389 +
 390 +static void raw_page_start(void)
 391 +{
 392 +       meta_idx = ARRAY_SIZE(meta_map->entries);
 393 +       meta_map_next = meta_map_start;
 394 +}
 395 +
 396 +static int raw_page_get_next(void *buffer)
 397 +{
 398 +       if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
 399 +               if (!meta_map_next)
 400 +                       return 0;
 401 +               if (meta_map_curr != meta_map_next) {
 402 +#ifdef PAGEMAP_DEBUG
 403 +                       PAGEMAP_INFO("META: %d (%08x)\n", (int)meta_map_next,
 404 +                               (unsigned int)(meta_map_next
 405 +                                       * PAGE_SIZE));
 406 +#endif
 407 +                       if (page_read_mem(meta_map_next, meta_map))
 408 +                               return -1;
 409 +                       meta_map_curr = meta_map_next;
 410 +                       meta_map_next = meta_map->next_swap;
 411 +               }
 412 +               meta_idx = 0;
 413 +       }
 414 +#ifdef PAGEMAP_DEBUG
 415 +       {
 416 +               static unsigned int pre;
 417 +               if ((pre + 1) != meta_map->entries[meta_idx]) {
 418 +                       PAGEMAP_INFO("DATA-Skiped: %d->%d (%08x->%08x)\n",
 419 +                       pre,  (unsigned int)meta_map->entries[meta_idx],
 420 +                       pre * PAGE_SIZE,
 421 +                       (unsigned int)(meta_map->entries[meta_idx]
 422 +                               * PAGE_SIZE));
 423 +       }
 424 +       pre = meta_map->entries[meta_idx];
 425 +       _last_read_pages = pre;
 426 +       }
 427 +#endif
 428 +       if (page_read_mem(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_pages_avail;
 441 +static unsigned char *unc_buf, *cmp_buf;
 442 +static int unc_offset;
 443 +
 444 +static int image_page_init(int compressed)
 445 +{
 446 +       if (!compressed)
 447 +               return 1;
 448 +
 449 +       unc_buf = malloc(LZO_UNC_SIZE);
 450 +       cmp_buf = malloc(LZO_CMP_SIZE);
 451 +       if (!unc_buf || !cmp_buf) {
 452 +               printf("not enogh memory\n");
 453 +               return 1;
 454 +       }
 455 +       return 0;
 456 +}
 457 +
 458 +static void image_page_start(void)
 459 +{
 460 +       image_pages_avail = 0;
 461 +}
 462 +
 463 +static int image_page_get_next(void *buffer)
 464 +{
 465 +#ifdef CONFIG_LZO
 466 +               if (!image_pages_avail) {
 467 +                       int ret;
 468 +                       size_t unc_len, cmp_len, cmp_avail;
 469 +
 470 +                       ret = raw_page_get_next(cmp_buf);
 471 +                       if (ret <= 0)
 472 +                               return ret;
 473 +
 474 +                       cmp_len = *(size_t *) cmp_buf;
 475 +                       cmp_avail = PAGE_SIZE;
 476 +
 477 +                       while (cmp_avail < cmp_len + LZO_HEADER) {
 478 +                               ret = raw_page_get_next(cmp_buf + cmp_avail);
 479 +                               if (unlikely(ret <= 0))
 480 +                                       return ret;
 481 +                               cmp_avail += PAGE_SIZE;
 482 +                       }
 483 +
 484 +                       unc_len = LZO_UNC_SIZE;
 485 +                       ret = lzo1x_decompress_safe(cmp_buf + LZO_HEADER,
 486 +                                               cmp_len, unc_buf, &unc_len);
 487 +                       if (unlikely(ret != LZO_E_OK)) {
 488 +                               printf("Decompression failure: %d,"
 489 +                                      " cmp_buf = %p,"
 490 +                                      " cmp_len = %d, unc_len = %d\n",
 491 +                                      ret, cmp_buf + LZO_HEADER, cmp_len,
 492 +                                      unc_len);
 493 +                               return ret;
 494 +                       }
 495 +                       image_pages_avail = unc_len / PAGE_SIZE;
 496 +                       unc_offset = 0;
 497 +               }
 498 +
 499 +               memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
 500 +               unc_offset += PAGE_SIZE;
 501 +               image_pages_avail--;
 502 +               return 1;
 503 +#else
 504 +               printf("No LZO support in u-boot.\n");
 505 +               return -1;
 506 +#endif
 507 +}
 508 +
 509 +static void image_page_exit(void)
 510 +{
 511 +       free(unc_buf);
 512 +       free(cmp_buf);
 513 +       unc_buf = cmp_buf = NULL;
 514 +}
 515 +
 516 +static void bitmap_set(u32 *bm, unsigned int bit)
 517 +{
 518 +       bm[bit >> 5] |= (1 << (bit & 0x1f));
 519 +}
 520 +
 521 +static int bitmap_is_set(u32 *bm, unsigned int bit)
 522 +{
 523 +       return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
 524 +}
 525 +
 526 +static u32 *used_bitmap;
 527 +static u32 next_free_page;
 528 +static u32 total_pages;
 529 +
 530 +static int free_page_init(void)
 531 +{
 532 +       total_pages = (u32)((TOP_ADDRESS -
 533 +                       LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
 534 +       used_bitmap = malloc(total_pages * sizeof(u32) / 32);
 535 +       if (!used_bitmap)
 536 +               return -1;
 537 +       return 0;
 538 +}
 539 +
 540 +static void free_page_start(int offset)
 541 +{
 542 +       memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
 543 +       next_free_page = pg_ub2zero(offset);
 544 +}
 545 +
 546 +static void free_page_mark_used(u32 page);
 547 +/* Returns full-address based pages */
 548 +static int free_page_get_next(void)
 549 +{
 550 +       while (bitmap_is_set(used_bitmap, next_free_page))
 551 +               next_free_page++;
 552 +       free_page_mark_used(next_free_page);
 553 +       return pg_zero2ub(next_free_page++);
 554 +}
 555 +
 556 +static void free_page_mark_used(u32 page)
 557 +{
 558 +       bitmap_set(used_bitmap, page);
 559 +}
 560 +
 561 +static void free_page_exit(void)
 562 +{
 563 +       free(used_bitmap);
 564 +       used_bitmap = NULL;
 565 +}
 566 +
 567 +void set_swsuspmem_hook(struct swsuspmem_hook *hook)
 568 +{
 569 +       _hook = hook;
 570 +}
 571 +
 572 +/*
 573 + * rtn = 1 : Hibernation image OK.
 574 + * rtn = 0 : Hibernation image NG.
 575 + * */
 576 +int do_checksnapimage(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 577 +{
 578 +       __u32 offset = 0;
 579 +       void *spare_page = NULL;
 580 +       struct swsusp_header *swsusp_header;
 581 +       CRC32_WORD4_t calc_crc;
 582 +
 583 +       /* Address hack */
 584 +       if (argc > 1) {
 585 +               char *ep;
 586 +               offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
 587 +               if (*ep) {
 588 +                       printf("Invalid address\n");
 589 +                       return 0;
 590 +               }
 591 +       }
 592 +
 593 +       spare_page = malloc(PAGE_SIZE);
 594 +       if (!spare_page)
 595 +               goto mem_err;
 596 +
 597 +       swsusp_header = spare_page;
 598 +       if (page_read_mem(offset, swsusp_header))
 599 +               goto read_err;
 600 +
 601 +#ifdef SWSUSP_DEBUG_INFO
 602 +       PAGEMAP_INFO("swssp_header:%x\n", swsusp_header);
 603 +       PAGEMAP_INFO("    comp_crc: <snip>\n");
 604 +       PAGEMAP_INFO("    img_size: %d\n", swsusp_header->img_size);
 605 +       PAGEMAP_INFO("    image(swap firest sector): %08x\n",
 606 +                       (unsigned int)swsusp_header->image);
 607 +       PAGEMAP_INFO("    flags: %08x\n", swsusp_header->flags);
 608 +       PAGEMAP_INFO("    orig_sig:%s\n", swsusp_header->orig_sig);
 609 +       PAGEMAP_INFO("    sig:%s\n",      swsusp_header->sig);
 610 +#endif /* SWSUSP_DEBUG_INFO */
 611 +
 612 +       if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
 613 +                       || (swsusp_header->img_size == 0)
 614 +                       || (swsusp_header->img_size > 0x03fff000)) {
 615 +               printf("No hibernation image present\n");
 616 +               CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
 617 +               return 0;
 618 +       }
 619 +       memset(&calc_crc, 0, sizeof(calc_crc));
 620 +
 621 +       calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
 622 +                       swsusp_header->img_size, &calc_crc);
 623 +
 624 +       if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
 625 +                               sizeof(CRC32_WORD4_t))) {
 626 +               printf("Bad CRC for image, image: %08x:%08x:"
 627 +                       "%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
 628 +                       swsusp_header->comp_crc32.crc_w[0],
 629 +                       swsusp_header->comp_crc32.crc_w[1],
 630 +                       swsusp_header->comp_crc32.crc_w[2],
 631 +                       swsusp_header->comp_crc32.crc_w[3],
 632 +                       calc_crc.crc_w[0], calc_crc.crc_w[1],
 633 +                       calc_crc.crc_w[2], calc_crc.crc_w[3]);
 634 +               CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
 635 +               return 0;
 636 +       }
 637 +       free(spare_page);
 638 +       printf("Hibernation image OK!.\n");
 639 +
 640 +       return 1;
 641 +
 642 +mem_err:
 643 +       printf("Not enough memory.\n");
 644 +       CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
 645 +       goto err;
 646 +
 647 +read_err:
 648 +       printf("Read error while restoring image.\n");
 649 +
 650 +err:
 651 +       __asm__ volatile (
 652 +       "mov    r0, #0\n"
 653 +       "mcr    p15, 0, r0, c7, c5, 0   @ invalidate icache\n"
 654 +       "mcr    p15, 0, r0, c7, c10, 4  @ DSB\n"
 655 +       "mcr    p15, 0, r0, c7, c5, 4   @ ISB\n"
 656 +       : : : "r0", "memory");
 657 +
 658 +       free(spare_page);
 659 +
 660 +       CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
 661 +       return 0;
 662 +}
 663 +
 664 +U_BOOT_CMD(checksnapimage, 2, 2, do_checksnapimage,
 665 +       "Check hibernation image data from memory",
 666 +       "<address>]"
 667 +);
 668 +
 669 +int do_swsuspmem(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 670 +{
 671 +       __u32 offset = 0;
 672 +       void *spare_page = NULL;
 673 +       struct swsusp_header *swsusp_header;
 674 +       struct swsusp_info *swsusp_info;
 675 +       struct swsusp_finish_context *context;
 676 +       int max_page;
 677 +       int i;
 678 +       u32 nr_pfn_pages;
 679 +       u32 **pfn_pages = NULL;
 680 +       u32 *remap_orig_page;
 681 +       u32 *remap_temp_page;
 682 +       u32 **remap_orig;
 683 +       u32 **remap_temp;
 684 +       int remap_idx;
 685 +       void (*swsusp_finish_copy)(void *);
 686 +       char *data_page;
 687 +       char *stack_addr;
 688 +       CRC32_WORD4_t calc_crc;
 689 +       int high_page;
 690 +
 691 +#ifdef PAGEMAP_DEBUG
 692 +       int high_page_images = 0;
 693 +       int total_remap = 0;
 694 +       if (getenv("hybdebug") != NULL)
 695 +               debugout = 1;
 696 +#endif
 697 +       /* Address hack */
 698 +       void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
 699 +       if (argc > 1) {
 700 +               char *ep;
 701 +               offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
 702 +               if (*ep) {
 703 +                       printf("Invalid address\n");
 704 +                       return 1;
 705 +               }
 706 +       }
 707 +
 708 +       /* Allow for 16 pages of stack */
 709 +       max_page = gd->start_addr_sp / PAGE_SIZE - 32;
 710 +       high_page = (((gd->relocaddr + _bss_end_ofs)
 711 +                               + (PAGE_SIZE - 1)) / PAGE_SIZE) + 1;
 712 +#define pfn_is_occupied(pfn) (page > max_page && page <= high_page)
 713 +#ifdef PAGEMAP_DEBUG
 714 +       PAGEMAP_INFO(" *gd->start_addr_sp:%p\n",
 715 +                       (void *)gd->start_addr_sp);
 716 +       PAGEMAP_INFO(" *gd->relocaddr:%p\n",
 717 +                       (void *)gd->relocaddr);
 718 +       PAGEMAP_INFO(" *bss_start_offset:%d bss_end_offset:%d\n",
 719 +                       (int)_bss_start_ofs, (int)_bss_end_ofs);
 720 +       PAGEMAP_INFO(" UBOOT own memory [%p-%p]\n",
 721 +                       pg2addr(max_page), pg2addr(high_page));
 722 +#endif
 723 +       if (free_page_init())
 724 +               goto mem_err;
 725 +       free_page_start(exclude_max_page + 1);
 726 +
 727 +       spare_page = malloc(PAGE_SIZE);
 728 +       if (!spare_page)
 729 +               goto mem_err;
 730 +
 731 +       swsusp_header = spare_page;
 732 +       if (page_read_mem(offset, swsusp_header))
 733 +               goto read_err;
 734 +
 735 +#ifdef SWSUSP_DEBUG_INFO
 736 +       PAGEMAP_INFO("swssp_header:\n");
 737 +       PAGEMAP_INFO("    comp_crc: <snip>\n");
 738 +       PAGEMAP_INFO("    img_size: %d\n", swsusp_header->img_size);
 739 +       PAGEMAP_INFO("    image(swap firest sector): %08x\n",
 740 +                       (unsigned int)swsusp_header->image);
 741 +       PAGEMAP_INFO("    flags: %08x\n", swsusp_header->flags);
 742 +       PAGEMAP_INFO("    orig_sig:%s\n", swsusp_header->orig_sig);
 743 +       PAGEMAP_INFO("    sig:%s\n",      swsusp_header->sig);
 744 +#endif /* SWSUSP_DEBUG_INFO */
 745 +
 746 +       if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
 747 +                       || (swsusp_header->img_size == 0)
 748 +                       || (swsusp_header->img_size > 0x03fff000)) {
 749 +               printf("No hibernation image present\n");
 750 +               CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
 751 +               return 0;
 752 +       }
 753 +       memset(&calc_crc, 0, sizeof(calc_crc));
 754 +
 755 +       calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
 756 +                       swsusp_header->img_size, &calc_crc);
 757 +
 758 +       if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
 759 +                               sizeof(CRC32_WORD4_t))) {
 760 +               printf("Bad CRC for image, image: %08x:%08x:"
 761 +                       "%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
 762 +                       swsusp_header->comp_crc32.crc_w[0],
 763 +                       swsusp_header->comp_crc32.crc_w[1],
 764 +                       swsusp_header->comp_crc32.crc_w[2],
 765 +                       swsusp_header->comp_crc32.crc_w[3],
 766 +                       calc_crc.crc_w[0], calc_crc.crc_w[1],
 767 +                       calc_crc.crc_w[2], calc_crc.crc_w[3]);
 768 +               CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
 769 +               return 0;
 770 +       }
 771 +
 772 +       /* Overwrite header if necessary */
 773 +#ifndef SWSUSP_KEEP_IMAGE
 774 +       if (memcmp(swsusp_header->sig, swsusp_header->orig_sig, 10)) {
 775 +               memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
 776 +               if (page_write_mem(offset, swsusp_header))
 777 +                       printf("Write error resetting header\n");
 778 +       }
 779 +#endif
 780 +
 781 +       if (raw_page_init(swsusp_header->image))
 782 +               goto mem_err;
 783 +       raw_page_start();
 784 +
 785 +       if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
 786 +               goto mem_err;
 787 +       image_page_start();
 788 +
 789 +       swsusp_info = spare_page;
 790 +       if (raw_page_get_next(swsusp_info) <= 0)
 791 +               goto read_err;
 792 +
 793 +#ifdef SWSUSP_DEBUG_INFO
 794 +       PAGEMAP_INFO("swsup_info:\n");
 795 +       PAGEMAP_INFO("  utsname.sysname:%s\n",
 796 +                       swsusp_info->uts.sysname);
 797 +       PAGEMAP_INFO("            nodename:%s\n",
 798 +                       swsusp_info->uts.nodename);
 799 +       PAGEMAP_INFO("            release:%s\n",
 800 +                       swsusp_info->uts.release);
 801 +       PAGEMAP_INFO("            version:%s\n",
 802 +                       swsusp_info->uts.version);
 803 +       PAGEMAP_INFO("            machine:%s\n",
 804 +                       swsusp_info->uts.machine);
 805 +       PAGEMAP_INFO("    vesion_code:%#08x\n",
 806 +                       (unsigned int)swsusp_info->version_code);
 807 +       PAGEMAP_INFO("    num_physpages:%d\n",
 808 +                       (unsigned int)swsusp_info->num_physpages);
 809 +       PAGEMAP_INFO("    pages        :%d\n",
 810 +                       (unsigned int)swsusp_info->pages);
 811 +       PAGEMAP_INFO("    size         :%d\n",
 812 +                       (unsigned int)swsusp_info->size);
 813 +#endif
 814 +
 815 +       nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
 816 +                                                               PAGE_SIZE;
 817 +       pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
 818 +       if (!pfn_pages)
 819 +               goto mem_err;
 820 +       memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
 821 +
 822 +       /* UBOOT using memory */
 823 +       for (i = max_page; i <= high_page; i++)
 824 +               free_page_mark_used(pg_ub2zero(i));
 825 +
 826 +       printf("Allocating %u bytes (nr_pfn_pages %u)\n",
 827 +                       nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
 828 +
 829 +       for (i = 0; i < nr_pfn_pages; i++) {
 830 +               u32 idx;
 831 +               pfn_pages[i] = malloc(PAGE_SIZE);
 832 +               memset(pfn_pages[i], 0xff, PAGE_SIZE);
 833 +               if (unlikely(!pfn_pages[i]))
 834 +                       goto mem_err;
 835 +               if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
 836 +                       goto read_err;
 837 +               for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
 838 +                       u32 page = pfn_pages[i][idx];
 839 +                       if (page == ~0UL) /* End of list of pages */
 840 +                               break;
 841 +                       free_page_mark_used(pg_ub2zero(page));
 842 +               }
 843 +       }
 844 +
 845 +       printf("Loading image data pages (%lu pages)\n",
 846 +                                               swsusp_info->image_pages);
 847 +
 848 +       remap_orig_page = pg2addr(free_page_get_next());
 849 +       remap_temp_page = pg2addr(free_page_get_next());
 850 +
 851 +       remap_orig = (u32 **)remap_orig_page;
 852 +       remap_temp = (u32 **)remap_temp_page;
 853 +       remap_idx = 0;
 854 +
 855 +       for (i = 0; i < swsusp_info->image_pages; i++) {
 856 +               u32 page = pfn_pages[i >> 10][i & 0x3ff];
 857 +               if (unlikely(!pfn_is_valid(page))) {
 858 +                       printf("Attempt to restore invalid address %llx\n",
 859 +                                       pg2phys(page));
 860 +                       continue;
 861 +               } else if (unlikely(pfn_is_excluded(page))) {
 862 +                       printf("Attempt to restore excluded address %llx\n",
 863 +                                       pg2phys(page));
 864 +                       continue;
 865 +               } else if (unlikely(pfn_is_low(page) &&
 866 +                                       pfn_is_occupied(page))) {
 867 +                       remap_orig[remap_idx] = pg2addr(page);
 868 +                       page = free_page_get_next();
 869 +                       remap_temp[remap_idx] = pg2addr(page);
 870 +                       remap_idx++;
 871 +#ifdef PAGEMAP_DEBUG
 872 +                       ++total_remap;
 873 +#endif
 874 +                       /* If we fill our current page, allocate a new one */
 875 +                       if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
 876 +                               u32 *next;
 877 +
 878 +                               next = pg2addr(free_page_get_next());
 879 +                               remap_orig[remap_idx] = next;
 880 +                               remap_orig = (u32 **)next;
 881 +
 882 +                               next = pg2addr(free_page_get_next());
 883 +                               remap_temp[remap_idx] = next;
 884 +                               remap_temp = (u32 **)next;
 885 +
 886 +                               remap_idx = 0;
 887 +                       }
 888 +               }
 889 +               if (image_page_get_next(pg2addr(page)) <= 0)
 890 +                       goto read_err;
 891 +       }
 892 +
 893 +       printf("Image loading done.\n");
 894 +       invalidate_icache_all();
 895 +
 896 +       CALL_HOOK(resume_boot, SWSUSPMEM_IMAGEDONE);
 897 +       /* put end markers on the remap list */
 898 +       remap_orig[remap_idx] = (void *) ~0UL;
 899 +       remap_temp[remap_idx] = (void *) ~0UL;
 900 +
 901 +#ifdef PAGEMAP_DEBUG
 902 +       PAGEMAP_INFO("Number of remap pages:%d\n", total_remap);
 903 +       PAGEMAP_INFO("Number of high pages:%d\n", high_page_images);
 904 +       PAGEMAP_INFO("Last read page %d (%08x)\n",
 905 +                    _last_read_pages, _last_read_pages * PAGE_SIZE);
 906 +#endif
 907 +       remap_orig = (u32 **)remap_orig_page;
 908 +       remap_temp = (u32 **)remap_temp_page;
 909 +
 910 +       /* Make a copy of swsusp_finish in a free data page */
 911 +       data_page = pg2addr(free_page_get_next());
 912 +       memcpy(data_page, swsusp_finish_p, PAGE_SIZE);
 913 +       swsusp_finish_copy = (void *) data_page;
 914 +
 915 +       /* Setup context for swsusp_finish at the end of the data_page */
 916 +       context = (struct swsusp_finish_context *) (data_page + PAGE_SIZE -
 917 +                                       sizeof(struct swsusp_finish_context));
 918 +       context->remap_orig_page = remap_orig_page;
 919 +       context->remap_temp_page = remap_temp_page;
 920 +       memcpy((void *)&context->archdata, (void *)swsusp_info->archdata,
 921 +                       sizeof(struct swsusp_archdata));
 922 +
 923 +       /* Get a stack pointer for swsusp_finish, growing down from context */
 924 +       stack_addr = (char *) context;
 925 +
 926 +#ifdef CONFIG_NETCONSOLE
 927 +       /*
 928 +        * Stop the ethernet stack if NetConsole could have
 929 +        * left it up
 930 +        */
 931 +       eth_halt();
 932 +#endif
 933 +#ifdef CONFIG_USB_DEVICE
 934 +       udc_disconnect();
 935 +#endif
 936 +       arch_preboot_os();
 937 +       cleanup_before_linux();
 938 +
 939 +       CALL_HOOK(resume_boot, SWSUSPMEM_RESUME);
 940 +       /* Copy the final data from a safe place */
 941 +       call_with_stack(swsusp_finish_copy, context, stack_addr);
 942 +
 943 +       return 0;
 944 +
 945 +mem_err:
 946 +       printf("Not enough memory.\n");
 947 +       CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
 948 +       goto err;
 949 +
 950 +read_err:
 951 +       printf("Read error while restoring image.\n");
 952 +
 953 +err:
 954 +       __asm__ volatile (
 955 +       "mov    r0, #0\n"
 956 +       "mcr    p15, 0, r0, c7, c5, 0   @ invalidate icache\n"
 957 +       "mcr    p15, 0, r0, c7, c10, 4  @ DSB\n"
 958 +       "mcr    p15, 0, r0, c7, c5, 4   @ ISB\n"
 959 +       : : : "r0", "memory");
 960 +
 961 +       raw_page_exit();
 962 +       image_page_exit();
 963 +       free_page_exit();
 964 +       if (pfn_pages) {
 965 +               for (i = 0; i < nr_pfn_pages; i++)
 966 +                       free(pfn_pages[i]);
 967 +               free(pfn_pages);
 968 +       }
 969 +       free(spare_page);
 970 +
 971 +       CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
 972 +       return 1;
 973 +}
 974 +
 975 +U_BOOT_CMD(swsuspmem, 2, 2, do_swsuspmem,
 976 +       "Restore SWSUSP hibernation image from memory",
 977 +       "<address>]"
 978 +);
 979 diff --git a/include/swsuspmem.h b/include/swsuspmem.h
 980 new file mode 100644
 981 index 0000000..3b353ea
 982 --- /dev/null
 983 +++ b/include/swsuspmem.h
 984 @@ -0,0 +1,24 @@
 985 +#ifndef _SWSUSPMEM_H_
 986 +#define _SWSUSPMEM_H_
 987 +
 988 +enum { SWSUSPMEM_NORM = 0,
 989 +       SWSUSPMEM_NOIMAGE = 0x01,
 990 +       SWSUSPMEM_BROKENIMAGE = 0x02,
 991 +       SWSUSPMEM_ENOMEM  = 0x80,
 992 +       SWSUSPMEM_RESTOREFAIL = 0x81,
 993 +};
 994 +
 995 +enum { SWSUSPMEM_IMAGEDONE = 0x01,
 996 +          SWSUSPMEM_RESUME = 0x02
 997 +};
 998 +
 999 +struct swsuspmem_hook {
1000 +       void (*err_hook)(int errcode);
1001 +       void (*resume_boot)(int  param);
1002 +};
1003 +
1004 +void set_swsuspmem_hook(struct swsuspmem_hook *hook);
1005 +void arch_preboot_os(void);
1006 +void call_with_stack(void (*fn)(void *),
1007 +               void *userdata, void *stack);
1008 +#endif
1009 diff --git a/lib/lzo/lzo1x_decompress.c b/lib/lzo/lzo1x_decompress.c
1010 index e6ff708..ebdf10b 100644
1011 --- a/lib/lzo/lzo1x_decompress.c
1012 +++ b/lib/lzo/lzo1x_decompress.c
1013 @@ -68,13 +68,14 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
1014         unsigned char *start = dst;
1015         const unsigned char *send = src + src_len;
1016         u32 slen, dlen;
1017 -       size_t tmp;
1018 +       size_t tmp, remaining;
1019         int r;
1020
1021         src = parse_header(src);
1022         if (!src)
1023                 return LZO_E_ERROR;
1024
1025 +       remaining = *dst_len;
1026         while (src < send) {
1027                 /* read uncompressed block size */
1028                 dlen = get_unaligned_be32(src);
1029 @@ -93,18 +94,25 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
1030                 if (slen <= 0 || slen > dlen)
1031                         return LZO_E_ERROR;
1032
1033 +               /* abort if buffer ran out of room */
1034 +               if (dlen > remaining)
1035 +                       return LZO_E_OUTPUT_OVERRUN;
1036 +
1037                 /* decompress */
1038                 tmp = dlen;
1039                 r = lzo1x_decompress_safe((u8 *) src, slen, dst, &tmp);
1040
1041 -               if (r != LZO_E_OK)
1042 +               if (r != LZO_E_OK) {
1043 +                       *dst_len = dst - start;
1044                         return r;
1045 +               }
1046
1047                 if (dlen != tmp)
1048                         return LZO_E_ERROR;
1049
1050                 src += slen;
1051                 dst += dlen;
1052 +               remaining -= dlen;
1053         }
1054
1055         return LZO_E_INPUT_OVERRUN;
1056 --
1057 1.8.3.1
1058