git.zerfleddert.de Git - proxmark3-svn/blob - client/hidcardformatutils.c

   1 //-----------------------------------------------------------------------------
   2 // Copyright (C) 2018 grauerfuchs
   3 //
   4 // This code is licensed to you under the terms of the GNU GPL, version 2 or,
   5 // at your option, any later version. See the LICENSE.txt file for the text of
   6 // the license.
   7 //-----------------------------------------------------------------------------
   8 // HID card format packing/unpacking support functions
   9 //-----------------------------------------------------------------------------
  10
  11 #include <stdbool.h>
  12 #include <stdint.h>
  13 #include <stdio.h>
  14 #include <string.h>
  15 #include "hidcardformatutils.h"
  16 #include "ui.h"
  17
  18 bool get_bit_by_position(/* in */hidproxmessage_t* data, /* in */uint8_t pos){
  19   if (pos >= data->Length) return false;
  20   pos = (data->Length - pos) - 1; // invert ordering; Indexing goes from 0 to 1. Subtract 1 for weight of bit.
  21   bool result = false;
  22   if (pos > 95)
  23     result = false;
  24   else if (pos > 63)
  25     result = (data->top >> (pos - 64)) & 1;
  26   else if (pos > 31)
  27     result = (data->mid >> (pos - 32)) & 1;
  28   else
  29     result = (data->bot >> pos) & 1;
  30   return result;
  31 }
  32 bool set_bit_by_position(/* inout */hidproxmessage_t* data, /* in */bool value, /* in */uint8_t pos){
  33   if (pos >= data->Length) return false;
  34   pos = (data->Length - pos) - 1; // invert ordering; Indexing goes from 0 to 1. Subtract 1 for weight of bit.
  35   if (pos > 95) {
  36     return false;
  37   } else if (pos > 63) {
  38     if (value)
  39       data->top |= (1 << (pos - 64));
  40     else
  41       data->top &= ~(1 << (pos - 64));
  42     return true;
  43   } else if (pos > 31) {
  44     if (value)
  45       data->mid |= (1 << (pos - 32));
  46     else
  47       data->mid &= ~(1 << (pos - 32));
  48     return true;
  49   } else {
  50     if (value)
  51       data->bot |= (1 << pos);
  52     else
  53       data->bot &= ~(1 << pos);
  54     return true;
  55   }
  56 }
  57 /**
  58  * Safeguard the data by doing a manual deep copy
  59  *
  60  * At the time of the initial writing, the struct does not contain pointers. That doesn't
  61  * mean it won't eventually contain one, however. To prevent memory leaks and erroneous
  62  * aliasing, perform the copy function manually instead. Hence, this function.
  63  *
  64  * If the definition of the hidproxmessage struct changes, this function must also
  65  * be updated to match.
  66  */
  67 void proxmessage_datacopy(/*in*/hidproxmessage_t* src, /*out*/hidproxmessage_t* dest){
  68   dest->bot = src->bot;
  69   dest->mid = src->mid;
  70   dest->top = src->top;
  71   dest->Length = src->Length;
  72 }
  73 /**
  74  *
  75  * Yes, this is horribly inefficient for linear data.
  76  * The current code is a temporary measure to have a working function in place
  77  * until all the bugs shaken from the block/chunk version of the code.
  78  *
  79  */
  80 uint64_t get_linear_field(/* in */hidproxmessage_t* data, uint8_t firstBit, uint8_t length){
  81   uint64_t result = 0;
  82   for (uint8_t i = 0; i < length; i++ ) {
  83     result = (result << 1) | get_bit_by_position(data, firstBit + i);
  84   }
  85   return result;
  86 }
  87 bool set_linear_field(/* inout */hidproxmessage_t* data, uint64_t value, uint8_t firstBit, uint8_t length){
  88   hidproxmessage_t tmpdata;
  89   proxmessage_datacopy(data, &tmpdata);
  90   bool result = true;
  91   for (int i = 0; i < length; i++){
  92     result &= set_bit_by_position(&tmpdata, (value >> ((length - i) - 1)) & 1, firstBit + i);
  93   }
  94   if (result) proxmessage_datacopy(&tmpdata, data);
  95   return result;
  96 }
  97
  98 uint64_t get_nonlinear_field(/* in */hidproxmessage_t* data, uint8_t numBits, uint8_t* bits){
  99   uint64_t result = 0;
 100   for (int i = 0; i < numBits; i++){
 101     result = (result << 1) | (get_bit_by_position(data, *(bits+i)) & 1);
 102   }
 103   return result;
 104 }
 105 bool set_nonlinear_field(/* inout */hidproxmessage_t* data, uint64_t value, uint8_t numBits, uint8_t* bits){
 106   hidproxmessage_t tmpdata;
 107   proxmessage_datacopy(data, &tmpdata);
 108   bool result = true;
 109   for (int i = 0; i < numBits; i++){
 110     result &= set_bit_by_position(&tmpdata, (value >> ((numBits - i) - 1)) & 1, *(bits + i));
 111   }
 112   if (result) proxmessage_datacopy(&tmpdata, data);
 113   return result;
 114 }
 115
 116 uint8_t get_length_from_header(/* inout */hidproxmessage_t* data) {
 117   uint8_t len = 0;
 118
 119   uint32_t hFmt; // for calculating card length
 120   if ((data->top & 0x000FFFFF) > 0) { // > 64 bits
 121     hFmt = data->top & 0x000FFFFF;
 122     len = 64;
 123   } else if ((data->mid & 0xFFFFFFC0) > 0) { // < 63-38 bits
 124     hFmt = data->mid & 0xFFFFFFC0;
 125     len = 32;
 126   } else if ((data->mid & 0x00000020) == 0) { // 37 bits
 127     hFmt = 0;
 128     len = 37;
 129   } else if ((data->mid & 0x0000001F) > 0){ // 36-32 bits
 130     hFmt = data->mid & 0x0000001F;
 131     len = 32;
 132   } else { // <32 bits
 133     hFmt = data->bot;
 134     len = 0;
 135   }
 136
 137   while (hFmt > 1) {
 138     hFmt >>= 1;
 139     len++;
 140   }
 141   return len;
 142 }
 143
 144 hidproxmessage_t initialize_proxmessage_object(uint32_t top, uint32_t mid, uint32_t bot){
 145   struct hidproxmessage_s result;
 146   memset(&result, 0, sizeof(hidproxmessage_t));
 147   result.top = top;
 148   result.mid = mid;
 149   result.bot = bot;
 150   result.Length = get_length_from_header(&result);
 151   return result;
 152 }
 153 bool add_HID_header(/* inout */hidproxmessage_t* data){
 154   if (data->Length > 84 || data->Length == 0) return false; // Invalid value
 155
 156   if (data->Length >= 64){
 157     data->top |= 1 << (data->Length - 64); // leading 1: start bit
 158     data->top |= 0x09e00000; // Extended-length header
 159   } else if (data->Length > 37){
 160     data->mid |= 1 << (data->Length - 32); // leading 1: start bit
 161     data->top |= 0x09e00000; // Extended-length header
 162   } else if (data->Length == 37){
 163     // No header bits added to 37-bit cards
 164   } else if (data->Length >= 32){
 165     data->mid |= 0x20; // Bit 37; standard header
 166     data->mid |= 1 << (data->Length - 32); // leading 1: start bit
 167   } else {
 168     data->mid |= 0x20; // Bit 37; standard header
 169     data->bot |= 1 << data->Length; // leading 1: start bit
 170   }
 171   return true;
 172 }