1 /*
2 * Copyright (c) 2020 Intel Corporation
3 * Copyright (c) 2021 Antmicro <www.antmicro.com>
4 * Copyright (c) 2022 Meta
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #undef _POSIX_C_SOURCE
10 #define _POSIX_C_SOURCE 200809L
11
12 #include <stdlib.h>
13 #include <zephyr/sys/sys_getopt.h>
14 #include <zephyr/device.h>
15 #include <zephyr/shell/shell.h>
16 #include <zephyr/sys/byteorder.h>
17 #include <zephyr/sys/util.h>
18
is_ascii(uint8_t data)19 static inline bool is_ascii(uint8_t data)
20 {
21 return (data >= 0x30 && data <= 0x39) || (data >= 0x61 && data <= 0x66) ||
22 (data >= 0x41 && data <= 0x46);
23 }
24
25 static unsigned char *bytes;
26 static uint32_t *data;
27 static int sum;
28 static int chunk_element;
29 static char chunk[2];
30 static bool littleendian;
31
32 #define CHAR_CAN 0x18
33 #define CHAR_DC1 0x11
34
memory_dump(const struct shell * sh,mem_addr_t phys_addr,size_t size,uint8_t width)35 static int memory_dump(const struct shell *sh, mem_addr_t phys_addr, size_t size, uint8_t width)
36 {
37 uint64_t value;
38 size_t data_offset;
39 mm_reg_t addr;
40 const size_t vsize = width / BITS_PER_BYTE;
41 uint8_t hex_data[SHELL_HEXDUMP_BYTES_IN_LINE];
42
43 #if defined(CONFIG_MMU) || defined(CONFIG_PCIE)
44 device_map((mm_reg_t *)&addr, phys_addr, size, K_MEM_CACHE_NONE);
45
46 shell_print(sh, "Mapped 0x%lx to 0x%lx\n", phys_addr, addr);
47 #else
48 addr = phys_addr;
49 #endif /* defined(CONFIG_MMU) || defined(CONFIG_PCIE) */
50
51 for (; size > 0;
52 addr += SHELL_HEXDUMP_BYTES_IN_LINE, size -= MIN(size, SHELL_HEXDUMP_BYTES_IN_LINE)) {
53 for (data_offset = 0;
54 size >= vsize && data_offset + vsize <= SHELL_HEXDUMP_BYTES_IN_LINE;
55 data_offset += vsize) {
56 switch (width) {
57 case 8:
58 value = sys_read8(addr + data_offset);
59 hex_data[data_offset] = value;
60 break;
61 case 16:
62 value = sys_le16_to_cpu(sys_read16(addr + data_offset));
63 sys_put_le16(value, &hex_data[data_offset]);
64 break;
65 case 32:
66 value = sys_le32_to_cpu(sys_read32(addr + data_offset));
67 sys_put_le32(value, &hex_data[data_offset]);
68 break;
69 #ifdef CONFIG_64BIT
70 case 64:
71 value = sys_le64_to_cpu(sys_read64(addr + data_offset));
72 sys_put_le64(value, &hex_data[data_offset]);
73 break;
74 #endif /* CONFIG_64BIT */
75 default:
76 shell_print(sh, "Incorrect data width");
77 return -EINVAL;
78 }
79 }
80
81 shell_hexdump_line(sh, addr, hex_data, MIN(size, SHELL_HEXDUMP_BYTES_IN_LINE));
82 }
83
84 return 0;
85 }
86
cmd_dump(const struct shell * sh,size_t argc,char ** argv)87 static int cmd_dump(const struct shell *sh, size_t argc, char **argv)
88 {
89 int rv;
90 int err = 0;
91 size_t size = -1;
92 size_t width = 32;
93 mem_addr_t addr = -1;
94
95 sys_getopt_optind = 1;
96 sys_getopt_init();
97
98 while ((rv = sys_getopt(argc, argv, "a:s:w:")) != -1) {
99 switch (rv) {
100 case 'a':
101 addr = (mem_addr_t)shell_strtoul(sys_getopt_optarg, 16, &err);
102 if (err != 0) {
103 shell_error(sh, "invalid addr '%s'", sys_getopt_optarg);
104 return -EINVAL;
105 }
106 break;
107 case 's':
108 size = (size_t)shell_strtoul(sys_getopt_optarg, 0, &err);
109 if (err != 0) {
110 shell_error(sh, "invalid size '%s'", sys_getopt_optarg);
111 return -EINVAL;
112 }
113 break;
114 case 'w':
115 width = (size_t)shell_strtoul(sys_getopt_optarg, 0, &err);
116 if (err != 0) {
117 shell_error(sh, "invalid width '%s'", sys_getopt_optarg);
118 return -EINVAL;
119 }
120 break;
121 case '?':
122 default:
123 return -EINVAL;
124 }
125 }
126
127 if (addr == -1) {
128 shell_error(sh, "'-a <address>' is mandatory");
129 return -EINVAL;
130 }
131
132 if (size == -1) {
133 shell_error(sh, "'-s <size>' is mandatory");
134 return -EINVAL;
135 }
136
137 return memory_dump(sh, addr, size, width);
138 }
139
set_bypass(const struct shell * sh,shell_bypass_cb_t bypass)140 static int set_bypass(const struct shell *sh, shell_bypass_cb_t bypass)
141 {
142 static bool in_use;
143
144 if (bypass && in_use) {
145 shell_error(sh, "devmem load supports setting bypass on a single instance.");
146
147 return -EBUSY;
148 }
149
150 in_use = !in_use;
151 if (in_use) {
152 shell_print(sh, "Loading...\npress ctrl-x ctrl-q to escape");
153 in_use = true;
154 }
155
156 shell_set_bypass(sh, bypass, NULL);
157
158 return 0;
159 }
160
bypass_cb(const struct shell * sh,uint8_t * recv,size_t len,void * user_data)161 static void bypass_cb(const struct shell *sh, uint8_t *recv, size_t len, void *user_data)
162 {
163 bool escape = false;
164 static uint8_t tail;
165 uint8_t byte;
166
167 ARG_UNUSED(user_data);
168
169 for (size_t i = 0; i < len; i++) {
170 if (tail == CHAR_CAN && recv[i] == CHAR_DC1) {
171 escape = true;
172 tail = 0;
173 break;
174 }
175 tail = recv[i];
176
177 if (is_ascii(recv[i])) {
178 chunk[chunk_element] = recv[i];
179 chunk_element++;
180 }
181
182 if (chunk_element == 2) {
183 byte = (uint8_t)strtoul(chunk, NULL, 16);
184 *bytes = byte;
185 bytes++;
186 sum++;
187 chunk_element = 0;
188 }
189 }
190
191 if (escape) {
192 shell_print(sh, "Number of bytes read: %d", sum);
193 set_bypass(sh, NULL);
194
195 if (!littleendian) {
196 while (sum > 4) {
197 *data = BSWAP_32(*data);
198 data++;
199 sum = sum - 4;
200 }
201 if (sum % 4 == 0) {
202 *data = BSWAP_32(*data);
203 } else if (sum % 4 == 2) {
204 *data = BSWAP_16(*data);
205 } else if (sum % 4 == 3) {
206 *data = BSWAP_24(*data);
207 }
208 }
209 return;
210 }
211 }
212
cmd_load(const struct shell * sh,size_t argc,char ** argv)213 static int cmd_load(const struct shell *sh, size_t argc, char **argv)
214 {
215 littleendian = false;
216 char *arg;
217
218 chunk_element = 0;
219 sum = 0;
220
221 while (argc >= 2) {
222 arg = argv[1] + (!strncmp(argv[1], "--", 2) && argv[1][2]);
223 if (!strncmp(arg, "-e", 2)) {
224 littleendian = true;
225 } else if (!strcmp(arg, "--")) {
226 argv++;
227 argc--;
228 break;
229 } else if (arg[0] == '-' && arg[1]) {
230 shell_print(sh, "Unknown option \"%s\"", arg);
231 } else {
232 break;
233 }
234 argv++;
235 argc--;
236 }
237
238 bytes = (unsigned char *)strtoul(argv[1], NULL, 0);
239 data = (uint32_t *)strtoul(argv[1], NULL, 0);
240
241 set_bypass(sh, bypass_cb);
242 return 0;
243 }
244
memory_read(const struct shell * sh,mem_addr_t addr,uint8_t width)245 static int memory_read(const struct shell *sh, mem_addr_t addr, uint8_t width)
246 {
247 uint64_t value;
248 int err = 0;
249
250 switch (width) {
251 case 8:
252 value = sys_read8(addr);
253 break;
254 case 16:
255 value = sys_read16(addr);
256 break;
257 case 32:
258 value = sys_read32(addr);
259 break;
260 #ifdef CONFIG_64BIT
261 case 64:
262 value = sys_read64(addr);
263 break;
264 #endif /* CONFIG_64BIT */
265 default:
266 shell_print(sh, "Incorrect data width");
267 err = -EINVAL;
268 break;
269 }
270
271 if (err == 0) {
272 shell_print(sh, "Read value 0x%llx", value);
273 }
274
275 return err;
276 }
277
memory_write(const struct shell * sh,mem_addr_t addr,uint8_t width,uint64_t value)278 static int memory_write(const struct shell *sh, mem_addr_t addr, uint8_t width, uint64_t value)
279 {
280 int err = 0;
281
282 switch (width) {
283 case 8:
284 sys_write8(value, addr);
285 break;
286 case 16:
287 sys_write16(value, addr);
288 break;
289 case 32:
290 sys_write32(value, addr);
291 break;
292 #ifdef CONFIG_64BIT
293 case 64:
294 sys_write64(value, addr);
295 break;
296 #endif /* CONFIG_64BIT */
297 default:
298 shell_print(sh, "Incorrect data width");
299 err = -EINVAL;
300 break;
301 }
302
303 return err;
304 }
305
306 /* The syntax of the command is similar to busybox's devmem */
cmd_devmem(const struct shell * sh,size_t argc,char ** argv)307 static int cmd_devmem(const struct shell *sh, size_t argc, char **argv)
308 {
309 mem_addr_t phys_addr, addr;
310 uint64_t value = 0;
311 uint8_t width;
312
313 phys_addr = strtoul(argv[1], NULL, 16);
314
315 #if defined(CONFIG_MMU) || defined(CONFIG_PCIE)
316 device_map((mm_reg_t *)&addr, phys_addr, 0x100, K_MEM_CACHE_NONE);
317
318 shell_print(sh, "Mapped 0x%lx to 0x%lx\n", phys_addr, addr);
319 #else
320 addr = phys_addr;
321 #endif /* defined(CONFIG_MMU) || defined(CONFIG_PCIE) */
322
323 if (argc < 3) {
324 width = 32;
325 } else {
326 width = strtoul(argv[2], NULL, 10);
327 }
328
329 shell_print(sh, "Using data width %d", width);
330
331 if (argc <= 3) {
332 return memory_read(sh, addr, width);
333 }
334
335 /* If there are more then 3 arguments, that means we are going to write
336 * this value at the address provided
337 */
338
339 value = (uint64_t)strtoull(argv[3], NULL, 16);
340
341 shell_print(sh, "Writing value 0x%llx", value);
342
343 return memory_write(sh, addr, width, value);
344 }
345
346 SHELL_STATIC_SUBCMD_SET_CREATE(sub_devmem,
347 SHELL_CMD_ARG(dump, NULL,
348 "Usage:\n"
349 "devmem dump -a <address> -s <size> [-w <width>]\n",
350 cmd_dump, 5, 2),
351 SHELL_CMD_ARG(load, NULL,
352 "Usage:\n"
353 "devmem load [options] [address]\n"
354 "Options:\n"
355 "-e\tlittle-endian parse",
356 cmd_load, 2, 1),
357 SHELL_SUBCMD_SET_END);
358
359 SHELL_CMD_ARG_REGISTER(devmem, &sub_devmem,
360 "Read/write physical memory\n"
361 "Usage:\n"
362 "Read memory at address with optional width:\n"
363 "devmem <address> [<width>]\n"
364 "Write memory at address with mandatory width and value:\n"
365 "devmem <address> <width> <value>",
366 cmd_devmem, 2, 2);
367
