1 // SPDX-License-Identifier: GPL-2.0+
2
3 #include <common.h>
4 #include <command.h>
5 #include <malloc.h>
6 #include <mapmem.h>
7 #include <linux/ctype.h>
8
9 enum {
10 OP_ID_XOR,
11 OP_ID_AND,
12 OP_ID_OR,
13 };
14
write_to_env_var(char * varname,u8 * result,ulong len)15 void write_to_env_var(char *varname, u8 *result, ulong len)
16 {
17 char *str_output;
18 char *str_ptr;
19 int i;
20
21 str_output = malloc(len * 2 + 1);
22 str_ptr = str_output;
23
24 for (i = 0; i < len; i++) {
25 sprintf(str_ptr, "%02x", result[i]);
26 str_ptr += 2;
27 }
28 *str_ptr = '\0';
29 setenv(varname, str_output);
30
31 free(str_output);
32 }
33
decode_hexstring(char * hexstr,u8 * result)34 void decode_hexstring(char *hexstr, u8 *result)
35 {
36 int i;
37 int acc = 0;
38
39 for (i = 0; i < strlen(hexstr); ++i) {
40 char d = hexstr[i];
41 int value;
42
43 if (isdigit(d))
44 value = (d - '0');
45 else
46 value = (islower(d) ? toupper(d) : d) - 'A' + 10;
47
48 if (i % 2 == 0) {
49 acc = value * 16;
50 } else {
51 result[i / 2] = acc + value;
52 acc = 0;
53 }
54 }
55 }
56
read_from_env_var(char * varname,u8 * result)57 void read_from_env_var(char *varname, u8 *result)
58 {
59 char *str_value;
60
61 str_value = getenv(varname);
62 if (str_value)
63 decode_hexstring(str_value, result);
64 else
65 decode_hexstring(varname, result);
66 }
67
read_from_mem(ulong addr,u8 * result,ulong len)68 void read_from_mem(ulong addr, u8 *result, ulong len)
69 {
70 u8 *src;
71
72 src = map_sysmem(addr, len);
73 memcpy(result, src, len);
74 unmap_sysmem(src);
75 }
76
write_to_mem(char * varname,u8 * result,ulong len)77 void write_to_mem(char *varname, u8 *result, ulong len)
78 {
79 ulong addr;
80 u8 *buf;
81
82 addr = simple_strtoul(varname, NULL, 16);
83 buf = map_sysmem(addr, len);
84 memcpy(buf, result, len);
85 unmap_sysmem(buf);
86 }
87
do_binop(cmd_tbl_t * cmdtp,int flag,int argc,char * const argv[])88 static int do_binop(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
89 {
90 ulong len;
91 u8 *result, *src1, *src2;
92 char *oparg, *lenarg, *src1arg, *src2arg, *destarg;
93 int i, op;
94
95 if (argc < 5)
96 return CMD_RET_USAGE;
97
98 oparg = argv[1];
99 lenarg = argv[2];
100 src1arg = argv[3];
101 src2arg = argv[4];
102
103 if (!strcmp(oparg, "xor"))
104 op = OP_ID_XOR;
105 else if (!strcmp(oparg, "or"))
106 op = OP_ID_OR;
107 else if (!strcmp(oparg, "and"))
108 op = OP_ID_AND;
109 else
110 return CMD_RET_USAGE;
111
112 len = simple_strtoul(lenarg, NULL, 10);
113
114 src1 = malloc(len);
115 src2 = malloc(len);
116
117 if (*src1arg == '*')
118 read_from_mem(simple_strtoul(src1arg + 1, NULL, 16), src1, len);
119 else
120 read_from_env_var(src1arg, src1);
121
122 if (*src2arg == '*')
123 read_from_mem(simple_strtoul(src2arg + 1, NULL, 16), src2, len);
124 else
125 read_from_env_var(src2arg, src2);
126
127 result = malloc(len);
128
129 switch (op) {
130 case OP_ID_XOR:
131 for (i = 0; i < len; i++)
132 result[i] = src1[i] ^ src2[i];
133 break;
134 case OP_ID_OR:
135 for (i = 0; i < len; i++)
136 result[i] = src1[i] | src2[i];
137 break;
138 case OP_ID_AND:
139 for (i = 0; i < len; i++)
140 result[i] = src1[i] & src2[i];
141 break;
142 }
143
144 if (argc == 5) {
145 for (i = 0; i < len; i++) {
146 printf("%02x ", result[i]);
147 if (i % 16 == 15)
148 puts("\n");
149 }
150 puts("\n");
151
152 goto exit;
153 }
154
155 destarg = argv[5];
156
157 if (*destarg == '*')
158 write_to_mem(destarg + 1, result, len); /* Skip asterisk */
159 else
160 write_to_env_var(destarg, result, len);
161 exit:
162 free(result);
163 free(src2);
164 free(src1);
165
166 return 0;
167 }
168
169 U_BOOT_CMD(
170 binop, 6, 1, do_binop,
171 "compute binary operation",
172 "op count [*]src1 [*]src2 [[*]dest]\n"
173 " - compute binary operation of data at/in src1 and\n src2 (either *memaddr, env var name or hex string)\n and store result in/at dest, where op is one of\n xor, or, and."
174 );
175