cvs.zerfleddert.de Git - proxmark3-svn/blob - client/loclass/cipherutils.c

   1 /*****************************************************************************
   2  * This file is part of iClassCipher. It is a reconstructon of the cipher engine
   3  * used in iClass, and RFID techology.
   4  *
   5  * The implementation is based on the work performed by
   6  * Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
   7  * Milosch Meriac in the paper "Dismantling IClass".
   8  *
   9  * Copyright (C) 2014 Martin Holst Swende
  10  *
  11  * This is free software: you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as published
  13  * by the Free Software Foundation.
  14  *
  15  * This file is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  *
  20  * You should have received a copy of the GNU General Public License
  21  * along with IClassCipher.  If not, see <http://www.gnu.org/licenses/>.
  22  ****************************************************************************/
  23
  24 #include <stdint.h>
  25 #include <stdio.h>
  26 #include <string.h>
  27 #include "fileutils.h"
  28 #include "cipherutils.h"
  29 /**
  30  *
  31  * @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
  32  * @param stream
  33  * @return
  34  */
  35 bool headBit( BitstreamIn *stream)
  36 {
  37         int bytepos = stream->position >> 3; // divide by 8
  38         int bitpos = (stream->position++) & 7; // mask out 00000111
  39         return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
  40 }
  41 /**
  42  * @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
  43  * @param stream
  44  * @return
  45  */
  46 bool tailBit( BitstreamIn *stream)
  47 {
  48         int bitpos = stream->numbits -1 - (stream->position++);
  49
  50         int bytepos= bitpos >> 3;
  51         bitpos &= 7;
  52         return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
  53 }
  54 /**
  55  * @brief Pushes bit onto the stream
  56  * @param stream
  57  * @param bit
  58  */
  59 void pushBit( BitstreamOut* stream, bool bit)
  60 {
  61         int bytepos = stream->position >> 3; // divide by 8
  62         int bitpos = stream->position & 7;
  63         *(stream->buffer+bytepos) |= (bit & 1) <<  (7 - bitpos);
  64         stream->position++;
  65         stream->numbits++;
  66 }
  67
  68 /**
  69  * @brief Pushes the lower six bits onto the stream
  70  * as b0 b1 b2 b3 b4 b5 b6
  71  * @param stream
  72  * @param bits
  73  */
  74 void push6bits( BitstreamOut* stream, uint8_t bits)
  75 {
  76         pushBit(stream, bits & 0x20);
  77         pushBit(stream, bits & 0x10);
  78         pushBit(stream, bits & 0x08);
  79         pushBit(stream, bits & 0x04);
  80         pushBit(stream, bits & 0x02);
  81         pushBit(stream, bits & 0x01);
  82 }
  83
  84 /**
  85  * @brief bitsLeft
  86  * @param stream
  87  * @return number of bits left in stream
  88  */
  89 int bitsLeft( BitstreamIn *stream)
  90 {
  91         return stream->numbits - stream->position;
  92 }
  93 /**
  94  * @brief numBits
  95  * @param stream
  96  * @return Number of bits stored in stream
  97  */
  98 int numBits(BitstreamOut *stream)
  99 {
 100         return stream->numbits;
 101 }
 102
 103 void x_num_to_bytes(uint64_t n, size_t len, uint8_t* dest)
 104 {
 105         while (len--) {
 106                 dest[len] = (uint8_t) n;
 107                 n >>= 8;
 108         }
 109 }
 110
 111 uint64_t x_bytes_to_num(uint8_t* src, size_t len)
 112 {
 113         uint64_t num = 0;
 114         while (len--)
 115         {
 116                 num = (num << 8) | (*src);
 117                 src++;
 118         }
 119         return num;
 120 }
 121 uint8_t reversebytes(uint8_t b) {
 122         b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
 123         b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
 124         b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
 125    return b;
 126 }
 127 void reverse_arraybytes(uint8_t* arr, size_t len)
 128 {
 129         uint8_t i;
 130         for( i =0; i< len ; i++)
 131         {
 132                 arr[i] = reversebytes(arr[i]);
 133         }
 134 }
 135 void reverse_arraycopy(uint8_t* arr, uint8_t* dest, size_t len)
 136 {
 137         uint8_t i;
 138         for( i =0; i< len ; i++)
 139         {
 140                 dest[i] = reversebytes(arr[i]);
 141         }
 142 }
 143
 144 void printarr(char * name, uint8_t* arr, int len)
 145 {
 146         int cx;
 147         size_t outsize = 40+strlen(name)+len*5;
 148         char* output = malloc(outsize);
 149         memset(output, 0,outsize);
 150
 151         int i ;
 152         cx = snprintf(output,outsize, "uint8_t %s[] = {", name);
 153         for(i =0 ;  i< len ; i++)
 154         {
 155                 cx += snprintf(output+cx,outsize-cx,"0x%02x,",*(arr+i));//5 bytes per byte
 156         }
 157         cx += snprintf(output+cx,outsize-cx,"};");
 158         prnlog(output);
 159 }
 160
 161 void printvar(char * name, uint8_t* arr, int len)
 162 {
 163         int cx;
 164         size_t outsize = 40+strlen(name)+len*2;
 165         char* output = malloc(outsize);
 166         memset(output, 0,outsize);
 167
 168         int i ;
 169         cx = snprintf(output,outsize,"%s = ", name);
 170         for(i =0 ;  i< len ; i++)
 171         {
 172                 cx += snprintf(output+cx,outsize-cx,"%02x",*(arr+i));//2 bytes per byte
 173         }
 174
 175         prnlog(output);
 176 }
 177
 178 void printarr_human_readable(char * title, uint8_t* arr, int len)
 179 {
 180         int cx;
 181         size_t outsize = 100+strlen(title)+len*4;
 182         char* output = malloc(outsize);
 183         memset(output, 0,outsize);
 184
 185
 186         int i;
 187         cx = snprintf(output,outsize,  "\n\t%s\n", title);
 188         for(i =0 ;  i< len ; i++)
 189         {
 190                 if(i % 16 == 0)
 191                         cx += snprintf(output+cx,outsize-cx,"\n%02x| ", i );
 192                 cx += snprintf(output+cx,outsize-cx, "%02x ",*(arr+i));
 193         }
 194         prnlog(output);
 195         free(output);
 196 }
 197
 198 //-----------------------------
 199 // Code for testing below
 200 //-----------------------------
 201
 202
 203 int testBitStream()
 204 {
 205         uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
 206         uint8_t output [] = {0,0,0,0,0,0,0,0};
 207         BitstreamIn in = { input, sizeof(input) * 8,0};
 208         BitstreamOut out ={ output, 0,0}
 209                                           ;
 210         while(bitsLeft(&in) > 0)
 211         {
 212                 pushBit(&out, headBit(&in));
 213                 //printf("Bits left: %d\n", bitsLeft(&in));
 214                 //printf("Bits out: %d\n", numBits(&out));
 215         }
 216         if(memcmp(input, output, sizeof(input)) == 0)
 217         {
 218                 prnlog("    Bitstream test 1 ok");
 219         }else
 220         {
 221                 prnlog("    Bitstream test 1 failed");
 222                 uint8_t i;
 223                 for(i = 0 ; i < sizeof(input) ; i++)
 224                 {
 225                         prnlog("    IN %02x, OUT %02x", input[i], output[i]);
 226                 }
 227                 return 1;
 228         }
 229         return 0;
 230 }
 231
 232 int testReversedBitstream()
 233 {
 234         uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
 235         uint8_t reverse [] = {0,0,0,0,0,0,0,0};
 236         uint8_t output [] = {0,0,0,0,0,0,0,0};
 237         BitstreamIn in = { input, sizeof(input) * 8,0};
 238         BitstreamOut out ={ output, 0,0};
 239         BitstreamIn reversed_in ={ reverse, sizeof(input)*8,0};
 240         BitstreamOut reversed_out ={ reverse,0 ,0};
 241
 242         while(bitsLeft(&in) > 0)
 243         {
 244                 pushBit(&reversed_out, tailBit(&in));
 245         }
 246         while(bitsLeft(&reversed_in) > 0)
 247         {
 248                 pushBit(&out, tailBit(&reversed_in));
 249         }
 250         if(memcmp(input, output, sizeof(input)) == 0)
 251         {
 252                 prnlog("    Bitstream test 2 ok");
 253         }else
 254         {
 255                 prnlog("    Bitstream test 2 failed");
 256                 uint8_t i;
 257                 for(i = 0 ; i < sizeof(input) ; i++)
 258                 {
 259                         prnlog("    IN %02x, MIDDLE: %02x, OUT %02x", input[i],reverse[i], output[i]);
 260                 }
 261                 return 1;
 262         }
 263         return 0;
 264 }
 265
 266
 267 int testCipherUtils(void)
 268 {
 269         prnlog("[+] Testing some internals...");
 270         int retval = 0;
 271         retval |= testBitStream();
 272         retval |= testReversedBitstream();
 273         return retval;
 274 }