utils/imxtools/sbtools/misc.c

   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 bool g_force = false;
  29
  30 /**
  31  * Misc
  32  */
  33
  34 void *memdup(const void *p, size_t len)
  35 {
  36     void *cpy = xmalloc(len);
  37     memcpy(cpy, p, len);
  38     return cpy;
  39 }
  40
  41 void generate_random_data(void *buf, size_t sz)
  42 {
  43     size_t i = 0;
  44     unsigned char* p = (unsigned char*)buf;
  45     while(i++ < sz)
  46         *p++ = rand();
  47 }
  48
  49 void *xmalloc(size_t s)
  50 {
  51     void * r = malloc(s);
  52     if(!r) bugp("malloc");
  53     return r;
  54 }
  55
  56 int convxdigit(char digit, byte *val)
  57 {
  58     if(digit >= '0' && digit <= '9')
  59     {
  60         *val = digit - '0';
  61         return 0;
  62     }
  63     else if(digit >= 'A' && digit <= 'F')
  64     {
  65         *val = digit - 'A' + 10;
  66         return 0;
  67     }
  68     else if(digit >= 'a' && digit <= 'f')
  69     {
  70         *val = digit - 'a' + 10;
  71         return 0;
  72     }
  73     else
  74         return 1;
  75 }
  76
  77 /* helper function to augment an array, free old array */
  78 void *augment_array(void *arr, size_t elem_sz, size_t cnt, void *aug, size_t aug_cnt)
  79 {
  80     void *p = xmalloc(elem_sz * (cnt + aug_cnt));
  81     memcpy(p, arr, elem_sz * cnt);
  82     memcpy(p + elem_sz * cnt, aug, elem_sz * aug_cnt);
  83     free(arr);
  84     return p;
  85 }
  86
  87 void augment_array_ex(void **arr, size_t elem_sz, int *cnt, int *capacity,
  88     void *aug, int aug_cnt)
  89 {
  90     /* if capacity is not large enough, double it */
  91     if(*cnt + aug_cnt > *capacity)
  92     {
  93         if(*capacity == 0)
  94             *capacity = 1;
  95         while(*cnt + aug_cnt > *capacity)
  96             *capacity *= 2;
  97         void *p = xmalloc(elem_sz * (*capacity));
  98         memcpy(p, *arr, elem_sz * (*cnt));
  99         free(*arr);
 100         *arr = p;
 101     }
 102     /* copy elements */
 103     memcpy(*arr + elem_sz * (*cnt), aug, elem_sz * aug_cnt);
 104     *cnt += aug_cnt;
 105 }
 106
 107 /**
 108  * Key file parsing
 109  */
 110 int g_nr_keys;
 111 key_array_t g_key_array;
 112
 113 bool parse_key(char **pstr, struct crypto_key_t *key)
 114 {
 115     char *str = *pstr;
 116     /* ignore spaces */
 117     while(isspace(*str))
 118         str++;
 119     /* CRYPTO_KEY: 32 hex characters
 120      * CRYPTO_USBOTP: usbotp(vid:pid) where vid and pid are hex numbers
 121      * CRYPTO_XOR_KEY: 256 hex characters */
 122     if(isxdigit(str[0]) && strlen(str) >= 256 && isxdigit(str[32]))
 123     {
 124         for(int j = 0; j < 128; j++)
 125         {
 126             byte a, b;
 127             if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b))
 128                 return false;
 129             key->u.xor_key[j / 64].key[j % 64] = (a << 4) | b;
 130         }
 131         /* skip key */
 132         *pstr = str + 256;
 133         key->method = CRYPTO_XOR_KEY;
 134         return true;
 135     }
 136     else if(isxdigit(str[0]))
 137     {
 138         if(strlen(str) < 32)
 139             return false;
 140         for(int j = 0; j < 16; j++)
 141         {
 142             byte a, b;
 143             if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b))
 144                 return false;
 145             key->u.key[j] = (a << 4) | b;
 146         }
 147         /* skip key */
 148         *pstr = str + 32;
 149         key->method = CRYPTO_KEY;
 150         return true;
 151     }
 152     else
 153     {
 154         const char *prefix = "usbotp(";
 155         if(strlen(str) < strlen(prefix))
 156             return false;
 157         if(strncmp(str, prefix, strlen(prefix)) != 0)
 158             return false;
 159         str += strlen(prefix);
 160         /* vid */
 161         long vid = strtol(str, &str, 16);
 162         if(vid < 0 || vid > 0xffff)
 163             return false;
 164         if(*str++ != ':')
 165             return false;
 166         /* pid */
 167         long pid = strtol(str, &str, 16);
 168         if(pid < 0 || pid > 0xffff)
 169             return false;
 170         if(*str++ != ')')
 171             return false;
 172         *pstr = str;
 173         key->method = CRYPTO_USBOTP;
 174         key->u.vid_pid = vid << 16 | pid;
 175         return true;
 176     }
 177 }
 178
 179 void add_keys(key_array_t ka, int kac)
 180 {
 181     key_array_t new_ka = xmalloc((g_nr_keys + kac) * sizeof(struct crypto_key_t));
 182     memcpy(new_ka, g_key_array, g_nr_keys * sizeof(struct crypto_key_t));
 183     memcpy(new_ka + g_nr_keys, ka, kac * sizeof(struct crypto_key_t));
 184     free(g_key_array);
 185     g_key_array = new_ka;
 186     g_nr_keys += kac;
 187 }
 188
 189 void clear_keys()
 190 {
 191     free(g_key_array);
 192     g_nr_keys = 0;
 193     g_key_array = NULL;
 194 }
 195
 196 bool add_keys_from_file(const char *key_file)
 197 {
 198     int size;
 199     FILE *fd = fopen(key_file, "r");
 200     if(fd == NULL)
 201     {
 202         if(g_debug)
 203             perror("cannot open key file");
 204         return false;
 205     }
 206     fseek(fd, 0, SEEK_END);
 207     size = ftell(fd);
 208     fseek(fd, 0, SEEK_SET);
 209     char *buf = xmalloc(size + 1);
 210     if(fread(buf, 1, size, fd) != (size_t)size)
 211     {
 212         if(g_debug)
 213             perror("Cannot read key file");
 214         fclose(fd);
 215         return false;
 216     }
 217     buf[size] = 0;
 218     fclose(fd);
 219
 220     if(g_debug)
 221         printf("Parsing key file '%s'...\n", key_file);
 222     char *p = buf;
 223     while(1)
 224     {
 225         struct crypto_key_t k;
 226         /* parse key */
 227         if(!parse_key(&p, &k))
 228         {
 229             if(g_debug)
 230                 printf("invalid key file\n");
 231             return false;
 232         }
 233         if(g_debug)
 234         {
 235             printf("Add key: ");
 236             print_key(&k, true);
 237         }
 238         add_keys(&k, 1);
 239         /* request at least one space character before next key, or end of file */
 240         if(*p != 0 && !isspace(*p))
 241         {
 242             if(g_debug)
 243                 printf("invalid key file\n");
 244             return false;
 245         }
 246         /* skip whitespace */
 247         while(isspace(*p))
 248             p++;
 249         if(*p == 0)
 250             break;
 251     }
 252     free(buf);
 253     return true;
 254 }
 255
 256 void print_hex(byte *data, int len, bool newline)
 257 {
 258     for(int i = 0; i < len; i++)
 259         printf("%02X ", data[i]);
 260     if(newline)
 261         printf("\n");
 262 }
 263
 264 void print_key(struct crypto_key_t *key, bool newline)
 265 {
 266     switch(key->method)
 267     {
 268         case CRYPTO_KEY:
 269             print_hex(key->u.key, 16, false);
 270             break;
 271         case CRYPTO_USBOTP:
 272             printf("USB-OTP(%04x:%04x)", key->u.vid_pid >> 16, key->u.vid_pid & 0xffff);
 273             break;
 274         case CRYPTO_NONE:
 275             printf("none");
 276             break;
 277         case CRYPTO_XOR_KEY:
 278             print_hex(&key->u.xor_key[0].key[0], 64, false);
 279             print_hex(&key->u.xor_key[1].key[0], 64, false);
 280             break;
 281         default:
 282             printf("unknown");
 283     }
 284     if(newline)
 285         printf("\n");
 286 }
 287
 288 char OFF[] = { 0x1b, 0x5b, 0x31, 0x3b, '0', '0', 0x6d, '\0' };
 289
 290 char GREY[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '0', 0x6d, '\0' };
 291 char RED[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '1', 0x6d, '\0' };
 292 char GREEN[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '2', 0x6d, '\0' };
 293 char YELLOW[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '3', 0x6d, '\0' };
 294 char BLUE[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '4', 0x6d, '\0' };
 295
 296 static bool g_color_enable = true;
 297
 298 void enable_color(bool enable)
 299 {
 300     g_color_enable = enable;
 301 }
 302
 303 void color(color_t c)
 304 {
 305     if(g_color_enable)
 306         printf("%s", (char *)c);
 307 }