348 lines
9 KiB
C
348 lines
9 KiB
C
/***************************************************************************
|
|
* __________ __ ___.
|
|
* Open \______ \ ____ ____ | | _\_ |__ _______ ___
|
|
* Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
|
|
* Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
|
|
* Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
|
|
* \/ \/ \/ \/ \/
|
|
* $Id$
|
|
*
|
|
* Copyright (C) 2010 Amaury Pouly
|
|
*
|
|
* This program is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU General Public License
|
|
* as published by the Free Software Foundation; either version 2
|
|
* of the License, or (at your option) any later version.
|
|
*
|
|
* This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
|
|
* KIND, either express or implied.
|
|
*
|
|
****************************************************************************/
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <time.h>
|
|
#include <ctype.h>
|
|
#include "misc.h"
|
|
|
|
bool g_debug = false;
|
|
bool g_force = false;
|
|
|
|
/**
|
|
* Misc
|
|
*/
|
|
|
|
void *memdup(const void *p, size_t len)
|
|
{
|
|
void *cpy = xmalloc(len);
|
|
memcpy(cpy, p, len);
|
|
return cpy;
|
|
}
|
|
|
|
void generate_random_data(void *buf, size_t sz)
|
|
{
|
|
size_t i = 0;
|
|
unsigned char* p = (unsigned char*)buf;
|
|
while(i++ < sz)
|
|
*p++ = rand();
|
|
}
|
|
|
|
void *xmalloc(size_t s)
|
|
{
|
|
void * r = malloc(s);
|
|
if(!r) bugp("malloc");
|
|
return r;
|
|
}
|
|
|
|
int convxdigit(char digit, byte *val)
|
|
{
|
|
if(digit >= '0' && digit <= '9')
|
|
{
|
|
*val = digit - '0';
|
|
return 0;
|
|
}
|
|
else if(digit >= 'A' && digit <= 'F')
|
|
{
|
|
*val = digit - 'A' + 10;
|
|
return 0;
|
|
}
|
|
else if(digit >= 'a' && digit <= 'f')
|
|
{
|
|
*val = digit - 'a' + 10;
|
|
return 0;
|
|
}
|
|
else
|
|
return 1;
|
|
}
|
|
|
|
/* helper function to augment an array, free old array */
|
|
void *augment_array(void *arr, size_t elem_sz, size_t cnt, void *aug, size_t aug_cnt)
|
|
{
|
|
void *p = xmalloc(elem_sz * (cnt + aug_cnt));
|
|
memcpy(p, arr, elem_sz * cnt);
|
|
memcpy(p + elem_sz * cnt, aug, elem_sz * aug_cnt);
|
|
free(arr);
|
|
return p;
|
|
}
|
|
|
|
void augment_array_ex(void **arr, size_t elem_sz, int *cnt, int *capacity,
|
|
void *aug, int aug_cnt)
|
|
{
|
|
/* if capacity is not large enough, double it */
|
|
if(*cnt + aug_cnt > *capacity)
|
|
{
|
|
if(*capacity == 0)
|
|
*capacity = 1;
|
|
while(*cnt + aug_cnt > *capacity)
|
|
*capacity *= 2;
|
|
void *p = xmalloc(elem_sz * (*capacity));
|
|
memcpy(p, *arr, elem_sz * (*cnt));
|
|
free(*arr);
|
|
*arr = p;
|
|
}
|
|
/* copy elements */
|
|
memcpy(*arr + elem_sz * (*cnt), aug, elem_sz * aug_cnt);
|
|
*cnt += aug_cnt;
|
|
}
|
|
|
|
/**
|
|
* Key file parsing
|
|
*/
|
|
int g_nr_keys;
|
|
key_array_t g_key_array;
|
|
|
|
bool parse_key(char **pstr, struct crypto_key_t *key)
|
|
{
|
|
char *str = *pstr;
|
|
/* ignore spaces */
|
|
while(isspace(*str))
|
|
str++;
|
|
/* CRYPTO_KEY: 32 hex characters
|
|
* CRYPTO_USBOTP: usbotp(vid:pid) where vid and pid are hex numbers
|
|
* CRYPTO_XOR_KEY: 256 hex characters */
|
|
if(isxdigit(str[0]) && strlen(str) >= 256 && isxdigit(str[32]))
|
|
{
|
|
for(int j = 0; j < 128; j++)
|
|
{
|
|
byte a, b;
|
|
if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b))
|
|
return false;
|
|
key->u.xor_key[j / 64].key[j % 64] = (a << 4) | b;
|
|
}
|
|
/* skip key */
|
|
*pstr = str + 256;
|
|
key->method = CRYPTO_XOR_KEY;
|
|
return true;
|
|
}
|
|
else if(isxdigit(str[0]))
|
|
{
|
|
if(strlen(str) < 32)
|
|
return false;
|
|
for(int j = 0; j < 16; j++)
|
|
{
|
|
byte a, b;
|
|
if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b))
|
|
return false;
|
|
key->u.key[j] = (a << 4) | b;
|
|
}
|
|
/* skip key */
|
|
*pstr = str + 32;
|
|
key->method = CRYPTO_KEY;
|
|
return true;
|
|
}
|
|
else
|
|
{
|
|
const char *prefix = "usbotp(";
|
|
if(strlen(str) < strlen(prefix))
|
|
return false;
|
|
if(strncmp(str, prefix, strlen(prefix)) != 0)
|
|
return false;
|
|
str += strlen(prefix);
|
|
/* vid */
|
|
long vid = strtol(str, &str, 16);
|
|
if(vid < 0 || vid > 0xffff)
|
|
return false;
|
|
if(*str++ != ':')
|
|
return false;
|
|
/* pid */
|
|
long pid = strtol(str, &str, 16);
|
|
if(pid < 0 || pid > 0xffff)
|
|
return false;
|
|
if(*str++ != ')')
|
|
return false;
|
|
*pstr = str;
|
|
key->method = CRYPTO_USBOTP;
|
|
key->u.vid_pid = vid << 16 | pid;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
void add_keys(key_array_t ka, int kac)
|
|
{
|
|
key_array_t new_ka = xmalloc((g_nr_keys + kac) * sizeof(struct crypto_key_t));
|
|
memcpy(new_ka, g_key_array, g_nr_keys * sizeof(struct crypto_key_t));
|
|
memcpy(new_ka + g_nr_keys, ka, kac * sizeof(struct crypto_key_t));
|
|
free(g_key_array);
|
|
g_key_array = new_ka;
|
|
g_nr_keys += kac;
|
|
}
|
|
|
|
void clear_keys()
|
|
{
|
|
free(g_key_array);
|
|
g_nr_keys = 0;
|
|
g_key_array = NULL;
|
|
}
|
|
|
|
bool add_keys_from_file(const char *key_file)
|
|
{
|
|
int size;
|
|
FILE *fd = fopen(key_file, "r");
|
|
if(fd == NULL)
|
|
{
|
|
if(g_debug)
|
|
perror("cannot open key file");
|
|
return false;
|
|
}
|
|
fseek(fd, 0, SEEK_END);
|
|
size = ftell(fd);
|
|
fseek(fd, 0, SEEK_SET);
|
|
char *buf = xmalloc(size + 1);
|
|
if(fread(buf, 1, size, fd) != (size_t)size)
|
|
{
|
|
if(g_debug)
|
|
perror("Cannot read key file");
|
|
fclose(fd);
|
|
return false;
|
|
}
|
|
buf[size] = 0;
|
|
fclose(fd);
|
|
|
|
if(g_debug)
|
|
printf("Parsing key file '%s'...\n", key_file);
|
|
char *p = buf;
|
|
while(1)
|
|
{
|
|
struct crypto_key_t k;
|
|
/* parse key */
|
|
if(!parse_key(&p, &k))
|
|
{
|
|
if(g_debug)
|
|
printf("invalid key file\n");
|
|
return false;
|
|
}
|
|
if(g_debug)
|
|
{
|
|
printf("Add key: ");
|
|
print_key(&k, true);
|
|
}
|
|
add_keys(&k, 1);
|
|
/* request at least one space character before next key, or end of file */
|
|
if(*p != 0 && !isspace(*p))
|
|
{
|
|
if(g_debug)
|
|
printf("invalid key file\n");
|
|
return false;
|
|
}
|
|
/* skip whitespace */
|
|
while(isspace(*p))
|
|
p++;
|
|
if(*p == 0)
|
|
break;
|
|
}
|
|
free(buf);
|
|
return true;
|
|
}
|
|
|
|
void print_hex(byte *data, int len, bool newline)
|
|
{
|
|
for(int i = 0; i < len; i++)
|
|
printf("%02X ", data[i]);
|
|
if(newline)
|
|
printf("\n");
|
|
}
|
|
|
|
void print_key(struct crypto_key_t *key, bool newline)
|
|
{
|
|
switch(key->method)
|
|
{
|
|
case CRYPTO_KEY:
|
|
print_hex(key->u.key, 16, false);
|
|
break;
|
|
case CRYPTO_USBOTP:
|
|
printf("USB-OTP(%04x:%04x)", key->u.vid_pid >> 16, key->u.vid_pid & 0xffff);
|
|
break;
|
|
case CRYPTO_NONE:
|
|
printf("none");
|
|
break;
|
|
case CRYPTO_XOR_KEY:
|
|
print_hex(&key->u.xor_key[0].key[0], 64, false);
|
|
print_hex(&key->u.xor_key[1].key[0], 64, false);
|
|
break;
|
|
default:
|
|
printf("unknown");
|
|
}
|
|
if(newline)
|
|
printf("\n");
|
|
}
|
|
|
|
char OFF[] = { 0x1b, 0x5b, 0x31, 0x3b, '0', '0', 0x6d, '\0' };
|
|
|
|
char GREY[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '0', 0x6d, '\0' };
|
|
char RED[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '1', 0x6d, '\0' };
|
|
char GREEN[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '2', 0x6d, '\0' };
|
|
char YELLOW[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '3', 0x6d, '\0' };
|
|
char BLUE[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '4', 0x6d, '\0' };
|
|
|
|
static bool g_color_enable = true;
|
|
|
|
void enable_color(bool enable)
|
|
{
|
|
g_color_enable = enable;
|
|
}
|
|
|
|
void color(color_t c)
|
|
{
|
|
if(g_color_enable)
|
|
printf("%s", (char *)c);
|
|
}
|
|
|
|
enum sb_version_guess_t guess_sb_version(const char *filename)
|
|
{
|
|
#define ret(x) do { if(f) fclose(f); return x; } while(0)
|
|
FILE *f = fopen(filename, "rb");
|
|
if(f == NULL)
|
|
ret(SB_VERSION_ERR);
|
|
// check signature
|
|
uint8_t sig[4];
|
|
if(fseek(f, 20, SEEK_SET))
|
|
ret(SB_VERSION_UNK);
|
|
if(fread(sig, 4, 1, f) != 1)
|
|
ret(SB_VERSION_UNK);
|
|
if(memcmp(sig, "STMP", 4) != 0)
|
|
ret(SB_VERSION_UNK);
|
|
// check header size (v1)
|
|
uint32_t hdr_size;
|
|
if(fseek(f, 8, SEEK_SET))
|
|
ret(SB_VERSION_UNK);
|
|
if(fread(&hdr_size, 4, 1, f) != 1)
|
|
ret(SB_VERSION_UNK);
|
|
if(hdr_size == 0x34)
|
|
ret(SB_VERSION_1);
|
|
// check header params relationship
|
|
struct
|
|
{
|
|
uint16_t nr_keys; /* Number of encryption keys */
|
|
uint16_t key_dict_off; /* Offset to key dictionary (in blocks) */
|
|
uint16_t header_size; /* In blocks */
|
|
uint16_t nr_sections; /* Number of sections */
|
|
uint16_t sec_hdr_size; /* Section header size (in blocks) */
|
|
} __attribute__((packed)) u;
|
|
if(fseek(f, 0x28, SEEK_SET))
|
|
ret(SB_VERSION_UNK);
|
|
if(fread(&u, sizeof(u), 1, f) != 1)
|
|
ret(SB_VERSION_UNK);
|
|
if(u.sec_hdr_size == 1 && u.header_size == 6 && u.key_dict_off == u.header_size + u.nr_sections)
|
|
ret(SB_VERSION_2);
|
|
ret(SB_VERSION_UNK);
|
|
#undef ret
|
|
}
|