meta-agl-bsp/meta-renesas/recipes-kernel/linux/linux/hibernation/0002-Add-Hibernation-arch-code-Only-R-CAR-M2W.patch

   1 From 34a419b3fd88a2275ca681c99a5787b937e0f39d Mon Sep 17 00:00:00 2001
   2 From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
   3 Date: Thu, 18 May 2017 16:47:29 +0900
   4 Subject: [PATCH 02/15] Add Hibernation arch code(Only R-CAR M2W)
   5
   6 Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
   7 ---
   8  arch/arm/Kconfig                         |   5 +
   9  arch/arm/include/asm/memory.h            |   1 +
  10  arch/arm/include/asm/smp.h               |   4 +
  11  arch/arm/include/asm/suspend.h           |   3 +
  12  arch/arm/kernel/Makefile                 |   1 +
  13  arch/arm/kernel/hibernate.c              | 139 ++++++++++++++
  14  arch/arm/kernel/process.c                |   7 +-
  15  arch/arm/kernel/sleep.S                  |   8 +
  16  arch/arm/kernel/smp.c                    |   6 +
  17  arch/arm/kernel/suspend.c                |  64 ++++---
  18  arch/arm/mach-shmobile/Kconfig           |  65 +++++++
  19  arch/arm/mach-shmobile/Makefile          |   1 +
  20  arch/arm/mach-shmobile/common.h          |   8 +
  21  arch/arm/mach-shmobile/crc32_word4.c     | 299 +++++++++++++++++++++++++++++++
  22  arch/arm/mach-shmobile/crc32_word4.h     |  23 +++
  23  arch/arm/mach-shmobile/headsmp.S         |   2 -
  24  arch/arm/mach-shmobile/hibernation.c     | 243 +++++++++++++++++++++++++
  25  arch/arm/mach-shmobile/platsmp-apmu.c    |  13 +-
  26  arch/arm/mach-shmobile/platsmp-rst.c     |   3 +-
  27  arch/arm/mach-shmobile/pm-r8a7791.c      |  27 +--
  28  arch/arm/mach-shmobile/rcar-gen2.h       |  39 ++++
  29  arch/arm/mach-shmobile/setup-r8a7791.c   |  10 +-
  30  arch/arm/mach-shmobile/setup-rcar-gen2.c |  14 +-
  31  arch/arm/mach-shmobile/smp-r8a7791.c     |   8 +-
  32  arch/arm/mm/proc-v7.S                    |  31 ++--
  33  25 files changed, 947 insertions(+), 77 deletions(-)
  34  create mode 100644 arch/arm/kernel/hibernate.c
  35  create mode 100644 arch/arm/mach-shmobile/crc32_word4.c
  36  create mode 100644 arch/arm/mach-shmobile/crc32_word4.h
  37  create mode 100644 arch/arm/mach-shmobile/hibernation.c
  38
  39 diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
  40 index 4dd95dd..eb76182 100644
  41 --- a/arch/arm/Kconfig
  42 +++ b/arch/arm/Kconfig
  43 @@ -2232,6 +2232,11 @@ config ARCH_SUSPEND_POSSIBLE
  44  config ARM_CPU_SUSPEND
  45         def_bool PM_SLEEP
  46
  47 +config ARCH_HIBERNATION_POSSIBLE
  48 +       bool
  49 +       depends on MMU
  50 +       default y if ARCH_SUSPEND_POSSIBLE
  51 +
  52  endmenu
  53
  54  source "net/Kconfig"
  55 diff --git a/arch/arm/include/asm/memory.h b/arch/arm/include/asm/memory.h
  56 index 48cb2b3..01158e7 100644
  57 --- a/arch/arm/include/asm/memory.h
  58 +++ b/arch/arm/include/asm/memory.h
  59 @@ -241,6 +241,7 @@ static inline void *phys_to_virt(phys_addr_t x)
  60  #define __pa(x)                        __virt_to_phys((unsigned long)(x))
  61  #define __va(x)                        ((void *)__phys_to_virt((phys_addr_t)(x)))
  62  #define pfn_to_kaddr(pfn)      __va((pfn) << PAGE_SHIFT)
  63 +#define virt_to_pfn(kaddr)      (__pa(kaddr) >> PAGE_SHIFT)
  64
  65  /*
  66   * Virtual <-> DMA view memory address translations
  67 diff --git a/arch/arm/include/asm/smp.h b/arch/arm/include/asm/smp.h
  68 index d3a22be..b718040 100644
  69 --- a/arch/arm/include/asm/smp.h
  70 +++ b/arch/arm/include/asm/smp.h
  71 @@ -42,6 +42,10 @@ void handle_IPI(int ipinr, struct pt_regs *regs);
  72   */
  73  extern void smp_init_cpus(void);
  74
  75 +/*
  76 + * Provide a function to call machine specific cpu initialization sequence
  77 + */
  78 +extern void arch_smp_prepare_cpus(unsigned int max_cpus);
  79
  80  /*
  81   * Provide a function to raise an IPI cross call on CPUs in callmap.
  82 diff --git a/arch/arm/include/asm/suspend.h b/arch/arm/include/asm/suspend.h
  83 index 1c0a551..709afa4 100644
  84 --- a/arch/arm/include/asm/suspend.h
  85 +++ b/arch/arm/include/asm/suspend.h
  86 @@ -3,5 +3,8 @@
  87
  88  extern void cpu_resume(void);
  89  extern int cpu_suspend(unsigned long, int (*)(unsigned long));
  90 +extern const void __nosave_begin, __nosave_end;
  91 +extern void cpu_resume_restore_nosave(u32, u32, u32);
  92 +extern void call_with_stack(void (*fn)(void *), void *arg, void *sp);
  93
  94  #endif
  95 diff --git a/arch/arm/kernel/Makefile b/arch/arm/kernel/Makefile
  96 index 5f3338e..70f439f 100644
  97 --- a/arch/arm/kernel/Makefile
  98 +++ b/arch/arm/kernel/Makefile
  99 @@ -32,6 +32,7 @@ obj-$(CONFIG_ARTHUR)          += arthur.o
 100  obj-$(CONFIG_ISA_DMA)          += dma-isa.o
 101  obj-$(CONFIG_PCI)              += bios32.o isa.o
 102  obj-$(CONFIG_ARM_CPU_SUSPEND)  += sleep.o suspend.o
 103 +obj-$(CONFIG_HIBERNATION)              += hibernate.o
 104  obj-$(CONFIG_SMP)              += smp.o smp_tlb.o
 105  obj-$(CONFIG_HAVE_ARM_SCU)     += smp_scu.o
 106  obj-$(CONFIG_HAVE_ARM_TWD)     += smp_twd.o
 107 diff --git a/arch/arm/kernel/hibernate.c b/arch/arm/kernel/hibernate.c
 108 new file mode 100644
 109 index 0000000..9380fe2
 110 --- /dev/null
 111 +++ b/arch/arm/kernel/hibernate.c
 112 @@ -0,0 +1,139 @@
 113 +/*
 114 + * Hibernation support specific for ARM
 115 + *
 116 + * Derived from work on ARM hibernation support by:
 117 + *
 118 + * Ubuntu project, hibernation support for mach-dove
 119 + * Copyright (C) 2010 Nokia Corporation (Hiroshi Doyu)
 120 + * Copyright (C) 2010 Texas Instruments, Inc. (Teerth Reddy et al.)
 121 + *  https://lkml.org/lkml/2010/6/18/4
 122 + *  https://lists.linux-foundation.org/pipermail/linux-pm/2010-June/027422.html
 123 + *  https://patchwork.kernel.org/patch/96442/
 124 + *
 125 + * Copyright (C) 2006 Rafael J. Wysocki <rjw at sisk.pl>
 126 + *
 127 + * License terms: GNU General Public License (GPL) version 2
 128 + */
 129 +
 130 +#include <linux/mm.h>
 131 +#include <linux/suspend.h>
 132 +#include <asm/system_misc.h>
 133 +#include <asm/idmap.h>
 134 +#include <asm/suspend.h>
 135 +#include <asm/memory.h>
 136 +
 137 +struct swsusp_archdata {
 138 +       u32 nosave_backup_phys;
 139 +       u32 nosave_begin_phys;
 140 +       u32 nosave_end_phys;
 141 +       /* Function pointer */
 142 +       u32 cpu_resume_restore_nosave;
 143 +};
 144 +
 145 +static struct swsusp_archdata __archdata;
 146 +
 147 +void swsusp_arch_add_info(char *archdata)
 148 +{
 149 +       memcpy((void *)archdata, (void *)&__archdata,
 150 +                       sizeof(struct swsusp_archdata));
 151 +}
 152 +
 153 +int pfn_is_nosave(unsigned long pfn)
 154 +{
 155 +       unsigned long nosave_begin_pfn = virt_to_pfn(&__nosave_begin);
 156 +       unsigned long nosave_end_pfn = virt_to_pfn(&__nosave_end - 1);
 157 +
 158 +       return (pfn >= nosave_begin_pfn) && (pfn <= nosave_end_pfn);
 159 +}
 160 +
 161 +void notrace save_processor_state(void)
 162 +{
 163 +       WARN_ON(num_online_cpus() != 1);
 164 +       local_fiq_disable();
 165 +}
 166 +
 167 +void notrace restore_processor_state(void)
 168 +{
 169 +       local_fiq_enable();
 170 +}
 171 +
 172 +/*
 173 + * Snapshot kernel memory and reset the system.
 174 + *
 175 + * swsusp_save() is executed in the suspend finisher so that the CPU
 176 + * context pointer and memory are part of the saved image, which is
 177 + * required by the resume kernel image to restart execution from
 178 + * swsusp_arch_suspend().
 179 + *
 180 + * soft_restart is not technically needed, but is used to get success
 181 + * returned from cpu_suspend.
 182 + *
 183 + * When soft reboot completes, the hibernation snapshot is written out.
 184 + */
 185 +static int notrace arch_save_image(unsigned long unused)
 186 +{
 187 +       int ret;
 188 +       ret = swsusp_save();
 189 +       if (ret == 0)
 190 +               soft_restart(virt_to_phys(cpu_resume));
 191 +       return ret;
 192 +}
 193 +
 194 +/*
 195 + * Save the current CPU state before suspend / poweroff.
 196 + */
 197 +int notrace swsusp_arch_suspend(void)
 198 +{
 199 +       return cpu_suspend(0, arch_save_image);
 200 +}
 201 +
 202 +/*
 203 + * Restore page contents for physical pages that were in use during loading
 204 + * hibernation image.  Switch to idmap_pgd so the physical page tables
 205 + * are overwritten with the same contents.
 206 + */
 207 +static void notrace arch_restore_image(void *unused)
 208 +{
 209 +       struct pbe *pbe;
 210 +
 211 +
 212 +       cpu_switch_mm(idmap_pgd, &init_mm);
 213 +       for (pbe = restore_pblist; pbe; pbe = pbe->next)
 214 +               copy_page(pbe->orig_address, pbe->address);
 215 +
 216 +       soft_restart(virt_to_phys(cpu_resume));
 217 +}
 218 +static u64 resume_stack[PAGE_SIZE/2/sizeof(u64)] __nosavedata;
 219 +
 220 +/*
 221 + * Resume from the hibernation image.
 222 + * Due to the kernel heap / data restore, stack contents change underneath
 223 + * and that would make function calls impossible; switch to a temporary
 224 + * stack within the nosave region to avoid that problem.
 225 + */
 226 +int swsusp_arch_resume(void)
 227 +{
 228 +       call_with_stack(arch_restore_image, 0,
 229 +               resume_stack + ARRAY_SIZE(resume_stack));
 230 +       return 0;
 231 +}
 232 +
 233 +static int __init swsusp_arch_init(void)
 234 +{
 235 +       char *backup;
 236 +       size_t len;
 237 +
 238 +       len = &__nosave_end - &__nosave_begin;
 239 +       backup = kmalloc(len, GFP_KERNEL);
 240 +       if (backup)
 241 +               memcpy(backup, &__nosave_begin, len);
 242 +
 243 +       __archdata.nosave_backup_phys = virt_to_phys(backup);
 244 +       __archdata.nosave_begin_phys = virt_to_phys(&__nosave_begin);
 245 +       __archdata.nosave_end_phys = virt_to_phys(&__nosave_end);
 246 +       __archdata.cpu_resume_restore_nosave =
 247 +               virt_to_phys(cpu_resume_restore_nosave);
 248 +
 249 +       return 0;
 250 +}
 251 +late_initcall(swsusp_arch_init);
 252 diff --git a/arch/arm/kernel/process.c b/arch/arm/kernel/process.c
 253 index 7927629..ae56f0b 100644
 254 --- a/arch/arm/kernel/process.c
 255 +++ b/arch/arm/kernel/process.c
 256 @@ -98,7 +98,7 @@ void soft_restart(unsigned long addr)
 257         u64 *stack = soft_restart_stack + ARRAY_SIZE(soft_restart_stack);
 258
 259         /* Disable interrupts first */
 260 -       local_irq_disable();
 261 +       raw_local_irq_disable();
 262         local_fiq_disable();
 263
 264         /* Disable the L2 if we're the last man standing. */
 265 @@ -284,12 +284,17 @@ void __show_regs(struct pt_regs *regs)
 266         buf[3] = flags & PSR_V_BIT ? 'V' : 'v';
 267         buf[4] = '\0';
 268
 269 +#ifndef CONFIG_CPU_V7M
 270         printk("Flags: %s  IRQs o%s  FIQs o%s  Mode %s  ISA %s  Segment %s\n",
 271                 buf, interrupts_enabled(regs) ? "n" : "ff",
 272                 fast_interrupts_enabled(regs) ? "n" : "ff",
 273                 processor_modes[processor_mode(regs)],
 274                 isa_modes[isa_mode(regs)],
 275                 get_fs() == get_ds() ? "kernel" : "user");
 276 +#else
 277 +       printk("xPSR: %08lx\n", regs->ARM_cpsr);
 278 +#endif
 279 +
 280  #ifdef CONFIG_CPU_CP15
 281         {
 282                 unsigned int ctrl;
 283 diff --git a/arch/arm/kernel/sleep.S b/arch/arm/kernel/sleep.S
 284 index 987dcf3..e4d092f 100644
 285 --- a/arch/arm/kernel/sleep.S
 286 +++ b/arch/arm/kernel/sleep.S
 287 @@ -98,6 +98,14 @@ THUMB(       mov     sp, r2                  )
 288  THUMB( bx      r3                      )
 289  ENDPROC(cpu_resume)
 290
 291 +       .align
 292 +ENTRY(cpu_resume_restore_nosave)
 293 +1:     ldmia   r0!, {r3-r10}
 294 +       stmia   r1!, {r3-r10}
 295 +       cmp     r1, r2
 296 +       bne     1b
 297 +       b       cpu_resume
 298 +
 299  sleep_save_sp:
 300         .rept   CONFIG_NR_CPUS
 301         .long   0                               @ preserve stack phys ptr here
 302 diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
 303 index 5919eb4..c9a2991 100644
 304 --- a/arch/arm/kernel/smp.c
 305 +++ b/arch/arm/kernel/smp.c
 306 @@ -125,6 +125,12 @@ void __init smp_init_cpus(void)
 307                 smp_ops.smp_init_cpus();
 308  }
 309
 310 +void arch_smp_prepare_cpus(unsigned int max_cpus)
 311 +{
 312 +       if (smp_ops.smp_prepare_cpus)
 313 +               smp_ops.smp_prepare_cpus(max_cpus);
 314 +}
 315 +
 316  int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 317  {
 318         if (smp_ops.smp_boot_secondary)
 319 diff --git a/arch/arm/kernel/suspend.c b/arch/arm/kernel/suspend.c
 320 index c59c97e..38a5067 100644
 321 --- a/arch/arm/kernel/suspend.c
 322 +++ b/arch/arm/kernel/suspend.c
 323 @@ -10,6 +10,42 @@
 324  extern int __cpu_suspend(unsigned long, int (*)(unsigned long));
 325  extern void cpu_resume_mmu(void);
 326
 327 +#ifdef CONFIG_MMU
 328 +/*
 329 + * Hide the first two arguments to __cpu_suspend - these are an implementation
 330 + * detail which platform code shouldn't have to know about.
 331 + */
 332 +int cpu_suspend(unsigned long arg, int (*fn)(unsigned long))
 333 +{
 334 +       struct mm_struct *mm = current->active_mm;
 335 +       int ret;
 336 +
 337 +       if (!idmap_pgd)
 338 +               return -EINVAL;
 339 +
 340 +       /*
 341 +        * Provide a temporary page table with an identity mapping for
 342 +        * the MMU-enable code, required for resuming.  On successful
 343 +        * resume (indicated by a zero return code), we need to switch
 344 +        * back to the correct page tables.
 345 +        */
 346 +       ret = __cpu_suspend(arg, fn);
 347 +       if (ret == 0) {
 348 +               cpu_switch_mm(mm->pgd, mm);
 349 +               local_flush_bp_all();
 350 +               local_flush_tlb_all();
 351 +       }
 352 +
 353 +       return ret;
 354 +}
 355 +#else
 356 +int cpu_suspend(unsigned long arg, int (*fn)(unsigned long))
 357 +{
 358 +       return __cpu_suspend(arg, fn);
 359 +}
 360 +#define        idmap_pgd       NULL
 361 +#endif
 362 +
 363  /*
 364   * This is called by __cpu_suspend() to save the state, and do whatever
 365   * flushing is required to ensure that when the CPU goes to sleep we have
 366 @@ -46,31 +82,3 @@ void __cpu_suspend_save(u32 *ptr, u32 ptrsz, u32 sp, u32 *save_ptr)
 367         outer_clean_range(virt_to_phys(save_ptr),
 368                           virt_to_phys(save_ptr) + sizeof(*save_ptr));
 369  }
 370 -
 371 -/*
 372 - * Hide the first two arguments to __cpu_suspend - these are an implementation
 373 - * detail which platform code shouldn't have to know about.
 374 - */
 375 -int cpu_suspend(unsigned long arg, int (*fn)(unsigned long))
 376 -{
 377 -       struct mm_struct *mm = current->active_mm;
 378 -       int ret;
 379 -
 380 -       if (!idmap_pgd)
 381 -               return -EINVAL;
 382 -
 383 -       /*
 384 -        * Provide a temporary page table with an identity mapping for
 385 -        * the MMU-enable code, required for resuming.  On successful
 386 -        * resume (indicated by a zero return code), we need to switch
 387 -        * back to the correct page tables.
 388 -        */
 389 -       ret = __cpu_suspend(arg, fn);
 390 -       if (ret == 0) {
 391 -               cpu_switch_mm(mm->pgd, mm);
 392 -               local_flush_bp_all();
 393 -               local_flush_tlb_all();
 394 -       }
 395 -
 396 -       return ret;
 397 -}
 398 diff --git a/arch/arm/mach-shmobile/Kconfig b/arch/arm/mach-shmobile/Kconfig
 399 index 7c15245..73371de 100644
 400 --- a/arch/arm/mach-shmobile/Kconfig
 401 +++ b/arch/arm/mach-shmobile/Kconfig
 402 @@ -64,6 +64,25 @@ config MACH_KOELSCH
 403         select MICREL_PHY if SH_ETH
 404         select SND_SOC_AK4642 if SND_SIMPLE_CARD
 405
 406 +config MACH_KOELSCH_FTEN
 407 +       bool "FTEN spf development environment"
 408 +       depends on MACH_KOELSCH
 409 +
 410 +config MACH_FTEN
 411 +       bool
 412 +
 413 +config MACH_FTEN_DT
 414 +       bool
 415 +
 416 +config MACH_FTEN_M2W
 417 +       bool "FTEN R-Car M2W board"
 418 +       depends on ARCH_R8A7791
 419 +       select MACH_FTEN
 420 +       select MACH_FTEN_DT
 421 +       select HAVE_IDE
 422 +       select FIQ
 423 +       select SND_SOC_DIRANA3 if SND_SIMPLE_CARD
 424 +
 425  config MACH_LAGER
 426         bool "Lager board"
 427         depends on ARCH_R8A7790
 428 @@ -76,6 +95,15 @@ config MACH_GOSE
 429         select MICREL_PHY if SH_ETH
 430         select SND_SOC_AK4642 if SND_SIMPLE_CARD
 431
 432 +config MACH_FTEN_M2N
 433 +       bool "FTEN R-Car M2N board"
 434 +       depends on ARCH_R8A7793
 435 +       select MACH_FTEN
 436 +       select MACH_FTEN_DT
 437 +       select HAVE_IDE
 438 +       select FIQ
 439 +       select SND_SOC_DIRANA3 if SND_SIMPLE_CARD
 440 +
 441  config MACH_ALT
 442         bool "Alt board"
 443         depends on ARCH_R8A7794
 444 @@ -287,6 +315,19 @@ config MACH_KOELSCH
 445         select USE_OF
 446         select MICREL_PHY if SH_ETH
 447
 448 +config MACH_FTEN
 449 +       bool "FTEN strawberry board"
 450 +       depends on ARCH_R8A7791
 451 +       select USE_OF
 452 +       select HAVE_IDE
 453 +
 454 +config MACH_FTEN_DT
 455 +       bool "FTEN strawberry board - Device Tree Implementation"
 456 +       depends on ARCH_R8A7791
 457 +       select USE_OF
 458 +       select HAVE_IDE
 459 +       select SND_SOC_DIRANA3 if SND_SIMPLE_CARD
 460 +
 461  config MACH_KZM9G
 462         bool "KZM-A9-GT board"
 463         depends on ARCH_SH73A0
 464 @@ -360,4 +401,28 @@ config EM_TIMER_STI
 465
 466  endmenu
 467
 468 +if HIBERNATION
 469 +
 470 +menu "Hibernation area parameters"
 471 +
 472 +config SWSUSP_AREA
 473 +       hex "RAM hibernation area address"
 474 +       default "0x44000000"
 475 +       depends on HIBERNATION
 476 +       ---help---
 477 +           RAM hibernation area address, this is required for CRC
 478 +           calculation of final compressed hibernation image
 479 +
 480 +config SWSUSP_AREA_SIZE
 481 +       hex "RAM hibernation area size"
 482 +       default "0x4000000"
 483 +       depends on HIBERNATION
 484 +       ---help---
 485 +           RAM hibernation area size, this is required for CRC
 486 +           calculation of final compressed hibernation image
 487 +
 488 +endmenu
 489 +
 490 +endif
 491 +
 492  endif
 493 diff --git a/arch/arm/mach-shmobile/Makefile b/arch/arm/mach-shmobile/Makefile
 494 index 43b4025..71cfcfa 100644
 495 --- a/arch/arm/mach-shmobile/Makefile
 496 +++ b/arch/arm/mach-shmobile/Makefile
 497 @@ -55,6 +55,7 @@ smp-$(CONFIG_ARCH_EMEV2)      += smp-emev2.o headsmp-scu.o platsmp-scu.o
 498
 499  # PM objects
 500  obj-$(CONFIG_SUSPEND)          += suspend.o
 501 +obj-$(CONFIG_HIBERNATION)      += hibernation.o
 502  obj-$(CONFIG_CPU_IDLE)         += cpuidle.o
 503  obj-$(CONFIG_CPU_FREQ)         += cpufreq.o
 504  obj-$(CONFIG_ARCH_SH7372)      += pm-sh7372.o sleep-sh7372.o pm-rmobile.o
 505 diff --git a/arch/arm/mach-shmobile/common.h b/arch/arm/mach-shmobile/common.h
 506 index 95a77a0..37c7f87 100644
 507 --- a/arch/arm/mach-shmobile/common.h
 508 +++ b/arch/arm/mach-shmobile/common.h
 509 @@ -25,6 +25,7 @@ struct clk;
 510  extern int shmobile_clk_init(void);
 511  extern void shmobile_handle_irq_intc(struct pt_regs *);
 512  extern struct platform_suspend_ops shmobile_suspend_ops;
 513 +extern const struct platform_hibernation_ops shmobile_hibernation_ops;
 514  struct cpuidle_driver;
 515  extern void shmobile_cpuidle_set_driver(struct cpuidle_driver *drv);
 516  extern void shmobile_smp_apmu_enter_cpuidle(void);
 517 @@ -37,6 +38,12 @@ static inline int shmobile_suspend_init(void) { return 0; }
 518  static inline void shmobile_smp_apmu_suspend_init(void) { }
 519  #endif
 520
 521 +#ifdef CONFIG_HIBERNATION
 522 +int shmobile_hibernation_init(void);
 523 +#else
 524 +static inline int shmobile_hibernation_init(void) { return 0; }
 525 +#endif
 526 +
 527  #ifdef CONFIG_CPU_IDLE
 528  int shmobile_cpuidle_init(void);
 529  #else
 530 @@ -59,6 +66,7 @@ extern unsigned int l2actlr_value;
 531  static inline void __init shmobile_init_late(void)
 532  {
 533         shmobile_suspend_init();
 534 +       shmobile_hibernation_init();
 535         shmobile_cpuidle_init();
 536         shmobile_cpufreq_init();
 537  }
 538 diff --git a/arch/arm/mach-shmobile/crc32_word4.c b/arch/arm/mach-shmobile/crc32_word4.c
 539 new file mode 100644
 540 index 0000000..8aaefc6
 541 --- /dev/null
 542 +++ b/arch/arm/mach-shmobile/crc32_word4.c
 543 @@ -0,0 +1,299 @@
 544 +/*************************************************************************
 545 + * crc32_word4.c: rapid CRC32
 546 + * Coptright (C) FUJITSUTEN Limited, 2015 All Rights Reserved.
 547 + *
 548 + * This program is free software; you can redistribute it and/or modify
 549 + * it under the terms of the GNU General Public License version 2
 550 + * as published by the Free Software Foundation.
 551 + *
 552 + * This program is distributed in the hope that it will be useful,
 553 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
 554 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 555 + * GNU General Public License for more details.
 556 + *
 557 + * You should have received a copy of the GNU General Public License
 558 + * along with this program; if not, write to the Free Software Foundation,
 559 + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 560 + *************************************************************************/
 561 +#ifdef OWNTEST
 562 +#include <stdio.h>
 563 +#include <stdlib.h>
 564 +#include <string.h>
 565 +#include <asm/types.h>
 566 +typedef unsigned int   u_int32_t;
 567 +#else
 568 +#endif
 569 +
 570 +#include "crc32_word4.h"
 571 +
 572 +#define CRC_INIT_VALUE  (-1)
 573 +#define CRC_FIX(_crc32) (~(_crc32))
 574 +
 575 +/* #define HWDPLS_ENABLE */
 576 +#define        __HWDTPLS_OUT()
 577 +#define MEASURE(msg)
 578 +
 579 +/**** calc_crc32.c *****/
 580 +
 581 +/*
 582 + * CRC32は、ISO 3309 で規程され
 583 + * そのサンプルは
 584 + * RFC 2083 :PNG(Poratble Network Graphics
 585 + * で公になっています。本プログラムは、RFC2083 で掲示された
 586 + * CRC32を独自に最適化したプログラムです。
 587 + */
 588 +const static u_int32_t CRC_Table[256] = {
 589 +       0x00000000 , 0x77073096 , 0xee0e612c , 0x990951ba , 0x076dc419 , 0x706af48f , 0xe963a535 , 0x9e6495a3 ,
 590 +       0x0edb8832 , 0x79dcb8a4 , 0xe0d5e91e , 0x97d2d988 , 0x09b64c2b , 0x7eb17cbd , 0xe7b82d07 , 0x90bf1d91 ,
 591 +       0x1db71064 , 0x6ab020f2 , 0xf3b97148 , 0x84be41de , 0x1adad47d , 0x6ddde4eb , 0xf4d4b551 , 0x83d385c7 ,
 592 +       0x136c9856 , 0x646ba8c0 , 0xfd62f97a , 0x8a65c9ec , 0x14015c4f , 0x63066cd9 , 0xfa0f3d63 , 0x8d080df5 ,
 593 +       0x3b6e20c8 , 0x4c69105e , 0xd56041e4 , 0xa2677172 , 0x3c03e4d1 , 0x4b04d447 , 0xd20d85fd , 0xa50ab56b ,
 594 +       0x35b5a8fa , 0x42b2986c , 0xdbbbc9d6 , 0xacbcf940 , 0x32d86ce3 , 0x45df5c75 , 0xdcd60dcf , 0xabd13d59 ,
 595 +       0x26d930ac , 0x51de003a , 0xc8d75180 , 0xbfd06116 , 0x21b4f4b5 , 0x56b3c423 , 0xcfba9599 , 0xb8bda50f ,
 596 +       0x2802b89e , 0x5f058808 , 0xc60cd9b2 , 0xb10be924 , 0x2f6f7c87 , 0x58684c11 , 0xc1611dab , 0xb6662d3d ,
 597 +       0x76dc4190 , 0x01db7106 , 0x98d220bc , 0xefd5102a , 0x71b18589 , 0x06b6b51f , 0x9fbfe4a5 , 0xe8b8d433 ,
 598 +       0x7807c9a2 , 0x0f00f934 , 0x9609a88e , 0xe10e9818 , 0x7f6a0dbb , 0x086d3d2d , 0x91646c97 , 0xe6635c01 ,
 599 +       0x6b6b51f4 , 0x1c6c6162 , 0x856530d8 , 0xf262004e , 0x6c0695ed , 0x1b01a57b , 0x8208f4c1 , 0xf50fc457 ,
 600 +       0x65b0d9c6 , 0x12b7e950 , 0x8bbeb8ea , 0xfcb9887c , 0x62dd1ddf , 0x15da2d49 , 0x8cd37cf3 , 0xfbd44c65 ,
 601 +       0x4db26158 , 0x3ab551ce , 0xa3bc0074 , 0xd4bb30e2 , 0x4adfa541 , 0x3dd895d7 , 0xa4d1c46d , 0xd3d6f4fb ,
 602 +       0x4369e96a , 0x346ed9fc , 0xad678846 , 0xda60b8d0 , 0x44042d73 , 0x33031de5 , 0xaa0a4c5f , 0xdd0d7cc9 ,
 603 +       0x5005713c , 0x270241aa , 0xbe0b1010 , 0xc90c2086 , 0x5768b525 , 0x206f85b3 , 0xb966d409 , 0xce61e49f ,
 604 +       0x5edef90e , 0x29d9c998 , 0xb0d09822 , 0xc7d7a8b4 , 0x59b33d17 , 0x2eb40d81 , 0xb7bd5c3b , 0xc0ba6cad ,
 605 +       0xedb88320 , 0x9abfb3b6 , 0x03b6e20c , 0x74b1d29a , 0xead54739 , 0x9dd277af , 0x04db2615 , 0x73dc1683 ,
 606 +       0xe3630b12 , 0x94643b84 , 0x0d6d6a3e , 0x7a6a5aa8 , 0xe40ecf0b , 0x9309ff9d , 0x0a00ae27 , 0x7d079eb1 ,
 607 +       0xf00f9344 , 0x8708a3d2 , 0x1e01f268 , 0x6906c2fe , 0xf762575d , 0x806567cb , 0x196c3671 , 0x6e6b06e7 ,
 608 +       0xfed41b76 , 0x89d32be0 , 0x10da7a5a , 0x67dd4acc , 0xf9b9df6f , 0x8ebeeff9 , 0x17b7be43 , 0x60b08ed5 ,
 609 +       0xd6d6a3e8 , 0xa1d1937e , 0x38d8c2c4 , 0x4fdff252 , 0xd1bb67f1 , 0xa6bc5767 , 0x3fb506dd , 0x48b2364b ,
 610 +       0xd80d2bda , 0xaf0a1b4c , 0x36034af6 , 0x41047a60 , 0xdf60efc3 , 0xa867df55 , 0x316e8eef , 0x4669be79 ,
 611 +       0xcb61b38c , 0xbc66831a , 0x256fd2a0 , 0x5268e236 , 0xcc0c7795 , 0xbb0b4703 , 0x220216b9 , 0x5505262f ,
 612 +       0xc5ba3bbe , 0xb2bd0b28 , 0x2bb45a92 , 0x5cb36a04 , 0xc2d7ffa7 , 0xb5d0cf31 , 0x2cd99e8b , 0x5bdeae1d ,
 613 +       0x9b64c2b0 , 0xec63f226 , 0x756aa39c , 0x026d930a , 0x9c0906a9 , 0xeb0e363f , 0x72076785 , 0x05005713 ,
 614 +       0x95bf4a82 , 0xe2b87a14 , 0x7bb12bae , 0x0cb61b38 , 0x92d28e9b , 0xe5d5be0d , 0x7cdcefb7 , 0x0bdbdf21 ,
 615 +       0x86d3d2d4 , 0xf1d4e242 , 0x68ddb3f8 , 0x1fda836e , 0x81be16cd , 0xf6b9265b , 0x6fb077e1 , 0x18b74777 ,
 616 +       0x88085ae6 , 0xff0f6a70 , 0x66063bca , 0x11010b5c , 0x8f659eff , 0xf862ae69 , 0x616bffd3 , 0x166ccf45 ,
 617 +       0xa00ae278 , 0xd70dd2ee , 0x4e048354 , 0x3903b3c2 , 0xa7672661 , 0xd06016f7 , 0x4969474d , 0x3e6e77db ,
 618 +       0xaed16a4a , 0xd9d65adc , 0x40df0b66 , 0x37d83bf0 , 0xa9bcae53 , 0xdebb9ec5 , 0x47b2cf7f , 0x30b5ffe9 ,
 619 +       0xbdbdf21c , 0xcabac28a , 0x53b39330 , 0x24b4a3a6 , 0xbad03605 , 0xcdd70693 , 0x54de5729 , 0x23d967bf ,
 620 +       0xb3667a2e , 0xc4614ab8 , 0x5d681b02 , 0x2a6f2b94 , 0xb40bbe37 , 0xc30c8ea1 , 0x5a05df1b , 0x2d02ef8d ,
 621 +};
 622 +
 623 +/***
 624 + * CRC Table creater.
 625 + *
 626 +void make_crc_table(void) {
 627 +       u_int32_t c;
 628 +       u_int32_t n, k;
 629 +       for (n = 0; n < 256; n++)
 630 +       {
 631 +               c = (u_int32_t) n;
 632 +               for (k = 0; k < 8; k++)
 633 +               {
 634 +                       if (c & 1)
 635 +                               c = 0xedb88320L ^ (c >> 1);
 636 +                       else
 637 +                               c = c >> 1;
 638 +               }
 639 +               CRC_Table[n] = c;
 640 +       }
 641 +}
 642 +***/
 643 +#define NEXT_PTR       (4)
 644 +
 645 +static __inline__
 646 +u_int32_t _update_crc(u_int32_t crc, unsigned char *buf, size_t len)
 647 +{
 648 +       u_int32_t c = crc;
 649 +       size_t n;
 650 +       for (n = 0; n < len; n++)
 651 +               c = CRC_Table[(c ^ buf[n]) & 0xff] ^ (c >> 8);
 652 +       return c;
 653 +}
 654 +/*********************************************************************
 655 + * update_crc4x4()()
 656 + * calc_crc32() をベースに、4 ワード毎に個別に CRC32 を計算する方法
 657 + *
 658 + *              +0        +1        +2        +3
 659 + *  +0x00    AAAAAAAA  BBBBBBBB  CCCCCCCC  DDDDDDDD
 660 + *  +0x04    EEEEEEEE  FFFFFFFF  00000000  11111111
 661 + *              :         :         :         :
 662 + *  CRC32    xxxxxxxx  xxxxxxxx  xxxxxxxx  xxxxxxxx
 663 + *
 664 + *********************************************************************/
 665 +
 666 +static __inline__
 667 +void update_crc4x4(u_int32_t crc[4], unsigned char *buf)
 668 +{
 669 +       u_int32_t c1, c2, c3, c4;
 670 +       u_int32_t *p = (void *)buf;
 671 +
 672 +       c1 = crc[0] ^ p[0];
 673 +       c2 = crc[1] ^ p[1];
 674 +       c3 = crc[2] ^ p[2];
 675 +       c4 = crc[3] ^ p[3];
 676 +
 677 +       c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
 678 +       c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
 679 +       c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
 680 +       c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
 681 +
 682 +       c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
 683 +       c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
 684 +       c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
 685 +       c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
 686 +
 687 +       c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
 688 +       c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
 689 +       c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
 690 +       c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
 691 +
 692 +       c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
 693 +       c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
 694 +       c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
 695 +       c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
 696 +
 697 +       crc[0] = c1;
 698 +       crc[1] = c2;
 699 +       crc[2] = c3;
 700 +       crc[3] = c4;
 701 +}
 702 +
 703 +
 704 +void calc_crc32x4(unsigned char *buf, size_t len, CRC32_WORD4_t *result)
 705 +{
 706 +       unsigned int crc_tmp[4] = {CRC_INIT_VALUE, CRC_INIT_VALUE, CRC_INIT_VALUE, CRC_INIT_VALUE};
 707 +       u_int32_t i;
 708 +       int res;
 709 +       u_int32_t n4;
 710 +       int xlen = len;
 711 +#ifdef HWDPLS_ENABLE
 712 +       unsigned long plstout  = 60;
 713 +       unsigned long plsstart = 0;
 714 +       if ((unsigned long)CONFIG_SYS_HZ > 100000)
 715 +               plstout *= (unsigned long)CONFIG_SYS_HZ / 1000;
 716 +       else
 717 +               plstout =  DIV_ROUND_UP(plstout * (unsigned long)CONFIG_SYS_HZ, 1000);
 718 +#endif
 719 +
 720 +       /**
 721 +        * 4バイト境界に合わない開始アドレスの場合
 722 +        * 境界までのCRCを crc_tmp[0] に求める。
 723 +        */
 724 +       if ((unsigned long)buf & 3) {
 725 +               crc_tmp[0]  = _update_crc(crc_tmp[0], buf, (unsigned long)buf & 3);
 726 +               buf = (unsigned char *)((unsigned long)buf & ~3);
 727 +               xlen -= (unsigned long)buf & 3;
 728 +       }
 729 +
 730 +       n4 = xlen/(NEXT_PTR*4);
 731 +       /**
 732 +        * 4バイト境界に合わない開始アドレスの場合
 733 +        * 境界までのCRCを crc_tmp[0] に求める。
 734 +        */
 735 +#ifdef HWDPLS_ENABLE
 736 +       reset_timer();
 737 +       plsstart = get_timer(0);
 738 +#endif
 739 +       for (i = 0; i < n4; i++) {
 740 +               update_crc4x4(crc_tmp, buf);
 741 +               buf += NEXT_PTR * 4;
 742 +#ifdef HWDPLS_ENABLE
 743 +               /**
 744 +                * WDを考慮
 745 +                */
 746 +               if (__builtin_expect((int)((i & 0x1f) == 0), 0)) {
 747 +                       if ((get_timer(plsstart)) > plstout) {
 748 +                               __HWDTPLS_OUT();
 749 +                               MEASURE("crc plsout")
 750 +                               plsstart += plstout;
 751 +                       }
 752 +               }
 753 +#endif /*HWPLS_ENABLE*/
 754 +       }
 755 +
 756 +       res = xlen % (NEXT_PTR * 4);
 757 +       if (res > 0)
 758 +               crc_tmp[3]  = _update_crc(crc_tmp[3], buf, res);
 759 +
 760 +       result->crc_w[0] = CRC_FIX(crc_tmp[0]);
 761 +       result->crc_w[1] = CRC_FIX(crc_tmp[1]);
 762 +       result->crc_w[2] = CRC_FIX(crc_tmp[2]);
 763 +       result->crc_w[3] = CRC_FIX(crc_tmp[3]);
 764 +
 765 +       MEASURE("calc_crc32x4 finish")
 766 +}
 767 +
 768 +#if defined(OWNTEST)
 769 +#define BUFSIZE (2 * 1024 * 1024)
 770 +#include <sys/time.h>
 771 +#include <malloc.h>
 772 +
 773 +int main()
 774 +{
 775 +       unsigned char *buf, *buf2;
 776 +       struct timeval start, end;
 777 +       unsigned long long diff;
 778 +       int i;
 779 +
 780 +       CRC32_WORD4_t result =  { .crc_w = {0, 0, 0, 0 } };
 781 +       CRC32_WORD4_t result2 = { .crc_w = {0, 0, 0, 0 } };
 782 +
 783 +       buf = malloc(BUFSIZE);
 784 +       if (!buf) {
 785 +               perror("malloc");
 786 +               return 1;
 787 +       }
 788 +       printf("Generate %dMB random data..\n", BUFSIZE / 1024 / 1024);
 789 +       srand(0);
 790 +       for (i = 0; i < BUFSIZE / 4; i++)
 791 +               ((int *)buf)[i] = rand();
 792 +
 793 +       /* Memory dup */
 794 +       buf2 = memalign(NEXT_PTR, BUFSIZE);
 795 +       if (!buf2) {
 796 +               perror("malloc");
 797 +               return 1;
 798 +       }
 799 +       memcpy(buf2, buf, BUFSIZE);
 800 +
 801 +
 802 +       gettimeofday(&start, NULL);
 803 +       calc_crc32x4(buf, BUFSIZE, &result);
 804 +       gettimeofday(&end, NULL);
 805 +
 806 +       diff = (end.tv_sec - start.tv_sec) * 1000000;
 807 +       diff += end.tv_usec - start.tv_usec;
 808 +
 809 +       printf("time=%lluus\n", diff);
 810 +       printf(" result.word[0] = %x\n", result.crc_w[0]);
 811 +       printf(" result.word[1] = %x\n", result.crc_w[1]);
 812 +       printf(" result.word[2] = %x\n", result.crc_w[2]);
 813 +       printf(" result.word[3] = %x\n", result.crc_w[3]);
 814 +
 815 +       /* Broken test */
 816 +#if 0  /* Destory test */
 817 +       buf[rand() % BUFSIZE]  ^= 1 << (rand()%7);
 818 +#endif
 819 +       for (i = 0; i < BUFSIZE; i++) {
 820 +               if (buf[i] != buf2[i])
 821 +                       printf("buf[%d] %02x : %02x\n", i, buf[i], buf2[i]);
 822 +       }
 823 +
 824 +       gettimeofday(&start, NULL);
 825 +       calc_crc32x4(buf, BUFSIZE, &result2);
 826 +       gettimeofday(&end, NULL);
 827 +
 828 +       diff = (end.tv_sec - start.tv_sec) * 1000000;
 829 +       diff += end.tv_usec - start.tv_usec;
 830 +
 831 +       printf("time=%lluus\n", diff);
 832 +       printf(" result.word[0] = %x:%s\n", result2.crc_w[0] ,
 833 +               result.crc_w[0] == result2.crc_w[0] ? "OK" : "NG");
 834 +       printf(" result.word[1] = %x:%s\n", result2.crc_w[1] ,
 835 +               result.crc_w[1] == result2.crc_w[1] ? "OK" : "NG");
 836 +       printf(" result.word[2] = %x:%s\n", result2.crc_w[2] ,
 837 +               result.crc_w[2] == result2.crc_w[2] ? "OK" : "NG");
 838 +       printf(" result.word[3] = %x:%s\n", result2.crc_w[3] ,
 839 +               result.crc_w[3] == result2.crc_w[3] ? "OK" : "NG");
 840 +       return 0;
 841 +}
 842 +#endif /* TEST */
 843 diff --git a/arch/arm/mach-shmobile/crc32_word4.h b/arch/arm/mach-shmobile/crc32_word4.h
 844 new file mode 100644
 845 index 0000000..6c04878
 846 --- /dev/null
 847 +++ b/arch/arm/mach-shmobile/crc32_word4.h
 848 @@ -0,0 +1,23 @@
 849 +/*************************************************************************
 850 + * Coptright (C) FUJITSUTEN Limited, 2012 All Rights Reserved.
 851 + *
 852 + *************************************************************************/
 853 +#ifndef __CRC32_WORD4_H__
 854 +#define __CRC32_WORD4_H__
 855 +
 856 +typedef struct {
 857 +       unsigned int crc_w[4];
 858 +} CRC32_WORD4_t;
 859 +
 860 +void calc_crc32x4(unsigned char *buf, size_t len, CRC32_WORD4_t *result);
 861 +
 862 +typedef struct {
 863 +       unsigned int size;
 864 +       CRC32_WORD4_t chksum;
 865 +       unsigned int dummy[3];
 866 +} CRC32_WORD4_TICKET_t;
 867 +
 868 +#define IS_CRC_WORD4_OK(_res1, _res2) (!memcmp(_res1, _res2, sizeof(CRC32_WORD4_t)))
 869 +#define IS_CRC_WORD4_ZERO(_w4) (((_w4)->crc_w[0] == 0) && ((_w4)->crc_w[1] == 0) && ((_w4)->crc_w[2] == 0) && ((_w4)->crc_w[3] == 0))
 870 +#define IS_CRC_WORD4_ALL_F(_w4) (((_w4)->crc_w[0] == 0xffffffff) && ((_w4)->crc_w[1] == 0xffffffff) && ((_w4)->crc_w[2] == 0xffffffff) && ((_w4)->crc_w[3] == 0xffffffff))
 871 +#endif
 872 diff --git a/arch/arm/mach-shmobile/headsmp.S b/arch/arm/mach-shmobile/headsmp.S
 873 index debf271..f99f8b2 100644
 874 --- a/arch/arm/mach-shmobile/headsmp.S
 875 +++ b/arch/arm/mach-shmobile/headsmp.S
 876 @@ -16,8 +16,6 @@
 877  #include <linux/threads.h>
 878  #include <asm/memory.h>
 879
 880 -       __CPUINIT
 881 -
 882  #ifdef CONFIG_SMP
 883  ENTRY(shmobile_invalidate_start)
 884         bl      v7_invalidate_l1
 885 diff --git a/arch/arm/mach-shmobile/hibernation.c b/arch/arm/mach-shmobile/hibernation.c
 886 new file mode 100644
 887 index 0000000..94fa78b
 888 --- /dev/null
 889 +++ b/arch/arm/mach-shmobile/hibernation.c
 890 @@ -0,0 +1,243 @@
 891 +/*
 892 + * Suspend-to-RAM support code for SH-Mobile ARM
 893 + *
 894 + *  Copyright (C) 2011 Magnus Damm
 895 + *
 896 + * This file is subject to the terms and conditions of the GNU General Public
 897 + * License.  See the file "COPYING" in the main directory of this archive
 898 + * for more details.
 899 + */
 900 +#include <linux/pm.h>
 901 +#include <linux/suspend.h>
 902 +#include <linux/module.h>
 903 +#include <linux/err.h>
 904 +#include <linux/cpu.h>
 905 +#include <linux/smp.h>
 906 +
 907 +#include <linux/io.h>
 908 +#include <asm/system_misc.h>
 909 +#include <asm/page.h>
 910 +#include <asm/smp_plat.h>
 911 +
 912 +#include "common.h"
 913 +#include "rcar-gen2.h"
 914 +
 915 +#include <linux/clk.h>
 916 +
 917 +#include "crc32_word4.c"
 918 +#include "pm-rcar.h"
 919 +
 920 +
 921 +struct swsusp_header {
 922 +       char reserved[PAGE_SIZE - 20 - sizeof(sector_t) - sizeof(int) -
 923 +                     sizeof(u32) - sizeof(CRC32_WORD4_t) - sizeof(u32)];
 924 +       CRC32_WORD4_t comp_crc32;
 925 +       u32 img_size;   /* add.  see. kernel/power/swap.c */
 926 +       u32     crc32;
 927 +       sector_t image;
 928 +       unsigned int flags;     /* Flags to pass to the "boot" kernel */
 929 +       char    orig_sig[10];
 930 +       char    sig[10];
 931 +} __packed;
 932 +static unsigned long swsusp_area = CONFIG_SWSUSP_AREA;
 933 +static unsigned long swsusp_area_size = CONFIG_SWSUSP_AREA_SIZE;
 934 +
 935 +enum {
 936 +       MSTP00, MSTP01, MSTP02, MSTP03, MSTP04, MSTP05,
 937 +       MSTP07, MSTP08, MSTP09, MSTP10, MSTP11,
 938 +       MSTP_NR,
 939 +};
 940 +
 941 +static struct {
 942 +       u32 s_offset;
 943 +       u32 s_val;
 944 +       u32 r_offset;
 945 +       u32 r_val;
 946 +} mstp_regs[] = {
 947 +       [MSTP00] = { SMSTPCR0,  0,
 948 +                    RMSTPCR0,  0},
 949 +       [MSTP01] = { SMSTPCR1,  0,
 950 +                    RMSTPCR1,  0},
 951 +       [MSTP02] = { SMSTPCR2,  0,
 952 +                    RMSTPCR2,  0},
 953 +       [MSTP03] = { SMSTPCR3,  0,
 954 +                    RMSTPCR3,  0},
 955 +       [MSTP04] = { SMSTPCR4,  0,
 956 +                    RMSTPCR4,  0},
 957 +       [MSTP05] = { SMSTPCR5,  0,
 958 +                    RMSTPCR5,  0},
 959 +       [MSTP07] = { SMSTPCR7,  0,
 960 +                    RMSTPCR7,  0},
 961 +       [MSTP08] = { SMSTPCR8,  0,
 962 +                    RMSTPCR8,  0},
 963 +       [MSTP09] = { SMSTPCR9,  0,
 964 +                    RMSTPCR9,  0},
 965 +       [MSTP10] = { SMSTPCR10, 0,
 966 +                    RMSTPCR10, 0},
 967 +       [MSTP11] = { SMSTPCR11, 0,
 968 +                    RMSTPCR11, 0},
 969 +};
 970 +
 971 +static void save_mstp_regs(void)
 972 +{
 973 +       int i;
 974 +       void *m = ioremap(CPG_BASE, CPG_LEN);
 975 +       for (i = MSTP00; i < MSTP_NR; i++) {
 976 +               mstp_regs[i].s_val = ioread32(m +mstp_regs[i].s_offset);
 977 +               mstp_regs[i].r_val = ioread32(m +mstp_regs[i].r_offset);
 978 +       }
 979 +       iounmap(m);
 980 +}
 981 +
 982 +static void restore_mstp_regs(void)
 983 +{
 984 +       int i;
 985 +       void *m = ioremap(CPG_BASE, CPG_LEN);
 986 +       for (i = MSTP00; i < MSTP_NR; i++) {
 987 +               iowrite32(mstp_regs[i].s_val, m +mstp_regs[i].s_offset);
 988 +               iowrite32(mstp_regs[i].r_val, m +mstp_regs[i].r_offset);
 989 +       }
 990 +       iounmap(m);
 991 +}
 992 +
 993 +static int shmobile_hibernation_begin(void)
 994 +{
 995 +       save_mstp_regs();
 996 +       return 0;
 997 +}
 998 +
 999 +static void shmobile_hibernation_end(void)
1000 +{
1001 +}
1002 +
1003 +static int shmobile_hibernation_pre_snapshot(void)
1004 +{
1005 +       return 0;
1006 +}
1007 +
1008 +
1009 +static void shmobile_hibernation_finish(void)
1010 +{
1011 +}
1012 +
1013 +static int shmobile_hibernation_prepare(void)
1014 +{
1015 +       return 0;
1016 +}
1017 +
1018 +static int shmobile_hibernation_enter(void)
1019 +{
1020 +       void *m, *l;
1021 +       struct swsusp_header *h;
1022 +       unsigned int calc_sz;
1023 +       if (swsusp_area_size > 0) {
1024 +               h = m = ioremap(swsusp_area, swsusp_area_size);
1025 +               if (h) {
1026 +                       if ((h->img_size > PAGE_SIZE)
1027 +                           && (h->img_size < (swsusp_area_size - PAGE_SIZE)))
1028 +                               calc_sz = h->img_size;
1029 +                       else
1030 +                               calc_sz = swsusp_area_size - PAGE_SIZE;
1031 +                       memset(&h->comp_crc32, 0, sizeof(h->comp_crc32));
1032 +                       calc_crc32x4(m + PAGE_SIZE, calc_sz, &h->comp_crc32);
1033 +                       mb();
1034 +                       iounmap(m);
1035 +               }
1036 +       }
1037 +       /* Resetting FDP0 */
1038 +       l = ioremap(0xfe940000, 0x4000);
1039 +       writel(1, l + 0x1c);
1040 +       mb();
1041 +       iounmap(l);
1042 +       /* Resetting FDP1 */
1043 +       l = ioremap(0xfe944000, 0x4000);
1044 +       writel(1, l + 0x1c);
1045 +       mb();
1046 +       iounmap(l);
1047 +       /* Doing board reset */
1048 +       l = ioremap(0xe6300200, 4);
1049 +       writel(0xa1b20001, l);
1050 +       mb();
1051 +       iounmap(l);
1052 +
1053 +       return 0;
1054 +}
1055 +
1056 +char *clks[] = {
1057 +       "ehci", "hsusb", "dmal", "dmah", "sys-dmac1",
1058 +       "sys-dmac0", "ssp", "ssp_dev", "ipmmu_gp",
1059 +       "audmac0", "audmac1",
1060 +};
1061 +
1062 +static int shmobile_hibernation_pre_restore(void)
1063 +{
1064 +       return 0;
1065 +}
1066 +
1067 +
1068 +static void shmobile_hibernation_restore_cleanup(void)
1069 +{
1070 +}
1071 +
1072 +extern int in_suspend;
1073 +
1074 +static void shmobile_hibernation_leave(void)
1075 +{
1076 +       int restore_highmem(void);
1077 +       struct clk *clk;
1078 +       unsigned int i;
1079 +
1080 +       if (!in_suspend) {
1081 +#ifdef CONFIG_SMP
1082 +               if (is_smp())
1083 +                       arch_smp_prepare_cpus(setup_max_cpus);
1084 +#endif
1085 +
1086 +#ifdef CONFIG_HIGHMEM
1087 +               restore_highmem();
1088 +#endif
1089 +               restore_mstp_regs();
1090 +       }
1091 +
1092 +       for (i = 0; i < ARRAY_SIZE(clks); ++i) {
1093 +               clk = clk_get(NULL, clks[i]);
1094 +               if (!IS_ERR(clk)) {
1095 +                       clk_prepare_enable(clk);
1096 +                       clk_put(clk);
1097 +               }
1098 +       }
1099 +}
1100 +
1101 +const struct platform_hibernation_ops shmobile_hibernation_ops = {
1102 +       .leave = shmobile_hibernation_leave,
1103 +       .begin = shmobile_hibernation_begin,
1104 +       .end = shmobile_hibernation_end,
1105 +       .pre_snapshot = shmobile_hibernation_pre_snapshot,
1106 +       .finish = shmobile_hibernation_finish,
1107 +       .prepare = shmobile_hibernation_prepare,
1108 +       .enter = shmobile_hibernation_enter,
1109 +       .pre_restore = shmobile_hibernation_pre_restore,
1110 +       .restore_cleanup = shmobile_hibernation_restore_cleanup,
1111 +};
1112 +
1113 +int __init shmobile_hibernation_init(void)
1114 +{
1115 +       hibernation_set_ops(&shmobile_hibernation_ops);
1116 +       return 0;
1117 +}
1118 +static int setup_swsusp_area(char *s)
1119 +{
1120 +       long tmp = 0;
1121 +       char *p;
1122 +       if (!kstrtol(s, 0, &tmp) && tmp > 0)
1123 +               swsusp_area = tmp;
1124 +       p = strchr(s, ',');
1125 +       if (!p)
1126 +               goto out;
1127 +       if (!kstrtol(p, 0, &tmp) && tmp > 0)
1128 +               swsusp_area_size = tmp;
1129 +out:
1130 +       return 1;
1131 +}
1132 +__setup("swsusp_area=", setup_swsusp_area);
1133 +
1134 diff --git a/arch/arm/mach-shmobile/platsmp-apmu.c b/arch/arm/mach-shmobile/platsmp-apmu.c
1135 index cff7a25..e382e26 100644
1136 --- a/arch/arm/mach-shmobile/platsmp-apmu.c
1137 +++ b/arch/arm/mach-shmobile/platsmp-apmu.c
1138 @@ -33,7 +33,10 @@
1139
1140  /* only enable the cluster that includes the boot CPU by default */
1141  static bool enable_multicluster = false;
1142 +#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_SUSPEND) || \
1143 +defined(CONFIG_CPU_IDLE)
1144  static bool is_last_cpu;
1145 +#endif
1146
1147  static __init int apmu_setup(char *opt)
1148  {
1149 @@ -71,12 +74,15 @@ static int __maybe_unused apmu_power_on(void __iomem *p, int bit)
1150         return 0;
1151  }
1152
1153 +#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_SUSPEND) || \
1154 +defined(CONFIG_CPU_IDLE)
1155  static int apmu_power_off(void __iomem *p, int bit)
1156  {
1157         /* request Core Standby for next WFI */
1158         writel_relaxed(3, p + CPUNCR_OFFS(bit));
1159         return 0;
1160  }
1161 +#endif
1162
1163  static int __maybe_unused apmu_power_off_poll(void __iomem *p, int bit)
1164  {
1165 @@ -92,12 +98,15 @@ static int __maybe_unused apmu_power_off_poll(void __iomem *p, int bit)
1166         return 0;
1167  }
1168
1169 +#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_SUSPEND) || \
1170 +defined(CONFIG_CPU_IDLE) || defined(CONFIG_SMP)
1171  static int apmu_wrap(int cpu, int (*fn)(void __iomem *p, int cpu))
1172  {
1173         void __iomem *p = apmu_cpus[cpu].iomem;
1174
1175         return p ? fn(p, apmu_cpus[cpu].bit) : -EINVAL;
1176  }
1177 +#endif
1178
1179  static void apmu_init_cpu(struct resource *res, int cpu, int bit)
1180  {
1181 @@ -141,7 +150,7 @@ static void apmu_parse_cfg(void (*fn)(struct resource *res, int cpu, int bit),
1182         }
1183  }
1184
1185 -void __init shmobile_smp_apmu_prepare_cpus(unsigned int max_cpus,
1186 +void shmobile_smp_apmu_prepare_cpus(unsigned int max_cpus,
1187                                            struct rcar_apmu_config *apmu_config,
1188                                            int num)
1189  {
1190 @@ -328,7 +337,7 @@ static int __cpuinit shmobile_smp_apmu_enter_suspend(suspend_state_t state)
1191         return 0;
1192  }
1193
1194 -void __init shmobile_smp_apmu_suspend_init(void)
1195 +void shmobile_smp_apmu_suspend_init(void)
1196  {
1197         cpucmcr_ca7  = ioremap_nocache(CPUCMCR_CA7, 0x4);
1198         cpucmcr_ca15 = ioremap_nocache(CPUCMCR_CA15, 0x4);
1199 diff --git a/arch/arm/mach-shmobile/platsmp-rst.c b/arch/arm/mach-shmobile/platsmp-rst.c
1200 index 70a2b6c..7ba9eeb 100644
1201 --- a/arch/arm/mach-shmobile/platsmp-rst.c
1202 +++ b/arch/arm/mach-shmobile/platsmp-rst.c
1203 @@ -11,8 +11,7 @@
1204  #include <linux/io.h>
1205  #include <asm/smp_plat.h>
1206  #include <mach/platsmp-rst.h>
1207 -
1208 -#define RST            0xe6160000
1209 +#include "rcar-gen2.h"
1210
1211  #define r8a779x_clst_id(cpu) (cpu_logical_map((cpu)) >> 8)
1212  #define r8a779x_cpu_id(cpu) (cpu_logical_map((cpu)) & 0xff)
1213 diff --git a/arch/arm/mach-shmobile/pm-r8a7791.c b/arch/arm/mach-shmobile/pm-r8a7791.c
1214 index f0ed98c..a13da84 100644
1215 --- a/arch/arm/mach-shmobile/pm-r8a7791.c
1216 +++ b/arch/arm/mach-shmobile/pm-r8a7791.c
1217 @@ -21,16 +21,9 @@
1218  #include <asm/io.h>
1219  #include "common.h"
1220  #include "pm-rcar.h"
1221 +#include "rcar-gen2.h"
1222  #include "r8a7791.h"
1223
1224 -#define RST            0xe6160000
1225 -#define CA15BAR                0x0020
1226 -#define RAM            0xe63c0000
1227 -
1228 -/* SYSC */
1229 -#define SYSCIER 0x0c
1230 -#define SYSCIMR 0x10
1231 -
1232  struct r8a7791_pm_domain {
1233         struct generic_pm_domain genpd;
1234         struct rcar_sysc_ch ch;
1235 @@ -43,13 +36,14 @@ static inline struct rcar_sysc_ch *to_r8a7791_ch(struct generic_pm_domain *d)
1236
1237  #if defined(CONFIG_PM) || defined(CONFIG_SMP)
1238
1239 -static void __init r8a7791_sysc_init(void)
1240 +static void r8a7791_sysc_init(void)
1241  {
1242 -       void __iomem *base = rcar_sysc_init(0xe6180000);
1243 +       void __iomem *base = rcar_sysc_init(SYSC_BASE);
1244
1245         /* enable all interrupt sources, but do not use interrupt handler */
1246         iowrite32(0x0131000e, base + SYSCIER);
1247 -       iowrite32(0, base + SYSCIMR);
1248 +       /* keep reserved bits as they are in TRM */
1249 +       iowrite32(0x012001ec, base + SYSCIMR);
1250  }
1251
1252  #else /* CONFIG_PM || CONFIG_SMP */
1253 @@ -60,9 +54,6 @@ static inline void r8a7791_sysc_init(void) {}
1254
1255  #ifdef CONFIG_PM
1256
1257 -#define CPG_BASE 0xe6150000
1258 -#define CPG_LEN 0x1000
1259 -
1260  /* Software Reset */
1261  #define SRCR0          0x00a0
1262  #define SRCR1          0x00a8
1263 @@ -243,14 +234,10 @@ static struct notifier_block platform_nb = {
1264
1265  #endif /* CONFIG_PM */
1266
1267 -void __init r8a7791_pm_init(void)
1268 +void r8a7791_pm_init(void)
1269  {
1270         void __iomem *p;
1271         u32 bar;
1272 -       static int once;
1273 -
1274 -       if (once++)
1275 -               return;
1276
1277         /* RAM for jump stub, because BAR requires 256KB aligned address */
1278         p = ioremap_nocache(RAM, shmobile_boot_size);
1279 @@ -258,7 +245,7 @@ void __init r8a7791_pm_init(void)
1280         iounmap(p);
1281
1282         /* setup reset vectors */
1283 -       p = ioremap_nocache(RST, 0x63);
1284 +       p = ioremap_nocache(RST, RST_LEN);
1285         bar = (RAM >> 8) & 0xfffffc00;
1286         writel_relaxed(bar, p + CA15BAR);
1287         writel_relaxed(bar | 0x10, p + CA15BAR);
1288 diff --git a/arch/arm/mach-shmobile/rcar-gen2.h b/arch/arm/mach-shmobile/rcar-gen2.h
1289 index ce53cb5..df7201a 100644
1290 --- a/arch/arm/mach-shmobile/rcar-gen2.h
1291 +++ b/arch/arm/mach-shmobile/rcar-gen2.h
1292 @@ -1,6 +1,45 @@
1293  #ifndef __ASM_RCAR_GEN2_H__
1294  #define __ASM_RCAR_GEN2_H__
1295
1296 +#define CPG_BASE       0xe6150000
1297 +#define CPG_LEN                0x1000
1298 +#define RMSTPCR0       0x110
1299 +#define RMSTPCR1       0x114
1300 +#define RMSTPCR2       0x118
1301 +#define RMSTPCR3       0x11c
1302 +#define RMSTPCR4       0x120
1303 +#define RMSTPCR5       0x124
1304 +#define RMSTPCR7       0x12c
1305 +#define RMSTPCR8       0x980
1306 +#define RMSTPCR9       0x984
1307 +#define RMSTPCR10      0x988
1308 +#define RMSTPCR11      0x98c
1309 +#define SMSTPCR0       0x130
1310 +#define SMSTPCR1       0x134
1311 +#define SMSTPCR2       0x138
1312 +#define SMSTPCR3       0x13c
1313 +#define SMSTPCR4       0x140
1314 +#define SMSTPCR5       0x144
1315 +#define SMSTPCR7       0x14c
1316 +#define SMSTPCR8       0x990
1317 +#define SMSTPCR9       0x994
1318 +#define SMSTPCR10      0x998
1319 +#define SMSTPCR11      0x99c
1320 +
1321 +#define SYSC_BASE      0xe6180000
1322 +#define SYSCIER                0x0c
1323 +#define SYSCIMR                0x10
1324 +
1325 +#define RST            0xe6160000
1326 +#define RST_LEN                0x64
1327 +
1328 +#define CA15BAR                0x0020
1329 +#define CA7BAR         0x0030
1330 +#define RAM            0xe63c0000
1331 +
1332 +#define CNTCR 0
1333 +#define CNTFID0 0x20
1334 +
1335  void rcar_gen2_timer_init(void);
1336  #define MD(nr) BIT(nr)
1337  u32 rcar_gen2_read_mode_pins(void);
1338 diff --git a/arch/arm/mach-shmobile/setup-r8a7791.c b/arch/arm/mach-shmobile/setup-r8a7791.c
1339 index 2aa431a..c48c6a9 100644
1340 --- a/arch/arm/mach-shmobile/setup-r8a7791.c
1341 +++ b/arch/arm/mach-shmobile/setup-r8a7791.c
1342 @@ -29,6 +29,7 @@
1343  #include <linux/sh_timer.h>
1344  #include <linux/spi/sh_msiof.h>
1345  #include <asm/mach/arch.h>
1346 +#include <asm/smp_plat.h>
1347
1348  #include "common.h"
1349  #include "dma-register.h"
1350 @@ -243,8 +244,6 @@ static const struct resource powervr_resources[] __initconst = {
1351                                         powervr_resources,              \
1352                                         ARRAY_SIZE(powervr_resources))
1353
1354 -#define CPG_BASE       0xe6150000
1355 -#define CPG_LEN                0x1000
1356  #define RGXCR          0x0B4
1357
1358  void __init r8a7791_register_pvrsrvkm(void)
1359 @@ -271,7 +270,12 @@ void __init r8a7791_register_ssp(void)
1360
1361  void __init r8a7791_add_dt_devices(void)
1362  {
1363 -       r8a7791_pm_init();
1364 +#ifdef CONFIG_SMP
1365 +       /* In case of SMP config pm_init already called from smp_prepare_cpus.
1366 +        * It is still needed to call pm_init if 'nosmp' was given */
1367 +       if (!setup_max_cpus)
1368 +#endif
1369 +               r8a7791_pm_init();
1370         r8a7791_init_pm_domains();
1371         r8a7791_register_cmt(00);
1372         r8a7791_register_pvrsrvkm();
1373 diff --git a/arch/arm/mach-shmobile/setup-rcar-gen2.c b/arch/arm/mach-shmobile/setup-rcar-gen2.c
1374 index da16ebd..641ee1d 100644
1375 --- a/arch/arm/mach-shmobile/setup-rcar-gen2.c
1376 +++ b/arch/arm/mach-shmobile/setup-rcar-gen2.c
1377 @@ -47,9 +47,6 @@ u32 rcar_gen2_read_mode_pins(void)
1378         return mode;
1379  }
1380
1381 -#define CNTCR 0
1382 -#define CNTFID0 0x20
1383 -
1384  void __init rcar_gen2_timer_init(void)
1385  {
1386  #if defined(CONFIG_ARM_ARCH_TIMER) || defined(CONFIG_COMMON_CLK)
1387 @@ -58,7 +55,7 @@ void __init rcar_gen2_timer_init(void)
1388  #ifdef CONFIG_ARM_ARCH_TIMER
1389         void __iomem *base;
1390         int extal_mhz = 0;
1391 -       u32 freq;
1392 +       u32 rcar_gen2_archtimer_freq;
1393
1394         /* At Linux boot time the r8a7790 arch timer comes up
1395          * with the counter disabled. Moreover, it may also report
1396 @@ -83,7 +80,7 @@ void __init rcar_gen2_timer_init(void)
1397         }
1398
1399         /* The arch timer frequency equals EXTAL / 2 */
1400 -       freq = extal_mhz * (1000000 / 2);
1401 +       rcar_gen2_archtimer_freq = extal_mhz * (1000000 / 2);
1402
1403         /* Remap "armgcnt address map" space */
1404         base = ioremap(0xe6080000, PAGE_SIZE);
1405 @@ -96,10 +93,11 @@ void __init rcar_gen2_timer_init(void)
1406          */
1407
1408         if ((ioread32(base + CNTCR) & 1) == 0 ||
1409 -           ioread32(base + CNTFID0) != freq) {
1410 +           ioread32(base + CNTFID0) != rcar_gen2_archtimer_freq) {
1411                 /* Update registers with correct frequency */
1412 -               iowrite32(freq, base + CNTFID0);
1413 -               asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (freq));
1414 +               iowrite32(rcar_gen2_archtimer_freq, base + CNTFID0);
1415 +               asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r"
1416 +                               (rcar_gen2_archtimer_freq));
1417
1418                 /* make sure arch timer is started by setting bit 0 of CNTCR */
1419                 iowrite32(1, base + CNTCR);
1420 diff --git a/arch/arm/mach-shmobile/smp-r8a7791.c b/arch/arm/mach-shmobile/smp-r8a7791.c
1421 index 24cad9f..4583cb6 100644
1422 --- a/arch/arm/mach-shmobile/smp-r8a7791.c
1423 +++ b/arch/arm/mach-shmobile/smp-r8a7791.c
1424 @@ -33,6 +33,11 @@
1425
1426  #define CA15RESCNT     0x0040
1427
1428 +static struct rcar_sysc_ch r8a7791_ca15_scu = {
1429 +       .chan_offs = 0x180, /* PWRSR5 .. PWRER5 */
1430 +       .isr_bit = 12, /* CA15-SCU */
1431 +};
1432 +
1433  static struct rcar_apmu_config r8a7791_apmu_config[] = {
1434         {
1435                 .iomem = DEFINE_RES_MEM(0xe6152000, 0x88),
1436 @@ -47,7 +52,7 @@ static struct rcar_rst_config r8a7791_rst_config[] = {
1437         }
1438  };
1439
1440 -static void __init r8a7791_smp_prepare_cpus(unsigned int max_cpus)
1441 +static void r8a7791_smp_prepare_cpus(unsigned int max_cpus)
1442  {
1443         void __iomem *p;
1444         u32 val;
1445 @@ -67,6 +72,7 @@ static void __init r8a7791_smp_prepare_cpus(unsigned int max_cpus)
1446         }
1447
1448         r8a7791_pm_init();
1449 +       rcar_sysc_power_up(&r8a7791_ca15_scu);
1450
1451         /* keep secondary CPU cores in reset */
1452         r8a779x_init_reset(r8a7791_rst_config);
1453 diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
1454 index 19da841..35c9048 100644
1455 --- a/arch/arm/mm/proc-v7.S
1456 +++ b/arch/arm/mm/proc-v7.S
1457 @@ -92,48 +92,59 @@ ENDPROC(cpu_v7_dcache_clean_area)
1458
1459  /* Suspend/resume support: derived from arch/arm/mach-s5pv210/sleep.S */
1460  .globl cpu_v7_suspend_size
1461 -.equ   cpu_v7_suspend_size, 4 * 8
1462 +.equ   cpu_v7_suspend_size, 4 * 9
1463  #ifdef CONFIG_ARM_CPU_SUSPEND
1464  ENTRY(cpu_v7_do_suspend)
1465         stmfd   sp!, {r4 - r10, lr}
1466         mrc     p15, 0, r4, c13, c0, 0  @ FCSE/PID
1467         mrc     p15, 0, r5, c13, c0, 3  @ User r/o thread ID
1468         stmia   r0!, {r4 - r5}
1469 +#ifdef CONFIG_MMU
1470         mrc     p15, 0, r6, c3, c0, 0   @ Domain ID
1471 +#ifdef CONFIG_ARM_LPAE
1472 +       mrrc    p15, 1, r5, r7, c2      @ TTB 1
1473 +#else
1474         mrc     p15, 0, r7, c2, c0, 1   @ TTB 1
1475 +#endif
1476         mrc     p15, 0, r11, c2, c0, 2  @ TTB control register
1477 +#endif
1478         mrc     p15, 0, r8, c1, c0, 0   @ Control register
1479         mrc     p15, 0, r9, c1, c0, 1   @ Auxiliary control register
1480         mrc     p15, 0, r10, c1, c0, 2  @ Co-processor access control
1481 -       stmia   r0, {r6 - r11}
1482 +       stmia   r0, {r5 - r11}
1483         ldmfd   sp!, {r4 - r10, pc}
1484  ENDPROC(cpu_v7_do_suspend)
1485
1486  ENTRY(cpu_v7_do_resume)
1487         mov     ip, #0
1488 -       mcr     p15, 0, ip, c8, c7, 0   @ invalidate TLBs
1489         mcr     p15, 0, ip, c7, c5, 0   @ invalidate I cache
1490         mcr     p15, 0, ip, c13, c0, 1  @ set reserved context ID
1491         ldmia   r0!, {r4 - r5}
1492         mcr     p15, 0, r4, c13, c0, 0  @ FCSE/PID
1493         mcr     p15, 0, r5, c13, c0, 3  @ User r/o thread ID
1494 -       ldmia   r0, {r6 - r11}
1495 +       ldmia   r0, {r5 - r11}
1496 +#ifdef CONFIG_MMU
1497 +       mcr     p15, 0, ip, c8, c7, 0   @ invalidate TLBs
1498         mcr     p15, 0, r6, c3, c0, 0   @ Domain ID
1499 -#ifndef CONFIG_ARM_LPAE
1500 +#ifdef CONFIG_ARM_LPAE
1501 +       mcrr    p15, 0, r1, ip, c2      @ TTB 0
1502 +       mcrr    p15, 1, r5, r7, c2      @ TTB 1
1503 +#else
1504         ALT_SMP(orr     r1, r1, #TTB_FLAGS_SMP)
1505         ALT_UP(orr      r1, r1, #TTB_FLAGS_UP)
1506 -#endif
1507         mcr     p15, 0, r1, c2, c0, 0   @ TTB 0
1508         mcr     p15, 0, r7, c2, c0, 1   @ TTB 1
1509 +#endif
1510         mcr     p15, 0, r11, c2, c0, 2  @ TTB control register
1511 -       mrc     p15, 0, r4, c1, c0, 1   @ Read Auxiliary control register
1512 -       teq     r4, r9                  @ Is it already set?
1513 -       mcrne   p15, 0, r9, c1, c0, 1   @ No, so write it
1514 -       mcr     p15, 0, r10, c1, c0, 2  @ Co-processor access control
1515         ldr     r4, =PRRR               @ PRRR
1516         ldr     r5, =NMRR               @ NMRR
1517         mcr     p15, 0, r4, c10, c2, 0  @ write PRRR
1518         mcr     p15, 0, r5, c10, c2, 1  @ write NMRR
1519 +#endif /* CONFIG_MMU */
1520 +       mrc     p15, 0, r4, c1, c0, 1   @ Read Auxiliary control register
1521 +       teq     r4, r9                  @ Is it already set?
1522 +       mcrne   p15, 0, r9, c1, c0, 1   @ No, so write it
1523 +       mcr     p15, 0, r10, c1, c0, 2  @ Co-processor access control
1524         isb
1525         dsb
1526         mov     r0, r8                  @ control register
1527 --
1528 1.8.3.1
1529