hm60x: Fix white screen bug.
[maemo-rb.git] / utils / imxtools / sbtools / sb1.c
blobe4eb470e99098aedbb6ef0424faf84a26a48496f
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2012 Amaury Pouly
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.
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
20 ****************************************************************************/
21 #include <stdio.h>
22 #include <time.h>
23 #include <stdlib.h>
24 #include <ctype.h>
25 #include "misc.h"
26 #include "crypto.h"
27 #include "sb1.h"
29 static int sdram_size_table[] = {2, 8, 16, 32, 64};
31 #define NR_SDRAM_ENTRIES (int)(sizeof(sdram_size_table) / sizeof(sdram_size_table[0]))
33 int sb1_sdram_size_by_index(int index)
35 if(index < 0 || index >= NR_SDRAM_ENTRIES)
36 return -1;
37 return sdram_size_table[index];
40 int sb1_sdram_index_by_size(int size)
42 for(int i = 0; i < NR_SDRAM_ENTRIES; i++)
43 if(sdram_size_table[i] == size)
44 return i;
45 return -1;
48 static uint16_t swap16(uint16_t t)
50 return (t << 8) | (t >> 8);
53 static void fix_version(struct sb1_version_t *ver)
55 ver->major = swap16(ver->major);
56 ver->minor = swap16(ver->minor);
57 ver->revision = swap16(ver->revision);
60 enum sb1_error_t sb1_write_file(struct sb1_file_t *sb, const char *filename)
62 if(sb->key.method != CRYPTO_XOR_KEY)
63 return SB1_NO_VALID_KEY;
64 /* compute image size (without userdata) */
65 uint32_t image_size = 0;
66 image_size += sizeof(struct sb1_header_t);
67 for(int i = 0; i < sb->nr_insts; i++)
69 switch(sb->insts[i].cmd)
71 case SB1_INST_LOAD:
72 image_size += 8 + ROUND_UP(sb->insts[i].size, 4);
73 break;
74 case SB1_INST_FILL:
75 case SB1_INST_JUMP:
76 case SB1_INST_CALL:
77 image_size += 12;
78 break;
79 case SB1_INST_MODE:
80 case SB1_INST_SDRAM:
81 image_size += 8;
82 break;
83 default:
84 bugp("Unknown SB instruction: %#x\n", sb->insts[i].cmd);
87 // now take crypto marks and sector size into account:
88 // there is one crypto mark per sector, ie 4 bytes for 508 = 512 (sector)
89 image_size += 4 * ((image_size + SECTOR_SIZE - 5) / (SECTOR_SIZE - 4));
90 image_size = ROUND_UP(image_size, SECTOR_SIZE);
92 /* allocate buffer and fill it (ignoring crypto for now) */
93 void *buf = xmalloc(image_size);
94 struct sb1_header_t *header = buf;
95 memset(buf, 0, image_size);
96 header->rom_version = sb->rom_version;
97 header->image_size = image_size + sb->userdata_size;
98 header->header_size = sizeof(struct sb1_header_t);
99 header->userdata_offset = sb->userdata ? image_size : 0;
100 memcpy(&header->product_ver, &sb->product_ver, sizeof(sb->product_ver));
101 fix_version(&header->product_ver);
102 memcpy(&header->component_ver, &sb->component_ver, sizeof(sb->component_ver));
103 fix_version(&header->component_ver);
104 header->drive_tag = sb->drive_tag;
105 strncpy((void *)header->signature, "STMP", 4);
107 struct sb1_cmd_header_t *cmd = (void *)(header + 1);
108 for(int i = 0; i < sb->nr_insts; i++)
110 int bytes = 0;
111 int size = 0;
112 switch(sb->insts[i].cmd)
114 case SB1_INST_LOAD:
115 bytes = sb->insts[i].size;
116 cmd->addr = sb->insts[i].addr;
117 memcpy(cmd + 1, sb->insts[i].data, sb->insts[i].size);
118 memset((void *)(cmd + 1) + sb->insts[i].size, 0,
119 bytes - sb->insts[i].size);
120 break;
121 case SB1_INST_FILL:
122 bytes = sb->insts[i].size;
123 size = 2;
124 memcpy(cmd + 1, &sb->insts[i].pattern, 4);
125 cmd->addr = sb->insts[i].addr;
126 break;
127 case SB1_INST_JUMP:
128 case SB1_INST_CALL:
129 bytes = 4;
130 cmd->addr = sb->insts[i].addr;
131 memcpy(cmd + 1, &sb->insts[i].argument, 4);
132 break;
133 case SB1_INST_MODE:
134 bytes = 0;
135 cmd->addr = sb->insts[i].mode;
136 break;
137 case SB1_INST_SDRAM:
138 bytes = 0;
139 cmd->addr = SB1_MK_ADDR_SDRAM(sb->insts[i].sdram.chip_select,
140 sb->insts[i].sdram.size_index);
141 break;
142 default:
143 bugp("Unknown SB instruction: %#x\n", sb->insts[i].cmd);
146 /* handle most common cases */
147 if(size == 0)
148 size = ROUND_UP(bytes, 4) / 4 + 1;
150 cmd->cmd = SB1_MK_CMD(sb->insts[i].cmd, sb->insts[i].datatype,
151 bytes, sb->insts[i].critical,
152 size);
154 cmd = (void *)cmd + 4 + size * 4;
157 /* move everything to prepare crypto marks (start at the end !) */
158 for(int i = image_size / SECTOR_SIZE - 1; i >= 0; i--)
159 memmove(buf + i * SECTOR_SIZE, buf + i * (SECTOR_SIZE - 4), SECTOR_SIZE - 4);
161 union xorcrypt_key_t key[2];
162 memcpy(key, sb->key.u.xor_key, sizeof(sb->key));
163 void *ptr = header + 1;
164 int offset = header->header_size;
165 for(unsigned i = 0; i < image_size / SECTOR_SIZE; i++)
167 int size = SECTOR_SIZE - 4 - offset;
168 uint32_t mark = xor_encrypt(key, ptr, size);
169 *(uint32_t *)(ptr + size) = mark;
171 ptr += size + 4;
172 offset = 0;
175 FILE *fd = fopen(filename, "wb");
176 if(fd == NULL)
177 return SB1_OPEN_ERROR;
178 if(fwrite(buf, image_size, 1, fd) != 1)
180 free(buf);
181 return SB1_WRITE_ERROR;
183 free(buf);
184 if(sb->userdata)
185 fwrite(sb->userdata, sb->userdata_size, 1, fd);
186 fclose(fd);
188 return SB1_SUCCESS;
191 struct sb1_file_t *sb1_read_file(const char *filename, void *u,
192 sb1_color_printf cprintf, enum sb1_error_t *err)
194 return sb1_read_file_ex(filename, 0, -1, u, cprintf, err);
197 struct sb1_file_t *sb1_read_file_ex(const char *filename, size_t offset, size_t size, void *u,
198 sb1_color_printf cprintf, enum sb1_error_t *err)
200 #define fatal(e, ...) \
201 do { if(err) *err = e; \
202 cprintf(u, true, GREY, __VA_ARGS__); \
203 free(buf); \
204 return NULL; } while(0)
206 FILE *f = fopen(filename, "rb");
207 void *buf = NULL;
208 if(f == NULL)
209 fatal(SB1_OPEN_ERROR, "Cannot open file for reading\n");
210 fseek(f, 0, SEEK_END);
211 size_t read_size = ftell(f);
212 fseek(f, offset, SEEK_SET);
213 if(size != (size_t)-1)
214 read_size = size;
215 buf = xmalloc(read_size);
216 if(fread(buf, read_size, 1, f) != 1)
218 fclose(f);
219 fatal(SB1_READ_ERROR, "Cannot read file\n");
221 fclose(f);
223 struct sb1_file_t *ret = sb1_read_memory(buf, read_size, u, cprintf, err);
224 free(buf);
225 return ret;
227 #undef fatal
230 static const char *sb1_cmd_name(int cmd)
232 switch(cmd)
234 case SB1_INST_LOAD: return "load";
235 case SB1_INST_FILL: return "fill";
236 case SB1_INST_JUMP: return "jump";
237 case SB1_INST_CALL: return "call";
238 case SB1_INST_MODE: return "mode";
239 case SB1_INST_SDRAM: return "sdram";
240 default: return "unknown";
244 static const char *sb1_datatype_name(int cmd)
246 switch(cmd)
248 case SB1_DATATYPE_UINT32: return "uint32";
249 case SB1_DATATYPE_UINT16: return "uint16";
250 case SB1_DATATYPE_UINT8: return "uint8";
251 default: return "unknown";
255 struct sb1_file_t *sb1_read_memory(void *_buf, size_t filesize, void *u,
256 sb1_color_printf cprintf, enum sb1_error_t *err)
258 struct sb1_file_t *file = NULL;
259 uint8_t *buf = _buf;
261 #define printf(c, ...) cprintf(u, false, c, __VA_ARGS__)
262 #define fatal(e, ...) \
263 do { if(err) *err = e; \
264 cprintf(u, true, GREY, __VA_ARGS__); \
265 sb1_free(file); \
266 return NULL; } while(0)
267 #define print_hex(c, p, len, nl) \
268 do { printf(c, ""); print_hex(p, len, nl); } while(0)
270 file = xmalloc(sizeof(struct sb1_file_t));
271 memset(file, 0, sizeof(struct sb1_file_t));
272 struct sb1_header_t *header = (struct sb1_header_t *)buf;
274 if(memcmp(header->signature, "STMP", 4) != 0)
275 fatal(SB1_FORMAT_ERROR, "Bad signature\n");
276 if(header->image_size > filesize)
277 fatal(SB1_FORMAT_ERROR, "File too small (should be at least %d bytes)\n",
278 header->image_size);
279 if(header->header_size != sizeof(struct sb1_header_t))
280 fatal(SB1_FORMAT_ERROR, "Bad header size\n");
282 printf(BLUE, "Basic info:\n");
283 printf(GREEN, " ROM version: ");
284 printf(YELLOW, "%x\n", header->rom_version);
285 printf(GREEN, " Userdata offset: ");
286 printf(YELLOW, "%x\n", header->userdata_offset);
287 printf(GREEN, " Pad: ");
288 printf(YELLOW, "%x\n", header->pad2);
290 struct sb1_version_t product_ver = header->product_ver;
291 fix_version(&product_ver);
292 struct sb1_version_t component_ver = header->component_ver;
293 fix_version(&component_ver);
295 printf(GREEN, " Product version: ");
296 printf(YELLOW, "%X.%X.%X\n", product_ver.major, product_ver.minor, product_ver.revision);
297 printf(GREEN, " Component version: ");
298 printf(YELLOW, "%X.%X.%X\n", component_ver.major, component_ver.minor, component_ver.revision);
300 printf(GREEN, " Drive tag: ");
301 printf(YELLOW, "%x\n", header->drive_tag);
303 /* copy rom version, padding and drive tag */
304 /* copy versions */
305 memcpy(&file->product_ver, &product_ver, sizeof(product_ver));
306 memcpy(&file->component_ver, &component_ver, sizeof(component_ver));
307 file->rom_version = header->rom_version;
308 file->pad2 = header->pad2;
309 file->drive_tag = header->drive_tag;
311 /* reduce size w.r.t to userdata part */
312 uint32_t userdata_size = 0;
313 if(header->userdata_offset != 0)
315 userdata_size = header->image_size - header->userdata_offset;
316 header->image_size -= userdata_size;
319 if(header->image_size % SECTOR_SIZE)
321 if(!g_force)
322 printf(GREY, "Image size is not a multiple of sector size\n");
323 else
324 fatal(SB1_FORMAT_ERROR, "Image size is not a multiple of sector size\n");
327 /* find key */
328 union xorcrypt_key_t key[2];
329 memset(key, 0, sizeof(key));
330 bool valid_key = false;
331 uint8_t sector[SECTOR_SIZE];
333 for(int i = 0; i < g_nr_keys; i++)
335 if(!g_key_array[i].method == CRYPTO_XOR_KEY)
336 continue;
337 /* copy key and data because it's modified by the crypto code */
338 memcpy(key, g_key_array[i].u.xor_key, sizeof(key));
339 memcpy(sector, header + 1, SECTOR_SIZE - header->header_size);
340 /* try to decrypt the first sector */
341 uint32_t mark = xor_decrypt(key, sector, SECTOR_SIZE - 4 - header->header_size);
342 /* copy key again it's modified by the crypto code */
343 memcpy(key, g_key_array[i].u.xor_key, sizeof(key));
344 if(mark != *(uint32_t *)&sector[SECTOR_SIZE - 4 - header->header_size])
345 continue;
346 /* found ! */
347 valid_key = true;
348 break;
351 if(!valid_key)
353 if(!g_force)
354 fatal(SB1_NO_VALID_KEY, "No valid key found\n");
355 printf(GREY, "No valid key found: forced to continue but this will fail\n");
358 printf(BLUE, "Crypto\n");
359 for(int i = 0; i < 2; i++)
361 printf(RED, " Key %d\n", i);
362 printf(OFF, " ");
363 for(int j = 0; j < 64; j++)
365 printf(YELLOW, "%02x ", key[i].key[j]);
366 if((j + 1) % 16 == 0)
368 printf(OFF, "\n");
369 if(j + 1 != 64)
370 printf(OFF, " ");
375 memcpy(file->key.u.xor_key, key, sizeof(key));
377 /* decrypt image in-place (and removing crypto markers) */
378 void *ptr = header + 1;
379 void *copy_ptr = header + 1;
380 int offset = header->header_size;
381 for(unsigned i = 0; i < header->image_size / SECTOR_SIZE; i++)
383 int size = SECTOR_SIZE - 4 - offset;
384 uint32_t mark = xor_decrypt(key, ptr, size);
385 if(mark != *(uint32_t *)(ptr + size) && !g_force)
386 fatal(SB1_CHECKSUM_ERROR, "Crypto mark mismatch\n");
387 memmove(copy_ptr, ptr, size);
389 ptr += size + 4;
390 copy_ptr += size;
391 offset = 0;
394 /* reduce image size given the removed marks */
395 header->image_size -= header->image_size / SECTOR_SIZE;
397 printf(BLUE, "Commands\n");
398 struct sb1_cmd_header_t *cmd = (void *)(header + 1);
399 while((void *)cmd < (void *)header + header->image_size)
401 printf(GREEN, " Command");
402 printf(YELLOW, " %#x\n", cmd->cmd);
403 printf(YELLOW, " Size:");
404 printf(RED, " %#x\n", SB1_CMD_SIZE(cmd->cmd));
405 printf(YELLOW, " Critical:");
406 printf(RED, " %d\n", SB1_CMD_CRITICAL(cmd->cmd));
407 printf(YELLOW, " Data Type:");
408 printf(RED, " %#x ", SB1_CMD_DATATYPE(cmd->cmd));
409 printf(GREEN, "(%s)\n", sb1_datatype_name(SB1_CMD_DATATYPE(cmd->cmd)));
410 printf(YELLOW, " Bytes:");
411 printf(RED, " %#x\n", SB1_CMD_BYTES(cmd->cmd));
412 printf(YELLOW, " Boot:");
413 printf(RED, " %#x ", SB1_CMD_BOOT(cmd->cmd));
414 printf(GREEN, "(%s)\n", sb1_cmd_name(SB1_CMD_BOOT(cmd->cmd)));
416 /* copy command */
417 struct sb1_inst_t inst;
418 memset(&inst, 0, sizeof(inst));
419 inst.cmd = SB1_CMD_BOOT(cmd->cmd);
420 inst.critical = SB1_CMD_CRITICAL(cmd->cmd);
421 inst.datatype = SB1_CMD_DATATYPE(cmd->cmd);
422 inst.size = SB1_CMD_BYTES(cmd->cmd);
424 switch(SB1_CMD_BOOT(cmd->cmd))
426 case SB1_INST_SDRAM:
427 inst.sdram.chip_select = SB1_ADDR_SDRAM_CS(cmd->addr);
428 inst.sdram.size_index = SB1_ADDR_SDRAM_SZ(cmd->addr);
429 printf(YELLOW, " Ram:");
430 printf(RED, " %#x", inst.addr);
431 printf(GREEN, " (Chip Select=%d, Size=%d)\n", SB1_ADDR_SDRAM_CS(cmd->addr),
432 sb1_sdram_size_by_index(SB1_ADDR_SDRAM_SZ(cmd->addr)));
433 break;
434 case SB1_INST_MODE:
435 inst.mode = cmd->addr;
436 printf(YELLOW, " Mode:");
437 printf(RED, " %#x\n", inst.mode);
438 break;
439 case SB1_INST_LOAD:
440 inst.data = malloc(inst.size);
441 memcpy(inst.data, cmd + 1, inst.size);
442 inst.addr = cmd->addr;
443 printf(YELLOW, " Addr:");
444 printf(RED, " %#x\n", inst.addr);
445 break;
446 case SB1_INST_FILL:
447 inst.addr = cmd->addr;
448 inst.pattern = *(uint32_t *)(cmd + 1);
449 printf(YELLOW, " Addr:");
450 printf(RED, " %#x\n", cmd->addr);
451 printf(YELLOW, " Pattern:");
452 printf(RED, " %#x\n", inst.pattern);
453 break;
454 case SB1_INST_CALL:
455 case SB1_INST_JUMP:
456 inst.addr = cmd->addr;
457 inst.argument = *(uint32_t *)(cmd + 1);
458 printf(YELLOW, " Addr:");
459 printf(RED, " %#x\n", cmd->addr);
460 printf(YELLOW, " Argument:");
461 printf(RED, " %#x\n", inst.pattern);
462 break;
463 default:
464 printf(GREY, "WARNING: unknown SB command !\n");
465 break;
468 file->insts = augment_array(file->insts, sizeof(inst), file->nr_insts, &inst, 1);
469 file->nr_insts++;
471 /* last instruction ? */
472 if(SB1_CMD_BOOT(cmd->cmd) == SB1_INST_JUMP ||
473 SB1_CMD_BOOT(cmd->cmd) == SB1_INST_MODE)
474 break;
476 cmd = (void *)cmd + 4 + 4 * SB1_CMD_SIZE(cmd->cmd);
479 /* copy userdata */
480 file->userdata_size = userdata_size;
481 if(userdata_size > 0)
483 file->userdata = malloc(userdata_size);
484 memcpy(file->userdata, (void *)header + header->userdata_offset, userdata_size);
487 return file;
488 #undef printf
489 #undef fatal
490 #undef print_hex
493 void sb1_free(struct sb1_file_t *file)
495 if(!file) return;
497 for(int i = 0; i < file->nr_insts; i++)
498 free(file->insts[i].data);
499 free(file->insts);
500 free(file->userdata);
501 free(file);
504 void sb1_dump(struct sb1_file_t *file, void *u, sb1_color_printf cprintf)
506 #define printf(c, ...) cprintf(u, false, c, __VA_ARGS__)
507 #define print_hex(c, p, len, nl) \
508 do { printf(c, ""); print_hex(p, len, nl); } while(0)
510 #define TREE RED
511 #define HEADER GREEN
512 #define TEXT YELLOW
513 #define TEXT2 BLUE
514 #define TEXT3 RED
515 #define SEP OFF
517 printf(BLUE, "SB1 File\n");
518 printf(TREE, "+-");
519 printf(HEADER, "Rom Ver: ");
520 printf(TEXT, "%x\n", file->rom_version);
521 printf(TREE, "+-");
522 printf(HEADER, "Pad: ");
523 printf(TEXT, "%x\n", file->pad2);
524 printf(TREE, "+-");
525 printf(HEADER, "Drive Tag: ");
526 printf(TEXT, "%x\n", file->drive_tag);
527 printf(TREE, "+-");
528 printf(HEADER, "Product Version: ");
529 printf(TEXT, "%X.%X.%X\n", file->product_ver.major, file->product_ver.minor,
530 file->product_ver.revision);
531 printf(TREE, "+-");
532 printf(HEADER, "Component Version: ");
533 printf(TEXT, "%X.%X.%X\n", file->component_ver.major, file->component_ver.minor,
534 file->component_ver.revision);
536 for(int j = 0; j < file->nr_insts; j++)
538 struct sb1_inst_t *inst = &file->insts[j];
539 printf(TREE, "+-");
540 printf(HEADER, "Command\n");
541 printf(TREE, "| +-");
542 switch(inst->cmd)
544 case SB1_INST_CALL:
545 case SB1_INST_JUMP:
546 printf(HEADER, "%s", inst->cmd == SB1_INST_CALL ? "CALL" : "JUMP");
547 printf(SEP, " | ");
548 printf(TEXT3, "crit=%d", inst->critical);
549 printf(SEP, " | ");
550 printf(TEXT, "addr=0x%08x\n", inst->addr);
551 break;
552 case SB1_INST_LOAD:
553 printf(HEADER, "LOAD");
554 printf(SEP, " | ");
555 printf(TEXT3, "crit=%d", inst->critical);
556 printf(SEP, " | ");
557 printf(TEXT, "addr=0x%08x", inst->addr);
558 printf(SEP, " | ");
559 printf(TEXT2, "len=0x%08x\n", inst->size);
560 break;
561 case SB1_INST_FILL:
562 printf(HEADER, "FILL");
563 printf(SEP, " | ");
564 printf(TEXT3, "crit=%d", inst->critical);
565 printf(SEP, " | ");
566 printf(TEXT, "addr=0x%08x", inst->addr);
567 printf(SEP, " | ");
568 printf(TEXT2, "len=0x%08x", inst->size);
569 printf(SEP, " | ");
570 printf(TEXT2, "pattern=0x%08x\n", inst->pattern);
571 break;
572 case SB1_INST_MODE:
573 printf(HEADER, "MODE");
574 printf(SEP, " | ");
575 printf(TEXT3, "crit=%d", inst->critical);
576 printf(SEP, " | ");
577 printf(TEXT, "mode=0x%08x\n", inst->addr);
578 break;
579 case SB1_INST_SDRAM:
580 printf(HEADER, "SRAM");
581 printf(SEP, " | ");
582 printf(TEXT3, "crit=%d", inst->critical);
583 printf(SEP, " | ");
584 printf(TEXT, "chip_select=%d", inst->sdram.chip_select);
585 printf(SEP, " | ");
586 printf(TEXT2, "chip_size=%d\n", sb1_sdram_size_by_index(inst->sdram.size_index));
587 break;
588 default:
589 printf(GREY, "[Unknown instruction %x]\n", inst->cmd);
590 break;
594 #undef printf
595 #undef print_hex
598 static struct crypto_key_t g_default_xor_key =
600 .method = CRYPTO_XOR_KEY,
601 .u.xor_key =
603 {.k = {0x67ECAEF6, 0xB31FB961, 0x118A9F4C, 0xA32A97DA,
604 0x6CC39617, 0x5BC00314, 0x9D430685, 0x4D7DB502,
605 0xA347685E, 0x3C87E86C, 0x8987AAA0, 0x24B78EF1,
606 0x893B9605, 0x9BB8C2BE, 0x6D9544E2, 0x375B525C}},
607 {.k = {0x3F424704, 0x53B5A331, 0x6AD345A5, 0x20DCEC51,
608 0x743C8D3B, 0x444B3792, 0x0AF429569, 0xB7EE1111,
609 0x583BF768, 0x9683BF9A, 0x0B032D799, 0xFE4E78ED,
610 0xF20D08C2, 0xFA0BE4A2, 0x4D89C317, 0x887B2D6F}}
614 void sb1_get_default_key(struct crypto_key_t *key)
616 memcpy(key, &g_default_xor_key, sizeof(g_default_xor_key));
617 /* decrypt the xor key which is xor'ed */
618 for(int i = 0; i < 2; i++)
619 for(int j = 0; j < 16; j++)
620 key->u.xor_key[i].k[j] ^= 0xaa55aa55;