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