+// Merlok, 2011\r
+// people from mifare@nethemba.com, 2010\r
+//\r
+// This code is licensed to you under the terms of the GNU GPL, version 2 or,\r
+// at your option, any later version. See the LICENSE.txt file for the text of\r
+// the license.\r
+//-----------------------------------------------------------------------------\r
+// High frequency ISO14443A commands\r
+//-----------------------------------------------------------------------------\r
+\r
+#include <stdio.h>\r
+#include <stdlib.h>\r
+#include "mifarehost.h"\r
+\r
+\r
+int compar_int(const void * a, const void * b) {\r
+ return (*(uint64_t*)b - *(uint64_t*)a);\r
+}\r
+\r
+// Compare countKeys structure\r
+int compar_special_int(const void * a, const void * b) {\r
+ return (((countKeys *)b)->count - ((countKeys *)a)->count);\r
+}\r
+\r
+countKeys * uniqsort(uint64_t * possibleKeys, uint32_t size) {\r
+ int i, j = 0;\r
+ int count = 0;\r
+ countKeys *our_counts;\r
+ \r
+ qsort(possibleKeys, size, sizeof (uint64_t), compar_int);\r
+ \r
+ our_counts = calloc(size, sizeof(countKeys));\r
+ if (our_counts == NULL) {\r
+ PrintAndLog("Memory allocation error for our_counts");\r
+ return NULL;\r
+ }\r
+ \r
+ for (i = 0; i < size; i++) {\r
+ if (possibleKeys[i+1] == possibleKeys[i]) { \r
+ count++;\r
+ } else {\r
+ our_counts[j].key = possibleKeys[i];\r
+ our_counts[j].count = count;\r
+ j++;\r
+ count=0;\r
+ }\r
+ }\r
+ qsort(our_counts, j, sizeof(countKeys), compar_special_int);\r
+ return (our_counts);\r
+}\r
+\r
+int mfnested(uint8_t blockNo, uint8_t keyType, uint8_t * key, uint8_t trgBlockNo, uint8_t trgKeyType, uint8_t * resultKeys) \r
+{\r
+ int i, m, len;\r
+ uint8_t isEOF;\r
+ uint32_t uid;\r
+ fnVector * vector = NULL;\r
+ countKeys *ck;\r
+ int lenVector = 0;\r
+ UsbCommand * resp = NULL;\r
+ \r
+ memset(resultKeys, 0x00, 16 * 6);\r
+\r
+ // flush queue\r
+ while (WaitForResponseTimeout(CMD_ACK, 500) != NULL) ;\r
+ \r
+ UsbCommand c = {CMD_MIFARE_NESTED, {blockNo, keyType, trgBlockNo + trgKeyType * 0x100}};\r
+ memcpy(c.d.asBytes, key, 6);\r
+ SendCommand(&c);\r
+\r
+ PrintAndLog("\n");\r
+\r
+ // wait cycle\r
+ while (true) {\r
+ printf(".");\r
+ if (ukbhit()) {\r
+ getchar();\r
+ printf("\naborted via keyboard!\n");\r
+ break;\r
+ }\r
+\r
+ resp = WaitForResponseTimeout(CMD_ACK, 1500);\r
+\r
+ if (resp != NULL) {\r
+ isEOF = resp->arg[0] & 0xff;\r
+\r
+ if (isEOF) break;\r
+ \r
+ len = resp->arg[1] & 0xff;\r
+ if (len == 0) continue;\r
+ \r
+ memcpy(&uid, resp->d.asBytes, 4); \r
+ PrintAndLog("uid:%08x len=%d trgbl=%d trgkey=%x", uid, len, resp->arg[2] & 0xff, (resp->arg[2] >> 8) & 0xff);\r
+ vector = (fnVector *) realloc((void *)vector, (lenVector + len) * sizeof(fnVector) + 200);\r
+ if (vector == NULL) {\r
+ PrintAndLog("Memory allocation error for fnVector. len: %d bytes: %d", lenVector + len, (lenVector + len) * sizeof(fnVector)); \r
+ break;\r
+ }\r
+ \r
+ for (i = 0; i < len; i++) {\r
+ vector[lenVector + i].blockNo = resp->arg[2] & 0xff;\r
+ vector[lenVector + i].keyType = (resp->arg[2] >> 8) & 0xff;\r
+ vector[lenVector + i].uid = uid;\r
+\r
+ memcpy(&vector[lenVector + i].nt, (void *)(resp->d.asBytes + 8 + i * 8 + 0), 4);\r
+ memcpy(&vector[lenVector + i].ks1, (void *)(resp->d.asBytes + 8 + i * 8 + 4), 4);\r
+ }\r
+\r
+ lenVector += len;\r
+ }\r
+ }\r
+ \r
+ if (!lenVector) {\r
+ PrintAndLog("Got 0 keys from proxmark."); \r
+ return 1;\r
+ }\r
+ printf("------------------------------------------------------------------\n");\r
+ \r
+ // calc keys\r
+ struct Crypto1State* revstate = NULL;\r
+ struct Crypto1State* revstate_start = NULL;\r
+ uint64_t lfsr;\r
+ int kcount = 0;\r
+ pKeys *pk;\r
+ \r
+ if ((pk = (void *) malloc(sizeof(pKeys))) == NULL) return 1;\r
+ memset(pk, 0x00, sizeof(pKeys));\r
+ \r
+ for (m = 0; m < lenVector; m++) {\r
+ // And finally recover the first 32 bits of the key\r
+ revstate = lfsr_recovery32(vector[m].ks1, vector[m].nt ^ vector[m].uid);\r
+ if (revstate_start == NULL) revstate_start = revstate;\r
+ \r
+ while ((revstate->odd != 0x0) || (revstate->even != 0x0)) {\r
+ lfsr_rollback_word(revstate, vector[m].nt ^ vector[m].uid, 0);\r
+ crypto1_get_lfsr(revstate, &lfsr);\r
+\r
+ // Allocate a new space for keys\r
+ if (((kcount % MEM_CHUNK) == 0) || (kcount >= pk->size)) {\r
+ pk->size += MEM_CHUNK;\r
+//fprintf(stdout, "New chunk by %d, sizeof %d\n", kcount, pk->size * sizeof(uint64_t));\r
+ pk->possibleKeys = (uint64_t *) realloc((void *)pk->possibleKeys, pk->size * sizeof(uint64_t));\r
+ if (pk->possibleKeys == NULL) {\r
+ PrintAndLog("Memory allocation error for pk->possibleKeys"); \r
+ return 1;\r
+ }\r
+ }\r
+ pk->possibleKeys[kcount] = lfsr;\r
+ kcount++;\r
+ revstate++;\r
+ }\r
+ free(revstate_start);\r
+ revstate_start = NULL;\r
+\r
+ }\r
+ \r
+ // Truncate\r
+ if (kcount != 0) {\r
+ pk->size = --kcount;\r
+ if ((pk->possibleKeys = (uint64_t *) realloc((void *)pk->possibleKeys, pk->size * sizeof(uint64_t))) == NULL) {\r
+ PrintAndLog("Memory allocation error for pk->possibleKeys"); \r
+ return 1;\r
+ } \r
+ }\r
+\r
+ PrintAndLog("Total keys count:%d", kcount);\r
+ ck = uniqsort(pk->possibleKeys, pk->size);\r
+\r
+ // fill key array\r
+ for (i = 0; i < 16 ; i++) {\r
+ num_to_bytes(ck[i].key, 6, (uint8_t*)(resultKeys + i * 6));\r
+ }\r
+\r
+ // finalize\r
+ free(pk->possibleKeys);\r
+ free(pk);\r
+ free(ck);\r
+ free(vector);\r
+\r
+ return 0;\r
+}\r
+\r
+int mfCheckKeys (uint8_t blockNo, uint8_t keyType, uint8_t keycnt, uint8_t * keyBlock, uint64_t * key){\r
+ *key = 0;\r
+\r
+ UsbCommand c = {CMD_MIFARE_CHKKEYS, {blockNo, keyType, keycnt}};\r
+ memcpy(c.d.asBytes, keyBlock, 6 * keycnt);\r
+\r
+ SendCommand(&c);\r
+\r
+ UsbCommand * resp = WaitForResponseTimeout(CMD_ACK, 3000);\r
+\r
+ if (resp == NULL) return 1;\r
+ if ((resp->arg[0] & 0xff) != 0x01) return 2;\r
+ *key = bytes_to_num(resp->d.asBytes, 6);\r
+ return 0;\r
+}\r