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     if(isxdigit(str[0]))
 122     {
 123         if(strlen(str) < 32)
 124             return false;
 125         for(int j = 0; j < 16; j++)
 126         {
 127             byte a, b;
 128             if(convxdigit(str[2 * j], &a) || convxdigit(str[2 * j + 1], &b))
 129                 return false;
 130             key->u.key[j] = (a << 4) | b;
 131         }
 132         /* skip key */
 133         *pstr = str + 32;
 134         key->method = CRYPTO_KEY;
 135         return true;
 136     }
 137     else
 138     {
 139         const char *prefix = "usbotp(";
 140         if(strlen(str) < strlen(prefix))
 141             return false;
 142         if(strncmp(str, prefix, strlen(prefix)) != 0)
 143             return false;
 144         str += strlen(prefix);
 145         /* vid */
 146         long vid = strtol(str, &str, 16);
 147         if(vid < 0 || vid > 0xffff)
 148             return false;
 149         if(*str++ != ':')
 150             return false;
 151         /* pid */
 152         long pid = strtol(str, &str, 16);
 153         if(pid < 0 || pid > 0xffff)
 154             return false;
 155         if(*str++ != ')')
 156             return false;
 157         *pstr = str;
 158         key->method = CRYPTO_USBOTP;
 159         key->u.vid_pid = vid << 16 | pid;
 160         return true;
 161     }
 162 }
 163
 164 void add_keys(key_array_t ka, int kac)
 165 {
 166     key_array_t new_ka = xmalloc((g_nr_keys + kac) * sizeof(struct crypto_key_t));
 167     memcpy(new_ka, g_key_array, g_nr_keys * sizeof(struct crypto_key_t));
 168     memcpy(new_ka + g_nr_keys, ka, kac * sizeof(struct crypto_key_t));
 169     free(g_key_array);
 170     g_key_array = new_ka;
 171     g_nr_keys += kac;
 172 }
 173
 174 void clear_keys()
 175 {
 176     free(g_key_array);
 177     g_nr_keys = 0;
 178     g_key_array = NULL;
 179 }
 180
 181 bool add_keys_from_file(const char *key_file)
 182 {
 183     int size;
 184     FILE *fd = fopen(key_file, "r");
 185     if(fd == NULL)
 186     {
 187         if(g_debug)
 188             perror("cannot open key file");
 189         return false;
 190     }
 191     fseek(fd, 0, SEEK_END);
 192     size = ftell(fd);
 193     fseek(fd, 0, SEEK_SET);
 194     char *buf = xmalloc(size + 1);
 195     if(fread(buf, 1, size, fd) != (size_t)size)
 196     {
 197         if(g_debug)
 198             perror("Cannot read key file");
 199         fclose(fd);
 200         return false;
 201     }
 202     buf[size] = 0;
 203     fclose(fd);
 204
 205     if(g_debug)
 206         printf("Parsing key file '%s'...\n", key_file);
 207     char *p = buf;
 208     while(1)
 209     {
 210         struct crypto_key_t k;
 211         /* parse key */
 212         if(!parse_key(&p, &k))
 213         {
 214             if(g_debug)
 215                 printf("invalid key file\n");
 216             return false;
 217         }
 218         if(g_debug)
 219         {
 220             printf("Add key: ");
 221             print_key(&k, true);
 222         }
 223         add_keys(&k, 1);
 224         /* request at least one space character before next key, or end of file */
 225         if(*p != 0 && !isspace(*p))
 226         {
 227             if(g_debug)
 228                 printf("invalid key file\n");
 229             return false;
 230         }
 231         /* skip whitespace */
 232         while(isspace(*p))
 233             p++;
 234         if(*p == 0)
 235             break;
 236     }
 237     free(buf);
 238     return true;
 239 }
 240
 241 void print_hex(byte *data, int len, bool newline)
 242 {
 243     for(int i = 0; i < len; i++)
 244         printf("%02X ", data[i]);
 245     if(newline)
 246         printf("\n");
 247 }
 248
 249 void print_key(struct crypto_key_t *key, bool newline)
 250 {
 251     switch(key->method)
 252     {
 253         case CRYPTO_KEY:
 254             print_hex(key->u.key, 16, false);
 255             break;
 256         case CRYPTO_USBOTP:
 257             printf("USB-OTP(%04x:%04x)", key->u.vid_pid >> 16, key->u.vid_pid & 0xffff);
 258             break;
 259         case CRYPTO_NONE:
 260             printf("none");
 261             break;
 262         case CRYPTO_XOR_KEY:
 263             print_hex(&key->u.xor_key[0].key[0], 64, false);
 264             print_hex(&key->u.xor_key[1].key[0], 64, false);
 265             break;
 266         default:
 267             printf("unknown");
 268     }
 269     if(newline)
 270         printf("\n");
 271 }
 272
 273 char OFF[] = { 0x1b, 0x5b, 0x31, 0x3b, '0', '0', 0x6d, '\0' };
 274
 275 char GREY[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '0', 0x6d, '\0' };
 276 char RED[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '1', 0x6d, '\0' };
 277 char GREEN[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '2', 0x6d, '\0' };
 278 char YELLOW[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '3', 0x6d, '\0' };
 279 char BLUE[] = { 0x1b, 0x5b, 0x31, 0x3b, '3', '4', 0x6d, '\0' };
 280
 281 static bool g_color_enable = true;
 282
 283 void enable_color(bool enable)
 284 {
 285     g_color_enable = enable;
 286 }
 287
 288 void color(color_t c)
 289 {
 290     if(g_color_enable)
 291         printf("%s", (char *)c);
 292 }