--- /dev/null
+From 0aae8f3fefc67bc07b7e4e42f885ef661f0921ab Mon Sep 17 00:00:00 2001
+From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+Date: Fri, 19 May 2017 14:25:38 +0900
+Subject: [PATCH 1/4] Add rcar-sdhi DMA support
+
+Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+---
+ drivers/dma/Makefile | 1 +
+ drivers/dma/sh_dma.c | 306 ++++++++++++++++++++++++++++++++++++++++++++++++++
+ drivers/mmc/sh_sdhi.c | 158 +++++++++++++++++++++++++-
+ drivers/mmc/sh_sdhi.h | 5 +
+ include/sh_dma.h | 58 ++++++++++
+ 5 files changed, 524 insertions(+), 4 deletions(-)
+ create mode 100644 drivers/dma/sh_dma.c
+ create mode 100644 include/sh_dma.h
+
+diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
+index 5d864b5..1129fc3 100644
+--- a/drivers/dma/Makefile
++++ b/drivers/dma/Makefile
+@@ -29,6 +29,7 @@ COBJS-$(CONFIG_FSLDMAFEC) += MCD_tasksInit.o MCD_dmaApi.o MCD_tasks.o
+ COBJS-$(CONFIG_APBH_DMA) += apbh_dma.o
+ COBJS-$(CONFIG_FSL_DMA) += fsl_dma.o
+ COBJS-$(CONFIG_OMAP3_DMA) += omap3_dma.o
++COBJS-$(CONFIG_SH_DMA) += sh_dma.o
+
+ COBJS := $(COBJS-y)
+ SRCS := $(COBJS:.o=.c)
+diff --git a/drivers/dma/sh_dma.c b/drivers/dma/sh_dma.c
+new file mode 100644
+index 0000000..0af2480
+--- /dev/null
++++ b/drivers/dma/sh_dma.c
+@@ -0,0 +1,306 @@
++/*
++ * SH SYS-DMA driver
++ *
++ * Copyright (C) 2014 Cogent Embedded, Inc.
++ *
++ * This program is free software; you can redistribute it and/or
++ * modify it under the terms of the GNU General Public License as
++ * published by the Free Software Foundation; either version 2 of
++ * the License, or (at your option) any later version.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ */
++
++#include <common.h>
++#include <malloc.h>
++#include <asm/io.h>
++#include <linux/list.h>
++#include <sh_dma.h>
++
++struct sh_dma {
++ u32 base;
++ u32 mask;
++ u32 nch;
++ struct list_head list;
++};
++
++struct sh_dma_chan {
++ struct sh_dma *dma;
++ u32 base;
++ u32 num;
++ u32 ts;
++ u32 bs;
++ u32 rounds;
++};
++
++#define SH_DMA_MAX_TC 0x1000000
++#define SH_DMA_MAX_CHAN 32
++#define SH_DMA_CHAN_OFFSET 0x8000
++#define SH_DMA_CHAN_SIZE 0x80
++
++/* Global registers */
++#define SH_DMAISTA 0x20
++#define SH_DMASEC 0x30
++#define SH_DMAOR 0x60
++#define SH_DMACHCL 0x80
++#define SH_DMADPSEC 0xA0
++
++/* DMA operation register bits */
++#define SH_DMAOR_DME (0x1 << 0)
++
++/* Channel registers */
++#define SH_DMASAR 0x00
++#define SH_DMADAR 0x04
++#define SH_DMATCR 0x08
++#define SH_DMACHCR 0x0C
++#define SH_DMATSR 0x28
++#define SH_DMATCRB 0x18
++#define SH_DMATSRB 0x38
++#define SH_DMACHCRB 0x1C
++#define SH_DMARS 0x40
++#define SH_DMABUFCR 0x48
++#define SH_DMADPBASE 0x50
++#define SH_DMADPCR 0x54
++#define SH_DMAFIXSAR 0x10
++#define SH_DMAFIXDAR 0x14
++#define SH_DMAFIXDPBASE 0x60
++
++/* Channel control register bits */
++#define SH_DMACHCR_SM(v) (((v) & 0x3) << 12)
++#define SH_DMACHCR_SM_MASK (0x3 << 12)
++#define SH_DMACHCR_DM(v) (((v) & 0x3) << 14)
++#define SH_DMACHCR_DM_MASK (0x3 << 14)
++#define SH_DMACHCR_TS_1 (0x0 << 3 | 0x0 << 20)
++#define SH_DMACHCR_TS_2 (0x1 << 3 | 0x0 << 20)
++#define SH_DMACHCR_TS_4 (0x2 << 3 | 0x0 << 20)
++#define SH_DMACHCR_TS_8 (0x3 << 3 | 0x1 << 20)
++#define SH_DMACHCR_TS_16 (0x3 << 3 | 0x0 << 20)
++#define SH_DMACHCR_TS_32 (0x0 << 3 | 0x1 << 20)
++#define SH_DMACHCR_TS_64 (0x1 << 3 | 0x1 << 20)
++#define SH_DMACHCR_TS_MASK (0x3 << 3 | 0x3 << 20)
++#define SH_DMACHCR_RS_AUTO (0x4 << 8)
++#define SH_DMACHCR_RS_SEL (0x8 << 8)
++#define SH_DMACHCR_RS_MASK (0xf << 8)
++#define SH_DMACHCR_CAE (0x1 << 31)
++#define SH_DMACHCR_TE (0x1 << 1)
++#define SH_DMACHCR_DE (0x1 << 0)
++
++#define sh_dma_writel(d, r, v) writel((v), (d)->base + (r))
++#define sh_dma_readl(d, r) readl((d)->base + (r))
++#define sh_dma_writew(d, r, v) writew((v), (d)->base + (r))
++#define sh_dma_readw(d, r) readw((d)->base + (r))
++
++static LIST_HEAD(sh_dma_list);
++
++struct sh_dma *sh_dma_add(u32 base, u32 nch)
++{
++ struct list_head *entry;
++ struct sh_dma *dma;
++ u32 mask;
++
++ if (nch > SH_DMA_MAX_CHAN)
++ return NULL;
++
++ mask = (1 << nch) - 1;
++ list_for_each(entry, &sh_dma_list) {
++ dma = list_entry(entry, struct sh_dma, list);
++ if (dma->base == base) {
++ if (nch > dma->nch) {
++ mask &= ~((1 << dma->nch) - 1);
++ sh_dma_writel(dma, SH_DMACHCL, mask);
++ dma->nch = nch;
++ }
++ return dma;
++ }
++ }
++
++ dma = malloc(sizeof(*dma));
++ if (!dma)
++ return NULL;
++
++ dma->base = base;
++ dma->mask = 0;
++ dma->nch = nch;
++ sh_dma_writel(dma, SH_DMACHCL, mask);
++ sh_dma_writew(dma, SH_DMAOR, SH_DMAOR_DME);
++ list_add(&dma->list, &sh_dma_list);
++ return dma;
++}
++
++void sh_dma_chan_src(struct sh_dma_chan *chan, u32 src)
++{
++ sh_dma_writel(chan, SH_DMASAR, src);
++}
++
++void sh_dma_chan_dst(struct sh_dma_chan *chan, u32 dst)
++{
++ sh_dma_writel(chan, SH_DMADAR, dst);
++}
++
++void sh_dma_chan_cfg(struct sh_dma_chan *chan, u8 midrid, u8 sm, u8 dm)
++{
++ u32 val;
++
++ sh_dma_writew(chan, SH_DMARS, midrid);
++ val = sh_dma_readl(chan, SH_DMACHCR);
++ val &= ~(SH_DMACHCR_RS_MASK |
++ SH_DMACHCR_SM_MASK | SH_DMACHCR_DM_MASK);
++ val |= midrid ? SH_DMACHCR_RS_SEL : SH_DMACHCR_RS_AUTO;
++ val |= SH_DMACHCR_SM(sm) | SH_DMACHCR_DM(dm);
++ sh_dma_writel(chan, SH_DMACHCR, val);
++}
++
++void sh_dma_chan_start(struct sh_dma_chan *chan, u32 ts, u8 bs)
++{
++ u32 val;
++
++ if (!ts)
++ return;
++
++ val = (ts + (1 << bs) - 1) >> bs;
++ val = val < SH_DMA_MAX_TC ? val : 0x0;
++ sh_dma_writel(chan, SH_DMATCR, val);
++
++ chan->ts = ts;
++ chan->bs = bs;
++ chan->rounds = val;
++
++ val = sh_dma_readl(chan, SH_DMACHCR);
++
++ val &= ~(SH_DMACHCR_TE | SH_DMACHCR_TS_MASK);
++ val |= SH_DMACHCR_DE;
++ switch (bs) {
++ default:
++ case 0:
++ val |= SH_DMACHCR_TS_1;
++ break;
++ case 1:
++ val |= SH_DMACHCR_TS_2;
++ break;
++ case 2:
++ val |= SH_DMACHCR_TS_4;
++ break;
++ case 3:
++ val |= SH_DMACHCR_TS_8;
++ break;
++ case 4:
++ val |= SH_DMACHCR_TS_16;
++ break;
++ case 5:
++ val |= SH_DMACHCR_TS_32;
++ break;
++ case 6:
++ val |= SH_DMACHCR_TS_64;
++ break;
++ }
++
++ sh_dma_writel(chan, SH_DMACHCR, val);
++}
++
++void sh_dma_chan_stop(struct sh_dma_chan *chan)
++{
++ u32 val;
++
++ chan->ts = 0;
++ chan->bs = 0;
++ chan->rounds = 0;
++
++ val = sh_dma_readl(chan, SH_DMACHCR);
++ val &= ~(SH_DMACHCR_CAE | SH_DMACHCR_TE | SH_DMACHCR_DE);
++ sh_dma_writel(chan, SH_DMACHCR, val);
++ do {
++ val = sh_dma_readl(chan, SH_DMACHCR);
++ } while (val & SH_DMACHCR_DE);
++}
++
++int sh_dma_chan_wait(struct sh_dma_chan *chan)
++{
++ u32 val;
++ u32 timeout = 10000000;
++ int retval = 0;
++
++ do {
++ val = sh_dma_readl(chan, SH_DMACHCR);
++ val &= SH_DMACHCR_CAE | SH_DMACHCR_TE | SH_DMACHCR_DE;
++ if (val == (SH_DMACHCR_TE | SH_DMACHCR_DE))
++ break;
++
++ if (!timeout)
++ return -ETIMEDOUT;
++
++ timeout--;
++ udelay(1);
++ } while (1);
++
++ if (!(val & SH_DMACHCR_DE))
++ return chan->ts ? -EINTR : 0;
++
++ if (val & SH_DMACHCR_CAE) {
++ retval = -EFAULT;
++ goto out;
++ }
++
++ val = chan->rounds < SH_DMA_MAX_TC ? chan->rounds : SH_DMA_MAX_TC;
++ val = chan->rounds - val;
++ if (val) {
++ puts("abnormal end\n");
++ sh_dma_chan_start(chan, val << chan->bs, 0);
++ return -EAGAIN;
++ }
++
++out:
++ sh_dma_chan_stop(chan);
++ return retval;
++}
++
++void sh_dma_chan_clr(struct sh_dma_chan *chan)
++{
++ chan->ts = 0;
++ chan->bs = 0;
++ chan->rounds = 0;
++
++ sh_dma_writel(chan->dma, SH_DMACHCL, 1 << chan->num);
++}
++
++struct sh_dma_chan *sh_dma_chan_init(struct sh_dma *dma, int ch)
++{
++ struct sh_dma_chan *chan;
++ u32 mask;
++
++ if (ch < 0) {
++ if (!~dma->mask)
++ return NULL;
++
++ ch = ffz(dma->mask);
++ }
++
++ if (!dma || ch > dma->nch)
++ return NULL;
++
++ mask = 1 << ch;
++ if (dma->mask & mask)
++ return NULL;
++
++ chan = malloc(sizeof(*chan));
++ if (!chan)
++ return NULL;
++
++ dma->mask |= mask;
++ chan->dma = dma;
++ chan->base = dma->base + SH_DMA_CHAN_OFFSET + ch * SH_DMA_CHAN_SIZE;
++ chan->num = ch;
++ sh_dma_chan_clr(chan);
++
++ return chan;
++}
++
++void sh_dma_chan_release(struct sh_dma_chan *chan)
++{
++ struct sh_dma *dma = chan->dma;
++
++ dma->mask &= ~(1 << chan->num);
++ free(chan);
++}
+diff --git a/drivers/mmc/sh_sdhi.c b/drivers/mmc/sh_sdhi.c
+index ddad43a..80dc7a8 100644
+--- a/drivers/mmc/sh_sdhi.c
++++ b/drivers/mmc/sh_sdhi.c
+@@ -17,7 +17,6 @@
+ #include <command.h>
+ #include <mmc.h>
+ #include <malloc.h>
+-#include <mmc.h>
+ #include <asm/errno.h>
+ #include <asm/io.h>
+
+@@ -33,6 +32,111 @@
+
+ #define DRIVER_NAME "sh-sdhi"
+
++#ifdef CONFIG_SH_DMA
++
++#ifdef CONFIG_SYS_DCACHE_OFF
++static inline void sh_sdhi_invalidate_dcache(u32 addr, int len) { }
++#else /* CONFIG_SYS_DCACHE_OFF */
++#define DCACHE_LINE_MASK (ARCH_DMA_MINALIGN - 1)
++
++static void sh_sdhi_invalidate_dcache(u32 addr, int len)
++{
++ u32 start, end;
++
++ start = addr & ~DCACHE_LINE_MASK;
++ if (start != addr) {
++ end = start + ARCH_DMA_MINALIGN;
++ flush_dcache_range(start, end);
++
++ len -= end - addr;
++ start = end;
++ }
++
++ if (len >= ARCH_DMA_MINALIGN) {
++ end = (start + len) & ~DCACHE_LINE_MASK;
++ invalidate_dcache_range(start, end);
++
++ len &= DCACHE_LINE_MASK;
++ start = end;
++ }
++
++ if (len > 0) {
++ end = start + ARCH_DMA_MINALIGN;
++ flush_dcache_range(start, end);
++ }
++}
++#endif /* CONFIG_SYS_DCACHE_OFF */
++
++static void sh_sdhi_dma_init(struct sdhi_host *host)
++{
++ struct sh_dma *dma;
++
++ dma = sh_dma_add(CONFIG_SH_SYS_DMAL_BASE, CONFIG_SH_SYS_DMAL_NCH);
++ if (!dma)
++ return;
++
++ host->dma_rx = sh_dma_chan_init(dma, 1);
++ if (!host->dma_rx)
++ return;
++
++ sh_dma_chan_cfg(host->dma_rx, SH_DMA_SDHI0_RX,
++ SH_DMA_AM_FIX, SH_DMA_AM_INC);
++ sh_dma_chan_src(host->dma_rx,
++ host->addr + (SDHI_BUF0 << host->bus_shift) +
++ 0x2000);
++}
++
++static void sh_sdhi_dma_release(struct sdhi_host *host)
++{
++ if (host->dma_rx) {
++ sh_dma_chan_release(host->dma_rx);
++ host->dma_rx = NULL;
++ }
++}
++
++static void sh_sdhi_start_dma_rx(struct sdhi_host *host,
++ struct mmc_data *data)
++{
++ int ret;
++ u32 blocksize = data->blocksize;
++ sh_sdhi_dma_init(host);
++ sdhi_writew(host, SDHI_SD_DMACR, 0xa0);
++ sdhi_writew(host, SDHI_CC_EXT_MODE, (1 << 1));
++
++ sh_sdhi_invalidate_dcache((u32)data->dest, blocksize);
++
++ sh_dma_chan_dst(host->dma_rx, (u32)data->dest);
++
++ /* sh_sdhi_bitset(BUF_ACC_DMAREN, &host->regs->ce_buf_acc); */
++
++ /* MMCIF is capable to transfer only 4 bytes at a time,
++ * provide size order as a param */
++ blocksize = sdhi_readw(host, SDHI_SIZE);
++ sh_dma_chan_start(host->dma_rx, blocksize, 1);
++
++ do {
++ ret = sh_dma_chan_wait(host->dma_rx);
++ } while (ret == -EAGAIN);
++ sdhi_writew(host, SDHI_CC_EXT_MODE, 0x0);
++ sh_dma_chan_clr(host->dma_rx);
++ sh_sdhi_dma_release(host);
++}
++
++static void sdhi_dma_transfer(struct sdhi_host *host,
++ struct mmc_data *data)
++{
++ sh_sdhi_start_dma_rx(host, data);
++}
++
++
++#else /* CONFIG_SH_DMA */
++static inline void sh_sdhi_dma_init(struct sdhi_host *host) { }
++static inline void sh_sdhi_dma_release(struct sdhi_host *host) { }
++static inline void sh_sdhi_start_dma_rx(struct sdhi_host *host,
++ struct mmc_data *data) { }
++
++#endif /* CONFIG_SH_DMA */
++
+ static void *mmc_priv(struct mmc *mmc)
+ {
+ return (void *)mmc->priv;
+@@ -253,7 +357,9 @@ static int sdhi_single_read(struct sdhi_host *host, struct mmc_data *data)
+ {
+ int ch = host->ch;
+ long time;
++#ifndef CONFIG_SH_DMA
+ unsigned short blocksize, i;
++#endif
+ unsigned short *p = (unsigned short *)data->dest;
+
+ if ((unsigned long)p & 0x00000001) {
+@@ -272,10 +378,14 @@ static int sdhi_single_read(struct sdhi_host *host, struct mmc_data *data)
+ return sdhi_error_manage(host);
+
+ g_wait_int[ch] = 0;
++#ifdef CONFIG_SH_DMA
++ sdhi_dma_transfer(host, data);
++#else
+ blocksize = sdhi_readw(host, SDHI_SIZE);
+ for (i = 0; i < blocksize / 2; i++)
+ *p++ = sdhi_readw(host, SDHI_BUF0);
+
++#endif
+ time = sdhi_wait_interrupt_flag(host);
+ if (time == 0 || g_sd_error[ch] != 0)
+ return sdhi_error_manage(host);
+@@ -537,7 +647,6 @@ static int sdhi_start_cmd(struct sdhi_host *host,
+ ;
+
+ sdhi_writew(host, SDHI_CMD, (unsigned short)(opc & CMD_MASK));
+-
+ g_wait_int[host->ch] = 0;
+ sdhi_writew(host, SDHI_INFO1_MASK,
+ ~INFO1M_RESP_END & sdhi_readw(host, SDHI_INFO1_MASK));
+@@ -546,7 +655,6 @@ static int sdhi_start_cmd(struct sdhi_host *host,
+ INFO2M_END_ERROR | INFO2M_TIMEOUT |
+ INFO2M_RESP_TIMEOUT | INFO2M_ILA) &
+ sdhi_readw(host, SDHI_INFO2_MASK));
+-
+ time = sdhi_wait_interrupt_flag(host);
+ if (time == 0)
+ return sdhi_error_manage(host);
+@@ -578,7 +686,6 @@ static int sdhi_start_cmd(struct sdhi_host *host,
+ }
+ if (host->data)
+ ret = sdhi_data_trans(host, data, opc);
+-
+ pr_debug("ret = %d, resp = %08x, %08x, %08x, %08x\n",
+ ret, cmd->response[0], cmd->response[1],
+ cmd->response[2], cmd->response[3]);
+@@ -697,3 +804,46 @@ int sdhi_mmc_init(unsigned long addr, int ch)
+ return ret;
+ }
+
++
++int sdhi_warmup_sdio(struct mmc *mmc)
++{
++ struct mmc_cmd cmd;
++ int err;
++ int32_t ocr;
++
++ udelay(10);
++
++ mmc->bus_width = 1;
++ mmc->clock = mmc->f_min;
++ sdhi_set_ios(mmc);
++ udelay(10);
++
++ cmd.cmdidx = MMC_CMD_GO_IDLE_STATE;
++ cmd.resp_type = MMC_RSP_NONE;
++ cmd.cmdarg = 0;
++ err = sdhi_request(mmc, &cmd, NULL);
++ if (err)
++ goto err_out;
++ cmd.cmdidx = 0x5;
++ cmd.resp_type = MMC_RSP_R4;
++ cmd.cmdarg = 0;
++ err = sdhi_request(mmc, &cmd, NULL);
++ if (err)
++ goto err_out;
++ ocr = cmd.response[0];
++ ocr |= (1 << 24);
++ cmd.cmdidx = 0x05;
++ cmd.resp_type = MMC_RSP_R4;
++ cmd.cmdarg = ocr;
++ err = sdhi_request(mmc, &cmd, NULL);
++ if (err)
++ goto err_out;
++ printf("SDIO OCR:%08x\n", cmd.response[0]);
++ return 0;
++err_out:
++ printf("cmd: CMD%02d err = %d, resp = %08x, %08x, %08x, %08x\n",
++ err, cmd.cmdidx, cmd.response[0], cmd.response[1],
++ cmd.response[2], cmd.response[3]);
++ return err;
++}
++
+diff --git a/drivers/mmc/sh_sdhi.h b/drivers/mmc/sh_sdhi.h
+index 4deded2..7b5d421 100644
+--- a/drivers/mmc/sh_sdhi.h
++++ b/drivers/mmc/sh_sdhi.h
+@@ -15,6 +15,8 @@
+ #ifndef _SH_SDHI_H_
+ #define _SH_SDHI_H_
+
++#include <sh_dma.h>
++
+ #define SDHI_CMD (0x0000 >> 1)
+ #define SDHI_PORTSEL (0x0004 >> 1)
+ #define SDHI_ARG0 (0x0008 >> 1)
+@@ -181,6 +183,9 @@ struct sdhi_host {
+ unsigned int power_mode;
+ int ch;
+ int bus_shift;
++#ifdef CONFIG_SH_DMA
++ struct sh_dma_chan *dma_rx;
++#endif
+ };
+
+ static unsigned short g_wait_int[CONFIG_MMC_SH_SDHI_NR_CHANNEL];
+diff --git a/include/sh_dma.h b/include/sh_dma.h
+new file mode 100644
+index 0000000..3f35c3a
+--- /dev/null
++++ b/include/sh_dma.h
+@@ -0,0 +1,58 @@
++#ifndef __SH_DMA_H__
++#define __SH_DMA_H__
++
++#include <asm/types.h>
++#include <errno.h>
++
++#define SH_DMA_MMCIF0_RX 0xD2
++#define SH_DMA_SDHI0_RX 0xCE
++
++/* Address mode */
++#define SH_DMA_AM_FIX 0
++#define SH_DMA_AM_INC 1
++#define SH_DMA_AM_DEC 2
++
++struct sh_dma;
++struct sh_dma_chan;
++
++#ifdef CONFIG_SH_DMA
++struct sh_dma *sh_dma_add(u32 base, u32 nch);
++struct sh_dma_chan *sh_dma_chan_init(struct sh_dma *dma, int ch);
++void sh_dma_chan_release(struct sh_dma_chan *chan);
++
++void sh_dma_chan_src(struct sh_dma_chan *chan, u32 src);
++void sh_dma_chan_dst(struct sh_dma_chan *chan, u32 dst);
++void sh_dma_chan_cfg(struct sh_dma_chan *chan, u8 midrid, u8 sm, u8 dm);
++void sh_dma_chan_start(struct sh_dma_chan *chan, u32 ts, u8 bs);
++void sh_dma_chan_stop(struct sh_dma_chan *chan);
++int sh_dma_chan_wait(struct sh_dma_chan *chan);
++void sh_dma_chan_clr(struct sh_dma_chan *chan);
++#else
++static inline struct sh_dma *sh_dma_add(u32 base, u32 nch)
++{
++ return NULL;
++}
++
++static inline struct sh_dma_chan *sh_dma_chan_init(struct sh_dma *dma,
++ int ch)
++{
++ return NULL;
++}
++
++static inline void sh_dma_chan_release(struct sh_dma_chan *chan) { }
++static inline void sh_dma_chan_src(struct sh_dma_chan *chan, u32 src) { }
++static inline void sh_dma_chan_dst(struct sh_dma_chan *chan, u32 dst) { }
++static inline void sh_dma_chan_cfg(struct sh_dma_chan *chan,
++ u8 midrid, u8 sm, u8 dm) { }
++static inline void sh_dma_chan_start(struct sh_dma_chan *chan,
++ u32 ts, u8 bs) { }
++static inline void sh_dma_chan_stop(struct sh_dma_chan *chan) { }
++static inline int sh_dma_chan_wait(struct sh_dma_chan *chan)
++{
++ return -ENOSYS;
++}
++
++static inline void sh_dma_chan_clr(struct sh_dma_chan *chan) { }
++#endif
++
++#endif /* __SH_DMA_H__ */
+--
+1.8.3.1
+
--- /dev/null
+From 45b3abc592bd685726a6b55693ab95e4cb6065fc Mon Sep 17 00:00:00 2001
+From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+Date: Fri, 19 May 2017 14:27:46 +0900
+Subject: [PATCH 2/4] Add Hibernation swsusp command support
+
+Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+---
+ common/cmd_swsusp.c | 889 ++++++++++++++++++++++++++++++++++++++++++++++++++++
+ 1 file changed, 889 insertions(+)
+ create mode 100644 common/cmd_swsusp.c
+
+diff --git a/common/cmd_swsusp.c b/common/cmd_swsusp.c
+new file mode 100644
+index 0000000..ba05aa4
+--- /dev/null
++++ b/common/cmd_swsusp.c
+@@ -0,0 +1,889 @@
++/*
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License as published by
++ * the Free Software Foundation; either version 2 of the License, or
++ * (at your option) any later version.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ *
++ * You should have received a copy of the GNU General Public License
++ * along with this program; if not, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++#include <common.h>
++#include <command.h>
++#include <part.h>
++#include <malloc.h>
++
++#include <linux/lzo.h>
++#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
++#include <swsuspmem.h>
++
++/* Note for Renesas--based boards:
++ * We have the following memory split here:
++ * 0x40000000 - u_boot_lowest - used to store pfns at physical addresses
++ * u_boot_lowest - 0x8000000 - pfns are relocated, and then later put
++ * on physical addresses (swsusp_finish)
++ * 0x8000000 - 0xc0000000 - used to store pfns with physical address
++ * of 0x200000000 (long address), we have to change offset for them.
++ * Any pfn with address > 0x8000000 but less than 0x200000000
++ * is an error.
++ * For boards which do not have memory above first GB, that will
++ * still work, as they won't have anything above 0x80000000
++ * in their image, so for standard 2GB setup ou should put
++ * your secong GB in 0x80000000-0xC0000000 range, you can
++ * use MMU for that or if your RAM is continous, it will
++ * naturally be there. */
++
++DECLARE_GLOBAL_DATA_PTR;
++
++/* #define PAGEMAP_DEBUG */
++
++#ifdef PAGEMAP_DEBUG
++#define SWSUSP_DEBUG_INFO
++#endif
++
++#define SWSUSP_KEEP_IMAGE
++
++#ifndef likely
++# define likely(x) __builtin_expect(!!(x), 1)
++# define unlikely(x) __builtin_expect(!!(x), 0)
++#endif
++
++#define HIBERNATE_SIG "S1SUSPEND"
++#define PAGE_SIZE 4096
++
++/* Define depending on CONFIG_LBDAF in kernel */
++typedef u64 sector_t;
++
++
++struct swsusp_header {
++ char reserved[PAGE_SIZE - 20 - sizeof(sector_t) -
++ sizeof(int) - sizeof(u32) -
++ sizeof(CRC32_WORD4_t) - sizeof(u32)];
++ CRC32_WORD4_t comp_crc32;
++ u32 img_size; /* append */
++ u32 crc32;
++ sector_t image;
++ unsigned int flags;
++ char orig_sig[10];
++ char sig[10];
++} __packed;
++
++#define __NEW_UTS_LEN 64
++
++struct new_utsname {
++ char sysname[__NEW_UTS_LEN + 1];
++ char nodename[__NEW_UTS_LEN + 1];
++ char release[__NEW_UTS_LEN + 1];
++ char version[__NEW_UTS_LEN + 1];
++ char machine[__NEW_UTS_LEN + 1];
++ char domainname[__NEW_UTS_LEN + 1];
++};
++
++struct swsusp_archdata {
++ u32 nosave_backup_phys;
++ u32 nosave_begin_phys;
++ u32 nosave_end_phys;
++ void (*cpu_resume_restore_nosave)(u32, u32, u32);
++};
++
++struct swsusp_info {
++ struct new_utsname uts;
++ u32 version_code;
++ unsigned long num_physpages;
++ int cpus;
++ unsigned long image_pages;
++ unsigned long pages;
++ unsigned long size;
++ char archdata[1024];
++};
++
++struct swap_map_page {
++ u64 entries[PAGE_SIZE / sizeof(u64) - 1];
++ u64 next_swap;
++};
++
++struct swsusp_finish_context {
++ void *remap_orig_page;
++ void *remap_temp_page;
++ struct swsusp_archdata archdata;
++};
++
++/* Do not specially exclude any bottom area */
++#define USED_ADDRESS_TOP (CONFIG_SYS_SDRAM_BASE)
++#define USED_ADDRESS_END (CONFIG_SYS_SDRAM_BASE)
++
++#define PG_UB2ZERO(pg) (pg - CONFIG_SYS_SDRAM_BASE / PAGE_SIZE)
++static u32 const exclude_min_page =
++ (USED_ADDRESS_TOP) / PAGE_SIZE;
++static u32 const exclude_max_page =
++ (USED_ADDRESS_END - 1) / PAGE_SIZE;
++static u32 const exclude_min_page_ub =
++ PG_UB2ZERO((USED_ADDRESS_TOP) / PAGE_SIZE);
++static u32 const exclude_max_page_ub =
++ PG_UB2ZERO((USED_ADDRESS_END-1) / PAGE_SIZE);
++#define SF_NOCOMPRESS_MODE 2
++
++#define LZO_HEADER sizeof(size_t)
++
++/* Number of pages/bytes we'll compress at one time. */
++#define LZO_UNC_PAGES 32
++#define LZO_UNC_SIZE (LZO_UNC_PAGES * PAGE_SIZE)
++
++/* Number of pages/bytes we need for compressed data (worst case). */
++#define LZO_CMP_PAGES DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
++ LZO_HEADER, PAGE_SIZE)
++#define LZO_CMP_SIZE (LZO_CMP_PAGES * PAGE_SIZE)
++
++static block_dev_desc_t *swap_dev;
++static disk_partition_t swap_info;
++
++static struct swap_map_page *meta_map;
++static u64 meta_map_next;
++static u64 meta_map_curr;
++static u64 meta_map_start;
++static int meta_idx;
++
++#ifdef PAGEMAP_DEBUG
++static int debugout;
++static int _last_read_pages;
++#define PAGEMAP_INFO(_msg, ...) do { if (debugout == 1) \
++ printf(_msg, ## __VA_ARGS__); } while (0)
++#endif
++
++#define HIGHMEM_PHYS_ADDR 0x200000000ULL
++#define HIGHMEM_VA 0x80000000UL
++#define HIGHMEM_PFN (HIGHMEM_PHYS_ADDR / PAGE_SIZE)
++#define LOW_TOP 0x80000000
++#define LOW_TOP_PFN (LOW_TOP / PAGE_SIZE)
++#define LOW_BOTTOM CONFIG_SYS_SDRAM_BASE
++#define LOW_BOTTOM_PFN (LOW_BOTTOM / PAGE_SIZE)
++#define TOP_ADDRESS 0x240000000ULL
++
++static int get_meta(void);
++
++static inline int pfn_is_low(u32 pfn)
++{
++ return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
++}
++
++static inline int pfn_is_high(u32 pfn)
++{
++ return (pfn >= HIGHMEM_PFN);
++}
++
++#define pfn_is_valid(p) (pfn_is_low(p) || pfn_is_high(p))
++
++static inline int pfn_is_excluded(u32 pfn)
++{
++ /* Allow bottom 2 pages for exception vectors */
++ if (pfn < (LOW_BOTTOM_PFN + 2))
++ return 0;
++ else if (exclude_min_page >= exclude_max_page)
++ return 0;
++ else
++ return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
++}
++
++/* PFN to zero-counted page */
++static inline u32 pg_ub2zero(u32 pg)
++{
++ return pg - LOW_BOTTOM_PFN;
++}
++
++/* zero-counted page to PFN */
++static inline u32 pg_zero2ub(u32 pg)
++{
++ return pg + LOW_BOTTOM_PFN;
++}
++
++/* PFN to physical address (64-bit (40-bit)) */
++static inline u64 pg2phys(u32 page)
++{
++ return (u64) page * PAGE_SIZE;
++}
++
++/* PFN to virtual address */
++static inline void *pg2addr(u32 page)
++{
++ void *addr;
++ if (page >= HIGHMEM_PFN)
++ addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
++ else
++ addr = (void *) (u32)pg2phys(page);
++
++ return addr;
++}
++
++#ifdef CONFIG_SH_DMA
++static inline void *malloc_aligned(u32 size, u32 align)
++{
++ return (void *)(((u32)malloc(size + align) + align - 1) & ~(align - 1));
++}
++
++#endif
++
++static int page_read(u32 page, void *addr)
++{
++ __u32 cnt;
++ int blk_per_page;
++
++ blk_per_page = PAGE_SIZE / swap_dev->blksz;
++ cnt = swap_dev->block_read(swap_dev->dev,
++ swap_info.start + (page * blk_per_page),
++ blk_per_page, addr);
++ return cnt != blk_per_page;
++}
++
++#ifndef SWSUSP_KEEP_IMAGE
++static int page_write(u32 page, void *addr)
++{
++ __u32 cnt;
++ int blk_per_page;
++
++ blk_per_page = PAGE_SIZE / swap_dev->blksz;
++ cnt = swap_dev->block_write(swap_dev->dev,
++ swap_info.start + (page * blk_per_page),
++ blk_per_page, addr);
++ return cnt != blk_per_page;
++}
++#endif
++
++#define FAST_COPY
++void __attribute__((section(".rodata")))
++ __attribute__((optimize("O6", "unroll-loops")))
++swsusp_finish(void *userdata)
++{
++ struct swsusp_finish_context *context = userdata;
++ u32 **remap_orig;
++ u32 **remap_temp;
++ int idx = 0;
++ const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
++
++ remap_orig = context->remap_orig_page;
++ remap_temp = context->remap_temp_page;
++
++ __asm__ volatile ("" : : : "memory");
++ for (;;) {
++ u32 *orig, *temp;
++ int count;
++
++ /* Linked list to next page */
++ if (idx == lastidx) {
++ remap_orig = (u32 **)remap_orig[idx];
++ remap_temp = (u32 **)remap_temp[idx];
++ idx = 0;
++ }
++ if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
++ break;
++ orig = remap_orig[idx];
++ temp = remap_temp[idx];
++#ifdef FAST_COPY
++ count = PAGE_SIZE / sizeof(u32) / 32;
++ __asm__ volatile (
++ "1:\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "subs %[count], %[count], #1\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "bgt 1b\n"
++ : /* No outputs */
++ :
++ [rorig]"h" (orig),
++ [rtemp]"h" (temp),
++ [count]"h" (count)
++ : "r0", "r1", "r2",
++ "r3", "r4", "r5",
++ "r6", "r7", "cc", "memory"
++ );
++#else
++ count = PAGE_SIZE / sizeof(u32);
++ while (count--)
++ *orig++ = *temp++;
++#endif
++#ifdef SWSUSP_CHECK_COPY_RESULT
++ count = PAGE_SIZE / sizeof(u32);
++ orig = remap_orig[idx];
++ temp = remap_temp[idx];
++ __asm__ volatile (
++ "1:\n"
++ "ldr r3, [%[rorig]]\n"
++ "ldr r4, [%[rtemp]]\n"
++ "cmp r3, r4\n"
++ "bne 2f\n"
++ "add %[rorig], %[rorig], #4\n"
++ "add %[rtemp], %[rtemp], #4\n"
++ "subs %[count], %[count], #1\n"
++ "bgt 1b\n"
++ "b 3f\n"
++ "2:b 2b\n"
++ "3:\n"
++ :
++ [rorig]"+r" (orig),
++ [rtemp]"+r" (temp),
++ [count]"+r" (count)
++ :
++ : "r3", "r4", "cc", "memory"
++ );
++#endif
++ idx++;
++ }
++ context->archdata.cpu_resume_restore_nosave(
++ context->archdata.nosave_backup_phys,
++ context->archdata.nosave_begin_phys,
++ context->archdata.nosave_end_phys);
++}
++
++static int raw_page_init(u64 start)
++{
++#ifdef CONFIG_SH_DMA
++ meta_map = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
++#else
++ meta_map = malloc(PAGE_SIZE);
++#endif
++ if (!meta_map)
++ return -1;
++ meta_map_next = 0;
++ meta_map_curr = 0;
++ meta_map_start = start;
++ return 0;
++}
++
++static void raw_page_start(void)
++{
++ meta_idx = ARRAY_SIZE(meta_map->entries);
++ meta_map_next = meta_map_start;
++}
++
++static int get_meta(void)
++{
++ if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
++ if (!meta_map_next)
++ return 0;
++ if (meta_map_curr != meta_map_next) {
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO("META: %d (%08x)\n",
++ (int)meta_map_next,
++ (unsigned int)
++ (meta_map_next * PAGE_SIZE));
++#endif
++ if (page_read(meta_map_next, meta_map))
++ return -1;
++ meta_map_curr = meta_map_next;
++ meta_map_next = meta_map->next_swap;
++ }
++ meta_idx = 0;
++ }
++#ifdef PAGEMAP_DEBUG
++ {
++ static unsigned int pre;
++ if ((pre+1) != meta_map->entries[meta_idx]) {
++ PAGEMAP_INFO("DATA-Skipped: %d->%d (%08x->%08x)\n",
++ pre,
++ (unsigned int)meta_map->entries[meta_idx],
++ pre*PAGE_SIZE,
++ (unsigned int)
++ (meta_map->entries[meta_idx] *
++ PAGE_SIZE));
++ }
++ pre = meta_map->entries[meta_idx];
++ _last_read_pages = pre;
++ }
++#endif
++ return 1;
++}
++
++static int raw_page_get_next(void *buffer)
++{
++ if (!get_meta())
++ return 0;
++
++ if (page_read(meta_map->entries[meta_idx++], buffer))
++ return -1;
++
++ return 1;
++}
++
++static void raw_page_exit(void)
++{
++ free(meta_map);
++ meta_map = NULL;
++}
++
++static int image_compressed;
++static int image_pages_avail;
++static unsigned char *unc_buf;
++static unsigned char *cmp_buf;
++static int unc_offset;
++
++static int image_page_init(int compressed)
++{
++ if (!compressed)
++ return 1;
++
++#ifdef CONFIG_SH_DMA
++ cmp_buf = malloc_aligned(LZO_CMP_SIZE, ARCH_DMA_MINALIGN);
++#else
++ cmp_buf = malloc(LZO_CMP_SIZE);
++#endif
++ unc_buf = malloc(LZO_UNC_SIZE);
++ if (!unc_buf || !cmp_buf) {
++ printf("not enogh memory\n");
++ return 1;
++ }
++ image_compressed = compressed;
++ return 0;
++}
++
++static void image_page_start(void)
++{
++ image_pages_avail = 0;
++}
++
++static int image_page_get_next(void *buffer)
++{
++ if (image_compressed) {
++#ifdef CONFIG_LZO
++ if (!image_pages_avail) {
++ int ret;
++ size_t unc_len, cmp_len, cmp_avail;
++
++ ret = raw_page_get_next(cmp_buf);
++ if (ret <= 0)
++ return ret;
++
++ cmp_len = *(size_t *) cmp_buf;
++ cmp_avail = PAGE_SIZE;
++
++ while (cmp_avail < cmp_len + LZO_HEADER) {
++ ret = raw_page_get_next(cmp_buf + cmp_avail);
++ if (unlikely(ret <= 0))
++ return ret;
++ cmp_avail += PAGE_SIZE;
++ }
++ unc_len = LZO_UNC_SIZE;
++ ret = lzo1x_decompress_safe(cmp_buf + LZO_HEADER,
++ cmp_len, unc_buf, &unc_len);
++ if (unlikely(ret != LZO_E_OK)) {
++ printf("Decompression failure:\n");
++ printf("ret = %d\n", ret);
++ printf("cmp_buf = %p\n", cmp_buf + LZO_HEADER);
++ printf("cmp_len = %zu\n", cmp_len);
++ printf("unc_len = %zu\n", unc_len);
++ return ret;
++ }
++ image_pages_avail = unc_len / PAGE_SIZE;
++ unc_offset = 0;
++ }
++
++ memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
++ unc_offset += PAGE_SIZE;
++ image_pages_avail--;
++ return 1;
++#else
++ printf("No LZO support in u-boot.\n");
++ return -1;
++#endif
++ } else {
++ return raw_page_get_next(buffer);
++ }
++}
++
++static void image_page_exit(void)
++{
++ free(unc_buf);
++ free(cmp_buf);
++ unc_buf = cmp_buf = NULL;
++}
++
++static void bitmap_set(u32 *bm, unsigned int bit)
++{
++ bm[bit >> 5] |= (1 << (bit & 0x1f));
++}
++
++static int bitmap_is_set(u32 *bm, unsigned int bit)
++{
++ return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
++}
++
++static u32 *used_bitmap;
++static u32 next_free_page;
++static u32 total_pages;
++
++static int free_page_init(void)
++{
++ total_pages = (u32)((TOP_ADDRESS -
++ LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
++ used_bitmap = malloc(total_pages * sizeof(u32) / 32);
++ if (!used_bitmap)
++ return -1;
++ return 0;
++}
++
++static void free_page_start(int offset)
++{
++ memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
++ next_free_page = pg_ub2zero(offset);
++}
++
++static void free_page_mark_used(u32 page);
++/* Returns full-address based pages */
++static int free_page_get_next(void)
++{
++ while (bitmap_is_set(used_bitmap, next_free_page))
++ next_free_page++;
++ free_page_mark_used(next_free_page);
++ return pg_zero2ub(next_free_page++);
++}
++
++static void free_page_mark_used(u32 page)
++{
++ bitmap_set(used_bitmap, page);
++}
++
++static void free_page_exit(void)
++{
++ free(used_bitmap);
++ used_bitmap = NULL;
++}
++
++int do_swsusp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
++{
++ int ret;
++ __u32 offset = 0;
++ void *spare_page = NULL;
++ struct swsusp_header *swsusp_header;
++ struct swsusp_info *swsusp_info;
++ struct swsusp_finish_context *context;
++ int max_page;
++ int i;
++ u32 nr_pfn_pages;
++ u32 **pfn_pages = NULL;
++ u32 *remap_orig_page;
++ u32 *remap_temp_page;
++ u32 **remap_orig;
++ u32 **remap_temp;
++ int remap_idx;
++ int m;
++ void (*swsusp_finish_copy)(void *);
++ char *data_page;
++ char *stack_addr;
++ int high_page;
++#ifdef USE_CRC_32x4
++ CRC32_WORD4_t calc_crc;
++#endif
++#ifdef PAGEMAP_DEBUG
++ int high_page_images = 0;
++ int total_remap = 0;
++ if (getenv("hybdebug") != NULL)
++ debugout = 1;
++#endif
++ /* Address hack */
++ void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
++
++ if (argc < 2) {
++ printf("usage: swsusp <interface> "
++ "[<dev[:part]>] [<offset>]\n");
++ return 0;
++ }
++
++ if (argc == 4) {
++ char *ep;
++ offset = simple_strtoul(argv[3], &ep, 16);
++ if (*ep) {
++ printf("Invalid block offset\n");
++ return 1;
++ }
++ }
++
++ /* Allow for 32 pages of stack */
++ max_page = gd->start_addr_sp / PAGE_SIZE - 32;
++ high_page = (((gd->relocaddr
++ + _bss_end_ofs)+(PAGE_SIZE-1)) / PAGE_SIZE) + 1;
++#define pfn_is_occupied(pfn) (page > max_page && page <= high_page)
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO(" *gd->start_addr_sp:%p\n", (void *)gd->start_addr_sp);
++ PAGEMAP_INFO(" *gd->relocaddr:%p\n", (void *)gd->relocaddr);
++ PAGEMAP_INFO(" *bss_start_offset:%d bss_end_offset:%d\n",
++ (int)_bss_start_ofs, (int)_bss_end_ofs);
++ PAGEMAP_INFO(" UBOOT own memory [%p-%p]\n",
++ pg2addr(max_page), pg2addr(high_page));
++#endif
++
++ if (free_page_init())
++ goto mem_err;
++ free_page_start(exclude_max_page + 1);
++
++#ifdef CONFIG_SH_DMA
++ spare_page = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
++#else
++ spare_page = malloc(PAGE_SIZE);
++#endif
++ if (!spare_page)
++ goto mem_err;
++
++ ret = get_device_and_partition(argv[1], argv[2], &swap_dev, &swap_info,
++ 1);
++ if (ret < 0)
++ goto err;
++
++ swsusp_header = spare_page;
++ if (page_read(offset, swsusp_header))
++ goto read_err;
++
++#ifdef SWSUSP_DEBUG_INFO
++ PAGEMAP_INFO("swssp_header:\n");
++ PAGEMAP_INFO(" comp_crc: <snip>\n");
++ PAGEMAP_INFO(" img_size: %d\n", swsusp_header->img_size);
++ PAGEMAP_INFO(" image(swap firest sector): %08x\n",
++ (unsigned int)swsusp_header->image);
++ PAGEMAP_INFO(" flags: %08x\n", swsusp_header->flags);
++ PAGEMAP_INFO(" orig_sig:%s\n", swsusp_header->orig_sig);
++ PAGEMAP_INFO(" sig:%s\n", swsusp_header->sig);
++#endif /* SWSUSP_DEBUG_INFO */
++
++ if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)) {
++ printf("No hibernation image present\n");
++ return 0;
++ }
++
++#ifdef USE_CRC_32x4
++ memset(&calc_crc, 0, sizeof(calc_crc));
++ calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
++ swsusp_header->img_size, &calc_crc);
++
++ if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
++ sizeof(CRC32_WORD4_t))) {
++ printf("Bad CRC for image, image: %08x:%08x:%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
++ swsusp_header->comp_crc32.crc_w[0],
++ swsusp_header->comp_crc32.crc_w[1],
++ swsusp_header->comp_crc32.crc_w[2],
++ swsusp_header->comp_crc32.crc_w[3],
++ calc_crc.crc_w[0], calc_crc.crc_w[1],
++ calc_crc.crc_w[2], calc_crc.crc_w[3]);
++ return 0;
++ }
++#endif
++
++ /* Overwrite header if necessary */
++#ifndef SWSUSP_KEEP_IMAGE
++ if (memcmp(swsusp_header->sig, swsusp_header->orig_sig, 10)) {
++ memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
++ if (page_write(offset, swsusp_header))
++ printf("Write error resetting header\n");
++ }
++#endif
++
++ if (raw_page_init(swsusp_header->image))
++ goto mem_err;
++ raw_page_start();
++
++ if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
++ goto mem_err;
++ image_page_start();
++
++ swsusp_info = spare_page;
++ if (raw_page_get_next(swsusp_info) <= 0)
++ goto read_err;
++
++#ifdef SWSUSP_DEBUG_INFO
++ PAGEMAP_INFO("swsup_info:\n");
++ PAGEMAP_INFO(" utsname.sysname:%s\n", swsusp_info->uts.sysname);
++ PAGEMAP_INFO(" nodename:%s\n", swsusp_info->uts.nodename);
++ PAGEMAP_INFO(" release:%s\n", swsusp_info->uts.release);
++ PAGEMAP_INFO(" version:%s\n", swsusp_info->uts.version);
++ PAGEMAP_INFO(" machine:%s\n", swsusp_info->uts.machine);
++ PAGEMAP_INFO(" vesion_code:%#08x\n",
++ (unsigned int)swsusp_info->version_code);
++ PAGEMAP_INFO(" num_physpages:%d\n",
++ (unsigned int)swsusp_info->num_physpages);
++ PAGEMAP_INFO(" pages :%d\n",
++ (unsigned int)swsusp_info->pages);
++ PAGEMAP_INFO(" size :%d\n",
++ (unsigned int)swsusp_info->size);
++#endif
++
++ nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
++ PAGE_SIZE;
++ pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
++ if (!pfn_pages)
++ goto mem_err;
++ memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
++
++ /* UBOOT using memory */
++ for (i = max_page; i <= high_page; i++)
++ free_page_mark_used(pg_ub2zero(i));
++
++ printf("Allocating %u bytes (nr_pfn_pages %u)\n",
++ nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
++
++ for (i = 0; i < nr_pfn_pages; i++) {
++ u32 idx;
++#ifdef CONFIG_SH_DMA
++ pfn_pages[i] = malloc_aligned(PAGE_SIZE, ARCH_DMA_MINALIGN);
++#else
++ pfn_pages[i] = malloc(PAGE_SIZE);
++#endif
++ if (unlikely(!pfn_pages[i]))
++ goto mem_err;
++ if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
++ goto read_err;
++ for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
++ u32 page = pfn_pages[i][idx];
++ if (page == ~0UL)
++ break;
++ free_page_mark_used(pg_ub2zero(page));
++ }
++ }
++
++ printf("Loading image data pages (%lu pages)\n",
++ swsusp_info->image_pages);
++
++ remap_orig_page = pg2addr(free_page_get_next());
++ remap_temp_page = pg2addr(free_page_get_next());
++
++ remap_orig = (u32 **)remap_orig_page;
++ remap_temp = (u32 **)remap_temp_page;
++ remap_idx = 0;
++
++ m = (swsusp_info->image_pages / 10) ? : 1;
++ for (i = 0; i < swsusp_info->image_pages; i++) {
++ u32 page = pfn_pages[i >> 10][i & 0x3ff];
++ if (unlikely(!pfn_is_valid(page))) {
++ printf("Attempt to restore invalid address %llx\n",
++ pg2phys(page));
++ continue;
++ } else if (unlikely(pfn_is_excluded(page))) {
++ printf("Attempt to restore excluded address %llx\n",
++ pg2phys(page));
++ continue;
++ } else if (unlikely(pfn_is_low(page) &&
++ pfn_is_occupied(page))) {
++ remap_orig[remap_idx] = pg2addr(page);
++ page = free_page_get_next();
++ remap_temp[remap_idx] = pg2addr(page);
++ remap_idx++;
++#ifdef PAGEMAP_DEBUG
++ ++total_remap;
++#endif
++ /* If we fill our current page, allocate a new one */
++ if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
++ u32 *next;
++
++ next = pg2addr(free_page_get_next());
++ remap_orig[remap_idx] = next;
++ remap_orig = (u32 **)next;
++
++ next = pg2addr(free_page_get_next());
++ remap_temp[remap_idx] = next;
++ remap_temp = (u32 **)next;
++
++ remap_idx = 0;
++ }
++ }
++ if (image_page_get_next(pg2addr(page)) <= 0)
++ goto read_err;
++ if (!(i % m))
++ printf("Image loading progress: %3d%%\n", 10 * i / m);
++ }
++
++ printf("Image loading done.\n");
++ invalidate_icache_all();
++
++ /* put end markers on the remap list */
++ remap_orig[remap_idx] = (void *) ~0UL;
++ remap_temp[remap_idx] = (void *) ~0UL;
++
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO("Number of remap pages:%d\n",
++ total_remap);
++ PAGEMAP_INFO("Number of high pages:%d\n",
++ high_page_images);
++ PAGEMAP_INFO("Last read page %d (%08x)\n",
++ _last_read_pages,
++ _last_read_pages * PAGE_SIZE);
++#endif
++ remap_orig = (u32 **)remap_orig_page;
++ remap_temp = (u32 **)remap_temp_page;
++
++ /* Make a copy of swsusp_finish in a free data page */
++ data_page = pg2addr(free_page_get_next());
++ memcpy(data_page, swsusp_finish_p, PAGE_SIZE);
++ swsusp_finish_copy = (void *) data_page;
++
++ /* Setup context for swsusp_finish at the end of the data_page */
++ context = (struct swsusp_finish_context *) (data_page + PAGE_SIZE -
++ sizeof(struct swsusp_finish_context));
++ context->remap_orig_page = remap_orig_page;
++ context->remap_temp_page = remap_temp_page;
++ memcpy((void *)&context->archdata, (void *)swsusp_info->archdata,
++ sizeof(struct swsusp_archdata));
++
++ /* Get a stack pointer for swsusp_finish, growing down from context */
++ stack_addr = (char *) context;
++
++#ifdef CONFIG_NETCONSOLE
++ /*
++ * Stop the ethernet stack if NetConsole could have
++ * left it up
++ */
++ eth_halt();
++#endif
++
++#ifdef CONFIG_USB_DEVICE
++ udc_disconnect();
++#endif
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO("Execution routine: %08x\n",
++ (u32)context->archdata.cpu_resume_restore_nosave);
++#endif
++ arch_preboot_os();
++ cleanup_before_linux();
++
++ /* Copy the final data from a safe place */
++ call_with_stack(swsusp_finish_copy, context, stack_addr);
++
++ return 0;
++
++mem_err:
++ printf("Not enough memory.\n");
++ goto err;
++
++read_err:
++ printf("Read error while restoring image.\n");
++
++err:
++ __asm__ volatile (
++ "mov r0, #0\n"
++ "mcr p15, 0, r0, c7, c5, 0 @ invalidate icache\n"
++ "mcr p15, 0, r0, c7, c10, 4 @ DSB\n"
++ "mcr p15, 0, r0, c7, c5, 4 @ ISB\n"
++ : : : "r0", "memory");
++
++ raw_page_exit();
++ image_page_exit();
++ free_page_exit();
++ if (pfn_pages) {
++ for (i = 0; i < nr_pfn_pages; i++)
++ free(pfn_pages[i]);
++ free(pfn_pages);
++ }
++ free(spare_page);
++
++ return 1;
++}
++
++U_BOOT_CMD(swsusp, 4, 0, do_swsusp,
++ "Restore SWSUSP hibernation image",
++ "<interface> [<dev[:part]>] [<offset>]"
++);
+--
+1.8.3.1
+
--- /dev/null
+From 4ce00daa904a40701ab6bed44506fe97b8f1da47 Mon Sep 17 00:00:00 2001
+From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+Date: Fri, 19 May 2017 14:48:38 +0900
+Subject: [PATCH 3/4] Add Hibernation swsuspmem command support
+
+Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+---
+ common/Makefile | 2 +
+ common/cmd_swsuspmem.c | 944 +++++++++++++++++++++++++++++++++++++++++++++
+ include/swsuspmem.h | 24 ++
+ lib/lzo/lzo1x_decompress.c | 12 +-
+ 4 files changed, 980 insertions(+), 2 deletions(-)
+ create mode 100644 common/cmd_swsuspmem.c
+ create mode 100644 include/swsuspmem.h
+
+diff --git a/common/Makefile b/common/Makefile
+index 54fcc81..7a18486 100644
+--- a/common/Makefile
++++ b/common/Makefile
+@@ -160,6 +160,8 @@ COBJS-$(CONFIG_CMD_SETEXPR) += cmd_setexpr.o
+ COBJS-$(CONFIG_CMD_SPI) += cmd_spi.o
+ COBJS-$(CONFIG_CMD_SPIBOOTLDR) += cmd_spibootldr.o
+ COBJS-$(CONFIG_CMD_STRINGS) += cmd_strings.o
++COBJS-$(CONFIG_CMD_SWSUSP) += cmd_swsusp.o
++COBJS-$(CONFIG_CMD_SWSUSPMEM) += cmd_swsuspmem.o
+ COBJS-$(CONFIG_CMD_TERMINAL) += cmd_terminal.o
+ COBJS-$(CONFIG_CMD_TIME) += cmd_time.o
+ COBJS-$(CONFIG_SYS_HUSH_PARSER) += cmd_test.o
+diff --git a/common/cmd_swsuspmem.c b/common/cmd_swsuspmem.c
+new file mode 100644
+index 0000000..6980aaf
+--- /dev/null
++++ b/common/cmd_swsuspmem.c
+@@ -0,0 +1,944 @@
++/*
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License as published by
++ * the Free Software Foundation; either version 2 of the License, or
++ * (at your option) any later version.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ *
++ * You should have received a copy of the GNU General Public License
++ * along with this program; if not, write to the Free Software
++ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
++ */
++
++#include <common.h>
++#include <command.h>
++#include <part.h>
++#include <malloc.h>
++
++#include <linux/lzo.h>
++#include "../arch/arm/cpu/armv7/rmobile/crc32_word4.h"
++#include <swsuspmem.h>
++
++/* Note for Renesas--based boards:
++ * We have the following memory split here:
++ * 0x40000000 - u_boot_lowest - used to store pfns at physical addresses
++ * u_boot_lowest - 0x8000000 - pfns are relocated, and then later put
++ * on physical addresses (swsusp_finish)
++ * 0x8000000 - 0xc0000000 - used to store pfns with physical address
++ * of 0x200000000 (long address), we have to change offset for them.
++ * Any pfn with address > 0x8000000 but less than 0x200000000
++ * is an error.
++ * For boards which do not have memory above first GB, that will
++ * still work, as they won't have anything above 0x80000000
++ * in their image, so for standard 2GB setup ou should put
++ * your secong GB in 0x80000000-0xC0000000 range, you can
++ * use MMU for that or if your RAM is continous, it will
++ * naturally be there. */
++
++DECLARE_GLOBAL_DATA_PTR;
++
++/* #define PAGEMAP_DEBUG */
++
++#ifdef PAGEMAP_DEBUG
++#define SWSUSP_DEBUG_INFO
++#endif
++
++#define SWSUSP_KEEP_IMAGE
++
++#ifndef likely
++# define likely(x) __builtin_expect(!!(x), 1)
++# define unlikely(x) __builtin_expect(!!(x), 0)
++#endif
++
++#define HIBERNATE_SIG "S1SUSPEND"
++#define PAGE_SIZE (4096)
++/* Define depending on CONFIG_LBDAF in kernel */
++
++typedef u64 sector_t;
++
++struct swsusp_header {
++ char reserved[PAGE_SIZE - 20
++ - sizeof(sector_t) - sizeof(int) - sizeof(u32)
++ - sizeof(CRC32_WORD4_t) - sizeof(u32)];
++ CRC32_WORD4_t comp_crc32;
++ u32 img_size; /* append */
++ u32 crc32;
++ sector_t image;
++ unsigned int flags;
++ char orig_sig[10];
++ char sig[10];
++} __packed;
++
++#define __NEW_UTS_LEN 64
++
++struct new_utsname {
++ char sysname[__NEW_UTS_LEN + 1];
++ char nodename[__NEW_UTS_LEN + 1];
++ char release[__NEW_UTS_LEN + 1];
++ char version[__NEW_UTS_LEN + 1];
++ char machine[__NEW_UTS_LEN + 1];
++ char domainname[__NEW_UTS_LEN + 1];
++};
++
++struct swsusp_archdata {
++ u32 nosave_backup_phys;
++ u32 nosave_begin_phys;
++ u32 nosave_end_phys;
++ void (*cpu_resume_restore_nosave)(u32, u32, u32);
++};
++
++struct swsusp_info {
++ struct new_utsname uts;
++ u32 version_code;
++ unsigned long num_physpages;
++ int cpus;
++ unsigned long image_pages;
++ unsigned long pages;
++ unsigned long size;
++ char archdata[1024];
++};
++
++struct swap_map_page {
++ u64 entries[PAGE_SIZE / sizeof(u64) - 1];
++ u64 next_swap;
++};
++
++struct swsusp_finish_context {
++ void *remap_orig_page;
++ void *remap_temp_page;
++ struct swsusp_archdata archdata;
++};
++#ifdef FTEN_SPF_SDRAM_BASE
++#define USED_ADDRESS_TOP (CONFIG_SYS_SDRAM_BASE)
++#define USED_ADDRESS_END (CONFIG_SYS_SDRAM_BASE + CONFIG_SYS_LOAD_OFFSET)
++#else
++#define USED_ADDRESS_TOP (0x40000000)
++#define USED_ADDRESS_END (0x48000000)
++#endif
++#define PG_UB2ZERO(pg) ((pg) - CONFIG_SYS_SDRAM_BASE / PAGE_SIZE)
++static u32 const exclude_min_page =
++ (USED_ADDRESS_TOP) / PAGE_SIZE;
++static u32 const exclude_max_page =
++ (USED_ADDRESS_END - 1) / PAGE_SIZE;
++static u32 const exclude_min_page_ub =
++ PG_UB2ZERO((USED_ADDRESS_TOP) / PAGE_SIZE);
++static u32 const exclude_max_page_ub =
++ PG_UB2ZERO((USED_ADDRESS_END-1) / PAGE_SIZE);
++
++/*
++ #define SD_PLATFORM_MODE 1
++ #define SD_CRC32_MODE 4
++ */
++#define SF_NOCOMPRESS_MODE 2
++
++#define LZO_HEADER sizeof(size_t)
++
++/* Number of pages/bytes we'll compress at one time. */
++#define LZO_UNC_PAGES 32
++#define LZO_UNC_SIZE (LZO_UNC_PAGES * PAGE_SIZE)
++
++/* Number of pages/bytes we need for compressed data (worst case). */
++#define LZO_CMP_PAGES DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
++ LZO_HEADER, PAGE_SIZE)
++#define LZO_CMP_SIZE (LZO_CMP_PAGES * PAGE_SIZE)
++
++static struct swsuspmem_hook *_hook;
++
++#define CALL_HOOK(f, param) \
++ do { \
++ if (_hook != NULL) { \
++ if (_hook->f != NULL) \
++ _hook->f(param); \
++ } \
++ } while (0)
++
++#ifdef PAGEMAP_DEBUG
++static int debugout;
++static int _last_read_pages;
++#define PAGEMAP_INFO(_msg, ...) \
++ do { \
++ if (debugout == 1) \
++ printf(_msg, ## __VA_ARGS__); \
++ } while (0)
++#endif
++
++#define HIGHMEM_PHYS_ADDR 0x200000000ULL
++#define HIGHMEM_VA 0x80000000UL
++#define HIGHMEM_PFN (HIGHMEM_PHYS_ADDR / PAGE_SIZE)
++#define LOW_TOP 0x80000000
++#define LOW_TOP_PFN (LOW_TOP / PAGE_SIZE)
++#define LOW_BOTTOM CONFIG_SYS_SDRAM_BASE
++#define LOW_BOTTOM_PFN (LOW_BOTTOM / PAGE_SIZE)
++#define TOP_ADDRESS 0x240000000ULL
++
++static inline int pfn_is_low(u32 pfn)
++{
++ return ((pfn >= LOW_BOTTOM_PFN) && (pfn < LOW_TOP_PFN));
++}
++
++static inline int pfn_is_high(u32 pfn)
++{
++ return (pfn >= HIGHMEM_PFN);
++}
++
++#define pfn_is_valid(p) (pfn_is_low(p) || pfn_is_high(p))
++
++static inline int pfn_is_excluded(u32 pfn)
++{
++ /* Allow bottom 2 pages for exception vectors */
++ if (pfn < (LOW_BOTTOM_PFN + 2))
++ return 0;
++ else if (exclude_min_page >= exclude_max_page)
++ return 0;
++ else
++ return (pfn >= exclude_min_page) && (pfn <= exclude_max_page);
++}
++/* PFN to zero-counted page */
++static inline u32 pg_ub2zero(u32 pg)
++{
++ return pg - LOW_BOTTOM_PFN;
++}
++
++/* zero-counted page to PFN */
++static inline u32 pg_zero2ub(u32 pg)
++{
++ return pg + LOW_BOTTOM_PFN;
++}
++
++/* PFN to physical address (64-bit (40-bit)) */
++static inline u64 pg2phys(u32 page)
++{
++ return (u64) page * PAGE_SIZE;
++}
++
++/* PFN to virtual address */
++static inline void *pg2addr(u32 page)
++{
++ void *addr;
++ if (page >= HIGHMEM_PFN)
++ addr = (void *) (u32)(pg2phys(page - HIGHMEM_PFN) + HIGHMEM_VA);
++ else
++ addr = (void *) (u32)pg2phys(page);
++
++ return addr;
++}
++/* Virtual address to PFN */
++static inline u32 addr2pg(void *addr)
++{
++ return ((u32)(addr)) / PAGE_SIZE;
++}
++static void *offt_addr = (void *)0x44000000;
++static int page_read_mem(u64 page, void *addr)
++{
++ memcpy(addr, (u8 *)offt_addr + page * PAGE_SIZE, PAGE_SIZE);
++ return 0;
++}
++
++#ifndef SWSUSP_KEEP_IMAGE
++static int page_write_mem(u32 page, void *addr)
++{
++ memcpy((u8 *)offt_addr + page * PAGE_SIZE, addr, PAGE_SIZE);
++ return 0;
++}
++#endif
++
++#define FAST_COPY
++static void __attribute__((section(".rodata")))
++ __attribute__((optimize("O6", "unroll-loops")))
++swsusp_finish(void *userdata)
++{
++ struct swsusp_finish_context *context = userdata;
++ u32 **remap_orig;
++ u32 **remap_temp;
++ int idx = 0;
++ const int lastidx = PAGE_SIZE / sizeof(u32) - 1;
++
++ remap_orig = context->remap_orig_page;
++ remap_temp = context->remap_temp_page;
++
++ __asm__ volatile ("" : : : "memory");
++ for (;;) {
++ u32 *orig, *temp;
++ int count;
++
++ /* Linked list to next page */
++ if (idx == lastidx) {
++ remap_orig = (u32 **)remap_orig[idx];
++ remap_temp = (u32 **)remap_temp[idx];
++ idx = 0;
++ }
++ if (unlikely(!remap_orig || remap_orig[idx] == (u32 *)~0UL))
++ break;
++ orig = remap_orig[idx];
++ temp = remap_temp[idx];
++#ifdef FAST_COPY
++ count = PAGE_SIZE / sizeof(u32) / 32;
++ __asm__ volatile (
++ "1:\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "ldmia %[rtemp]!, {r0-r7}\n"
++ "subs %[count], %[count], #1\n"
++ "stmia %[rorig]!, {r0-r7}\n"
++ "bgt 1b\n"
++ : /* No outputs */
++ :
++ [rorig]"h" (orig),
++ [rtemp]"h" (temp),
++ [count]"h" (count)
++ : "r0", "r1", "r2", "r3",
++ "r4", "r5", "r6", "r7",
++ "cc", "memory"
++ );
++#else
++ count = PAGE_SIZE / sizeof(u32);
++ while (count--)
++ *orig++ = *temp++;
++#endif
++#ifdef SWSUSP_CHECK_COPY_RESULT
++ count = PAGE_SIZE / sizeof(u32);
++ orig = remap_orig[idx];
++ temp = remap_temp[idx];
++ __asm__ volatile (
++ "1:\n"
++ "ldr r3, [%[rorig]]\n"
++ "ldr r4, [%[rtemp]]\n"
++ "cmp r3, r4\n"
++ "bne 2f\n"
++ "add %[rorig], %[rorig], #4\n"
++ "add %[rtemp], %[rtemp], #4\n"
++ "subs %[count], %[count], #1\n"
++ "bgt 1b\n"
++ "b 3f\n"
++ "2:b 2b\n"
++ "3:\n"
++ :
++ [rorig]"+r" (orig),
++ [rtemp]"+r" (temp),
++ [count]"+r" (count)
++ :
++ : "r3", "r4", "cc", "memory"
++ );
++#endif
++ idx++;
++ }
++ context->archdata.cpu_resume_restore_nosave(
++ context->archdata.nosave_backup_phys,
++ context->archdata.nosave_begin_phys,
++ context->archdata.nosave_end_phys);
++}
++
++static struct swap_map_page *meta_map;
++static u64 meta_map_next;
++static u64 meta_map_curr;
++static u64 meta_map_start;
++static int meta_idx;
++
++static int raw_page_init(u64 start)
++{
++ meta_map = malloc(PAGE_SIZE);
++ if (!meta_map)
++ return -1;
++ meta_map_next = 0;
++ meta_map_curr = 0;
++ meta_map_start = start;
++ return 0;
++}
++
++static void raw_page_start(void)
++{
++ meta_idx = ARRAY_SIZE(meta_map->entries);
++ meta_map_next = meta_map_start;
++}
++
++static int raw_page_get_next(void *buffer)
++{
++ if (meta_idx == ARRAY_SIZE(meta_map->entries)) {
++ if (!meta_map_next)
++ return 0;
++ if (meta_map_curr != meta_map_next) {
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO("META: %d (%08x)\n", (int)meta_map_next,
++ (unsigned int)(meta_map_next
++ * PAGE_SIZE));
++#endif
++ if (page_read_mem(meta_map_next, meta_map))
++ return -1;
++ meta_map_curr = meta_map_next;
++ meta_map_next = meta_map->next_swap;
++ }
++ meta_idx = 0;
++ }
++#ifdef PAGEMAP_DEBUG
++ {
++ static unsigned int pre;
++ if ((pre + 1) != meta_map->entries[meta_idx]) {
++ PAGEMAP_INFO("DATA-Skiped: %d->%d (%08x->%08x)\n",
++ pre, (unsigned int)meta_map->entries[meta_idx],
++ pre * PAGE_SIZE,
++ (unsigned int)(meta_map->entries[meta_idx]
++ * PAGE_SIZE));
++ }
++ pre = meta_map->entries[meta_idx];
++ _last_read_pages = pre;
++ }
++#endif
++ if (page_read_mem(meta_map->entries[meta_idx++], buffer))
++ return -1;
++
++ return 1;
++}
++
++static void raw_page_exit(void)
++{
++ free(meta_map);
++ meta_map = NULL;
++}
++
++static int image_pages_avail;
++static unsigned char *unc_buf, *cmp_buf;
++static int unc_offset;
++
++static int image_page_init(int compressed)
++{
++ if (!compressed)
++ return 1;
++
++ unc_buf = malloc(LZO_UNC_SIZE);
++ cmp_buf = malloc(LZO_CMP_SIZE);
++ if (!unc_buf || !cmp_buf) {
++ printf("not enogh memory\n");
++ return 1;
++ }
++ return 0;
++}
++
++static void image_page_start(void)
++{
++ image_pages_avail = 0;
++}
++
++static int image_page_get_next(void *buffer)
++{
++#ifdef CONFIG_LZO
++ if (!image_pages_avail) {
++ int ret;
++ size_t unc_len, cmp_len, cmp_avail;
++
++ ret = raw_page_get_next(cmp_buf);
++ if (ret <= 0)
++ return ret;
++
++ cmp_len = *(size_t *) cmp_buf;
++ cmp_avail = PAGE_SIZE;
++
++ while (cmp_avail < cmp_len + LZO_HEADER) {
++ ret = raw_page_get_next(cmp_buf + cmp_avail);
++ if (unlikely(ret <= 0))
++ return ret;
++ cmp_avail += PAGE_SIZE;
++ }
++
++ unc_len = LZO_UNC_SIZE;
++ ret = lzo1x_decompress_safe(cmp_buf + LZO_HEADER,
++ cmp_len, unc_buf, &unc_len);
++ if (unlikely(ret != LZO_E_OK)) {
++ printf("Decompression failure: %d,"
++ " cmp_buf = %p,"
++ " cmp_len = %d, unc_len = %d\n",
++ ret, cmp_buf + LZO_HEADER, cmp_len,
++ unc_len);
++ return ret;
++ }
++ image_pages_avail = unc_len / PAGE_SIZE;
++ unc_offset = 0;
++ }
++
++ memcpy(buffer, unc_buf + unc_offset, PAGE_SIZE);
++ unc_offset += PAGE_SIZE;
++ image_pages_avail--;
++ return 1;
++#else
++ printf("No LZO support in u-boot.\n");
++ return -1;
++#endif
++}
++
++static void image_page_exit(void)
++{
++ free(unc_buf);
++ free(cmp_buf);
++ unc_buf = cmp_buf = NULL;
++}
++
++static void bitmap_set(u32 *bm, unsigned int bit)
++{
++ bm[bit >> 5] |= (1 << (bit & 0x1f));
++}
++
++static int bitmap_is_set(u32 *bm, unsigned int bit)
++{
++ return !!(bm[bit >> 5] & (1 << (bit & 0x1f)));
++}
++
++static u32 *used_bitmap;
++static u32 next_free_page;
++static u32 total_pages;
++
++static int free_page_init(void)
++{
++ total_pages = (u32)((TOP_ADDRESS -
++ LOW_BOTTOM) / PAGE_SIZE); /* 2GB */
++ used_bitmap = malloc(total_pages * sizeof(u32) / 32);
++ if (!used_bitmap)
++ return -1;
++ return 0;
++}
++
++static void free_page_start(int offset)
++{
++ memset(used_bitmap, 0, total_pages * sizeof(u32) / 32);
++ next_free_page = pg_ub2zero(offset);
++}
++
++static void free_page_mark_used(u32 page);
++/* Returns full-address based pages */
++static int free_page_get_next(void)
++{
++ while (bitmap_is_set(used_bitmap, next_free_page))
++ next_free_page++;
++ free_page_mark_used(next_free_page);
++ return pg_zero2ub(next_free_page++);
++}
++
++static void free_page_mark_used(u32 page)
++{
++ bitmap_set(used_bitmap, page);
++}
++
++static void free_page_exit(void)
++{
++ free(used_bitmap);
++ used_bitmap = NULL;
++}
++
++void set_swsuspmem_hook(struct swsuspmem_hook *hook)
++{
++ _hook = hook;
++}
++
++/*
++ * rtn = 1 : Hibernation image OK.
++ * rtn = 0 : Hibernation image NG.
++ * */
++int do_checksnapimage(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
++{
++ __u32 offset = 0;
++ void *spare_page = NULL;
++ struct swsusp_header *swsusp_header;
++ CRC32_WORD4_t calc_crc;
++
++ /* Address hack */
++ if (argc > 1) {
++ char *ep;
++ offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
++ if (*ep) {
++ printf("Invalid address\n");
++ return 0;
++ }
++ }
++
++ spare_page = malloc(PAGE_SIZE);
++ if (!spare_page)
++ goto mem_err;
++
++ swsusp_header = spare_page;
++ if (page_read_mem(offset, swsusp_header))
++ goto read_err;
++
++#ifdef SWSUSP_DEBUG_INFO
++ PAGEMAP_INFO("swssp_header:%x\n", swsusp_header);
++ PAGEMAP_INFO(" comp_crc: <snip>\n");
++ PAGEMAP_INFO(" img_size: %d\n", swsusp_header->img_size);
++ PAGEMAP_INFO(" image(swap firest sector): %08x\n",
++ (unsigned int)swsusp_header->image);
++ PAGEMAP_INFO(" flags: %08x\n", swsusp_header->flags);
++ PAGEMAP_INFO(" orig_sig:%s\n", swsusp_header->orig_sig);
++ PAGEMAP_INFO(" sig:%s\n", swsusp_header->sig);
++#endif /* SWSUSP_DEBUG_INFO */
++
++ if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
++ || (swsusp_header->img_size == 0)
++ || (swsusp_header->img_size > 0x03fff000)) {
++ printf("No hibernation image present\n");
++ CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
++ return 0;
++ }
++ memset(&calc_crc, 0, sizeof(calc_crc));
++
++ calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
++ swsusp_header->img_size, &calc_crc);
++
++ if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
++ sizeof(CRC32_WORD4_t))) {
++ printf("Bad CRC for image, image: %08x:%08x:"
++ "%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
++ swsusp_header->comp_crc32.crc_w[0],
++ swsusp_header->comp_crc32.crc_w[1],
++ swsusp_header->comp_crc32.crc_w[2],
++ swsusp_header->comp_crc32.crc_w[3],
++ calc_crc.crc_w[0], calc_crc.crc_w[1],
++ calc_crc.crc_w[2], calc_crc.crc_w[3]);
++ CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
++ return 0;
++ }
++ free(spare_page);
++ printf("Hibernation image OK!.\n");
++
++ return 1;
++
++mem_err:
++ printf("Not enough memory.\n");
++ CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
++ goto err;
++
++read_err:
++ printf("Read error while restoring image.\n");
++
++err:
++ __asm__ volatile (
++ "mov r0, #0\n"
++ "mcr p15, 0, r0, c7, c5, 0 @ invalidate icache\n"
++ "mcr p15, 0, r0, c7, c10, 4 @ DSB\n"
++ "mcr p15, 0, r0, c7, c5, 4 @ ISB\n"
++ : : : "r0", "memory");
++
++ free(spare_page);
++
++ CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
++ return 0;
++}
++
++U_BOOT_CMD(checksnapimage, 2, 2, do_checksnapimage,
++ "Check hibernation image data from memory",
++ "<address>]"
++);
++
++int do_swsuspmem(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
++{
++ __u32 offset = 0;
++ void *spare_page = NULL;
++ struct swsusp_header *swsusp_header;
++ struct swsusp_info *swsusp_info;
++ struct swsusp_finish_context *context;
++ int max_page;
++ int i;
++ u32 nr_pfn_pages;
++ u32 **pfn_pages = NULL;
++ u32 *remap_orig_page;
++ u32 *remap_temp_page;
++ u32 **remap_orig;
++ u32 **remap_temp;
++ int remap_idx;
++ void (*swsusp_finish_copy)(void *);
++ char *data_page;
++ char *stack_addr;
++ CRC32_WORD4_t calc_crc;
++ int high_page;
++
++#ifdef PAGEMAP_DEBUG
++ int high_page_images = 0;
++ int total_remap = 0;
++ if (getenv("hybdebug") != NULL)
++ debugout = 1;
++#endif
++ /* Address hack */
++ void *swsusp_finish_p = (void *)((u32)swsusp_finish & ~0x1);
++ if (argc > 1) {
++ char *ep;
++ offt_addr = (void *)simple_strtoul(argv[1], &ep, 16);
++ if (*ep) {
++ printf("Invalid address\n");
++ return 1;
++ }
++ }
++
++ /* Allow for 16 pages of stack */
++ max_page = gd->start_addr_sp / PAGE_SIZE - 32;
++ high_page = (((gd->relocaddr + _bss_end_ofs)
++ + (PAGE_SIZE - 1)) / PAGE_SIZE) + 1;
++#define pfn_is_occupied(pfn) (page > max_page && page <= high_page)
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO(" *gd->start_addr_sp:%p\n",
++ (void *)gd->start_addr_sp);
++ PAGEMAP_INFO(" *gd->relocaddr:%p\n",
++ (void *)gd->relocaddr);
++ PAGEMAP_INFO(" *bss_start_offset:%d bss_end_offset:%d\n",
++ (int)_bss_start_ofs, (int)_bss_end_ofs);
++ PAGEMAP_INFO(" UBOOT own memory [%p-%p]\n",
++ pg2addr(max_page), pg2addr(high_page));
++#endif
++ if (free_page_init())
++ goto mem_err;
++ free_page_start(exclude_max_page + 1);
++
++ spare_page = malloc(PAGE_SIZE);
++ if (!spare_page)
++ goto mem_err;
++
++ swsusp_header = spare_page;
++ if (page_read_mem(offset, swsusp_header))
++ goto read_err;
++
++#ifdef SWSUSP_DEBUG_INFO
++ PAGEMAP_INFO("swssp_header:\n");
++ PAGEMAP_INFO(" comp_crc: <snip>\n");
++ PAGEMAP_INFO(" img_size: %d\n", swsusp_header->img_size);
++ PAGEMAP_INFO(" image(swap firest sector): %08x\n",
++ (unsigned int)swsusp_header->image);
++ PAGEMAP_INFO(" flags: %08x\n", swsusp_header->flags);
++ PAGEMAP_INFO(" orig_sig:%s\n", swsusp_header->orig_sig);
++ PAGEMAP_INFO(" sig:%s\n", swsusp_header->sig);
++#endif /* SWSUSP_DEBUG_INFO */
++
++ if (memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)
++ || (swsusp_header->img_size == 0)
++ || (swsusp_header->img_size > 0x03fff000)) {
++ printf("No hibernation image present\n");
++ CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
++ return 0;
++ }
++ memset(&calc_crc, 0, sizeof(calc_crc));
++
++ calc_crc32x4((u8 *)((unsigned long)offt_addr + PAGE_SIZE),
++ swsusp_header->img_size, &calc_crc);
++
++ if (memcmp(&calc_crc, &swsusp_header->comp_crc32,
++ sizeof(CRC32_WORD4_t))) {
++ printf("Bad CRC for image, image: %08x:%08x:"
++ "%08x:%08x, calc: %08x:%08x:%08x:%08x\n",
++ swsusp_header->comp_crc32.crc_w[0],
++ swsusp_header->comp_crc32.crc_w[1],
++ swsusp_header->comp_crc32.crc_w[2],
++ swsusp_header->comp_crc32.crc_w[3],
++ calc_crc.crc_w[0], calc_crc.crc_w[1],
++ calc_crc.crc_w[2], calc_crc.crc_w[3]);
++ CALL_HOOK(err_hook, SWSUSPMEM_BROKENIMAGE);
++ return 0;
++ }
++
++ /* Overwrite header if necessary */
++#ifndef SWSUSP_KEEP_IMAGE
++ if (memcmp(swsusp_header->sig, swsusp_header->orig_sig, 10)) {
++ memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
++ if (page_write_mem(offset, swsusp_header))
++ printf("Write error resetting header\n");
++ }
++#endif
++
++ if (raw_page_init(swsusp_header->image))
++ goto mem_err;
++ raw_page_start();
++
++ if (image_page_init(!(swsusp_header->flags & SF_NOCOMPRESS_MODE)))
++ goto mem_err;
++ image_page_start();
++
++ swsusp_info = spare_page;
++ if (raw_page_get_next(swsusp_info) <= 0)
++ goto read_err;
++
++#ifdef SWSUSP_DEBUG_INFO
++ PAGEMAP_INFO("swsup_info:\n");
++ PAGEMAP_INFO(" utsname.sysname:%s\n",
++ swsusp_info->uts.sysname);
++ PAGEMAP_INFO(" nodename:%s\n",
++ swsusp_info->uts.nodename);
++ PAGEMAP_INFO(" release:%s\n",
++ swsusp_info->uts.release);
++ PAGEMAP_INFO(" version:%s\n",
++ swsusp_info->uts.version);
++ PAGEMAP_INFO(" machine:%s\n",
++ swsusp_info->uts.machine);
++ PAGEMAP_INFO(" vesion_code:%#08x\n",
++ (unsigned int)swsusp_info->version_code);
++ PAGEMAP_INFO(" num_physpages:%d\n",
++ (unsigned int)swsusp_info->num_physpages);
++ PAGEMAP_INFO(" pages :%d\n",
++ (unsigned int)swsusp_info->pages);
++ PAGEMAP_INFO(" size :%d\n",
++ (unsigned int)swsusp_info->size);
++#endif
++
++ nr_pfn_pages = (swsusp_info->image_pages * 4 + PAGE_SIZE - 1) /
++ PAGE_SIZE;
++ pfn_pages = malloc(nr_pfn_pages * sizeof(u32 *));
++ if (!pfn_pages)
++ goto mem_err;
++ memset(pfn_pages, 0, nr_pfn_pages * sizeof(u32 *));
++
++ /* UBOOT using memory */
++ for (i = max_page; i <= high_page; i++)
++ free_page_mark_used(pg_ub2zero(i));
++
++ printf("Allocating %u bytes (nr_pfn_pages %u)\n",
++ nr_pfn_pages * PAGE_SIZE, nr_pfn_pages);
++
++ for (i = 0; i < nr_pfn_pages; i++) {
++ u32 idx;
++ pfn_pages[i] = malloc(PAGE_SIZE);
++ memset(pfn_pages[i], 0xff, PAGE_SIZE);
++ if (unlikely(!pfn_pages[i]))
++ goto mem_err;
++ if (unlikely(image_page_get_next(pfn_pages[i]) <= 0))
++ goto read_err;
++ for (idx = 0; idx < PAGE_SIZE / sizeof(u32); idx++) {
++ u32 page = pfn_pages[i][idx];
++ if (page == ~0UL) /* End of list of pages */
++ break;
++ free_page_mark_used(pg_ub2zero(page));
++ }
++ }
++
++ printf("Loading image data pages (%lu pages)\n",
++ swsusp_info->image_pages);
++
++ remap_orig_page = pg2addr(free_page_get_next());
++ remap_temp_page = pg2addr(free_page_get_next());
++
++ remap_orig = (u32 **)remap_orig_page;
++ remap_temp = (u32 **)remap_temp_page;
++ remap_idx = 0;
++
++ for (i = 0; i < swsusp_info->image_pages; i++) {
++ u32 page = pfn_pages[i >> 10][i & 0x3ff];
++ if (unlikely(!pfn_is_valid(page))) {
++ printf("Attempt to restore invalid address %llx\n",
++ pg2phys(page));
++ continue;
++ } else if (unlikely(pfn_is_excluded(page))) {
++ printf("Attempt to restore excluded address %llx\n",
++ pg2phys(page));
++ continue;
++ } else if (unlikely(pfn_is_low(page) &&
++ pfn_is_occupied(page))) {
++ remap_orig[remap_idx] = pg2addr(page);
++ page = free_page_get_next();
++ remap_temp[remap_idx] = pg2addr(page);
++ remap_idx++;
++#ifdef PAGEMAP_DEBUG
++ ++total_remap;
++#endif
++ /* If we fill our current page, allocate a new one */
++ if (remap_idx + 1 == PAGE_SIZE / sizeof(u32)) {
++ u32 *next;
++
++ next = pg2addr(free_page_get_next());
++ remap_orig[remap_idx] = next;
++ remap_orig = (u32 **)next;
++
++ next = pg2addr(free_page_get_next());
++ remap_temp[remap_idx] = next;
++ remap_temp = (u32 **)next;
++
++ remap_idx = 0;
++ }
++ }
++ if (image_page_get_next(pg2addr(page)) <= 0)
++ goto read_err;
++ }
++
++ printf("Image loading done.\n");
++ invalidate_icache_all();
++
++ CALL_HOOK(resume_boot, SWSUSPMEM_IMAGEDONE);
++ /* put end markers on the remap list */
++ remap_orig[remap_idx] = (void *) ~0UL;
++ remap_temp[remap_idx] = (void *) ~0UL;
++
++#ifdef PAGEMAP_DEBUG
++ PAGEMAP_INFO("Number of remap pages:%d\n", total_remap);
++ PAGEMAP_INFO("Number of high pages:%d\n", high_page_images);
++ PAGEMAP_INFO("Last read page %d (%08x)\n",
++ _last_read_pages, _last_read_pages * PAGE_SIZE);
++#endif
++ remap_orig = (u32 **)remap_orig_page;
++ remap_temp = (u32 **)remap_temp_page;
++
++ /* Make a copy of swsusp_finish in a free data page */
++ data_page = pg2addr(free_page_get_next());
++ memcpy(data_page, swsusp_finish_p, PAGE_SIZE);
++ swsusp_finish_copy = (void *) data_page;
++
++ /* Setup context for swsusp_finish at the end of the data_page */
++ context = (struct swsusp_finish_context *) (data_page + PAGE_SIZE -
++ sizeof(struct swsusp_finish_context));
++ context->remap_orig_page = remap_orig_page;
++ context->remap_temp_page = remap_temp_page;
++ memcpy((void *)&context->archdata, (void *)swsusp_info->archdata,
++ sizeof(struct swsusp_archdata));
++
++ /* Get a stack pointer for swsusp_finish, growing down from context */
++ stack_addr = (char *) context;
++
++#ifdef CONFIG_NETCONSOLE
++ /*
++ * Stop the ethernet stack if NetConsole could have
++ * left it up
++ */
++ eth_halt();
++#endif
++#ifdef CONFIG_USB_DEVICE
++ udc_disconnect();
++#endif
++ arch_preboot_os();
++ cleanup_before_linux();
++
++ CALL_HOOK(resume_boot, SWSUSPMEM_RESUME);
++ /* Copy the final data from a safe place */
++ call_with_stack(swsusp_finish_copy, context, stack_addr);
++
++ return 0;
++
++mem_err:
++ printf("Not enough memory.\n");
++ CALL_HOOK(err_hook, SWSUSPMEM_ENOMEM);
++ goto err;
++
++read_err:
++ printf("Read error while restoring image.\n");
++
++err:
++ __asm__ volatile (
++ "mov r0, #0\n"
++ "mcr p15, 0, r0, c7, c5, 0 @ invalidate icache\n"
++ "mcr p15, 0, r0, c7, c10, 4 @ DSB\n"
++ "mcr p15, 0, r0, c7, c5, 4 @ ISB\n"
++ : : : "r0", "memory");
++
++ raw_page_exit();
++ image_page_exit();
++ free_page_exit();
++ if (pfn_pages) {
++ for (i = 0; i < nr_pfn_pages; i++)
++ free(pfn_pages[i]);
++ free(pfn_pages);
++ }
++ free(spare_page);
++
++ CALL_HOOK(err_hook, SWSUSPMEM_RESTOREFAIL);
++ return 1;
++}
++
++U_BOOT_CMD(swsuspmem, 2, 2, do_swsuspmem,
++ "Restore SWSUSP hibernation image from memory",
++ "<address>]"
++);
+diff --git a/include/swsuspmem.h b/include/swsuspmem.h
+new file mode 100644
+index 0000000..3b353ea
+--- /dev/null
++++ b/include/swsuspmem.h
+@@ -0,0 +1,24 @@
++#ifndef _SWSUSPMEM_H_
++#define _SWSUSPMEM_H_
++
++enum { SWSUSPMEM_NORM = 0,
++ SWSUSPMEM_NOIMAGE = 0x01,
++ SWSUSPMEM_BROKENIMAGE = 0x02,
++ SWSUSPMEM_ENOMEM = 0x80,
++ SWSUSPMEM_RESTOREFAIL = 0x81,
++};
++
++enum { SWSUSPMEM_IMAGEDONE = 0x01,
++ SWSUSPMEM_RESUME = 0x02
++};
++
++struct swsuspmem_hook {
++ void (*err_hook)(int errcode);
++ void (*resume_boot)(int param);
++};
++
++void set_swsuspmem_hook(struct swsuspmem_hook *hook);
++void arch_preboot_os(void);
++void call_with_stack(void (*fn)(void *),
++ void *userdata, void *stack);
++#endif
+diff --git a/lib/lzo/lzo1x_decompress.c b/lib/lzo/lzo1x_decompress.c
+index e6ff708..ebdf10b 100644
+--- a/lib/lzo/lzo1x_decompress.c
++++ b/lib/lzo/lzo1x_decompress.c
+@@ -68,13 +68,14 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
+ unsigned char *start = dst;
+ const unsigned char *send = src + src_len;
+ u32 slen, dlen;
+- size_t tmp;
++ size_t tmp, remaining;
+ int r;
+
+ src = parse_header(src);
+ if (!src)
+ return LZO_E_ERROR;
+
++ remaining = *dst_len;
+ while (src < send) {
+ /* read uncompressed block size */
+ dlen = get_unaligned_be32(src);
+@@ -93,18 +94,25 @@ int lzop_decompress(const unsigned char *src, size_t src_len,
+ if (slen <= 0 || slen > dlen)
+ return LZO_E_ERROR;
+
++ /* abort if buffer ran out of room */
++ if (dlen > remaining)
++ return LZO_E_OUTPUT_OVERRUN;
++
+ /* decompress */
+ tmp = dlen;
+ r = lzo1x_decompress_safe((u8 *) src, slen, dst, &tmp);
+
+- if (r != LZO_E_OK)
++ if (r != LZO_E_OK) {
++ *dst_len = dst - start;
+ return r;
++ }
+
+ if (dlen != tmp)
+ return LZO_E_ERROR;
+
+ src += slen;
+ dst += dlen;
++ remaining -= dlen;
+ }
+
+ return LZO_E_INPUT_OVERRUN;
+--
+1.8.3.1
+
--- /dev/null
+From 80d5c1269bd16fedce41611e45f25d156425b0c9 Mon Sep 17 00:00:00 2001
+From: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+Date: Fri, 19 May 2017 16:16:18 +0900
+Subject: [PATCH 4/4] Add porter board Hibernation code
+
+Signed-off-by: Yuichi Kusakabe <yuichi.kusakabe@jp.fujitsu.com>
+---
+ arch/arm/cpu/armv7/Makefile | 4 +
+ arch/arm/cpu/armv7/arch_timer.c | 58 ++++++
+ arch/arm/cpu/armv7/cache_v7.c | 14 +-
+ arch/arm/cpu/armv7/rmobile/Makefile | 4 +
+ arch/arm/cpu/armv7/rmobile/arm_arch_timer.c | 61 ++++++
+ arch/arm/cpu/armv7/rmobile/crc32_word4.c | 299 ++++++++++++++++++++++++++++
+ arch/arm/cpu/armv7/rmobile/crc32_word4.h | 23 +++
+ arch/arm/cpu/armv7/rmobile/sh_timer.c | 209 +++++++++++++++++++
+ arch/arm/include/asm/arch-rmobile/rmobile.h | 2 +
+ arch/arm/include/asm/armv7.h | 16 +-
+ arch/arm/include/asm/system.h | 136 ++++++++++++-
+ arch/arm/lib/Makefile | 2 +
+ arch/arm/lib/board.c | 2 +-
+ arch/arm/lib/cache-cp15.c | 123 +++++++++++-
+ arch/arm/lib/call_with_stack.S | 20 ++
+ board/renesas/porter/porter.c | 10 +
+ include/configs/porter.h | 19 +-
+ 17 files changed, 977 insertions(+), 25 deletions(-)
+ create mode 100644 arch/arm/cpu/armv7/arch_timer.c
+ create mode 100644 arch/arm/cpu/armv7/rmobile/arm_arch_timer.c
+ create mode 100644 arch/arm/cpu/armv7/rmobile/crc32_word4.c
+ create mode 100644 arch/arm/cpu/armv7/rmobile/crc32_word4.h
+ create mode 100644 arch/arm/cpu/armv7/rmobile/sh_timer.c
+ create mode 100644 arch/arm/lib/call_with_stack.S
+
+diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
+index 4fdbee4..f68ce5c 100644
+--- a/arch/arm/cpu/armv7/Makefile
++++ b/arch/arm/cpu/armv7/Makefile
+@@ -32,6 +32,10 @@ COBJS += cache_v7.o
+ COBJS += cpu.o
+ COBJS += syslib.o
+
++ifneq ($(CONFIG_SYS_ARCH_TIMER),)
++COBJS += arch_timer.o
++endif
++
+ ifneq ($(CONFIG_AM33XX)$(CONFIG_OMAP44XX)$(CONFIG_OMAP54XX)$(CONFIG_TEGRA20),)
+ SOBJS += lowlevel_init.o
+ endif
+diff --git a/arch/arm/cpu/armv7/arch_timer.c b/arch/arm/cpu/armv7/arch_timer.c
+new file mode 100644
+index 0000000..747b6e9
+--- /dev/null
++++ b/arch/arm/cpu/armv7/arch_timer.c
+@@ -0,0 +1,58 @@
++/*
++ * (C) Copyright 2012-2014
++ * Texas Instruments Incorporated, <www.ti.com>
++ *
++ * SPDX-License-Identifier: GPL-2.0+
++ */
++
++#include <common.h>
++#include <asm/io.h>
++#include <div64.h>
++
++DECLARE_GLOBAL_DATA_PTR;
++
++int timer_init(void)
++{
++ gd->tbl = 0;
++ gd->tbu = 0;
++
++ gd->timer_rate_hz = CONFIG_SYS_HZ_CLOCK / CONFIG_SYS_HZ;
++
++ return 0;
++}
++
++unsigned long long get_ticks(void)
++{
++ ulong nowl, nowu;
++
++ asm volatile("mrrc p15, 0, %0, %1, c14" : "=r" (nowl), "=r" (nowu));
++
++ gd->tbl = nowl;
++ gd->tbu = nowu;
++
++ return (((unsigned long long)gd->tbu) << 32) | gd->tbl;
++}
++
++
++ulong get_timer(ulong base)
++{
++ return lldiv(get_ticks(), gd->timer_rate_hz) - base;
++}
++
++void __udelay(unsigned long usec)
++{
++ unsigned long long endtime;
++
++ endtime = lldiv((unsigned long long)usec * gd->timer_rate_hz,
++ 1000UL);
++
++ endtime += get_ticks();
++
++ while (get_ticks() < endtime)
++ ;
++}
++
++ulong get_tbclk(void)
++{
++ return gd->timer_rate_hz;
++}
+diff --git a/arch/arm/cpu/armv7/cache_v7.c b/arch/arm/cpu/armv7/cache_v7.c
+index 5f6d039..5a0bdb8 100644
+--- a/arch/arm/cpu/armv7/cache_v7.c
++++ b/arch/arm/cpu/armv7/cache_v7.c
+@@ -82,7 +82,7 @@ static void v7_inval_dcache_level_setway(u32 level, u32 num_sets,
+ }
+ }
+ /* DSB to make sure the operation is complete */
+- CP15DSB;
++ DSB();
+ }
+
+ static void v7_clean_inval_dcache_level_setway(u32 level, u32 num_sets,
+@@ -109,7 +109,7 @@ static void v7_clean_inval_dcache_level_setway(u32 level, u32 num_sets,
+ }
+ }
+ /* DSB to make sure the operation is complete */
+- CP15DSB;
++ DSB();
+ }
+
+ static void v7_maint_dcache_level_setway(u32 level, u32 operation)
+@@ -230,7 +230,7 @@ static void v7_dcache_maint_range(u32 start, u32 stop, u32 range_op)
+ }
+
+ /* DSB to make sure the operation is complete */
+- CP15DSB;
++ DSB();
+ }
+
+ /* Invalidate TLB */
+@@ -243,9 +243,9 @@ static void v7_inval_tlb(void)
+ /* Invalidate entire instruction TLB */
+ asm volatile ("mcr p15, 0, %0, c8, c5, 0" : : "r" (0));
+ /* Full system DSB - make sure that the invalidation is complete */
+- CP15DSB;
++ DSB();
+ /* Full system ISB - make sure the instruction stream sees it */
+- CP15ISB;
++ ISB();
+ }
+
+ void invalidate_dcache_all(void)
+@@ -356,10 +356,10 @@ void invalidate_icache_all(void)
+ asm volatile ("mcr p15, 0, %0, c7, c5, 6" : : "r" (0));
+
+ /* Full system DSB - make sure that the invalidation is complete */
+- CP15DSB;
++ DSB();
+
+ /* ISB - make sure the instruction stream sees it */
+- CP15ISB;
++ ISB();
+ }
+ #else
+ void invalidate_icache_all(void)
+diff --git a/arch/arm/cpu/armv7/rmobile/Makefile b/arch/arm/cpu/armv7/rmobile/Makefile
+index b8c04c6..0a3623a 100644
+--- a/arch/arm/cpu/armv7/rmobile/Makefile
++++ b/arch/arm/cpu/armv7/rmobile/Makefile
+@@ -46,6 +46,10 @@ COBJS-$(CONFIG_R8A7740) += pfc-r8a7740.o
+ COBJS-$(CONFIG_SH73A0) += cpu_info-sh73a0.o
+ COBJS-$(CONFIG_SH73A0) += pfc-sh73a0.o
+ COBJS_LN-$(CONFIG_TMU_TIMER) += sh_timer.o
++COBJS-$(CONFIG_SYS_ARCH_TIMER) += arm_arch_timer.o
++ifeq ($(CONFIG_CMD_SWSUSP),y)
++COBJS-y += crc32_word4.o
++endif
+
+ COBJS := $(COBJS-y)
+ SRCS := $(SOBJS:.o=.S) $(COBJS:.o=.c)
+diff --git a/arch/arm/cpu/armv7/rmobile/arm_arch_timer.c b/arch/arm/cpu/armv7/rmobile/arm_arch_timer.c
+new file mode 100644
+index 0000000..a499e71
+--- /dev/null
++++ b/arch/arm/cpu/armv7/rmobile/arm_arch_timer.c
+@@ -0,0 +1,61 @@
++/*
++ * Copyright (C) 2014 Cogent Embedded Inc.
++ *
++ * Licensed under the GPL-2 or later.
++ */
++
++#include <common.h>
++#include <asm/io.h>
++
++#define MODEMR 0xe6160060
++#define MD(x) (1 << (x))
++#define CNTCR 0xe6080000
++#define CNTFID0 0xe6080020
++
++void arm_arch_timer_init(void)
++{
++ u32 mode = readl(MODEMR);
++ u32 freq;
++
++ switch (mode & (MD(14) | MD(13))) {
++ case MD(13):
++ freq = 20;
++ break;
++ case MD(14):
++ freq = 26;
++ break;
++ case MD(13) | MD(14):
++ freq = 30;
++ break;
++ default:
++ freq = 15;
++ break;
++ }
++
++ freq *= (1000000 / 2);
++
++#ifdef CONFIG_VE_ENABLED
++ /* CNTVOFF has to be initialized either from non-secure Hypervisor
++ * mode or secure Monitor mode with SCR.NS==1. If TrustZone is enabled
++ * then it should be handled by the secure code
++ */
++ asm volatile(
++ " cps 0x16\n"
++ " mrc p15, 0, r1, c1, c1, 0\n"
++ " orr r0, r1, #1\n"
++ " mcr p15, 0, r0, c1, c1, 0\n"
++ " isb\n"
++ " mov r0, #0\n"
++ " mcrr p15, 4, r0, r0, c14\n"
++ " isb\n"
++ " mcr p15, 0, r1, c1, c1, 0\n"
++ " isb\n"
++ " cps 0x13\n"
++ : : : "r0", "r1");
++#endif
++
++ /* Start Generic ARM timer */
++ writel(freq, CNTFID0);
++ asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (freq));
++ writel(1, CNTCR);
++}
+diff --git a/arch/arm/cpu/armv7/rmobile/crc32_word4.c b/arch/arm/cpu/armv7/rmobile/crc32_word4.c
+new file mode 100644
+index 0000000..b813899
+--- /dev/null
++++ b/arch/arm/cpu/armv7/rmobile/crc32_word4.c
+@@ -0,0 +1,299 @@
++/*************************************************************************
++ * crc32_word4.c: rapid CRC32
++ * Coptright (C) FUJITSUTEN Limited, 2015 All Rights Reserved.
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2
++ * as published by the Free Software Foundation.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ *
++ * You should have received a copy of the GNU General Public License
++ * along with this program; if not, write to the Free Software Foundation,
++ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
++ *************************************************************************/
++#ifdef OWNTEST
++#include <stdio.h>
++#include <stdlib.h>
++#include <string.h>
++#include <asm/types.h>
++typedef unsigned int u_int32_t;
++#else
++#include <common.h>
++#endif
++
++#include "crc32_word4.h"
++
++#define CRC_INIT_VALUE (-1)
++#define CRC_FIX(_crc32) (~(_crc32))
++
++#define __HWDTPLS_OUT()
++#define MEASURE(msg)
++
++/**** calc_crc32.c *****/
++
++/*
++ * CRC32は、ISO 3309 で規程され
++ * そのサンプルは
++ * RFC 2083 :PNG(Poratble Network Graphics
++ * で公になっています。本プログラムは、RFC2083 で掲示された
++ * CRC32を独自に最適化したプログラムです。
++ */
++const static u_int32_t CRC_Table[256] = {
++ 0x00000000 , 0x77073096 , 0xee0e612c , 0x990951ba , 0x076dc419 , 0x706af48f , 0xe963a535 , 0x9e6495a3 ,
++ 0x0edb8832 , 0x79dcb8a4 , 0xe0d5e91e , 0x97d2d988 , 0x09b64c2b , 0x7eb17cbd , 0xe7b82d07 , 0x90bf1d91 ,
++ 0x1db71064 , 0x6ab020f2 , 0xf3b97148 , 0x84be41de , 0x1adad47d , 0x6ddde4eb , 0xf4d4b551 , 0x83d385c7 ,
++ 0x136c9856 , 0x646ba8c0 , 0xfd62f97a , 0x8a65c9ec , 0x14015c4f , 0x63066cd9 , 0xfa0f3d63 , 0x8d080df5 ,
++ 0x3b6e20c8 , 0x4c69105e , 0xd56041e4 , 0xa2677172 , 0x3c03e4d1 , 0x4b04d447 , 0xd20d85fd , 0xa50ab56b ,
++ 0x35b5a8fa , 0x42b2986c , 0xdbbbc9d6 , 0xacbcf940 , 0x32d86ce3 , 0x45df5c75 , 0xdcd60dcf , 0xabd13d59 ,
++ 0x26d930ac , 0x51de003a , 0xc8d75180 , 0xbfd06116 , 0x21b4f4b5 , 0x56b3c423 , 0xcfba9599 , 0xb8bda50f ,
++ 0x2802b89e , 0x5f058808 , 0xc60cd9b2 , 0xb10be924 , 0x2f6f7c87 , 0x58684c11 , 0xc1611dab , 0xb6662d3d ,
++ 0x76dc4190 , 0x01db7106 , 0x98d220bc , 0xefd5102a , 0x71b18589 , 0x06b6b51f , 0x9fbfe4a5 , 0xe8b8d433 ,
++ 0x7807c9a2 , 0x0f00f934 , 0x9609a88e , 0xe10e9818 , 0x7f6a0dbb , 0x086d3d2d , 0x91646c97 , 0xe6635c01 ,
++ 0x6b6b51f4 , 0x1c6c6162 , 0x856530d8 , 0xf262004e , 0x6c0695ed , 0x1b01a57b , 0x8208f4c1 , 0xf50fc457 ,
++ 0x65b0d9c6 , 0x12b7e950 , 0x8bbeb8ea , 0xfcb9887c , 0x62dd1ddf , 0x15da2d49 , 0x8cd37cf3 , 0xfbd44c65 ,
++ 0x4db26158 , 0x3ab551ce , 0xa3bc0074 , 0xd4bb30e2 , 0x4adfa541 , 0x3dd895d7 , 0xa4d1c46d , 0xd3d6f4fb ,
++ 0x4369e96a , 0x346ed9fc , 0xad678846 , 0xda60b8d0 , 0x44042d73 , 0x33031de5 , 0xaa0a4c5f , 0xdd0d7cc9 ,
++ 0x5005713c , 0x270241aa , 0xbe0b1010 , 0xc90c2086 , 0x5768b525 , 0x206f85b3 , 0xb966d409 , 0xce61e49f ,
++ 0x5edef90e , 0x29d9c998 , 0xb0d09822 , 0xc7d7a8b4 , 0x59b33d17 , 0x2eb40d81 , 0xb7bd5c3b , 0xc0ba6cad ,
++ 0xedb88320 , 0x9abfb3b6 , 0x03b6e20c , 0x74b1d29a , 0xead54739 , 0x9dd277af , 0x04db2615 , 0x73dc1683 ,
++ 0xe3630b12 , 0x94643b84 , 0x0d6d6a3e , 0x7a6a5aa8 , 0xe40ecf0b , 0x9309ff9d , 0x0a00ae27 , 0x7d079eb1 ,
++ 0xf00f9344 , 0x8708a3d2 , 0x1e01f268 , 0x6906c2fe , 0xf762575d , 0x806567cb , 0x196c3671 , 0x6e6b06e7 ,
++ 0xfed41b76 , 0x89d32be0 , 0x10da7a5a , 0x67dd4acc , 0xf9b9df6f , 0x8ebeeff9 , 0x17b7be43 , 0x60b08ed5 ,
++ 0xd6d6a3e8 , 0xa1d1937e , 0x38d8c2c4 , 0x4fdff252 , 0xd1bb67f1 , 0xa6bc5767 , 0x3fb506dd , 0x48b2364b ,
++ 0xd80d2bda , 0xaf0a1b4c , 0x36034af6 , 0x41047a60 , 0xdf60efc3 , 0xa867df55 , 0x316e8eef , 0x4669be79 ,
++ 0xcb61b38c , 0xbc66831a , 0x256fd2a0 , 0x5268e236 , 0xcc0c7795 , 0xbb0b4703 , 0x220216b9 , 0x5505262f ,
++ 0xc5ba3bbe , 0xb2bd0b28 , 0x2bb45a92 , 0x5cb36a04 , 0xc2d7ffa7 , 0xb5d0cf31 , 0x2cd99e8b , 0x5bdeae1d ,
++ 0x9b64c2b0 , 0xec63f226 , 0x756aa39c , 0x026d930a , 0x9c0906a9 , 0xeb0e363f , 0x72076785 , 0x05005713 ,
++ 0x95bf4a82 , 0xe2b87a14 , 0x7bb12bae , 0x0cb61b38 , 0x92d28e9b , 0xe5d5be0d , 0x7cdcefb7 , 0x0bdbdf21 ,
++ 0x86d3d2d4 , 0xf1d4e242 , 0x68ddb3f8 , 0x1fda836e , 0x81be16cd , 0xf6b9265b , 0x6fb077e1 , 0x18b74777 ,
++ 0x88085ae6 , 0xff0f6a70 , 0x66063bca , 0x11010b5c , 0x8f659eff , 0xf862ae69 , 0x616bffd3 , 0x166ccf45 ,
++ 0xa00ae278 , 0xd70dd2ee , 0x4e048354 , 0x3903b3c2 , 0xa7672661 , 0xd06016f7 , 0x4969474d , 0x3e6e77db ,
++ 0xaed16a4a , 0xd9d65adc , 0x40df0b66 , 0x37d83bf0 , 0xa9bcae53 , 0xdebb9ec5 , 0x47b2cf7f , 0x30b5ffe9 ,
++ 0xbdbdf21c , 0xcabac28a , 0x53b39330 , 0x24b4a3a6 , 0xbad03605 , 0xcdd70693 , 0x54de5729 , 0x23d967bf ,
++ 0xb3667a2e , 0xc4614ab8 , 0x5d681b02 , 0x2a6f2b94 , 0xb40bbe37 , 0xc30c8ea1 , 0x5a05df1b , 0x2d02ef8d ,
++};
++
++/***
++ * CRC Table creater.
++ *
++void make_crc_table(void) {
++ u_int32_t c;
++ u_int32_t n, k;
++ for (n = 0; n < 256; n++)
++ {
++ c = (u_int32_t) n;
++ for (k = 0; k < 8; k++)
++ {
++ if (c & 1)
++ c = 0xedb88320L ^ (c >> 1);
++ else
++ c = c >> 1;
++ }
++ CRC_Table[n] = c;
++ }
++}
++***/
++#define NEXT_PTR (4)
++
++static __inline__
++u_int32_t _update_crc(u_int32_t crc, unsigned char *buf, size_t len)
++{
++ u_int32_t c = crc;
++ size_t n;
++ for (n = 0; n < len; n++)
++ c = CRC_Table[(c ^ buf[n]) & 0xff] ^ (c >> 8);
++ return c;
++}
++/*********************************************************************
++ * update_crc4x4()()
++ * calc_crc32() をベースに、4 ワード毎に個別に CRC32 を計算する方法
++ *
++ * +0 +1 +2 +3
++ * +0x00 AAAAAAAA BBBBBBBB CCCCCCCC DDDDDDDD
++ * +0x04 EEEEEEEE FFFFFFFF 00000000 11111111
++ * : : : :
++ * CRC32 xxxxxxxx xxxxxxxx xxxxxxxx xxxxxxxx
++ *
++ *********************************************************************/
++
++static __inline__
++void update_crc4x4(u_int32_t crc[4], unsigned char *buf)
++{
++ u_int32_t c1, c2, c3, c4;
++ u_int32_t *p = (void *)buf;
++
++ c1 = crc[0] ^ p[0];
++ c2 = crc[1] ^ p[1];
++ c3 = crc[2] ^ p[2];
++ c4 = crc[3] ^ p[3];
++
++ c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
++ c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
++ c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
++ c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
++
++ c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
++ c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
++ c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
++ c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
++
++ c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
++ c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
++ c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
++ c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
++
++ c1 = CRC_Table[c1 & 0xff] ^ (c1 >> 8);
++ c2 = CRC_Table[c2 & 0xff] ^ (c2 >> 8);
++ c3 = CRC_Table[c3 & 0xff] ^ (c3 >> 8);
++ c4 = CRC_Table[c4 & 0xff] ^ (c4 >> 8);
++
++ crc[0] = c1;
++ crc[1] = c2;
++ crc[2] = c3;
++ crc[3] = c4;
++}
++
++
++void calc_crc32x4(unsigned char *buf, size_t len, CRC32_WORD4_t *result)
++{
++ unsigned int crc_tmp[4] = {CRC_INIT_VALUE, CRC_INIT_VALUE, CRC_INIT_VALUE, CRC_INIT_VALUE};
++ u_int32_t i;
++ int res;
++ u_int32_t n4;
++ int xlen = len;
++#ifdef HWDPLS_ENABLE
++ unsigned long plstout = 60;
++ unsigned long plsstart = 0;
++ if ((unsigned long)CONFIG_SYS_HZ > 100000)
++ plstout *= (unsigned long)CONFIG_SYS_HZ / 1000;
++ else
++ plstout = DIV_ROUND_UP(plstout * (unsigned long)CONFIG_SYS_HZ, 1000);
++#endif
++
++ /**
++ * 4バイト境界に合わない開始アドレスの場合
++ * 境界までのCRCを crc_tmp[0] に求める。
++ */
++ if ((unsigned long)buf & 3) {
++ crc_tmp[0] = _update_crc(crc_tmp[0], buf, (unsigned long)buf & 3);
++ buf = (unsigned char *)((unsigned long)buf & ~3);
++ xlen -= (unsigned long)buf & 3;
++ }
++
++ n4 = xlen/(NEXT_PTR*4);
++ /**
++ * 4バイト境界に合わない開始アドレスの場合
++ * 境界までのCRCを crc_tmp[0] に求める。
++ */
++#ifdef HWDPLS_ENABLE
++ reset_timer();
++ plsstart = get_timer(0);
++#endif
++ for (i = 0; i < n4 ; i++) {
++ update_crc4x4(crc_tmp, buf);
++ buf += NEXT_PTR * 4;
++#ifdef HWDPLS_ENABLE
++ /**
++ * WDを考慮
++ */
++ if (__builtin_expect((int)((i & 0x1f) == 0), 0)) {
++ if ((get_timer(plsstart)) > plstout) {
++ __HWDTPLS_OUT();
++ MEASURE("crc plsout")
++ plsstart += plstout;
++ }
++ }
++#endif /*HWPLS_ENABLE*/
++ }
++
++ res = xlen % (NEXT_PTR * 4);
++ if (res > 0)
++ crc_tmp[3] = _update_crc(crc_tmp[3], buf, res);
++
++ result->crc_w[0] = CRC_FIX(crc_tmp[0]);
++ result->crc_w[1] = CRC_FIX(crc_tmp[1]);
++ result->crc_w[2] = CRC_FIX(crc_tmp[2]);
++ result->crc_w[3] = CRC_FIX(crc_tmp[3]);
++
++ MEASURE("calc_crc32x4 finish")
++}
++
++#if defined(OWNTEST)
++#define BUFSIZE (2 * 1024 * 1024)
++#include <sys/time.h>
++#include <malloc.h>
++
++int main()
++{
++ unsigned char *buf, *buf2;
++ struct timeval start, end;
++ unsigned long long diff;
++ int i;
++
++ CRC32_WORD4_t result = { .crc_w = {0, 0, 0, 0 } };
++ CRC32_WORD4_t result2 = { .crc_w = {0, 0, 0, 0 } };
++
++ buf = malloc(BUFSIZE);
++ if (!buf) {
++ perror("malloc");
++ return 1;
++ }
++ printf("Generate %dMB random data..\n", BUFSIZE / 1024 / 1024);
++ srand(0);
++ for (i = 0; i < BUFSIZE / 4; i++)
++ ((int *)buf)[i] = rand();
++
++ /* Memory dup */
++ buf2 = memalign(NEXT_PTR, BUFSIZE);
++ if (!buf2) {
++ perror("malloc");
++ return 1;
++ }
++ memcpy(buf2, buf, BUFSIZE);
++
++
++ gettimeofday(&start, NULL);
++ calc_crc32x4(buf, BUFSIZE, &result);
++ gettimeofday(&end, NULL);
++
++ diff = (end.tv_sec - start.tv_sec) * 1000000;
++ diff += end.tv_usec - start.tv_usec;
++
++ printf("time=%lluus\n", diff);
++ printf(" result.word[0] = %x\n", result.crc_w[0]);
++ printf(" result.word[1] = %x\n", result.crc_w[1]);
++ printf(" result.word[2] = %x\n", result.crc_w[2]);
++ printf(" result.word[3] = %x\n", result.crc_w[3]);
++
++ /* Broken test */
++#if 0 /* Destory test */
++ buf[rand() % BUFSIZE] ^= 1 << (rand()%7);
++#endif
++ for (i = 0; i < BUFSIZE ; i++) {
++ if (buf[i] != buf2[i])
++ printf("buf[%d] %02x : %02x\n", i, buf[i], buf2[i]);
++ }
++
++ gettimeofday(&start, NULL);
++ calc_crc32x4(buf, BUFSIZE, &result2);
++ gettimeofday(&end, NULL);
++
++ diff = (end.tv_sec - start.tv_sec) * 1000000;
++ diff += end.tv_usec - start.tv_usec;
++
++ printf("time=%lluus\n", diff);
++ printf(" result.word[0] = %x:%s\n", result2.crc_w[0] ,
++ result.crc_w[0] == result2.crc_w[0] ? "OK" : "NG");
++ printf(" result.word[1] = %x:%s\n", result2.crc_w[1] ,
++ result.crc_w[1] == result2.crc_w[1] ? "OK" : "NG");
++ printf(" result.word[2] = %x:%s\n", result2.crc_w[2] ,
++ result.crc_w[2] == result2.crc_w[2] ? "OK" : "NG");
++ printf(" result.word[3] = %x:%s\n", result2.crc_w[3] ,
++ result.crc_w[3] == result2.crc_w[3] ? "OK" : "NG");
++ return 0;
++}
++#endif /* TEST */
+diff --git a/arch/arm/cpu/armv7/rmobile/crc32_word4.h b/arch/arm/cpu/armv7/rmobile/crc32_word4.h
+new file mode 100644
+index 0000000..2b64218
+--- /dev/null
++++ b/arch/arm/cpu/armv7/rmobile/crc32_word4.h
+@@ -0,0 +1,23 @@
++/*************************************************************************
++ * Coptright (C) FUJITSUTEN Limited, 2012 All Rights Reserved.
++ *
++ *************************************************************************/
++#ifndef __CRC32_WORD4_H__
++#define __CRC32_WORD4_H__
++
++typedef struct {
++ unsigned int crc_w[4];
++} CRC32_WORD4_t;
++
++void calc_crc32x4(unsigned char *buf, size_t len, CRC32_WORD4_t *result);
++
++typedef struct {
++ unsigned int size;
++ CRC32_WORD4_t chksum;
++ unsigned int dummy[3];
++} CRC32_WORD4_TICKET_t;
++
++#define IS_CRC_WORD4_OK(_res1, _res2) (!memcmp((_res1), (_res2), sizeof(CRC32_WORD4_t)))
++#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))
++#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))
++#endif
+diff --git a/arch/arm/cpu/armv7/rmobile/sh_timer.c b/arch/arm/cpu/armv7/rmobile/sh_timer.c
+new file mode 100644
+index 0000000..1c64950
+--- /dev/null
++++ b/arch/arm/cpu/armv7/rmobile/sh_timer.c
+@@ -0,0 +1,209 @@
++/*
++ * Copyright (C) 2013-2014 Renesas Electronics Corporation
++ *
++ * (C) Copyright 2009
++ * Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
++ *
++ * (C) Copyright 2007-2012
++ * Nobobuhiro Iwamatsu <iwamatsu@nigauri.org>
++ *
++ * (C) Copyright 2003
++ * Wolfgang Denk, DENX Software Engineering, wd@denx.de.
++ *
++ * See file CREDITS for list of people who contributed to this
++ * project.
++ *
++ * This program is free software; you can redistribute it and/or
++ * modify it under the terms of the GNU General Public License as
++ * published by the Free Software Foundation; either version 2 of
++ * the License, or (at your option) any later version.
++ *
++ * This program is distributed in the hope that it will be useful,
++ * but WITHOUT ANY WARRANTY; without even the implied warranty of
++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
++ * GNU General Public License for more details.
++ *
++ * You should have received a copy of the GNU General Public License
++ * along with this program; if not, write to the Free Software
++ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
++ * MA 02111-1307 USA
++ */
++
++#include <common.h>
++#include <div64.h>
++#include <asm/processor.h>
++#include <asm/io.h>
++#include <sh_tmu.h>
++
++#if defined(CONFIG_R8A7790) || defined(CONFIG_R8A7791) || \
++ defined(CONFIG_R8A7793) || defined(CONFIG_R8A7794)
++
++DECLARE_GLOBAL_DATA_PTR;
++
++#endif
++
++static struct tmu_regs *tmu = (struct tmu_regs *)TMU_BASE;
++
++static u16 bit;
++static unsigned long last_tcnt;
++static unsigned long long overflow_ticks;
++
++#if defined(CONFIG_R8A7790) || defined(CONFIG_R8A7791) || \
++ defined(CONFIG_R8A7793) || defined(CONFIG_R8A7794)
++
++unsigned long get_tbclk(void)
++{
++ if (gd->flags & GD_FLG_RELOC)
++ return get_tmu0_clk_rate() >> ((bit + 1) * 2);
++ else {
++ u16 bit;
++
++ bit = (ffs(CONFIG_SYS_TMU_CLK_DIV) >> 1) - 1;
++ return get_tmu0_clk_rate() >> ((bit + 1) * 2);
++ }
++}
++
++#else
++
++unsigned long get_tbclk(void)
++{
++ return get_tmu0_clk_rate() >> ((bit + 1) * 2);
++}
++
++#endif
++
++static inline unsigned long long tick_to_time(unsigned long long tick)
++{
++ tick *= CONFIG_SYS_HZ;
++ do_div(tick, get_tbclk());
++
++ return tick;
++}
++
++static inline unsigned long long usec_to_tick(unsigned long long usec)
++{
++ usec *= get_tbclk();
++ do_div(usec, 1000000);
++
++ return usec;
++}
++
++static void tmu_timer_start(unsigned int timer)
++{
++ if (timer > 2)
++ return;
++ writeb(readb(&tmu->tstr) | (1 << timer), &tmu->tstr);
++}
++
++static void tmu_timer_stop(unsigned int timer)
++{
++ if (timer > 2)
++ return;
++ writeb(readb(&tmu->tstr) & ~(1 << timer), &tmu->tstr);
++}
++
++#if defined(CONFIG_R8A7790) || defined(CONFIG_R8A7791) || \
++ defined(CONFIG_R8A7793) || defined(CONFIG_R8A7794)
++
++int sh_timer_init(void)
++{
++ bit = (ffs(CONFIG_SYS_TMU_CLK_DIV) >> 1) - 1;
++ writew((readw(&tmu->tcr0) & ~0x7) | bit, &tmu->tcr0);
++
++ tmu_timer_stop(0);
++ tmu_timer_start(0);
++
++ last_tcnt = 0;
++ overflow_ticks = 0;
++
++ return 0;
++}
++
++int timer_init(void)
++{
++ tmu_timer_stop(0);
++ tmu_timer_start(0);
++
++ return 0;
++}
++
++#else
++
++int timer_init(void)
++{
++ bit = (ffs(CONFIG_SYS_TMU_CLK_DIV) >> 1) - 1;
++ writew((readw(&tmu->tcr0) & ~0x7) | bit, &tmu->tcr0);
++
++ tmu_timer_stop(0);
++ tmu_timer_start(0);
++
++ last_tcnt = 0;
++ overflow_ticks = 0;
++
++ return 0;
++}
++
++#endif
++
++#if defined(CONFIG_R8A7790) || defined(CONFIG_R8A7791) || \
++ defined(CONFIG_R8A7793) || defined(CONFIG_R8A7794)
++
++unsigned long long get_ticks(void)
++{
++ unsigned long tcnt = 0 - readl(&tmu->tcnt0);
++
++ if (gd->flags & GD_FLG_RELOC) {
++ if (last_tcnt > tcnt) /* overflow */
++ overflow_ticks++;
++ last_tcnt = tcnt;
++
++ return (overflow_ticks << 32) | tcnt;
++ }
++ else
++ return tcnt;
++}
++
++#else
++
++unsigned long long get_ticks(void)
++{
++ unsigned long tcnt = 0 - readl(&tmu->tcnt0);
++
++ if (last_tcnt > tcnt) /* overflow */
++ overflow_ticks++;
++ last_tcnt = tcnt;
++
++ return (overflow_ticks << 32) | tcnt;
++}
++
++#endif
++
++void __udelay(unsigned long usec)
++{
++ unsigned long long tmp;
++ ulong tmo;
++
++ tmo = usec_to_tick(usec);
++ tmp = get_ticks() + tmo; /* get current timestamp */
++
++ while (get_ticks() < tmp) /* loop till event */
++ /*NOP*/;
++}
++
++unsigned long get_timer(unsigned long base)
++{
++ /* return msec */
++ return tick_to_time(get_ticks()) - base;
++}
++
++void set_timer(unsigned long t)
++{
++ writel((0 - t), &tmu->tcnt0);
++}
++
++void reset_timer(void)
++{
++ tmu_timer_stop(0);
++ set_timer(0);
++ tmu_timer_start(0);
++}
+diff --git a/arch/arm/include/asm/arch-rmobile/rmobile.h b/arch/arm/include/asm/arch-rmobile/rmobile.h
+index 33a302e..12276e0 100644
+--- a/arch/arm/include/asm/arch-rmobile/rmobile.h
++++ b/arch/arm/include/asm/arch-rmobile/rmobile.h
+@@ -26,6 +26,8 @@ u32 rmobile_get_cpu_type(void);
+ u32 rmobile_get_cpu_rev_integer(void);
+ u32 rmobile_get_cpu_rev_fraction(void);
+
++void arm_arch_timer_init(void);
++
+ #endif /* __ASSEMBLY__ */
+
+ #endif /* __KERNEL__ */
+diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
+index ad9a875..aad5bf7 100644
+--- a/arch/arm/include/asm/armv7.h
++++ b/arch/arm/include/asm/armv7.h
+@@ -62,9 +62,19 @@
+ * However, we use the CP15 based instructtions because we use
+ * -march=armv5 in U-Boot
+ */
+-#define CP15ISB asm volatile ("mcr p15, 0, %0, c7, c5, 4" : : "r" (0))
+-#define CP15DSB asm volatile ("mcr p15, 0, %0, c7, c10, 4" : : "r" (0))
+-#define CP15DMB asm volatile ("mcr p15, 0, %0, c7, c10, 5" : : "r" (0))
++#define CP15ISB() asm volatile ("mcr p15, 0, %0, c7, c5, 4" : : "r" (0))
++#define CP15DSB() asm volatile ("mcr p15, 0, %0, c7, c10, 4" : : "r" (0))
++#define CP15DMB() asm volatile ("mcr p15, 0, %0, c7, c10, 5" : : "r" (0))
++
++#ifdef __ARM_ARCH_7A__
++#define ISB() asm volatile ("isb" : : : "memory")
++#define DSB() asm volatile ("dsb" : : : "memory")
++#define DMB() asm volatile ("dmb" : : : "memory")
++#else
++#define ISB() CP15ISB
++#define DSB() CP15DSB
++#define DMB() CP15DMB
++#endif
+
+ void v7_outer_cache_enable(void);
+ void v7_outer_cache_disable(void);
+diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
+index cd0de6b..3906646 100644
+--- a/arch/arm/include/asm/system.h
++++ b/arch/arm/include/asm/system.h
+@@ -45,6 +45,12 @@
+ #define CR_AFE (1 << 29) /* Access flag enable */
+ #define CR_TE (1 << 30) /* Thumb exception enable */
+
++#if defined(CONFIG_ARMV7_LPAE) && !defined(PGTABLE_SIZE)
++#define PGTABLE_SIZE (4096 * 5)
++#elif !defined(PGTABLE_SIZE)
++#define PGTABLE_SIZE (4096 * 4)
++#endif
++
+ /*
+ * This is used to ensure the compiler did actually allocate the register we
+ * asked it for some inline assembly sequences. Apparently we can't trust
+@@ -61,17 +67,50 @@
+
+ #define nop() __asm__ __volatile__("mov\tr0,r0\t@ nop\n\t");
+
++static inline unsigned long get_cpsr(void)
++{
++ unsigned long cpsr;
++
++ asm volatile("mrs %0, cpsr" : "=r"(cpsr): );
++ return cpsr;
++}
++
++static inline int is_hyp(void)
++{
++#ifdef CONFIG_ARMV7_LPAE
++ /* HYP mode requires LPAE ... */
++ return ((get_cpsr() & 0x1f) == 0x1a);
++#else
++ /* ... so without LPAE support we can optimize all hyp code away */
++ return 0;
++#endif
++}
++
+ static inline unsigned int get_cr(void)
+ {
+ unsigned int val;
+- asm("mrc p15, 0, %0, c1, c0, 0 @ get CR" : "=r" (val) : : "cc");
++
++ if (is_hyp())
++ asm volatile("mrc p15, 4, %0, c1, c0, 0 @ get CR" : "=r" (val)
++ :
++ : "cc");
++ else
++ asm volatile("mrc p15, 0, %0, c1, c0, 0 @ get CR" : "=r" (val)
++ :
++ : "cc");
+ return val;
+ }
+
+ static inline void set_cr(unsigned int val)
+ {
+- asm volatile("mcr p15, 0, %0, c1, c0, 0 @ set CR"
+- : : "r" (val) : "cc");
++ if (is_hyp())
++ asm volatile("mcr p15, 4, %0, c1, c0, 0 @ set CR" :
++ : "r" (val)
++ : "cc");
++ else
++ asm volatile("mcr p15, 0, %0, c1, c0, 0 @ set CR" :
++ : "r" (val)
++ : "cc");
+ isb();
+ }
+
+@@ -105,19 +144,108 @@ static inline void set_actlr(unsigned int val)
+ isb();
+ }
+
++#ifdef CONFIG_ARMV7_LPAE
++/* Long-Descriptor Translation Table Level 1/2 Bits */
++#define TTB_SECT_XN_MASK (1ULL << 54)
++#define TTB_SECT_NG_MASK (1 << 11)
++#define TTB_SECT_AF (1 << 10)
++#define TTB_SECT_SH_MASK (3 << 8)
++#define TTB_SECT_NS_MASK (1 << 5)
++#define TTB_SECT_AP (1 << 6)
++/* Note: TTB AP bits are set elsewhere */
++#define TTB_SECT_MAIR(x) ((x & 0x7) << 2) /* Index into MAIR */
++#define TTB_SECT (1 << 0)
++#define TTB_PAGETABLE (3 << 0)
++
++/* TTBCR flags */
++#define TTBCR_EAE (1 << 31)
++#define TTBCR_T0SZ(x) ((x) << 0)
++#define TTBCR_T1SZ(x) ((x) << 16)
++#define TTBCR_USING_TTBR0 (TTBCR_T0SZ(0) | TTBCR_T1SZ(0))
++#define TTBCR_IRGN0_NC (0 << 8)
++#define TTBCR_IRGN0_WBWA (1 << 8)
++#define TTBCR_IRGN0_WT (2 << 8)
++#define TTBCR_IRGN0_WBNWA (3 << 8)
++#define TTBCR_IRGN0_MASK (3 << 8)
++#define TTBCR_ORGN0_NC (0 << 10)
++#define TTBCR_ORGN0_WBWA (1 << 10)
++#define TTBCR_ORGN0_WT (2 << 10)
++#define TTBCR_ORGN0_WBNWA (3 << 10)
++#define TTBCR_ORGN0_MASK (3 << 10)
++#define TTBCR_SHARED_NON (0 << 12)
++#define TTBCR_SHARED_OUTER (2 << 12)
++#define TTBCR_SHARED_INNER (3 << 12)
++#define TTBCR_EPD0 (0 << 7)
++
++/*
++ * Memory types
++ */
++#define MEMORY_ATTRIBUTES ((0x00 << (0 * 8)) | (0x88 << (1 * 8)) | \
++ (0xcc << (2 * 8)) | (0xff << (3 * 8)))
++
++/* options available for data cache on each page */
++enum dcache_option {
++ DCACHE_OFF = TTB_SECT | TTB_SECT_MAIR(0),
++ DCACHE_WRITETHROUGH = TTB_SECT | TTB_SECT_MAIR(1),
++ DCACHE_WRITEBACK = TTB_SECT | TTB_SECT_MAIR(2),
++ DCACHE_WRITEALLOC = TTB_SECT | TTB_SECT_MAIR(3),
++};
++#elif defined(CONFIG_ARMV7)
++/* Short-Descriptor Translation Table Level 1 Bits */
++#define TTB_SECT_NS_MASK (1 << 19)
++#define TTB_SECT_NG_MASK (1 << 17)
++#define TTB_SECT_S_MASK (1 << 16)
++/* Note: TTB AP bits are set elsewhere */
++#define TTB_SECT_AP (3 << 10)
++#define TTB_SECT_TEX(x) ((x & 0x7) << 12)
++#define TTB_SECT_DOMAIN(x) ((x & 0xf) << 5)
++#define TTB_SECT_XN_MASK (1 << 4)
++#define TTB_SECT_C_MASK (1 << 3)
++#define TTB_SECT_B_MASK (1 << 2)
++#define TTB_SECT (2 << 0)
++
++/* options available for data cache on each page */
++enum dcache_option {
++ DCACHE_OFF = TTB_SECT_DOMAIN(0) | TTB_SECT_XN_MASK | TTB_SECT,
++ DCACHE_WRITETHROUGH = DCACHE_OFF | TTB_SECT_C_MASK,
++ DCACHE_WRITEBACK = DCACHE_WRITETHROUGH | TTB_SECT_B_MASK,
++ DCACHE_WRITEALLOC = DCACHE_WRITEBACK | TTB_SECT_TEX(1),
++};
++#else
++#define TTB_SECT_AP (3 << 10)
+ /* options available for data cache on each page */
+ enum dcache_option {
+ DCACHE_OFF = 0x12,
+ DCACHE_WRITETHROUGH = 0x1a,
+ DCACHE_WRITEBACK = 0x1e,
++ DCACHE_WRITEALLOC = 0x16,
+ };
++#endif
+
+ /* Size of an MMU section */
+ enum {
+- MMU_SECTION_SHIFT = 20,
++#ifdef CONFIG_ARMV7_LPAE
++ MMU_SECTION_SHIFT = 21, /* 2MB */
++#else
++ MMU_SECTION_SHIFT = 20, /* 1MB */
++#endif
+ MMU_SECTION_SIZE = 1 << MMU_SECTION_SHIFT,
+ };
+
++#ifdef CONFIG_ARMV7
++/* TTBR0 bits */
++#define TTBR0_BASE_ADDR_MASK 0xFFFFC000
++#define TTBR0_RGN_NC (0 << 3)
++#define TTBR0_RGN_WBWA (1 << 3)
++#define TTBR0_RGN_WT (2 << 3)
++#define TTBR0_RGN_WB (3 << 3)
++/* TTBR0[6] is IRGN[0] and TTBR[0] is IRGN[1] */
++#define TTBR0_IRGN_NC (0 << 0 | 0 << 6)
++#define TTBR0_IRGN_WBWA (0 << 0 | 1 << 6)
++#define TTBR0_IRGN_WT (1 << 0 | 0 << 6)
++#define TTBR0_IRGN_WB (1 << 0 | 1 << 6)
++#endif
++
+ /**
+ * Change the cache settings for a region.
+ *
+diff --git a/arch/arm/lib/Makefile b/arch/arm/lib/Makefile
+index 57111af..d8634be 100644
+--- a/arch/arm/lib/Makefile
++++ b/arch/arm/lib/Makefile
+@@ -54,6 +54,8 @@ COBJS-y += reset.o
+ COBJS-y += cache.o
+ COBJS-y += cache-cp15.o
+
++COBJS-y += call_with_stack.o
++
+ SRCS := $(GLSOBJS:.o=.S) $(GLCOBJS:.o=.c) \
+ $(SOBJS-y:.o=.S) $(COBJS-y:.o=.c)
+ OBJS := $(addprefix $(obj),$(SOBJS-y) $(COBJS-y))
+diff --git a/arch/arm/lib/board.c b/arch/arm/lib/board.c
+index 9f861cc..3c2bf55 100644
+--- a/arch/arm/lib/board.c
++++ b/arch/arm/lib/board.c
+@@ -355,7 +355,7 @@ void board_init_f(ulong bootflag)
+
+ #if !(defined(CONFIG_SYS_ICACHE_OFF) && defined(CONFIG_SYS_DCACHE_OFF))
+ /* reserve TLB table */
+- gd->tlb_size = 4096 * 4;
++ gd->tlb_size = PGTABLE_SIZE;
+ addr -= gd->tlb_size;
+
+ /* round down to next 64 kB limit */
+diff --git a/arch/arm/lib/cache-cp15.c b/arch/arm/lib/cache-cp15.c
+index 75cf7b1..aefa2ae 100644
+--- a/arch/arm/lib/cache-cp15.c
++++ b/arch/arm/lib/cache-cp15.c
+@@ -44,13 +44,50 @@ static void cp_delay (void)
+ asm volatile("" : : : "memory");
+ }
+
++#ifdef CONFIG_ARMV7_LPAE
++struct special_addr {
++ u32 page;
++ u32 size;
++ u64 addr;
++};
++
++/* This hack is for 2GB board with second GB attached
++ * to LPAE-only address at 0x200000000ULL */
++#define SDRAM2_ADDR 0x200000000ULL
++#define SDRAM2_SIZE 0x40000000 /* 1G */
++static struct special_addr offsets[] = {
++ { 0x80000000 >> MMU_SECTION_SHIFT, SDRAM2_SIZE >> MMU_SECTION_SHIFT, SDRAM2_ADDR, },
++};
++#endif
++
+ void set_section_dcache(int section, enum dcache_option option)
+ {
++#ifdef CONFIG_ARMV7_LPAE
++ int i;
++ u64 *page_table = (u64 *)gd->tlb_addr;
++ /* Need to set the access flag to not fault */
++ u64 value = TTB_SECT_AP | TTB_SECT_AF;
++#else
+ u32 *page_table = (u32 *)gd->tlb_addr;
+- u32 value;
++ u32 value = TTB_SECT_AP;
++#endif
++
++ /* Add the page offset */
++#ifdef CONFIG_ARMV7_LPAE
++ for (i = 0; i < ARRAY_SIZE(offsets); i++)
++ if (section >= offsets[i].page &&
++ section < offsets[i].page + offsets[i].size)
++ value |= offsets[i].addr + ((section - offsets[i].page) << MMU_SECTION_SHIFT);
++ else
++ value |= ((u32)section << MMU_SECTION_SHIFT);
++#else
++ value |= ((u32)section << MMU_SECTION_SHIFT);
++#endif
+
+- value = (section << MMU_SECTION_SHIFT) | (3 << 10);
++ /* Add caching bits */
+ value |= option;
++
++ /* Set PTE */
+ page_table[section] = value;
+ }
+
+@@ -66,11 +103,11 @@ void mmu_set_region_dcache_behaviour(u32 start, int size,
+ enum dcache_option option)
+ {
+ u32 *page_table = (u32 *)gd->tlb_addr;
+- u32 upto, end;
++ unsigned long upto, end;
+
+ end = ALIGN(start + size, MMU_SECTION_SIZE) >> MMU_SECTION_SHIFT;
+ start = start >> MMU_SECTION_SHIFT;
+- debug("%s: start=%x, size=%x, option=%d\n", __func__, start, size,
++ debug("%s: start=%pa, size=%zu, option=%d\n", __func__, &start, size,
+ option);
+ for (upto = start; upto < end; upto++)
+ set_section_dcache(upto, option);
+@@ -83,11 +120,14 @@ static inline void dram_bank_mmu_setup(int bank)
+ int i;
+
+ debug("%s: bank: %d\n", __func__, bank);
+- for (i = bd->bi_dram[bank].start >> 20;
+- i < (bd->bi_dram[bank].start + bd->bi_dram[bank].size) >> 20;
++ for (i = bd->bi_dram[bank].start >> MMU_SECTION_SHIFT;
++ i < (bd->bi_dram[bank].start >> MMU_SECTION_SHIFT) +
++ (bd->bi_dram[bank].size >> MMU_SECTION_SHIFT);
+ i++) {
+ #if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
+ set_section_dcache(i, DCACHE_WRITETHROUGH);
++#elif defined(CONFIG_SYS_ARM_CACHE_WRITEALLOC)
++ set_section_dcache(i, DCACHE_WRITEALLOC);
+ #else
+ set_section_dcache(i, DCACHE_WRITEBACK);
+ #endif
+@@ -102,19 +142,88 @@ static inline void mmu_setup(void)
+
+ arm_init_before_mmu();
+ /* Set up an identity-mapping for all 4GB, rw for everyone */
+- for (i = 0; i < 4096; i++)
++ for (i = 0; i < ((4096ULL * 1024 * 1024) >> MMU_SECTION_SHIFT); i++)
+ set_section_dcache(i, DCACHE_OFF);
+
+ for (i = 0; i < CONFIG_NR_DRAM_BANKS; i++) {
+ dram_bank_mmu_setup(i);
+ }
++ /* Enabling d-cache for remapped region of memory
++ *
++ */
++ for (i = (0x80000000 >> MMU_SECTION_SHIFT);
++ i < 0xc0000000 >> MMU_SECTION_SHIFT; i++)
++#if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
++ set_section_dcache(i, DCACHE_WRITETHROUGH);
++#elif defined(CONFIG_SYS_ARM_CACHE_WRITEALLOC)
++ set_section_dcache(i, DCACHE_WRITEALLOC);
++#else
++ set_section_dcache(i, DCACHE_WRITEBACK);
++#endif
++
++#ifdef CONFIG_ARMV7_LPAE
++ /* Set up 4 PTE entries pointing to our 4 1GB page tables */
++ for (i = 0; i < 4; i++) {
++ u64 *page_table = (u64 *)(gd->tlb_addr + (4096 * 4));
++ u64 tpt = gd->tlb_addr + (4096 * i);
++ page_table[i] = tpt | TTB_PAGETABLE;
++ }
+
++ reg = TTBCR_EAE;
++#if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
++ reg |= TTBCR_ORGN0_WT | TTBCR_IRGN0_WT;
++#elif defined(CONFIG_SYS_ARM_CACHE_WRITEALLOC)
++ reg |= TTBCR_ORGN0_WBWA | TTBCR_IRGN0_WBWA;
++#else
++ reg |= TTBCR_ORGN0_WBNWA | TTBCR_IRGN0_WBNWA;
++#endif
++
++ if (is_hyp()) {
++ /* Set HCTR to enable LPAE */
++ asm volatile("mcr p15, 4, %0, c2, c0, 2"
++ : : "r" (reg) : "memory");
++ /* Set HTTBR0 */
++ asm volatile("mcrr p15, 4, %0, %1, c2"
++ :
++ : "r"(gd->tlb_addr + (4096 * 4)), "r"(0)
++ : "memory");
++ /* Set HMAIR */
++ asm volatile("mcr p15, 4, %0, c10, c2, 0"
++ : : "r" (MEMORY_ATTRIBUTES) : "memory");
++ } else {
++ /* Set TTBCR to enable LPAE */
++ asm volatile("mcr p15, 0, %0, c2, c0, 2"
++ : : "r" (reg) : "memory");
++ /* Set 64-bit TTBR0 */
++ asm volatile("mcrr p15, 0, %0, %1, c2"
++ :
++ : "r"(gd->tlb_addr + (4096 * 4)), "r"(0)
++ : "memory");
++ /* Set MAIR */
++ asm volatile("mcr p15, 0, %0, c10, c2, 0"
++ : : "r" (MEMORY_ATTRIBUTES) : "memory");
++ }
++#elif defined(CONFIG_ARMV7)
++ /* Set TTBR0 */
++ reg = gd->tlb_addr & TTBR0_BASE_ADDR_MASK;
++#if defined(CONFIG_SYS_ARM_CACHE_WRITETHROUGH)
++ reg |= TTBR0_RGN_WT | TTBR0_IRGN_WT;
++#elif defined(CONFIG_SYS_ARM_CACHE_WRITEALLOC)
++ reg |= TTBR0_RGN_WBWA | TTBR0_IRGN_WBWA;
++#else
++ reg |= TTBR0_RGN_WB | TTBR0_IRGN_WB;
++#endif
++ asm volatile("mcr p15, 0, %0, c2, c0, 0"
++ : : "r" (reg) : "memory");
++#else
+ /* Copy the page table address to cp15 */
+ asm volatile("mcr p15, 0, %0, c2, c0, 0"
+ : : "r" (gd->tlb_addr) : "memory");
++#endif
+ /* Set the access control to all-supervisor */
+ asm volatile("mcr p15, 0, %0, c3, c0, 0"
+ : : "r" (~0));
++
+ /* and enable the mmu */
+ reg = get_cr(); /* get control reg. */
+ cp_delay();
+diff --git a/arch/arm/lib/call_with_stack.S b/arch/arm/lib/call_with_stack.S
+new file mode 100644
+index 0000000..651d869
+--- /dev/null
++++ b/arch/arm/lib/call_with_stack.S
+@@ -0,0 +1,20 @@
++.globl call_with_stack
++.syntax unified /* use unified assembler syntax */
++#ifdef __thumb__
++.thumb /* assemble in Thumb-2 (.thumb" can also be used) */
++#endif
++call_with_stack:
++ str sp, [r2, #-4]!
++ str lr, [r2, #-4]!
++
++ mov sp, r2
++ mov r2, r0
++ mov r0, r1
++
++ adr lr, 1f
++ mov pc, r2
++
++1: ldr lr, [sp]
++ ldr sp, [sp, #4]
++ mov pc, lr
++
+diff --git a/board/renesas/porter/porter.c b/board/renesas/porter/porter.c
+index 71836e2..6c4fd1a 100644
+--- a/board/renesas/porter/porter.c
++++ b/board/renesas/porter/porter.c
+@@ -42,6 +42,10 @@ void s_init(void)
+ struct r8a7791_swdt *swdt = (struct r8a7791_swdt *)SWDT_BASE;
+ u32 val;
+
++#ifdef CONFIG_SYS_ARCH_TIMER
++ arm_arch_timer_init();
++#endif
++
+ /* Watchdog init */
+ writel(0xA5A5A500, &rwdt->rwtcsra);
+ writel(0xA5A5A500, &swdt->swtcsra);
+@@ -71,10 +75,12 @@ int board_early_init_f(void)
+ {
+ u32 val;
+
++#ifdef CONFIG_TMU_TIMER
+ /* TMU0 */
+ val = readl(MSTPSR1);
+ val &= ~TMU0_MSTP125;
+ writel(val, SMSTPCR1);
++#endif
+
+ val = readl(MSTPSR7);
+ val &= ~SCIF0_MSTP721;
+@@ -114,7 +120,9 @@ int board_init(void)
+ /* Init PFC controller */
+ r8a7791_pinmux_init();
+
++#ifdef CONFIG_TMU_TIMER
+ sh_timer_init();
++#endif
+
+ /* ETHER Enable */
+ gpio_request(GPIO_FN_ETH_CRS_DV, NULL);
+@@ -288,10 +296,12 @@ void arch_preboot_os()
+ u32 val;
+ int i;
+
++#ifdef CONFIG_TMU_TIMER
+ /* stop TMU0 */
+ val = readb(TMU_BASE + TSTR0);
+ val &= ~TSTR0_STR0;
+ writeb(val, TMU_BASE + TSTR0);
++#endif
+
+ /* stop all module clock*/
+ for (i = MSTP00; i < MSTP_NR; i++) {
+diff --git a/include/configs/porter.h b/include/configs/porter.h
+index 7ab0643..5567c7c 100644
+--- a/include/configs/porter.h
++++ b/include/configs/porter.h
+@@ -53,6 +53,9 @@
+ #define CONFIG_CMD_EXT4_WRITE
+ #define CONFIG_CMD_SF
+ #define CONFIG_CMD_SPI
++#define CONFIG_CMD_SWSUSP
++#define CONFIG_CMD_SWSUSPMEM
++#define CONFIG_LZO
+
+ #define CONFIG_CMDLINE_TAG
+ #define CONFIG_SETUP_MEMORY_TAGS
+@@ -75,7 +78,6 @@
+ #define CONFIG_BOARD_EARLY_INIT_F
+ #define CONFIG_USE_ARCH_MEMSET
+ #define CONFIG_USE_ARCH_MEMCPY
+-#define CONFIG_TMU_TIMER
+
+ /* STACK */
+ #if defined(CONFIG_EXTRAM_BOOT)
+@@ -89,8 +91,8 @@
+
+ /* MEMORY */
+ #define PORTER_SDRAM_BASE 0x40000000
+-#define PORTER_SDRAM_SIZE 0x40000000
+-#define PORTER_UBOOT_SDRAM_SIZE 0x20000000
++#define PORTER_SDRAM_SIZE 0x48000000
++#define PORTER_UBOOT_SDRAM_SIZE 0x40000000
+
+ #define CONFIG_SYS_LONGHELP
+ #define CONFIG_SYS_PROMPT "=> "
+@@ -203,4 +205,15 @@
+ #define CONFIG_USB_HOST_ETHER /* Enable USB Ethernet adapters */
+ #define CONFIG_USB_ETHER_ASIX /* Asix, or whatever driver(s) you want */
+
++#define CONFIG_ARMV7_LPAE /* 64-bit MMU descriptors */
++#define CONFIG_SYS_ARM_CACHE_WRITEALLOC /* Make memory operations faster */
++
++#define CONFIG_SYS_ARCH_TIMER /* Init arch timer */
++#define CONFIG_VE_ENABLED /* Virtualization Extensions are enabled*/
++#define CONFIG_SYS_HZ_CLOCK CONFIG_SYS_CLK_FREQ
++
++#define CONFIG_SH_DMA
++#define CONFIG_SH_SYS_DMAL_BASE 0xE6700000
++#define CONFIG_SH_SYS_DMAL_NCH 15
++
+ #endif /* __PORTER_H */
+--
+1.8.3.1
+
FILESEXTRAPATHS_append := ":${THISDIR}/${PN}"
SRC_URI_append_porter_sota = "file://0001-Autoload-uEnv.txt-on-boot.patch"
+
+SRC_URI_append_agl-porter-hibernate = " file://hibernation/0001-Add-rcar-sdhi-DMA-support.patch \
+ file://hibernation/0002-Add-Hibernation-swsusp-command-support.patch \
+ file://hibernation/0003-Add-Hibernation-swsuspmem-command-support.patch \
+ file://hibernation/0004-Add-porter-board-Hibernation-code.patch \
+ "
+
Code Review / AGL / meta-agl.git / commitdiff