diff options
Diffstat (limited to 'utils/imxtools/sbtools/misc.c')
-rw-r--r-- | utils/imxtools/sbtools/misc.c | 265 |
1 files changed, 265 insertions, 0 deletions
diff --git a/utils/imxtools/sbtools/misc.c b/utils/imxtools/sbtools/misc.c new file mode 100644 index 0000000000..fce71ae0cd --- /dev/null +++ b/utils/imxtools/sbtools/misc.c | |||
@@ -0,0 +1,265 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2010 Amaury Pouly | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include <stdlib.h> | ||
22 | #include <stdio.h> | ||
23 | #include <time.h> | ||
24 | #include <ctype.h> | ||
25 | #include "misc.h" | ||
26 | |||
27 | bool g_debug = false; | ||
28 | |||
29 | /** | ||
30 | * Misc | ||
31 | */ | ||
32 | |||
33 | void *memdup(const void *p, size_t len) | ||
34 | { | ||
35 | void *cpy = xmalloc(len); | ||
36 | memcpy(cpy, p, len); | ||
37 | return cpy; | ||
38 | } | ||
39 | |||
40 | void generate_random_data(void *buf, size_t sz) | ||
41 | { | ||
42 | size_t i = 0; | ||
43 | unsigned char* p = (unsigned char*)buf; | ||
44 | while(i++ < sz) | ||
45 | *p++ = rand(); | ||
46 | } | ||
47 | |||
48 | void *xmalloc(size_t s) | ||
49 | { | ||
50 | void * r = malloc(s); | ||
51 | if(!r) bugp("malloc"); | ||
52 | return r; | ||
53 | } | ||
54 | |||
55 | int convxdigit(char digit, byte *val) | ||
56 | { | ||
57 | if(digit >= '0' && digit <= '9') | ||
58 | { | ||
59 | *val = digit - '0'; | ||
60 | return 0; | ||
61 | } | ||
62 | else if(digit >= 'A' && digit <= 'F') | ||
63 | { | ||
64 | *val = digit - 'A' + 10; | ||
65 | return 0; | ||
66 | } | ||
67 | else if(digit >= 'a' && digit <= 'f') | ||
68 | { | ||
69 | *val = digit - 'a' + 10; | ||
70 | return 0; | ||
71 | } | ||
72 | else | ||
73 | return 1; | ||
74 | } | ||
75 | |||
76 | /* helper function to augment an array, free old array */ | ||
77 | void *augment_array(void *arr, size_t elem_sz, size_t cnt, void *aug, size_t aug_cnt) | ||
78 | { | ||
79 | void *p = xmalloc(elem_sz * (cnt + aug_cnt)); | ||
80 | memcpy(p, arr, elem_sz * cnt); | ||
81 | memcpy(p + elem_sz * cnt, aug, elem_sz * aug_cnt); | ||
82 | free(arr); | ||
83 | return p; | ||
84 | } | ||
85 | |||
86 | /** | ||
87 | * Key file parsing | ||
88 | */ | ||
89 | int g_nr_keys; | ||
90 | key_array_t g_key_array; | ||
91 | |||
92 | bool parse_key(char **pstr, struct crypto_key_t *key) | ||
93 | { | ||
94 | char *str = *pstr; | ||
95 | /* ignore spaces */ | ||
96 | while(isspace(*str)) | ||
97 | str++; | ||
98 | /* CRYPTO_KEY: 32 hex characters | ||
99 | * CRYPTO_USBOTP: usbotp(vid:pid) where vid and pid are hex numbers */ | ||
100 | if(isxdigit(str[0])) | ||
101 | { | ||
102 | if(strlen(str) < 32) | ||
103 | return false; | ||
104 | for(int j = 0; j < 16; j++) | ||
105 | { | ||
106 | byte a, b; | ||
107 | if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b)) | ||
108 | return false; | ||
109 | key->u.key[j] = (a << 4) | b; | ||
110 | } | ||
111 | /* skip key */ | ||
112 | *pstr = str + 32; | ||
113 | key->method = CRYPTO_KEY; | ||
114 | return true; | ||
115 | } | ||
116 | else | ||
117 | { | ||
118 | const char *prefix = "usbotp("; | ||
119 | if(strlen(str) < strlen(prefix)) | ||
120 | return false; | ||
121 | if(strncmp(str, prefix, strlen(prefix)) != 0) | ||
122 | return false; | ||
123 | str += strlen(prefix); | ||
124 | /* vid */ | ||
125 | long vid = strtol(str, &str, 16); | ||
126 | if(vid < 0 || vid > 0xffff) | ||
127 | return false; | ||
128 | if(*str++ != ':') | ||
129 | return false; | ||
130 | /* pid */ | ||
131 | long pid = strtol(str, &str, 16); | ||
132 | if(pid < 0 || pid > 0xffff) | ||
133 | return false; | ||
134 | if(*str++ != ')') | ||
135 | return false; | ||
136 | *pstr = str; | ||
137 | key->method = CRYPTO_USBOTP; | ||
138 | key->u.vid_pid = vid << 16 | pid; | ||
139 | return true; | ||
140 | } | ||
141 | } | ||
142 | |||
143 | void add_keys(key_array_t ka, int kac) | ||
144 | { | ||
145 | key_array_t new_ka = xmalloc((g_nr_keys + kac) * sizeof(struct crypto_key_t)); | ||
146 | memcpy(new_ka, g_key_array, g_nr_keys * sizeof(struct crypto_key_t)); | ||
147 | memcpy(new_ka + g_nr_keys, ka, kac * sizeof(struct crypto_key_t)); | ||
148 | free(g_key_array); | ||
149 | g_key_array = new_ka; | ||
150 | g_nr_keys += kac; | ||
151 | } | ||
152 | |||
153 | void clear_keys() | ||
154 | { | ||
155 | free(g_key_array); | ||
156 | g_nr_keys = 0; | ||
157 | g_key_array = NULL; | ||
158 | } | ||
159 | |||
160 | bool add_keys_from_file(const char *key_file) | ||
161 | { | ||
162 | int size; | ||
163 | FILE *fd = fopen(key_file, "r"); | ||
164 | if(fd == NULL) | ||
165 | { | ||
166 | if(g_debug) | ||
167 | perror("cannot open key file"); | ||
168 | return false; | ||
169 | } | ||
170 | fseek(fd, 0, SEEK_END); | ||
171 | size = ftell(fd); | ||
172 | fseek(fd, 0, SEEK_SET); | ||
173 | char *buf = xmalloc(size + 1); | ||
174 | if(fread(buf, 1, size, fd) != (size_t)size) | ||
175 | { | ||
176 | if(g_debug) | ||
177 | perror("Cannot read key file"); | ||
178 | fclose(fd); | ||
179 | return false; | ||
180 | } | ||
181 | buf[size] = 0; | ||
182 | fclose(fd); | ||
183 | |||
184 | if(g_debug) | ||
185 | printf("Parsing key file '%s'...\n", key_file); | ||
186 | char *p = buf; | ||
187 | while(1) | ||
188 | { | ||
189 | struct crypto_key_t k; | ||
190 | /* parse key */ | ||
191 | if(!parse_key(&p, &k)) | ||
192 | { | ||
193 | if(g_debug) | ||
194 | printf("invalid key file\n"); | ||
195 | return false; | ||
196 | } | ||
197 | if(g_debug) | ||
198 | { | ||
199 | printf("Add key: "); | ||
200 | print_key(&k, true); | ||
201 | } | ||
202 | add_keys(&k, 1); | ||
203 | /* request at least one space character before next key, or end of file */ | ||
204 | if(*p != 0 && !isspace(*p)) | ||
205 | { | ||
206 | if(g_debug) | ||
207 | printf("invalid key file\n"); | ||
208 | return false; | ||
209 | } | ||
210 | /* skip whitespace */ | ||
211 | while(isspace(*p)) | ||
212 | p++; | ||
213 | if(*p == 0) | ||
214 | break; | ||
215 | } | ||
216 | free(buf); | ||
217 | return true; | ||
218 | } | ||
219 | |||
220 | void print_hex(byte *data, int len, bool newline) | ||
221 | { | ||
222 | for(int i = 0; i < len; i++) | ||
223 | printf("%02X ", data[i]); | ||
224 | if(newline) | ||
225 | printf("\n"); | ||
226 | } | ||
227 | |||
228 | void print_key(struct crypto_key_t *key, bool newline) | ||
229 | { | ||
230 | switch(key->method) | ||
231 | { | ||
232 | case CRYPTO_KEY: | ||
233 | print_hex(key->u.key, 16, false); | ||
234 | break; | ||
235 | case CRYPTO_USBOTP: | ||
236 | printf("USB-OTP(%04x:%04x)", key->u.vid_pid >> 16, key->u.vid_pid & 0xffff); | ||
237 | break; | ||
238 | case CRYPTO_NONE: | ||
239 | printf("none"); | ||
240 | break; | ||
241 | } | ||
242 | if(newline) | ||
243 | printf("\n"); | ||
244 | } | ||
245 | |||
246 | char OFF[] = { 0x1b, 0x5b, 0x31, 0x3b, '0', '0', 0x6d, '\0' }; | ||
247 | |||
248 | char GREY[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '0', 0x6d, '\0' }; | ||
249 | char RED[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '1', 0x6d, '\0' }; | ||
250 | char GREEN[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '2', 0x6d, '\0' }; | ||
251 | char YELLOW[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '3', 0x6d, '\0' }; | ||
252 | char BLUE[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '4', 0x6d, '\0' }; | ||
253 | |||
254 | static bool g_color_enable = true; | ||
255 | |||
256 | void enable_color(bool enable) | ||
257 | { | ||
258 | g_color_enable = enable; | ||
259 | } | ||
260 | |||
261 | void color(color_t c) | ||
262 | { | ||
263 | if(g_color_enable) | ||
264 | printf("%s", (char *)c); | ||
265 | } | ||