git.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 "cipherutils.h"
  25 #include "../util.h"
  26 #include <stdint.h>
  27 #include <stdio.h>
  28 #include <string.h>
  29
  30 /**
  31  *
  32  * @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
  33  * @param stream
  34  * @return
  35  */
  36 bool headBit( BitstreamIn *stream)
  37 {
  38         int bytepos = stream->position >> 3; // divide by 8
  39         int bitpos = (stream->position++) & 7; // mask out 00000111
  40         return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
  41 }
  42 /**
  43  * @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
  44  * @param stream
  45  * @return
  46  */
  47 bool tailBit( BitstreamIn *stream)
  48 {
  49         int bitpos = stream->numbits -1 - (stream->position++);
  50
  51         int bytepos= bitpos >> 3;
  52         bitpos &= 7;
  53         return (*(stream->buffer + bytepos) >> (7-bitpos)) & 1;
  54 }
  55 /**
  56  * @brief Pushes bit onto the stream
  57  * @param stream
  58  * @param bit
  59  */
  60 void pushBit( BitstreamOut* stream, bool bit)
  61 {
  62         int bytepos = stream->position >> 3; // divide by 8
  63         int bitpos = stream->position & 7;
  64         *(stream->buffer+bytepos) |= (bit & 1) <<  (7 - bitpos);
  65         stream->position++;
  66         stream->numbits++;
  67 }
  68
  69 /**
  70  * @brief Pushes the lower six bits onto the stream
  71  * as b0 b1 b2 b3 b4 b5 b6
  72  * @param stream
  73  * @param bits
  74  */
  75 void push6bits( BitstreamOut* stream, uint8_t bits)
  76 {
  77         pushBit(stream, bits & 0x20);
  78         pushBit(stream, bits & 0x10);
  79         pushBit(stream, bits & 0x08);
  80         pushBit(stream, bits & 0x04);
  81         pushBit(stream, bits & 0x02);
  82         pushBit(stream, bits & 0x01);
  83 }
  84
  85 /**
  86  * @brief bitsLeft
  87  * @param stream
  88  * @return number of bits left in stream
  89  */
  90 int bitsLeft( BitstreamIn *stream)
  91 {
  92         return stream->numbits - stream->position;
  93 }
  94 /**
  95  * @brief numBits
  96  * @param stream
  97  * @return Number of bits stored in stream
  98  */
  99 int numBits(BitstreamOut *stream)
 100 {
 101         return stream->numbits;
 102 }
 103
 104 uint8_t reversebytes(uint8_t b) {
 105         b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
 106         b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
 107         b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
 108    return b;
 109 }
 110 void reverse_arraybytes(uint8_t* arr, size_t len)
 111 {
 112         uint8_t i;
 113         for( i =0; i< len ; i++)
 114         {
 115                 arr[i] = reversebytes(arr[i]);
 116         }
 117 }
 118
 119
 120 //-----------------------------
 121 // Code for testing below
 122 //-----------------------------
 123
 124
 125 int testBitStream()
 126 {
 127         uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
 128         uint8_t output [] = {0,0,0,0,0,0,0,0};
 129         BitstreamIn in = { input, sizeof(input) * 8,0};
 130         BitstreamOut out ={ output, 0,0}
 131                                           ;
 132         while(bitsLeft(&in) > 0)
 133         {
 134                 pushBit(&out, headBit(&in));
 135                 //printf("Bits left: %d\n", bitsLeft(&in));
 136                 //printf("Bits out: %d\n", numBits(&out));
 137         }
 138         if(memcmp(input, output, sizeof(input)) == 0)
 139         {
 140                 printf("Bitstream test 1 ok\n");
 141         }else
 142         {
 143                 printf("Bitstream test 1 failed\n");
 144                 uint8_t i;
 145                 for(i = 0 ; i < sizeof(input) ; i++)
 146                 {
 147                         printf("IN %02x, OUT %02x\n", input[i], output[i]);
 148                 }
 149                 return 1;
 150         }
 151         return 0;
 152 }
 153
 154 int testReversedBitstream()
 155 {
 156         uint8_t input [] = {0xDE,0xAD,0xBE,0xEF,0xDE,0xAD,0xBE,0xEF};
 157         uint8_t reverse [] = {0,0,0,0,0,0,0,0};
 158         uint8_t output [] = {0,0,0,0,0,0,0,0};
 159         BitstreamIn in = { input, sizeof(input) * 8,0};
 160         BitstreamOut out ={ output, 0,0};
 161         BitstreamIn reversed_in ={ reverse, sizeof(input)*8,0};
 162         BitstreamOut reversed_out ={ reverse,0 ,0};
 163
 164         while(bitsLeft(&in) > 0)
 165         {
 166                 pushBit(&reversed_out, tailBit(&in));
 167         }
 168         while(bitsLeft(&reversed_in) > 0)
 169         {
 170                 pushBit(&out, tailBit(&reversed_in));
 171         }
 172         if(memcmp(input, output, sizeof(input)) == 0)
 173         {
 174                 printf("Bitstream test 2 ok\n");
 175         }else
 176         {
 177                 printf("Bitstream test 2 failed\n");
 178                 uint8_t i;
 179                 for(i = 0 ; i < sizeof(input) ; i++)
 180                 {
 181                         printf("IN %02x, MIDDLE: %02x, OUT %02x\n", input[i],reverse[i], output[i]);
 182                 }
 183                 return 1;
 184         }
 185         return 0;
 186 }
 187
 188
 189 int testCipherUtils(void)
 190 {
 191         int retval = 0;
 192         retval |= testBitStream();
 193         retval |= testReversedBitstream();
 194         return retval;
 195 }