client/src/loclass/cipherutils.c

   1 //-----------------------------------------------------------------------------
   2 // Borrowed initially from https://github.com/holiman/loclass
   3 // Copyright (C) 2014 Martin Holst Swende
   4 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
   5 //
   6 // This program is free software: you can redistribute it and/or modify
   7 // it under the terms of the GNU General Public License as published by
   8 // the Free Software Foundation, either version 3 of the License, or
   9 // (at your option) any later version.
  10 //
  11 // This program is distributed in the hope that it will be useful,
  12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 // GNU General Public License for more details.
  15 //
  16 // See LICENSE.txt for the text of the license.
  17 //-----------------------------------------------------------------------------
  18 // WARNING
  19 //
  20 // THIS CODE IS CREATED FOR EXPERIMENTATION AND EDUCATIONAL USE ONLY.
  21 //
  22 // USAGE OF THIS CODE IN OTHER WAYS MAY INFRINGE UPON THE INTELLECTUAL
  23 // PROPERTY OF OTHER PARTIES, SUCH AS INSIDE SECURE AND HID GLOBAL,
  24 // AND MAY EXPOSE YOU TO AN INFRINGEMENT ACTION FROM THOSE PARTIES.
  25 //
  26 // THIS CODE SHOULD NEVER BE USED TO INFRINGE PATENTS OR INTELLECTUAL PROPERTY RIGHTS.
  27 //-----------------------------------------------------------------------------
  28 // It is a reconstruction of the cipher engine used in iClass, and RFID techology.
  29 //
  30 // The implementation is based on the work performed by
  31 // Flavio D. Garcia, Gerhard de Koning Gans, Roel Verdult and
  32 // Milosch Meriac in the paper "Dismantling IClass".
  33 //-----------------------------------------------------------------------------
  34 #include "cipherutils.h"
  35
  36 #include <stdint.h>
  37 #include <stdio.h>
  38 #include <string.h>
  39 #include "util.h" // sprint_hex
  40 #include "commonutil.h"  // ARRAYLEN
  41
  42 #include "fileutils.h"
  43 /**
  44  *
  45  * @brief Return and remove the first bit (x0) in the stream : <x0 x1 x2 x3 ... xn >
  46  * @param stream
  47  * @return
  48  */
  49 bool headBit(BitstreamIn_t *stream) {
  50     int bytepos = stream->position >> 3; // divide by 8
  51     int bitpos = (stream->position++) & 7; // mask out 00000111
  52     return (*(stream->buffer + bytepos) >> (7 - bitpos)) & 1;
  53 }
  54 /**
  55  * @brief Return and remove the last bit (xn) in the stream: <x0 x1 x2 ... xn>
  56  * @param stream
  57  * @return
  58  */
  59 bool tailBit(BitstreamIn_t *stream) {
  60     int bitpos = stream->numbits - 1 - (stream->position++);
  61
  62     int bytepos = bitpos >> 3;
  63     bitpos &= 7;
  64     return (*(stream->buffer + bytepos) >> (7 - bitpos)) & 1;
  65 }
  66 /**
  67  * @brief Pushes bit onto the stream
  68  * @param stream
  69  * @param bit
  70  */
  71 void pushBit(BitstreamOut_t *stream, bool bit) {
  72     int bytepos = stream->position >> 3; // divide by 8
  73     int bitpos = stream->position & 7;
  74     *(stream->buffer + bytepos) |= (bit) << (7 - bitpos);
  75     stream->position++;
  76     stream->numbits++;
  77 }
  78
  79 /**
  80  * @brief Pushes the lower six bits onto the stream
  81  * as b0 b1 b2 b3 b4 b5 b6
  82  * @param stream
  83  * @param bits
  84  */
  85 void push6bits(BitstreamOut_t *stream, uint8_t bits) {
  86     pushBit(stream, bits & 0x20);
  87     pushBit(stream, bits & 0x10);
  88     pushBit(stream, bits & 0x08);
  89     pushBit(stream, bits & 0x04);
  90     pushBit(stream, bits & 0x02);
  91     pushBit(stream, bits & 0x01);
  92 }
  93
  94 /**
  95  * @brief bitsLeft
  96  * @param stream
  97  * @return number of bits left in stream
  98  */
  99 int bitsLeft(BitstreamIn_t *stream) {
 100     return stream->numbits - stream->position;
 101 }
 102 /**
 103  * @brief numBits
 104  * @param stream
 105  * @return Number of bits stored in stream
 106  */
 107 /*
 108 static int numBits(BitstreamOut_t *stream) {
 109     return stream->numbits;
 110 }
 111 */
 112 void x_num_to_bytes(uint64_t n, size_t len, uint8_t *dest) {
 113     while (len--) {
 114         dest[len] = (uint8_t) n;
 115         n >>= 8;
 116     }
 117 }
 118
 119 uint64_t x_bytes_to_num(uint8_t *src, size_t len) {
 120     uint64_t num = 0;
 121     while (len--) {
 122         num = (num << 8) | (*src);
 123         src++;
 124     }
 125     return num;
 126 }
 127
 128 void printarr(const char *name, uint8_t *arr, int len) {
 129     if (name == NULL || arr == NULL) return;
 130
 131     int cx, i;
 132     size_t outsize = 40 + strlen(name) + len * 5;
 133     char *output = calloc(outsize, sizeof(char));
 134     cx = snprintf(output, outsize, "uint8_t %s[] = {", name);
 135     for (i = 0; i < len; i++) {
 136         if (cx < outsize)
 137             cx += snprintf(output + cx, outsize - cx, "0x%02x,", *(arr + i)); //5 bytes per byte
 138     }
 139     if (cx < outsize)
 140         snprintf(output + cx, outsize - cx, "};");
 141     PrintAndLogEx(INFO, output);
 142     free(output);
 143 }
 144
 145 void printarr_human_readable(const char *title, uint8_t *arr, int len) {
 146
 147     if (arr == NULL) return;
 148
 149     int cx = 0, i;
 150     size_t outsize = 100 + strlen(title) + (len * 4);
 151     char *output = calloc(outsize, sizeof(char));
 152     PrintAndLogEx(INFO, "%s", title);
 153     for (i = 0;  i < len; i++) {
 154         if (i % 16 == 0) {
 155
 156             if (i == 0) {
 157                 if (cx < outsize)
 158                     cx += snprintf(output + cx, outsize - cx, "%02x| ", i);
 159             } else {
 160                 if (cx < outsize)
 161                     cx += snprintf(output + cx, outsize - cx, "\n%02x| ", i);
 162             }
 163         }
 164         if (cx < outsize)
 165             cx += snprintf(output + cx, outsize - cx, "%02x ", *(arr + i));
 166     }
 167     PrintAndLogEx(INFO, output);
 168     free(output);
 169 }
 170
 171 //-----------------------------
 172 // Code for testing below
 173 //-----------------------------
 174
 175 #ifndef ON_DEVICE
 176 static int testBitStream(void) {
 177     uint8_t input [] = {0xDE, 0xAD, 0xBE, 0xEF, 0xDE, 0xAD, 0xBE, 0xEF};
 178     uint8_t output [] = {0, 0, 0, 0, 0, 0, 0, 0};
 179     BitstreamIn_t in = { input, sizeof(input) * 8, 0};
 180     BitstreamOut_t out = { output, 0, 0}
 181                          ;
 182     while (bitsLeft(&in) > 0) {
 183         pushBit(&out, headBit(&in));
 184         //printf("Bits left: %d\n", bitsLeft(&in));
 185         //printf("Bits out: %d\n", numBits(&out));
 186     }
 187
 188     if (memcmp(input, output, sizeof(input)) == 0) {
 189         PrintAndLogEx(SUCCESS, "    Bitstream test 1 ( %s )", _GREEN_("ok"));
 190     } else {
 191         PrintAndLogEx(FAILED, "    Bitstream test 1 ( %s )", _RED_("fail"));
 192         uint8_t i;
 193         for (i = 0 ; i < ARRAYLEN(input) ; i++) {
 194             PrintAndLogEx(NORMAL, "    IN %02x, OUT %02x", input[i], output[i]);
 195         }
 196         return PM3_ESOFT;
 197     }
 198     return PM3_SUCCESS;
 199 }
 200
 201 static int testReversedBitstream(void) {
 202     uint8_t input [] = {0xDE, 0xAD, 0xBE, 0xEF, 0xDE, 0xAD, 0xBE, 0xEF};
 203     uint8_t reverse [] = {0, 0, 0, 0, 0, 0, 0, 0};
 204     uint8_t output [] = {0, 0, 0, 0, 0, 0, 0, 0};
 205     BitstreamIn_t in = { input, sizeof(input) * 8, 0};
 206     BitstreamOut_t out = { output, 0, 0};
 207     BitstreamIn_t reversed_in = { reverse, sizeof(input) * 8, 0};
 208     BitstreamOut_t reversed_out = { reverse, 0, 0};
 209
 210     while (bitsLeft(&in) > 0) {
 211         pushBit(&reversed_out, tailBit(&in));
 212     }
 213
 214     while (bitsLeft(&reversed_in) > 0) {
 215         pushBit(&out, tailBit(&reversed_in));
 216     }
 217
 218     if (memcmp(input, output, sizeof(input)) == 0) {
 219         PrintAndLogEx(SUCCESS, "    Bitstream test 2 ( %s )", _GREEN_("ok"));
 220     } else {
 221         PrintAndLogEx(FAILED, "    Bitstream test 2 ( %s )", _RED_("fail"));
 222         uint8_t i;
 223         for (i = 0 ; i < ARRAYLEN(input) ; i++) {
 224             PrintAndLogEx(NORMAL, "    IN %02x, MIDDLE: %02x, OUT %02x", input[i], reverse[i], output[i]);
 225         }
 226         return PM3_ESOFT;
 227     }
 228     return PM3_SUCCESS;
 229 }
 230
 231 int testCipherUtils(void) {
 232     PrintAndLogEx(INFO, "Testing some internals...");
 233     int retval = testBitStream();
 234     if (retval == PM3_SUCCESS)
 235         retval = testReversedBitstream();
 236
 237     return retval;
 238 }
 239 #endif