1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
|
/* coresight.c
*
* Copyright (C) 2011 Google, Inc.
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* 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 <asm/hardware/coresight.h>
#include <linux/amba/bus.h>
#include <linux/io.h>
#include <plat/pm.h>
#define CS_ETB_BASE (0xE0D00000 + 0x1000)
#define CS_FUNNEL_BASE (0xE0D00000 + 0x4000)
#define CS_ETM_BASE (0xE0D00000 + 0x6000)
#define TZPC3_BASE (0xE1C00000)
static void __iomem *cs_etb_regs;
static void __iomem *cs_funnel_regs;
static void __iomem *cs_etm_regs;
static void __iomem *tzpc3_regs;
static void cs_unlock(void *regs)
{
writel(UNLOCK_MAGIC, regs + CSMR_LOCKACCESS);
readl(regs + CSMR_LOCKSTATUS);
}
static void cs_relock(void *regs)
{
writel(0, regs + CSMR_LOCKACCESS);
}
static void cs_init_static_regs(void)
{
/* Enable NIDEN and SPNIDEN */
writel(0x5, tzpc3_regs + 0x828);
/* Set funnel control: Enable ATB port0 */
cs_unlock(cs_funnel_regs);
writel(0x301, cs_funnel_regs);
cs_relock(cs_funnel_regs);
}
#ifdef CONFIG_PM
static void (*orig_pm_cpu_prep)(void);
static void (*orig_pm_cpu_restore)(void);
static u32 etm_state[64];
static int etm_state_count;
static u32 etb_ffcr;
static u32 etb_ctl;
static void cs_pm_prep(void)
{
int save_count;
if (orig_pm_cpu_prep)
orig_pm_cpu_prep();
cs_unlock(cs_etb_regs);
etb_ctl = readl(cs_etb_regs + ETBR_CTRL);
etb_ffcr = readl(cs_etb_regs + ETBR_FORMATTERCTRL);
cs_relock(cs_etb_regs);
cs_unlock(cs_etm_regs);
writel(UNLOCK_MAGIC, cs_etm_regs + ETMMR_OSLAR);
save_count = readl(cs_etm_regs + ETMMR_OSSRR);
pr_debug("%s: trace_pm_prep reg count %d\n", __func__, save_count);
if (save_count > ARRAY_SIZE(etm_state))
save_count = 0;
etm_state_count = 0;
while (save_count--)
etm_state[etm_state_count++] = readl(cs_etm_regs + ETMMR_OSSRR);
writel(0, cs_etm_regs + ETMMR_OSLAR);
cs_relock(cs_etm_regs);
}
static void cs_pm_resume(void)
{
int i;
cs_init_static_regs();
cs_unlock(cs_etb_regs);
writel(etb_ffcr, cs_etb_regs + ETBR_FORMATTERCTRL);
writel(etb_ctl, cs_etb_regs + ETBR_CTRL);
cs_relock(cs_etb_regs);
cs_unlock(cs_etm_regs);
readl(cs_etm_regs + ETMMR_PDSR);
writel(UNLOCK_MAGIC, cs_etm_regs + ETMMR_OSLAR);
readl(cs_etm_regs + ETMMR_OSSRR);
for (i = 0; i < etm_state_count; i++) {
writel(etm_state[i], cs_etm_regs + ETMMR_OSSRR);
pr_debug("%s: restore %d %08x\n", __func__, i, etm_state[i]);
}
writel(0, cs_etm_regs + ETMMR_OSLAR);
cs_relock(cs_etm_regs);
if (orig_pm_cpu_restore)
orig_pm_cpu_restore();
}
static void cs_pm_init(void)
{
orig_pm_cpu_restore = pm_cpu_restore;
orig_pm_cpu_prep = pm_cpu_prep;
pm_cpu_restore = cs_pm_resume;
pm_cpu_prep = cs_pm_prep;
}
#else
static inline void cs_pm_init(void) {}
#endif
static struct amba_device s5pv210_etb_device = {
.dev = {
.init_name = "etb",
},
.res = {
.start = CS_ETB_BASE,
.end = CS_ETB_BASE + SZ_4K - 1,
.flags = IORESOURCE_MEM,
},
.periphid = 0x000bb907,
};
static struct amba_device s5pv210_etm_device = {
.dev = {
.init_name = "etm",
},
.res = {
.start = CS_ETM_BASE,
.end = CS_ETM_BASE + SZ_4K - 1,
.flags = IORESOURCE_MEM,
},
.periphid = 0x005bb921,
};
static int trace_init(void)
{
cs_etb_regs = ioremap(CS_ETB_BASE, SZ_4K);
cs_funnel_regs = ioremap(CS_FUNNEL_BASE, SZ_4K);
cs_etm_regs = ioremap(CS_ETM_BASE, SZ_4K);
tzpc3_regs = ioremap(TZPC3_BASE, SZ_4K);
cs_init_static_regs();
cs_pm_init();
amba_device_register(&s5pv210_etb_device, &iomem_resource);
amba_device_register(&s5pv210_etm_device, &iomem_resource);
return 0;
}
device_initcall(trace_init);
|
