mag_helpers.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651
  1. #include "mag_helpers.h"
  2. #define TAG "MagHelpers"
  3. #define GPIO_PIN_A &gpio_ext_pa6
  4. #define GPIO_PIN_B &gpio_ext_pa7
  5. #define GPIO_PIN_ENABLE &gpio_ext_pa4
  6. #define RFID_PIN_OUT &gpio_rfid_carrier_out
  7. #define ZERO_PREFIX 25 // n zeros prefix
  8. #define ZERO_BETWEEN 53 // n zeros between tracks
  9. #define ZERO_SUFFIX 25 // n zeros suffix
  10. // bits per char on a given track
  11. const uint8_t bitlen[] = {7, 5, 5};
  12. // char offset by track
  13. const int sublen[] = {32, 48, 48};
  14. uint8_t bit_dir = 0;
  15. void bitbang_raw(bool value, MagSetting* setting)
  16. {
  17. switch(setting->tx) {
  18. case MagTxStateRFID:
  19. furi_hal_gpio_write(RFID_PIN_OUT, value);
  20. break;
  21. case MagTxStateGPIOA6A7:
  22. furi_hal_gpio_write(GPIO_PIN_A, value);
  23. furi_hal_gpio_write(GPIO_PIN_B, !value);
  24. break;
  25. case MagTxCC1101_434:
  26. case MagTxCC1101_868:
  27. furi_hal_gpio_write(&gpio_cc1101_g0, true);
  28. furi_delay_us(64);
  29. furi_hal_gpio_write(&gpio_cc1101_g0, false);
  30. break;
  31. default:
  32. break;
  33. }
  34. }
  35. void play_bit_rf(bool bit, MagSetting* setting) {
  36. bit_dir ^= 1;
  37. furi_hal_gpio_write(&gpio_cc1101_g0, bit_dir);
  38. furi_delay_us(setting->us_clock);
  39. if(bit) {
  40. bit_dir ^= 1;
  41. furi_hal_gpio_write(&gpio_cc1101_g0, bit_dir);
  42. }
  43. furi_delay_us(setting->us_clock);
  44. furi_delay_us(setting->us_interpacket);
  45. }
  46. void play_bit_rfid(uint8_t send_bit, MagSetting* setting) {
  47. // internal TX over RFID coil
  48. bit_dir ^= 1;
  49. furi_hal_gpio_write(RFID_PIN_OUT, bit_dir);
  50. furi_delay_us(setting->us_clock);
  51. if(send_bit) {
  52. bit_dir ^= 1;
  53. furi_hal_gpio_write(RFID_PIN_OUT, bit_dir);
  54. }
  55. furi_delay_us(setting->us_clock);
  56. furi_delay_us(setting->us_interpacket);
  57. }
  58. void play_bit_gpio(uint8_t send_bit, MagSetting* setting) {
  59. // external TX over motor driver wired to PIN_A and PIN_B
  60. bit_dir ^= 1;
  61. furi_hal_gpio_write(GPIO_PIN_A, bit_dir);
  62. furi_hal_gpio_write(GPIO_PIN_B, !bit_dir);
  63. furi_delay_us(setting->us_clock);
  64. if(send_bit) {
  65. bit_dir ^= 1;
  66. furi_hal_gpio_write(GPIO_PIN_A, bit_dir);
  67. furi_hal_gpio_write(GPIO_PIN_B, !bit_dir);
  68. }
  69. furi_delay_us(setting->us_clock);
  70. furi_delay_us(setting->us_interpacket);
  71. }
  72. bool play_bit(uint8_t send_bit, MagSetting* setting) {
  73. // Initialize configured TX method
  74. switch(setting->tx) {
  75. case MagTxStateRFID:
  76. play_bit_rfid(send_bit, setting);
  77. break;
  78. case MagTxStateGPIOA6A7:
  79. play_bit_gpio(send_bit, setting);
  80. break;
  81. case MagTxCC1101_434:
  82. case MagTxCC1101_868:
  83. play_bit_rf(send_bit & 0x01, setting);
  84. break;
  85. default:
  86. return false;
  87. }
  88. return true;
  89. }
  90. void tx_init_rfid() {
  91. // initialize RFID system for TX
  92. furi_hal_power_enable_otg();
  93. furi_hal_ibutton_start_drive();
  94. furi_hal_ibutton_pin_low();
  95. // Initializing at GpioSpeedLow seems sufficient for our needs; no improvements seen by increasing speed setting
  96. // this doesn't seem to make a difference, leaving it in
  97. furi_hal_gpio_init(&gpio_rfid_data_in, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  98. furi_hal_gpio_write(&gpio_rfid_data_in, false);
  99. // false->ground RFID antenna; true->don't ground
  100. // skotopes (RFID dev) say normally you'd want RFID_PULL in high for signal forming, while modulating RFID_OUT
  101. // dunaevai135 had it low in their old code. Leaving low, as it doesn't seem to make a difference on my janky antenna
  102. furi_hal_gpio_init(&gpio_nfc_irq_rfid_pull, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  103. furi_hal_gpio_write(&gpio_nfc_irq_rfid_pull, false);
  104. furi_hal_gpio_init(RFID_PIN_OUT, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  105. // confirm this delay is needed / sufficient? legacy from hackathon...
  106. furi_delay_ms(300);
  107. }
  108. void tx_reset_rfid() {
  109. // reset RFID system
  110. furi_hal_gpio_write(RFID_PIN_OUT, 0);
  111. furi_hal_rfid_pins_reset();
  112. furi_hal_power_disable_otg();
  113. }
  114. void tx_init_gpio() {
  115. furi_hal_power_enable_otg();
  116. // gpio_item_configure_all_pins(GpioModeOutputPushPull);
  117. furi_hal_gpio_init(GPIO_PIN_A, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  118. furi_hal_gpio_init(GPIO_PIN_B, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  119. furi_hal_gpio_init(GPIO_PIN_ENABLE, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  120. furi_hal_gpio_write(GPIO_PIN_ENABLE, 1);
  121. furi_delay_ms(500);
  122. }
  123. void tx_reset_gpio() {
  124. furi_hal_gpio_write(GPIO_PIN_A, 0);
  125. furi_hal_gpio_write(GPIO_PIN_B, 0);
  126. furi_hal_gpio_write(GPIO_PIN_ENABLE, 0);
  127. // set back to analog output mode?
  128. //gpio_item_configure_all_pins(GpioModeAnalog);
  129. furi_hal_power_disable_otg();
  130. }
  131. void tx_init_rf(int hz)
  132. {
  133. // presets and frequency will need some experimenting
  134. furi_hal_subghz_reset();
  135. furi_hal_subghz_load_preset(FuriHalSubGhzPresetOok650Async);
  136. // furi_hal_subghz_load_preset(FuriHalSubGhzPresetGFSK9_99KbAsync);
  137. // furi_hal_subghz_load_preset(FuriHalSubGhzPresetMSK99_97KbAsync);
  138. // furi_hal_subghz_load_preset(FuriHalSubGhzPreset2FSKDev238Async);
  139. // furi_hal_subghz_load_preset(FuriHalSubGhzPreset2FSKDev476Async);
  140. furi_hal_gpio_init(&gpio_cc1101_g0, GpioModeOutputPushPull, GpioPullNo, GpioSpeedLow);
  141. furi_hal_subghz_set_frequency_and_path(hz);
  142. furi_hal_subghz_tx();
  143. furi_hal_gpio_write(&gpio_cc1101_g0, false);
  144. }
  145. void tx_deinit_rf()
  146. {
  147. furi_hal_gpio_write(&gpio_cc1101_g0, false);
  148. furi_hal_subghz_reset();
  149. furi_hal_subghz_idle();
  150. }
  151. bool tx_init(MagSetting* setting) {
  152. // Initialize configured TX method
  153. switch(setting->tx) {
  154. case MagTxStateRFID:
  155. tx_init_rfid();
  156. break;
  157. case MagTxStateGPIOA6A7:
  158. tx_init_gpio();
  159. break;
  160. case MagTxCC1101_434:
  161. tx_init_rf(434000000);
  162. break;
  163. case MagTxCC1101_868:
  164. tx_init_rf(868000000);
  165. break;
  166. default:
  167. return false;
  168. }
  169. return true;
  170. }
  171. bool tx_reset(MagSetting* setting) {
  172. // Reset configured TX method
  173. switch(setting->tx) {
  174. case MagTxStateRFID:
  175. tx_reset_rfid();
  176. break;
  177. case MagTxStateGPIOA6A7:
  178. tx_reset_gpio();
  179. break;
  180. case MagTxCC1101_434:
  181. case MagTxCC1101_868:
  182. tx_deinit_rf();
  183. break;
  184. default:
  185. return false;
  186. }
  187. return true;
  188. }
  189. void track_to_bits(uint8_t* bit_array, const char* track_data, uint8_t track_index) {
  190. // convert individual track to bits
  191. int tmp, crc, lrc = 0;
  192. int i = 0;
  193. // Please forgive the mess. This was a bug battlezone. Will clean up over the weekend
  194. // So many stupid things done here, many learnings lol
  195. //FURI_LOG_D(TAG, "%d", strlen(track_data));
  196. //FURI_LOG_D(TAG, "%d", strlen(track_data) * bitlen[track_index]);
  197. // convert track data to bits
  198. for(uint8_t j = 0; track_data[j] != '\0'; j++) {
  199. crc = 1;
  200. tmp = track_data[j] - sublen[track_index];
  201. for(uint8_t k = 0; k < bitlen[track_index] - 1; k++) {
  202. crc ^= tmp & 1;
  203. lrc ^= (tmp & 1) << k;
  204. bit_array[i] = tmp & 1;
  205. //FURI_LOG_D(
  206. // TAG, "i, j, k: %d %d %d char %s bit %d", i, j, k, &track_data[j], bit_array[i]);
  207. i++;
  208. tmp >>= 1;
  209. }
  210. bit_array[i] = crc;
  211. //FURI_LOG_D(TAG, "i, j: %d %d char %s bit %d", i, j, &track_data[j], bit_array[i]);
  212. i++;
  213. }
  214. FURI_LOG_D(TAG, "LRC");
  215. // finish calculating final "byte" (LRC)
  216. tmp = lrc;
  217. crc = 1;
  218. for(uint8_t j = 0; j < bitlen[track_index] - 1; j++) {
  219. crc ^= tmp & 1;
  220. bit_array[i] = tmp & 1;
  221. //FURI_LOG_D(TAG, "i, j: %d %d bit %d", i, j, bit_array[i]);
  222. i++;
  223. tmp >>= 1;
  224. }
  225. bit_array[i] = crc;
  226. //FURI_LOG_D(TAG, "i: %d bit %d", i, bit_array[i]);
  227. i++;
  228. // My makeshift end sentinel. All other values 0/1
  229. bit_array[i] = 2;
  230. //FURI_LOG_D(TAG, "i: %d bit %d", i, bit_array[i]);
  231. i++;
  232. // Log the output (messy but works)
  233. //char output[500] = {0x0};
  234. /*FuriString* tmp_str;
  235. tmp_str = furi_string_alloc();
  236. for(uint8_t j = 0; bit_array[j] != 2; j++) {
  237. furi_string_cat_printf(tmp_str, "%d", (bit_array[j] & 1));
  238. //strcat(output, furi_string_get_cstr(tmp_str));
  239. }
  240. FURI_LOG_D(TAG, "Track %d: %s", (track_index + 1), track_data);
  241. FURI_LOG_D(TAG, "Track %d: %s", (track_index + 1), furi_string_get_cstr(tmp_str));*/
  242. //furi_string_free(tmp_str);
  243. }
  244. void mag_spoof_bitwise(Mag* mag) {
  245. MagSetting* setting = mag->setting;
  246. FuriString* ft1 = mag->mag_dev->dev_data.track[0].str;
  247. FuriString* ft2 = mag->mag_dev->dev_data.track[1].str;
  248. char* data1; char* data2;
  249. data1 = malloc(furi_string_size(ft1)+1);
  250. data2 = malloc(furi_string_size(ft2)+1);
  251. strncpy(data1, furi_string_get_cstr(ft1), furi_string_size(ft1));
  252. strncpy(data2, furi_string_get_cstr(ft2), furi_string_size(ft2));
  253. if(furi_log_get_level() >= FuriLogLevelDebug) {
  254. debug_msr_string(data1, BITS_TRACK1, OFFSET_TRACK1);
  255. debug_msr_string(data2, BITS_TRACK2, OFFSET_TRACK2);
  256. }
  257. uint8_t bits_t1_raw[64] = {0x00}; // 68 chars max track 1 + 1 char crc * 7 approx =~ 483 bits
  258. uint8_t bits_t1_manchester[128] = {0x00}; // twice the above
  259. uint16_t bits_t1_count = msr_encode(data1, (uint8_t*) bits_t1_manchester, (uint8_t*) bits_t1_raw, BITS_TRACK1, OFFSET_TRACK1);
  260. uint8_t bits_t2_raw[64] = {0x00}; // 68 chars max track 1 + 1 char crc * 7 approx =~ 483 bits
  261. uint8_t bits_t2_manchester[128] = {0x00}; // twice the above
  262. uint16_t bits_t2_count = msr_encode(data2, (uint8_t*) bits_t2_manchester, (uint8_t*) bits_t2_raw, BITS_TRACK2, OFFSET_TRACK2);
  263. if(furi_log_get_level() >= FuriLogLevelDebug) {
  264. printf("Manchester bitcount: T1: %d, T2: %d\r\n", bits_t1_count, bits_t2_count);
  265. printf("T1 raw: ");
  266. for (int i = 0; i < bits_t1_count / 16; i++) printf("%02x ", bits_t1_raw[i]);
  267. printf("\r\n");
  268. printf("T1 manchester: ");
  269. for (int i = 0; i < bits_t1_count / 8; i++) printf("%02x ", bits_t1_manchester[i]);
  270. printf("\r\n");
  271. printf("T2 raw: ");
  272. for (int i = 0; i < bits_t2_count / 16; i++) printf("%02x ", bits_t2_raw[i]);
  273. printf("\r\n");
  274. printf("T2 manchester: ");
  275. for (int i = 0; i < bits_t2_count / 8; i++) printf("%02x ", bits_t2_manchester[i]);
  276. printf("\r\n");
  277. printf("Bitwise emulation done\r\n\r\n");
  278. }
  279. if(!tx_init(setting)) return;
  280. FURI_CRITICAL_ENTER();
  281. bool bit = false;
  282. if((setting->track == MagTrackStateAll))
  283. for(uint16_t i = 0; i < ZERO_PREFIX; i++)
  284. {
  285. bit ^= 0xFF;
  286. bitbang_raw(bit, setting);
  287. furi_delay_us(setting->us_clock*2);
  288. }
  289. if((setting->track == MagTrackStateAll) || (setting->track == MagTrackStateOne))
  290. for(uint16_t i = 0; i < bits_t1_count; i++)
  291. {
  292. uint8_t byte = i / 8;
  293. uint8_t bitmask = 1 << (7-(i % 8));
  294. /* this comment is mostly for zw's convenience:
  295. *
  296. * bits are stored in their arrays like on a card (LSB first). This is not how usually bits are stored in a
  297. * byte, with the MSB first. the var bitmask creates the pattern to iterate through each bit, LSB first, like so
  298. * 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01, 0x80... masking bits one by one from the current byte
  299. *
  300. * i've chosen this LSB approach since bits and bytes are hard enough to visualize with the 5/8 and 7/8 encoding
  301. * MSR uses. It's a biiit more complicated to process, but visualizing it with printf or a debugger is
  302. * infinitely easier
  303. *
  304. * Encoding the following pairs of 5 bits as 5/8: A1234 B1234 C1234 D1234
  305. * using this LSB format looks like: A1234B12 34C1234D 12340000
  306. * using the MSB format, looks like: 21B4321A D4321C43 00004321
  307. * this means reading each byte backwards when printing/debugging, and the jumping 16 bits ahead, reading 8 more
  308. * bits backward, jumping 16 more bits ahead.
  309. *
  310. * I find this much more convenient for debugging, with the tiny incovenience of reading the bits in reverse
  311. * order. THus, the reason for the bitmask above
  312. */
  313. bit = !!(bits_t1_manchester[byte] & bitmask);
  314. // TODO: reimplement timing delays. Replace fixed furi_hal_cortex_delay_us to wait instead to a specific value
  315. // for DWT->CYCCNT. Note timer is aliased to 64us as per
  316. // #define FURI_HAL_CORTEX_INSTRUCTIONS_PER_MICROSECOND (SystemCoreClock / 1000000) | furi_hal_cortex.c
  317. bitbang_raw(bit, setting);
  318. furi_delay_us(setting->us_clock);
  319. // if (i % 2 == 1) furi_delay_us(setting->us_interpacket);
  320. }
  321. if((setting->track == MagTrackStateAll))
  322. for(uint16_t i = 0; i < ZERO_BETWEEN; i++)
  323. {
  324. bit ^= 0xFF;
  325. bitbang_raw(bit, setting);
  326. furi_delay_us(setting->us_clock*2);
  327. }
  328. if((setting->track == MagTrackStateAll) || (setting->track == MagTrackStateTwo))
  329. for(uint16_t i = 0; i < bits_t2_count; i++)
  330. {
  331. uint16_t j = bits_t2_count - i - 1;
  332. uint8_t byte = j / 8;
  333. uint8_t bitmask = 1 << (7-(j % 8));
  334. bool bit = !!(bits_t2_manchester[byte] & bitmask);
  335. bitbang_raw(bit, setting);
  336. furi_delay_us(setting->us_clock);
  337. // if (i % 2 == 1) furi_delay_us(setting->us_interpacket);
  338. }
  339. if((setting->track == MagTrackStateAll))
  340. for(uint16_t i = 0; i < ZERO_SUFFIX; i++)
  341. {
  342. bit ^= 0xFF;
  343. bitbang_raw(bit, setting);
  344. furi_delay_us(setting->us_clock*2);
  345. }
  346. FURI_CRITICAL_EXIT();
  347. free(data1);
  348. free(data2);
  349. tx_reset(setting);
  350. }
  351. void mag_spoof(Mag* mag) {
  352. MagSetting* setting = mag->setting;
  353. // precompute tracks (WIP; ignores reverse and 3rd track)
  354. // likely will be reworked to antirez's bitmap method anyway...
  355. const char* data1 = furi_string_get_cstr(mag->mag_dev->dev_data.track[0].str);
  356. const char* data2 = furi_string_get_cstr(mag->mag_dev->dev_data.track[1].str);
  357. uint8_t bit_array1[2 * (strlen(data1) * bitlen[0]) + 1];
  358. uint8_t bit_array2[2 * (strlen(data2) * bitlen[1]) + 1];
  359. track_to_bits(bit_array1, data1, 0);
  360. track_to_bits(bit_array2, data2, 1);
  361. bool spoofed = false;
  362. do {
  363. // Initialize configured TX method
  364. if(!tx_init(setting)) break;
  365. // Critical timing section (need to eliminate ifs? does this impact timing?)
  366. FURI_CRITICAL_ENTER();
  367. // Prefix of zeros
  368. for(uint16_t i = 0; i < ZERO_PREFIX; i++) {
  369. if(!play_bit(0, setting)) break;
  370. }
  371. // Track 1
  372. if((setting->track == MagTrackStateAll) || (setting->track == MagTrackStateOne)) {
  373. for(uint16_t i = 0; bit_array1[i] != 2; i++) {
  374. if(!play_bit((bit_array1[i] & 1), setting)) break;
  375. }
  376. }
  377. // Zeros between tracks
  378. if(setting->track == MagTrackStateAll) {
  379. for(uint16_t i = 0; i < ZERO_BETWEEN; i++) {
  380. if(!play_bit(0, setting)) break;
  381. }
  382. }
  383. // Track 2 (TODO: Reverse track)
  384. if((setting->track == MagTrackStateAll) || (setting->track == MagTrackStateTwo)) {
  385. for(uint16_t i = 0; bit_array2[i] != 2; i++) {
  386. if(!play_bit((bit_array2[i] & 1), setting)) break;
  387. }
  388. }
  389. // Suffix of zeros
  390. for(uint16_t i = 0; i < ZERO_SUFFIX; i++) {
  391. if(!play_bit(0, setting)) break;
  392. }
  393. FURI_CRITICAL_EXIT();
  394. // Reset configured TX method
  395. if(!tx_reset(setting)) break;
  396. spoofed = true;
  397. } while(0);
  398. UNUSED(spoofed);
  399. /*if(!spoofed) {
  400. // error handling?
  401. // cleanup?
  402. }*/
  403. }
  404. //// @antirez's code from protoview for bitmapping. May want to refactor to use this...
  405. /* Set the 'bitpos' bit to value 'val', in the specified bitmap
  406. * 'b' of len 'blen'.
  407. * Out of range bits will silently be discarded. */
  408. void set_bit(uint8_t* b, uint32_t blen, uint32_t bitpos, bool val) {
  409. uint32_t byte = bitpos / 8;
  410. uint32_t bit = bitpos & 7;
  411. if(byte >= blen) return;
  412. if(val)
  413. b[byte] |= 1 << bit;
  414. else
  415. b[byte] &= ~(1 << bit);
  416. }
  417. /* Get the bit 'bitpos' of the bitmap 'b' of 'blen' bytes.
  418. * Out of range bits return false (not bit set). */
  419. bool get_bit(uint8_t* b, uint32_t blen, uint32_t bitpos) {
  420. uint32_t byte = bitpos / 8;
  421. uint32_t bit = bitpos & 7;
  422. if(byte >= blen) return 0;
  423. return (b[byte] & (1 << bit)) != 0;
  424. }
  425. /*uint32_t convert_signal_to_bits(uint8_t *b, uint32_t blen, RawSamplesBuffer *s, uint32_t idx, uint32_t count, uint32_t rate) {
  426. if (rate == 0) return 0; // We can't perform the conversion.
  427. uint32_t bitpos = 0;
  428. for (uint32_t j = 0; j < count; j++) {
  429. uint32_t dur;
  430. bool level;
  431. raw_samples_get(s, j+idx, &level, &dur);
  432. uint32_t numbits = dur / rate; // full bits that surely fit.
  433. uint32_t rest = dur % rate; // How much we are left with.
  434. if (rest > rate/2) numbits++; // There is another one.
  435. while(numbits--) set_bit(b,blen,bitpos++,s[j].level);
  436. }
  437. return bitpos;
  438. }*/
  439. uint16_t add_bit(bool value, uint8_t* out, uint16_t count)
  440. {
  441. uint8_t bit = count % 8;
  442. uint8_t byte = count / 8;
  443. if (value)
  444. {
  445. out[byte] |= 0x01;
  446. }
  447. if (bit < 7) out[byte] <<= 1;
  448. return count+1;
  449. }
  450. uint16_t add_bit_manchester(bool value, uint8_t* out, uint16_t count)
  451. {
  452. static bool toggle = 0;
  453. toggle ^= 0x01;
  454. count = add_bit(toggle, out, count);
  455. if (value) toggle ^= 0x01;
  456. count = add_bit(toggle, out, count);
  457. return count;
  458. }
  459. uint16_t msr_encode(char* data, uint8_t* out_manchester, uint8_t* out_raw, uint8_t track_bits, uint8_t track_ascii_offset)
  460. {
  461. /*
  462. * track_bits - the number of raw (data) bits on the track. on ISO cards, that's 7 for track 5, or 4 for 2/3 - this is samy's bitlen
  463. * - this count includes the parity bit
  464. * track_ascii_offset - how much the ascii values are offset. track 1 makes space (ascii 32) become data 0x00,
  465. * - tracks 2/3 make ascii "0" become data 0x00 - this is samy's sublen
  466. *
  467. */
  468. uint16_t raw_bits_count = 0;
  469. uint16_t output_count = 0;
  470. int tmp, crc, lrc = 0;
  471. for (int i = 0; i < PREFIX_NUM_ZEROES; i++)
  472. {
  473. output_count = add_bit_manchester(0, out_manchester, output_count);
  474. raw_bits_count = add_bit(0, out_raw, raw_bits_count);
  475. }
  476. for (int i = 0; *(data+i) != 0; i++)
  477. {
  478. crc = 1;
  479. tmp = *(data+i) - track_ascii_offset;
  480. for (int j = 0; j < track_bits-1; j++)
  481. {
  482. crc ^= tmp & 1;
  483. lrc ^= (tmp & 1) << j;
  484. raw_bits_count = add_bit(tmp & 0x01, out_raw, raw_bits_count);
  485. output_count = add_bit_manchester(tmp & 0x01, out_manchester, output_count);
  486. tmp >>= 1;
  487. }
  488. raw_bits_count = add_bit(crc, out_raw, raw_bits_count);
  489. output_count = add_bit_manchester(crc, out_manchester, output_count);
  490. }
  491. // LRC byte
  492. tmp = lrc;
  493. crc = 1;
  494. for (int j = 0; j < track_bits-1; j++)
  495. {
  496. crc ^= tmp & 0x01;
  497. raw_bits_count = add_bit(tmp & 0x01, out_raw, raw_bits_count);
  498. output_count = add_bit_manchester(tmp & 0x01, out_manchester, output_count);
  499. tmp >>= 1;
  500. }
  501. raw_bits_count = add_bit(crc, out_raw, raw_bits_count);
  502. output_count = add_bit_manchester(crc, out_manchester, output_count);
  503. return output_count;
  504. }
  505. void debug_msr_string(char* data, uint8_t track_bits, uint8_t track_ascii_offset)
  506. {
  507. uint8_t bits_raw[64] = {0}; // 68 chars max track 1 + 1 char crc * 7 approx =~ 483 bits
  508. uint8_t bits_manchester[128] = {0}; // twice the above
  509. int numbits = 0;
  510. printf("Encoding [%s] with %d bits\r\n", data, track_bits);
  511. numbits = msr_encode(data, (uint8_t*)bits_manchester, (uint8_t*)bits_raw, track_bits, track_ascii_offset);
  512. printf("Got %d bits\r\n", numbits);
  513. printf("Raw byte stream: ");
  514. for(int i = 0; i < numbits / 8 / 2; i++)
  515. {
  516. printf("%02x", bits_raw[i]);
  517. if (i%4==3) printf(" ");
  518. }
  519. printf("\r\n");
  520. printf("Bits ");
  521. int space_counter = 0;
  522. for(int i = 0; i < numbits / 2; i++)
  523. {
  524. if (i < PREFIX_NUM_ZEROES)
  525. {
  526. printf("X");
  527. continue;
  528. }
  529. else if (i == PREFIX_NUM_ZEROES)
  530. {
  531. printf (" ");
  532. space_counter = 0;
  533. }
  534. printf("%01x", (bits_raw[i/8] & (1 << (7-(i%8)))) != 0);
  535. if ((space_counter) % track_bits == track_bits-1) printf(" ");
  536. space_counter++;
  537. }
  538. printf("\r\n");
  539. printf("Manchester encoded, byte stream: ");
  540. for(int i = 0; i < numbits / 8; i++)
  541. {
  542. printf("%02x", bits_manchester[i]);
  543. if (i%4==3) printf(" ");
  544. }
  545. printf("\r\n\r\n");
  546. }