1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
2 /*
3 * Copyright (c) 2023 Rockchip Electronics Co., Ltd.
4 */
5
6 #include <asm/arch_timer.h>
7 #include <linux/init.h>
8 #include <linux/io.h>
9 #include <linux/kernel.h>
10 #include <linux/slab.h>
11
12 #include "rkpm_helpers.h"
13
14 /* REG region */
15 #define RGN_LEN(_rgn) (((_rgn)->end - (_rgn)->start) / (_rgn)->stride + 1)
16
17 static u32 *region_mem;
18 static u32 region_mem_size;
19 static int region_mem_idx;
20
alloc_region_mem(u32 * buf,int max_len,struct reg_region * rgns,u32 rgn_num)21 static int alloc_region_mem(u32 *buf, int max_len,
22 struct reg_region *rgns, u32 rgn_num)
23 {
24 int i;
25 int total_len = 0, len = 0;
26 struct reg_region *r = rgns;
27
28 if (!buf || !rgns) {
29 pr_err("%s invalid parameter\n", __func__);
30 return 0;
31 }
32
33 for (i = 0; i < rgn_num; i++, r++) {
34 if (total_len < max_len)
35 r->buf = &buf[total_len];
36
37 len = RGN_LEN(r);
38 total_len += len;
39 }
40
41 if (len >= max_len) {
42 pr_err("%s The buffer remain length:%d is too small for region:0x%x, at least %d\n",
43 __func__, max_len, rgns[0].start, total_len);
44 return -ENOMEM;
45 }
46
47 return total_len;
48 }
49
50 /**
51 * Alloc memory to reg_region->buf from region_mem.
52 * @rgns - struct reg_region array.
53 * @rgn_num - struct reg_region array length.
54 */
rkpm_alloc_region_mem(struct reg_region * rgns,u32 rgn_num)55 void rkpm_alloc_region_mem(struct reg_region *rgns, u32 rgn_num)
56 {
57 int max_len = 0, len;
58
59 max_len = region_mem_size / sizeof(u32) -
60 region_mem_idx;
61
62 len = alloc_region_mem(region_mem + region_mem_idx, max_len,
63 rgns, rgn_num);
64
65 region_mem_idx += len;
66 }
67
rkpm_region_mem_init(u32 size)68 void rkpm_region_mem_init(u32 size)
69 {
70 if (!size) {
71 pr_err("%s invalid param\n", __func__);
72 return;
73 }
74
75 region_mem = kmalloc(size, GFP_KERNEL);
76 if (!region_mem) {
77 pr_err("%s malloc region memory (0x%x) err\n", __func__, size);
78 return;
79 }
80
81 region_mem_size = size;
82 }
83
84 /**
85 * Save (reg_region->start ~ reg_region->end) to reg_region->buf.
86 * @rgns - struct reg_region array.
87 * @rgn_num - struct reg_region array length.
88 */
rkpm_reg_rgn_save(struct reg_region * rgns,u32 rgn_num)89 void rkpm_reg_rgn_save(struct reg_region *rgns, u32 rgn_num)
90 {
91 struct reg_region *r;
92 u8 *addr;
93 u8 *start, *end;
94 int i, j;
95
96 for (i = 0; i < rgn_num; i++) {
97 r = &rgns[i];
98 start = (char *)(*r->base) + r->start;
99 end = (char *)(*r->base) + r->end;
100 for (j = 0, addr = start; addr <= end; addr += r->stride, j++)
101 r->buf[j] = readl_relaxed(addr);
102 }
103 }
104
105 /**
106 * Restore reg_region->buf to (reg_region->start ~ reg_region->end).
107 * @rgns - struct reg_region array.
108 * @rgn_num - struct reg_region array length.
109 */
rkpm_reg_rgn_restore(struct reg_region * rgns,u32 rgn_num)110 void rkpm_reg_rgn_restore(struct reg_region *rgns, u32 rgn_num)
111 {
112 struct reg_region *r;
113 u8 *addr;
114 u8 *start, *end;
115 int i, j;
116
117 for (i = 0; i < rgn_num; i++) {
118 r = &rgns[i];
119 start = (char *)(*r->base) + r->start;
120 end = (char *)(*r->base) + r->end;
121 for (j = 0, addr = start; addr <= end; addr += r->stride, j++)
122 writel_relaxed(r->buf[j] | r->wmsk, addr);
123 }
124 }
125
rkpm_reg_rgn_restore_reverse(struct reg_region * rgns,u32 rgn_num)126 void rkpm_reg_rgn_restore_reverse(struct reg_region *rgns, u32 rgn_num)
127 {
128 struct reg_region *r;
129 u8 *addr;
130 u8 *start, *end;
131 int i, j;
132
133 for (i = rgn_num - 1; i >= 0; i--) {
134 r = &rgns[i];
135 start = (char *)(*r->base) + r->start;
136 end = (char *)(*r->base) + r->end;
137 j = RGN_LEN(r) - 1;
138 for (addr = end; addr >= start; addr -= r->stride, j--)
139 writel_relaxed(r->buf[j] | r->wmsk, addr);
140 }
141 }
142
143 /**
144 * Dump reg regions
145 * @rgns - struct reg_region array.
146 * @rgn_num - struct reg_region array length.
147 */
rkpm_dump_reg_rgns(struct reg_region * rgns,u32 rgn_num)148 void rkpm_dump_reg_rgns(struct reg_region *rgns, u32 rgn_num)
149 {
150 struct reg_region *r;
151 int i;
152
153 for (i = 0; i < rgn_num; i++) {
154 r = &rgns[i];
155 rkpm_regs_dump(*r->base, r->start, r->end, r->stride);
156 }
157 }
158
159 #pragma weak rkpm_printch
rkpm_printch(int c)160 void rkpm_printch(int c)
161 {
162 }
163
rkpm_printstr(const char * s)164 void rkpm_printstr(const char *s)
165 {
166 while (*s) {
167 rkpm_printch(*s);
168 s++;
169 }
170 }
171
rkpm_printhex(u32 hex)172 void rkpm_printhex(u32 hex)
173 {
174 u8 i = 8;
175 u8 c;
176
177 rkpm_printch('0');
178 rkpm_printch('x');
179 while (i--) {
180 c = (hex & 0xf0000000) >> 28;
181 rkpm_printch(c < 0xa ? c + '0' : c - 0xa + 'a');
182 hex <<= 4;
183 }
184 }
185
rkpm_printdec(int dec)186 void rkpm_printdec(int dec)
187 {
188 int i, tmp = dec;
189
190 if (dec < 0) {
191 rkpm_printch('-');
192 tmp = -dec;
193 dec = -dec;
194 }
195
196 for (i = 1; tmp / 10; tmp /= 10, i *= 10)
197 ;
198
199 for (; i >= 1; i /= 10) {
200 rkpm_printch('0' + (char)(dec / i));
201 dec %= i;
202 }
203 }
204
rkpm_regs_dump(void __iomem * base,u32 start_offset,u32 end_offset,u32 stride)205 void rkpm_regs_dump(void __iomem *base,
206 u32 start_offset,
207 u32 end_offset,
208 u32 stride)
209 {
210 u32 i;
211
212 for (i = start_offset; i <= end_offset; i += stride) {
213 if ((i - start_offset) % 16 == 0) {
214 rkpm_printch('\n');
215 rkpm_printhex((u32)base + i);
216 rkpm_printch(':');
217 rkpm_printch(' ');
218 rkpm_printch(' ');
219 rkpm_printch(' ');
220 rkpm_printch(' ');
221 }
222 rkpm_printhex(readl_relaxed(base + i));
223 rkpm_printch(' ');
224 rkpm_printch(' ');
225 rkpm_printch(' ');
226 rkpm_printch(' ');
227 }
228 rkpm_printch('\n');
229 }
230
rkpm_raw_udelay(int us)231 void rkpm_raw_udelay(int us)
232 {
233 u64 cur_cnt = __arch_counter_get_cntpct();
234 u64 del = us * 24;
235
236 while (__arch_counter_get_cntpct() - cur_cnt < del)
237 ;
238 }
239