| 1 | //----------------------------------------------------------------------------- |
| 2 | // This code is licensed to you under the terms of the GNU GPL, version 2 or, |
| 3 | // at your option, any later version. See the LICENSE.txt file for the text of |
| 4 | // the license. |
| 5 | //----------------------------------------------------------------------------- |
| 6 | // Wiegand functions |
| 7 | //----------------------------------------------------------------------------- |
| 8 | |
| 9 | #include "wiegand.h" |
| 10 | |
| 11 | /* |
| 12 | * @brief getParity |
| 13 | * @param bits pointer to the source bitstream of binary values 0|1 |
| 14 | * @param len how long shall parity be calculated |
| 15 | * @param type use the defined values EVEN|ODD |
| 16 | * @return parity bit required to match type |
| 17 | */ |
| 18 | uint8_t getParity( uint8_t *bits, uint8_t len, uint8_t type ) { |
| 19 | uint8_t x = 0; |
| 20 | for(; len > 0; --len) |
| 21 | x += bits[len - 1]; |
| 22 | |
| 23 | return (x & 1 ) ^ type; |
| 24 | } |
| 25 | |
| 26 | // by marshmellow |
| 27 | /* pass bits to be tested in bits, length bits passed in bitLen, and parity type EVEN|ODD in type |
| 28 | * @brief checkParity |
| 29 | * @param bits pointer to the source bitstream of binary values 0|1 |
| 30 | * @param len number of bits to be checked |
| 31 | * @param type use the defined values EVEN|ODD |
| 32 | * @return 1 if passed |
| 33 | */ |
| 34 | uint8_t checkParity(uint32_t bits, uint8_t len, uint8_t type); |
| 35 | |
| 36 | // by marshmellow |
| 37 | // takes a array of binary values, start position, length of bits per parity (includes parity bit), |
| 38 | // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) |
| 39 | size_t removeParity(uint8_t *bitstream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) { |
| 40 | uint32_t parityWd = 0; |
| 41 | size_t j = 0, bitcount = 0; |
| 42 | for (int word = 0; word < (bLen); word += pLen){ |
| 43 | for (int bit = 0; bit < pLen; ++bit){ |
| 44 | parityWd = (parityWd << 1) | bitstream[startIdx+word+bit]; |
| 45 | bitstream[j++] = (bitstream[startIdx + word + bit]); |
| 46 | } |
| 47 | j--; // overwrite parity with next data |
| 48 | // if parity fails then return 0 |
| 49 | switch (pType) { |
| 50 | case 3: if (bitstream[j] == 1) return 0; break; //should be 0 spacer bit |
| 51 | case 2: if (bitstream[j] == 0) return 0; break; //should be 1 spacer bit |
| 52 | default: //test parity |
| 53 | if (parityTest(parityWd, pLen, pType) == 0) return 0; break; |
| 54 | } |
| 55 | bitcount += ( pLen - 1 ); |
| 56 | parityWd = 0; |
| 57 | } |
| 58 | // if we got here then all the parities passed |
| 59 | //return ID start index and size |
| 60 | return bitcount; |
| 61 | } |
| 62 | |
| 63 | |
| 64 | // by marshmellow |
| 65 | // takes a array of binary values, length of bits per parity (includes parity bit), |
| 66 | // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run) |
| 67 | // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added |
| 68 | /* |
| 69 | * @brief addParity |
| 70 | * @param bits pointer to the source bitstream of binary values |
| 71 | * @param dest pointer to the destination where parities together with bits are added. |
| 72 | * @param sourceLen number of |
| 73 | * @param pLen length bits to be checked |
| 74 | * @param pType EVEN|ODD|2 (always 1's)|3 (always 0's) |
| 75 | * @return |
| 76 | */ |
| 77 | size_t addParity(uint8_t *bits, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType) |
| 78 | { |
| 79 | uint32_t parityWd = 0; |
| 80 | size_t j = 0, bitCnt = 0; |
| 81 | for (int word = 0; word < sourceLen; word += pLen-1) { |
| 82 | for (int bit = 0; bit < pLen-1; ++bit){ |
| 83 | parityWd = (parityWd << 1) | bits[word+bit]; |
| 84 | dest[j++] = (bits[word+bit]); |
| 85 | } |
| 86 | |
| 87 | // if parity fails then return 0 |
| 88 | switch (pType) { |
| 89 | case 3: dest[j++] = 0; break; // marker bit which should be a 0 |
| 90 | case 2: dest[j++] = 1; break; // marker bit which should be a 1 |
| 91 | default: |
| 92 | dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1; |
| 93 | break; |
| 94 | } |
| 95 | bitCnt += pLen; |
| 96 | parityWd = 0; |
| 97 | } |
| 98 | // if we got here then all the parities passed |
| 99 | //return ID start index and size |
| 100 | return bitCnt; |
| 101 | } |
| 102 | |
| 103 | // by marshmellow |
| 104 | /* |
| 105 | * add HID parity to binary array: EVEN prefix for 1st half of ID, ODD suffix for 2nd half |
| 106 | * @brief wiegand_add_parity |
| 107 | * @param source pointer to source of binary data |
| 108 | * @param dest pointer to the destination where wiegandparity has been appended |
| 109 | * @param len number of bits which wiegand parity shall be calculated over. This number is without parities, so a wiegand 26 has 24 bits of data |
| 110 | */ |
| 111 | void wiegand_add_parity(uint8_t *source, uint8_t *dest, uint8_t len) { |
| 112 | |
| 113 | // Copy to destination, shifted one step to make room for EVEN parity |
| 114 | memcpy(dest+1, source, length); |
| 115 | |
| 116 | // half length, Even and Odd is calculated to the middle. |
| 117 | uint8_t len_h2 = length >> 1; |
| 118 | |
| 119 | // add EVEN parity at the beginning |
| 120 | *(dest) = GetParity(source, EVEN, len_h2); |
| 121 | |
| 122 | dest += length + 1; |
| 123 | |
| 124 | // add ODD parity at the very end |
| 125 | *(dest) = GetParity(source + len_h2, ODD, len_h2); |
| 126 | } |
| 127 | |
| 128 | //uint32_t bytebits_to_byte(uint8_t* src, size_t numbits); |
| 129 | #define MAX_BITS_TXX55 6*4*8 |
| 130 | #define MAX_BYTES_TXX55 6*4 |
| 131 | /* |
| 132 | * @brief num_to_wiegand_bytes |
| 133 | * @param oem Sometimes call FF Fixfield, SiteCode. Used in a few formats |
| 134 | * @param fc Facility code |
| 135 | * @param cn Card number |
| 136 | * @param dest pointer to the destination where wiegand bytes will be stored |
| 137 | * @param formatlen |
| 138 | */ |
| 139 | void num_to_wiegand_bytes(uint64_t oem, uint64_t fc, uint64_t cn, uint8_t *dest, uint8_t formatlen){ |
| 140 | |
| 141 | uint8_t data[MAX_BITS_TXX55] = {0}; |
| 142 | memset(data, 0, sizeof(data)); |
| 143 | |
| 144 | num_to_wiegand_bits(oem, fc, cn, data, formatlen); |
| 145 | |
| 146 | // loop |
| 147 | // (formatlen / 32 ) + 1 |
| 148 | // (formatlen >> 5) + 1 |
| 149 | for (int i = 0; i < formatlen ; ++i){ |
| 150 | uint32_t value = bytebits_to_byte( data + (i * 32), 32); |
| 151 | num_to_bytes(value, 32, dest + (i*4) ); |
| 152 | } |
| 153 | |
| 154 | } |
| 155 | /* |
| 156 | * @brief num_to_wiegand_bits |
| 157 | * @param oem Sometimes call FF Fixfield, SiteCode. Used in a few formats |
| 158 | * @param fc Facility code |
| 159 | * @param cn Card number |
| 160 | * @param dest pointer to the destination where wiegand bits will be stored |
| 161 | * @param formatlen |
| 162 | */ |
| 163 | void num_to_wiegand_bits(uint64_t oem, uint64_t fc, uint64_t cn, uint8_t *dest, uint8_t formatlen){ |
| 164 | |
| 165 | uint8_t bits[MAX_BITS_TXX55] = {0}; |
| 166 | memset(bits, 0, sizeof(bits)); |
| 167 | uint8_t *temp = bits; |
| 168 | uint64_t value = 0; |
| 169 | |
| 170 | switch ( formatlen ){ |
| 171 | case 26 : // 26bit HID H10301 |
| 172 | fc &= 0xFF; // 8bits |
| 173 | cn &= 0xFFFF; // 16bits |
| 174 | value = fc << 16 | cn; |
| 175 | num_to_bytebits(value, 24, temp); |
| 176 | wiegand_add_parity(temp, dest, 24); |
| 177 | break; |
| 178 | case 261: // 26bit Indala |
| 179 | fc &= 0xFFF; // 12bits |
| 180 | cn &= 0xFFF; // 12bits |
| 181 | value = fc << 12 | cn; |
| 182 | num_to_bytebits(value, 24, temp); |
| 183 | wiegand_add_parity(temp, dest, 24); |
| 184 | break; |
| 185 | case 34 : // 34bits HID |
| 186 | fc &= 0xFFFF; // 16bits |
| 187 | cn &= 0xFFFF; // 16bits |
| 188 | value = fc << 16 | cn; |
| 189 | num_to_bytebits(value, 32, temp); |
| 190 | wiegand_add_parity(temp, dest, 32); |
| 191 | break; |
| 192 | case 35 : // 35bits HID |
| 193 | fc &= 0xFFF; // 12bits |
| 194 | cn &= 0xFFFFFF; // 20bits |
| 195 | value = fc << 20 | cn; |
| 196 | num_to_bytebits(value, 32, temp); |
| 197 | wiegand_add_parity(temp, dest, 32); |
| 198 | break; |
| 199 | case 37 : // H10304 |
| 200 | fc &= 0xFFFF; // 16bits |
| 201 | cn &= 0x7FFFF; // 19bits |
| 202 | value = fc << 19 | cn; |
| 203 | num_to_bytebits(value, 35, temp); |
| 204 | wiegand_add_parity(temp, dest, 35); |
| 205 | break; |
| 206 | case 39 : // 39bit KERI System Pyramid |
| 207 | fc &= 0x1FFFF; // 17bits |
| 208 | cn &= 0xFFFFFFFF; // 20bits |
| 209 | value = fc << 20 | cn; |
| 210 | num_to_bytebits(value, 37, temp); |
| 211 | wiegand_add_parity(temp, dest, 37); |
| 212 | break; |
| 213 | case 44 : // 44bit KERI system Pyramid |
| 214 | oem &= 0xFF; // 8bits |
| 215 | fc &= 0xFFF; // 12bits |
| 216 | cn &= 0xFFFFFFFF; // 21bits |
| 217 | value = oem << 20 | fc << 12 | cn; |
| 218 | num_to_bytebits(value, 42, temp); |
| 219 | wiegand_add_parity(temp, dest, 42); |
| 220 | break; |
| 221 | case 50 : // AWID 50 RBH |
| 222 | fc &= 0xFFFF; // 16bits |
| 223 | cn &= 0xFFFFFFFF // 32bits |
| 224 | value = fc << 32 | cn; |
| 225 | num_to_bytebits(value, 48, temp); |
| 226 | wiegand_add_parity(temp, dest, 48); // verify! |
| 227 | break; |
| 228 | default: |
| 229 | break; |
| 230 | } |
| 231 | } |