client/src/wiegand_formatutils.c

   1 //-----------------------------------------------------------------------------
   2 // Copyright (C) Proxmark3 contributors. See AUTHORS.md for details.
   3 //
   4 // This program is free software: you can redistribute it and/or modify
   5 // it under the terms of the GNU General Public License as published by
   6 // the Free Software Foundation, either version 3 of the License, or
   7 // (at your option) any later version.
   8 //
   9 // This program is distributed in the hope that it will be useful,
  10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 // GNU General Public License for more details.
  13 //
  14 // See LICENSE.txt for the text of the license.
  15 //-----------------------------------------------------------------------------
  16 // Wiegand card format packing/unpacking support functions
  17 //-----------------------------------------------------------------------------
  18
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include "wiegand_formatutils.h"
  24 #include "ui.h"
  25
  26 uint8_t get_bit_by_position(wiegand_message_t *data, uint8_t pos) {
  27     if (pos >= data->Length) return false;
  28     pos = (data->Length - pos) - 1; // invert ordering; Indexing goes from 0 to 1. Subtract 1 for weight of bit.
  29     uint8_t result = 0;
  30     if (pos > 95)
  31         result = 0;
  32     else if (pos > 63)
  33         result = (data->Top >> (pos - 64)) & 1;
  34     else if (pos > 31)
  35         result = (data->Mid >> (pos - 32)) & 1;
  36     else
  37         result = (data->Bot >> pos) & 1;
  38     return result;
  39 }
  40 bool set_bit_by_position(wiegand_message_t *data, bool value, uint8_t pos) {
  41     if (pos >= data->Length) return false;
  42     pos = (data->Length - pos) - 1; // invert ordering; Indexing goes from 0 to 1. Subtract 1 for weight of bit.
  43     if (pos > 95) {
  44         return false;
  45     } else if (pos > 63) {
  46         if (value)
  47             data->Top |= (1UL << (pos - 64));
  48         else
  49             data->Top &= ~(1UL << (pos - 64));
  50         return true;
  51     } else if (pos > 31) {
  52         if (value)
  53             data->Mid |= (1UL << (pos - 32));
  54         else
  55             data->Mid &= ~(1UL << (pos - 32));
  56         return true;
  57     } else {
  58         if (value)
  59             data->Bot |= (1UL << pos);
  60         else
  61             data->Bot &= ~(1UL << pos);
  62         return true;
  63     }
  64 }
  65 /**
  66  * Safeguard the data by doing a manual deep copy
  67  *
  68  * At the time of the initial writing, the struct does not contain pointers. That doesn't
  69  * mean it won't eventually contain one, however. To prevent memory leaks and erroneous
  70  * aliasing, perform the copy function manually instead. Hence, this function.
  71  *
  72  * If the definition of the wiegand_message struct changes, this function must also
  73  * be updated to match.
  74  */
  75 static void message_datacopy(wiegand_message_t *src, wiegand_message_t *dest) {
  76     dest->Bot = src->Bot;
  77     dest->Mid = src->Mid;
  78     dest->Top = src->Top;
  79     dest->Length = src->Length;
  80 }
  81 /**
  82  *
  83  * Yes, this is horribly inefficient for linear data.
  84  * The current code is a temporary measure to have a working function in place
  85  * until all the bugs shaken from the block/chunk version of the code.
  86  *
  87  */
  88 uint64_t get_linear_field(wiegand_message_t *data, uint8_t firstBit, uint8_t length) {
  89     uint64_t result = 0;
  90     for (uint8_t i = 0; i < length; i++) {
  91         result = (result << 1) | get_bit_by_position(data, firstBit + i);
  92     }
  93     return result;
  94 }
  95 bool set_linear_field(wiegand_message_t *data, uint64_t value, uint8_t firstBit, uint8_t length) {
  96     wiegand_message_t tmpdata;
  97     message_datacopy(data, &tmpdata);
  98     bool result = true;
  99     for (int i = 0; i < length; i++) {
 100         result &= set_bit_by_position(&tmpdata, (value >> ((length - i) - 1)) & 1, firstBit + i);
 101     }
 102     if (result)
 103         message_datacopy(&tmpdata, data);
 104
 105     return result;
 106 }
 107
 108 uint64_t get_nonlinear_field(wiegand_message_t *data, uint8_t numBits, uint8_t *bits) {
 109     uint64_t result = 0;
 110     for (int i = 0; i < numBits; i++) {
 111         result = (result << 1) | get_bit_by_position(data, *(bits + i));
 112     }
 113     return result;
 114 }
 115 bool set_nonlinear_field(wiegand_message_t *data, uint64_t value, uint8_t numBits, uint8_t *bits) {
 116
 117     wiegand_message_t tmpdata;
 118     message_datacopy(data, &tmpdata);
 119
 120     bool result = true;
 121     for (int i = 0; i < numBits; i++) {
 122         result &= set_bit_by_position(&tmpdata, (value >> ((numBits - i) - 1)) & 1, *(bits + i));
 123     }
 124
 125     if (result)
 126         message_datacopy(&tmpdata, data);
 127
 128     return result;
 129 }
 130
 131 static uint8_t get_length_from_header(wiegand_message_t *data) {
 132     /**
 133      * detect if message has "preamble" / "sentinel bit"
 134      * Right now we just calculate the highest bit set
 135      * 37 bit formats is hard to detect since it doesnt have a sentinel bit
 136      */
 137     uint8_t len = 0;
 138     uint32_t hfmt = 0; // for calculating card length
 139
 140     if ((data->Top & 0x000FFFFF) > 0) { // > 64 bits
 141         hfmt = data->Top & 0x000FFFFF;
 142         len = 64;
 143     } else if (data->Mid > 0) { // < 63-32 bits
 144
 145         // detect HID format b38 set
 146         if (data->Mid & 0xFFFFFFC0) {
 147             hfmt = data->Mid;
 148             len = 32;
 149         } else {
 150
 151             PrintAndLogEx(DEBUG, "hid preamble detected");
 152             len = 32;
 153
 154             if ((data->Mid ^ 0x20) == 0) { hfmt = data->Bot; len = 0; }
 155             else if ((data->Mid & 0x10) == 0) { hfmt = data->Mid & 0x1F; }
 156             else if ((data->Mid & 0x08) == 0) { hfmt = data->Mid & 0x0F; }
 157             else if ((data->Mid & 0x04) == 0) { hfmt = data->Mid & 0x07; }
 158             else if ((data->Mid & 0x02) == 0) { hfmt = data->Mid & 0x03; }
 159             else if ((data->Mid & 0x01) == 0) { hfmt = data->Mid & 0x01; }
 160             else { hfmt = data->Mid & 0x3F;}
 161         }
 162
 163     } else {
 164         hfmt = data->Bot;
 165         len = 0;
 166     }
 167
 168     while (hfmt > 1) {
 169         hfmt >>= 1;
 170         len++;
 171     }
 172
 173     // everything less than 26 bits found, assume 26 bits
 174     if (len < 26)
 175         len = 26;
 176
 177     return len;
 178 }
 179
 180 wiegand_message_t initialize_message_object(uint32_t top, uint32_t mid, uint32_t bot, int n) {
 181     wiegand_message_t result;
 182     memset(&result, 0, sizeof(wiegand_message_t));
 183
 184     result.Top = top;
 185     result.Mid = mid;
 186     result.Bot = bot;
 187     if (n > 0)
 188         result.Length = n;
 189     else
 190         result.Length = get_length_from_header(&result);
 191     return result;
 192 }
 193
 194 bool add_HID_header(wiegand_message_t *data) {
 195     // Invalid value
 196     if (data->Length > 84 || data->Length == 0)
 197         return false;
 198
 199     if (data->Length == 48) {
 200         data->Mid |= 1U << (data->Length - 32); // Example leading 1: start bit
 201         return true;
 202     }
 203     if (data->Length >= 64) {
 204         data->Top |= 0x09e00000; // Extended-length header
 205         data->Top |= 1U << (data->Length - 64); // leading 1: start bit
 206     } else if (data->Length > 37) {
 207         data->Top |= 0x09e00000; // Extended-length header
 208         data->Mid |= 1U << (data->Length - 32); // leading 1: start bit
 209     } else if (data->Length == 37) {
 210         // No header bits added to 37-bit cards
 211     } else if (data->Length >= 32) {
 212         data->Mid |= 0x20; // Bit 37; standard header
 213         data->Mid |= 1U << (data->Length - 32); // leading 1: start bit
 214     } else {
 215         data->Mid |= 0x20; // Bit 37; standard header
 216         data->Bot |= 1U << data->Length; // leading 1: start bit
 217     }
 218     return true;
 219 }