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