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