client/src/wiegand_formatutils.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 // Wiegand 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 "wiegand_formatutils.h"
  16 #include "ui.h"
  17
  18 uint8_t get_bit_by_position(wiegand_message_t *data, 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     uint8_t result = 0;
  22     if (pos > 95)
  23         result = 0;
  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(wiegand_message_t *data, bool value, 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 |= (1UL << (pos - 64));
  40         else
  41             data->Top &= ~(1UL << (pos - 64));
  42         return true;
  43     } else if (pos > 31) {
  44         if (value)
  45             data->Mid |= (1UL << (pos - 32));
  46         else
  47             data->Mid &= ~(1UL << (pos - 32));
  48         return true;
  49     } else {
  50         if (value)
  51             data->Bot |= (1UL << pos);
  52         else
  53             data->Bot &= ~(1UL << 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 wiegand_message struct changes, this function must also
  65  * be updated to match.
  66  */
  67 static void message_datacopy(wiegand_message_t *src, wiegand_message_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(wiegand_message_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(wiegand_message_t *data, uint64_t value, uint8_t firstBit, uint8_t length) {
  88     wiegand_message_t tmpdata;
  89     message_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)
  95         message_datacopy(&tmpdata, data);
  96
  97     return result;
  98 }
  99
 100 uint64_t get_nonlinear_field(wiegand_message_t *data, uint8_t numBits, uint8_t *bits) {
 101     uint64_t result = 0;
 102     for (int i = 0; i < numBits; i++) {
 103         result = (result << 1) | get_bit_by_position(data, *(bits + i));
 104     }
 105     return result;
 106 }
 107 bool set_nonlinear_field(wiegand_message_t *data, uint64_t value, uint8_t numBits, uint8_t *bits) {
 108
 109     wiegand_message_t tmpdata;
 110     message_datacopy(data, &tmpdata);
 111
 112     bool result = true;
 113     for (int i = 0; i < numBits; i++) {
 114         result &= set_bit_by_position(&tmpdata, (value >> ((numBits - i) - 1)) & 1, *(bits + i));
 115     }
 116
 117     if (result)
 118         message_datacopy(&tmpdata, data);
 119
 120     return result;
 121 }
 122
 123 static uint8_t get_length_from_header(wiegand_message_t *data) {
 124     /**
 125      * detect if message has "preamble" / "sentinel bit"
 126      *
 127      */
 128
 129
 130     uint8_t len = 0;
 131     uint32_t hfmt = 0; // for calculating card length
 132
 133     if ((data->Top & 0x000FFFFF) > 0) { // > 64 bits
 134         hfmt = data->Top & 0x000FFFFF;
 135         len = 64;
 136     } else if ((data->Mid & 0xFFFFFFC0) > 0) { // < 63-38 bits
 137         hfmt = data->Mid & 0xFFFFFFC0;
 138         len = 32;
 139     } else if (data->Mid && (data->Mid & 0x00000020) == 0) { // 37 bits;
 140         hfmt = 0;
 141         len = 37;
 142     } else if ((data->Mid & 0x0000001F) > 0) { // 36-32 bits
 143         hfmt = data->Mid & 0x0000001F;
 144         len = 32;
 145     } else {
 146         hfmt = data->Bot;
 147         len = 0;
 148     }
 149
 150     while (hfmt > 1) {
 151         hfmt >>= 1;
 152         len++;
 153     }
 154     if (len < 26)
 155         len = 26;
 156     return len;
 157 }
 158
 159 wiegand_message_t initialize_message_object(uint32_t top, uint32_t mid, uint32_t bot, int n) {
 160     wiegand_message_t result;
 161     memset(&result, 0, sizeof(wiegand_message_t));
 162
 163     result.Top = top;
 164     result.Mid = mid;
 165     result.Bot = bot;
 166     if (n > 0)
 167         result.Length = n;
 168     else
 169         result.Length = get_length_from_header(&result);
 170     return result;
 171 }
 172
 173 bool add_HID_header(wiegand_message_t *data) {
 174     // Invalid value
 175     if (data->Length > 84 || data->Length == 0)
 176         return false;
 177
 178     if (data->Length >= 64) {
 179         data->Top |= 0x09e00000; // Extended-length header
 180         data->Top |= 1U << (data->Length - 64); // leading 1: start bit
 181     } else if (data->Length > 37) {
 182         data->Top |= 0x09e00000; // Extended-length header
 183         data->Mid |= 1U << (data->Length - 32); // leading 1: start bit
 184     } else if (data->Length == 37) {
 185         // No header bits added to 37-bit cards
 186     } else if (data->Length >= 32) {
 187         data->Mid |= 0x20; // Bit 37; standard header
 188         data->Mid |= 1U << (data->Length - 32); // leading 1: start bit
 189     } else {
 190         data->Mid |= 0x20; // Bit 37; standard header
 191         data->Bot |= 1U << data->Length; // leading 1: start bit
 192     }
 193     return true;
 194 }