diff --git a/common/bucketsort.c b/common/bucketsort.c index 162869e38..c42b8673f 100644 --- a/common/bucketsort.c +++ b/common/bucketsort.c @@ -1,47 +1,47 @@ #include "bucketsort.h" extern void bucket_sort_intersect(uint32_t* const estart, uint32_t* const estop, - uint32_t* const ostart, uint32_t* const ostop, - bucket_info_t *bucket_info, bucket_array_t bucket) + uint32_t* const ostart, uint32_t* const ostop, + bucket_info_t *bucket_info, bucket_array_t bucket) { - uint32_t *p1, *p2; - uint32_t *start[2]; - uint32_t *stop[2]; + uint32_t *p1, *p2; + uint32_t *start[2]; + uint32_t *stop[2]; - start[0] = estart; - stop[0] = estop; - start[1] = ostart; - stop[1] = ostop; + start[0] = estart; + stop[0] = estop; + start[1] = ostart; + stop[1] = ostop; - // init buckets to be empty - for (uint32_t i = 0; i < 2; i++) { - for (uint32_t j = 0x00; j <= 0xff; j++) { - bucket[i][j].bp = bucket[i][j].head; - } - } + // init buckets to be empty + for (uint32_t i = 0; i < 2; i++) { + for (uint32_t j = 0x00; j <= 0xff; j++) { + bucket[i][j].bp = bucket[i][j].head; + } + } - // sort the lists into the buckets based on the MSB (contribution bits) - for (uint32_t i = 0; i < 2; i++) { - for (p1 = start[i]; p1 <= stop[i]; p1++) { - uint32_t bucket_index = (*p1 & 0xff000000) >> 24; - *(bucket[i][bucket_index].bp++) = *p1; - } - } + // sort the lists into the buckets based on the MSB (contribution bits) + for (uint32_t i = 0; i < 2; i++) { + for (p1 = start[i]; p1 <= stop[i]; p1++) { + uint32_t bucket_index = (*p1 & 0xff000000) >> 24; + *(bucket[i][bucket_index].bp++) = *p1; + } + } - // write back intersecting buckets as sorted list. - // fill in bucket_info with head and tail of the bucket contents in the list and number of non-empty buckets. - uint32_t nonempty_bucket; - for (uint32_t i = 0; i < 2; i++) { - p1 = start[i]; - nonempty_bucket = 0; - for (uint32_t j = 0x00; j <= 0xff; j++) { - if (bucket[0][j].bp != bucket[0][j].head && bucket[1][j].bp != bucket[1][j].head) { // non-empty intersecting buckets only - bucket_info->bucket_info[i][nonempty_bucket].head = p1; - for (p2 = bucket[i][j].head; p2 < bucket[i][j].bp; *p1++ = *p2++); - bucket_info->bucket_info[i][nonempty_bucket].tail = p1 - 1; - nonempty_bucket++; - } - } - bucket_info->numbuckets = nonempty_bucket; - } + // write back intersecting buckets as sorted list. + // fill in bucket_info with head and tail of the bucket contents in the list and number of non-empty buckets. + uint32_t nonempty_bucket; + for (uint32_t i = 0; i < 2; i++) { + p1 = start[i]; + nonempty_bucket = 0; + for (uint32_t j = 0x00; j <= 0xff; j++) { + if (bucket[0][j].bp != bucket[0][j].head && bucket[1][j].bp != bucket[1][j].head) { // non-empty intersecting buckets only + bucket_info->bucket_info[i][nonempty_bucket].head = p1; + for (p2 = bucket[i][j].head; p2 < bucket[i][j].bp; *p1++ = *p2++); + bucket_info->bucket_info[i][nonempty_bucket].tail = p1 - 1; + nonempty_bucket++; + } + } + bucket_info->numbuckets = nonempty_bucket; + } } diff --git a/common/bucketsort.h b/common/bucketsort.h index be504508a..30da87f2c 100644 --- a/common/bucketsort.h +++ b/common/bucketsort.h @@ -5,20 +5,20 @@ #include typedef struct bucket { - uint32_t *head; - uint32_t *bp; + uint32_t *head; + uint32_t *bp; } bucket_t; typedef bucket_t bucket_array_t[2][0x100]; typedef struct bucket_info { - struct { - uint32_t *head, *tail; - } bucket_info[2][0x100]; - uint32_t numbuckets; + struct { + uint32_t *head, *tail; + } bucket_info[2][0x100]; + uint32_t numbuckets; } bucket_info_t; void bucket_sort_intersect(uint32_t* const estart, uint32_t* const estop, - uint32_t* const ostart, uint32_t* const ostop, - bucket_info_t *bucket_info, bucket_array_t bucket); + uint32_t* const ostart, uint32_t* const ostop, + bucket_info_t *bucket_info, bucket_array_t bucket); #endif \ No newline at end of file diff --git a/common/cmd.c b/common/cmd.c index ebeaf4f7c..bc042b068 100644 --- a/common/cmd.c +++ b/common/cmd.c @@ -32,33 +32,33 @@ #include "cmd.h" uint8_t cmd_send(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void* data, size_t len) { - UsbCommand txcmd; + UsbCommand txcmd; - for (size_t i=0; i < sizeof(UsbCommand); i++) - ((uint8_t*)&txcmd)[i] = 0x00; + for (size_t i=0; i < sizeof(UsbCommand); i++) + ((uint8_t*)&txcmd)[i] = 0x00; - // Compose the outgoing command frame - txcmd.cmd = cmd; - txcmd.arg[0] = arg0; - txcmd.arg[1] = arg1; - txcmd.arg[2] = arg2; + // Compose the outgoing command frame + txcmd.cmd = cmd; + txcmd.arg[0] = arg0; + txcmd.arg[1] = arg1; + txcmd.arg[2] = arg2; - // Add the (optional) content to the frame, with a maximum size of USB_CMD_DATA_SIZE - if (data && len) { - len = MIN(len, USB_CMD_DATA_SIZE); - for (size_t i=0; i */ @@ -26,9 +26,9 @@ static uint8_t filterlut[1 << 20]; static void __attribute__((constructor)) fill_lut() { - uint32_t i; - for(i = 0; i < 1 << 20; ++i) - filterlut[i] = filter(i); + uint32_t i; + for(i = 0; i < 1 << 20; ++i) + filterlut[i] = filter(i); } #define filter(x) (filterlut[(x) & 0xfffff]) #endif @@ -38,11 +38,11 @@ static void __attribute__((constructor)) fill_lut() */ static inline void update_contribution(uint32_t *item, const uint32_t mask1, const uint32_t mask2) { - uint32_t p = *item >> 25; + uint32_t p = *item >> 25; - p = p << 1 | evenparity32(*item & mask1); - p = p << 1 | evenparity32(*item & mask2); - *item = p << 24 | (*item & 0xffffff); + p = p << 1 | evenparity32(*item & mask1); + p = p << 1 | evenparity32(*item & mask2); + *item = p << 24 | (*item & 0xffffff); } /** extend_table @@ -50,83 +50,83 @@ static inline void update_contribution(uint32_t *item, const uint32_t mask1, con */ static inline void extend_table(uint32_t *tbl, uint32_t **end, int bit, int m1, int m2, uint32_t in) { - in <<= 24; - for(*tbl <<= 1; tbl <= *end; *++tbl <<= 1) - if(filter(*tbl) ^ filter(*tbl | 1)) { - *tbl |= filter(*tbl) ^ bit; - update_contribution(tbl, m1, m2); - *tbl ^= in; - } else if(filter(*tbl) == bit) { - *++*end = tbl[1]; - tbl[1] = tbl[0] | 1; - update_contribution(tbl, m1, m2); - *tbl++ ^= in; - update_contribution(tbl, m1, m2); - *tbl ^= in; - } else - *tbl-- = *(*end)--; + in <<= 24; + for(*tbl <<= 1; tbl <= *end; *++tbl <<= 1) + if(filter(*tbl) ^ filter(*tbl | 1)) { + *tbl |= filter(*tbl) ^ bit; + update_contribution(tbl, m1, m2); + *tbl ^= in; + } else if(filter(*tbl) == bit) { + *++*end = tbl[1]; + tbl[1] = tbl[0] | 1; + update_contribution(tbl, m1, m2); + *tbl++ ^= in; + update_contribution(tbl, m1, m2); + *tbl ^= in; + } else + *tbl-- = *(*end)--; } /** extend_table_simple * using a bit of the keystream extend the table of possible lfsr states */ static inline void extend_table_simple(uint32_t *tbl, uint32_t **end, int bit) { - for(*tbl <<= 1; tbl <= *end; *++tbl <<= 1) { - if(filter(*tbl) ^ filter(*tbl | 1)) { // replace - *tbl |= filter(*tbl) ^ bit; - } else if(filter(*tbl) == bit) { // insert - *++*end = *++tbl; - *tbl = tbl[-1] | 1; - } else { // drop - *tbl-- = *(*end)--; + for(*tbl <<= 1; tbl <= *end; *++tbl <<= 1) { + if(filter(*tbl) ^ filter(*tbl | 1)) { // replace + *tbl |= filter(*tbl) ^ bit; + } else if(filter(*tbl) == bit) { // insert + *++*end = *++tbl; + *tbl = tbl[-1] | 1; + } else { // drop + *tbl-- = *(*end)--; } - } + } } /** recover * recursively narrow down the search space, 4 bits of keystream at a time */ static struct Crypto1State* recover(uint32_t *o_head, uint32_t *o_tail, uint32_t oks, - uint32_t *e_head, uint32_t *e_tail, uint32_t eks, int rem, - struct Crypto1State *sl, uint32_t in, bucket_array_t bucket) + uint32_t *e_head, uint32_t *e_tail, uint32_t eks, int rem, + struct Crypto1State *sl, uint32_t in, bucket_array_t bucket) { - uint32_t *o, *e; - bucket_info_t bucket_info; + uint32_t *o, *e; + bucket_info_t bucket_info; - if(rem == -1) { - for(e = e_head; e <= e_tail; ++e) { - *e = *e << 1 ^ evenparity32(*e & LF_POLY_EVEN) ^ !!(in & 4); - for(o = o_head; o <= o_tail; ++o, ++sl) { - sl->even = *o; - sl->odd = *e ^ evenparity32(*o & LF_POLY_ODD); - sl[1].odd = sl[1].even = 0; - } - } - return sl; - } + if(rem == -1) { + for(e = e_head; e <= e_tail; ++e) { + *e = *e << 1 ^ evenparity32(*e & LF_POLY_EVEN) ^ !!(in & 4); + for(o = o_head; o <= o_tail; ++o, ++sl) { + sl->even = *o; + sl->odd = *e ^ evenparity32(*o & LF_POLY_ODD); + sl[1].odd = sl[1].even = 0; + } + } + return sl; + } - for(uint32_t i = 0; i < 4 && rem--; i++) { - oks >>= 1; - eks >>= 1; - in >>= 2; - extend_table(o_head, &o_tail, oks & 1, LF_POLY_EVEN << 1 | 1, LF_POLY_ODD << 1, 0); - if(o_head > o_tail) - return sl; + for(uint32_t i = 0; i < 4 && rem--; i++) { + oks >>= 1; + eks >>= 1; + in >>= 2; + extend_table(o_head, &o_tail, oks & 1, LF_POLY_EVEN << 1 | 1, LF_POLY_ODD << 1, 0); + if(o_head > o_tail) + return sl; - extend_table(e_head, &e_tail, eks & 1, LF_POLY_ODD, LF_POLY_EVEN << 1 | 1, in & 3); - if(e_head > e_tail) - return sl; - } + extend_table(e_head, &e_tail, eks & 1, LF_POLY_ODD, LF_POLY_EVEN << 1 | 1, in & 3); + if(e_head > e_tail) + return sl; + } - bucket_sort_intersect(e_head, e_tail, o_head, o_tail, &bucket_info, bucket); + bucket_sort_intersect(e_head, e_tail, o_head, o_tail, &bucket_info, bucket); - for (int i = bucket_info.numbuckets - 1; i >= 0; i--) { - sl = recover(bucket_info.bucket_info[1][i].head, bucket_info.bucket_info[1][i].tail, oks, - bucket_info.bucket_info[0][i].head, bucket_info.bucket_info[0][i].tail, eks, - rem, sl, in, bucket); - } + for (int i = bucket_info.numbuckets - 1; i >= 0; i--) { + sl = recover(bucket_info.bucket_info[1][i].head, bucket_info.bucket_info[1][i].tail, oks, + bucket_info.bucket_info[0][i].head, bucket_info.bucket_info[0][i].tail, eks, + rem, sl, in, bucket); + } - return sl; + return sl; } /** lfsr_recovery * recover the state of the lfsr given 32 bits of the keystream @@ -135,87 +135,87 @@ recover(uint32_t *o_head, uint32_t *o_tail, uint32_t oks, */ struct Crypto1State* lfsr_recovery32(uint32_t ks2, uint32_t in) { - struct Crypto1State *statelist; - uint32_t *odd_head = 0, *odd_tail = 0, oks = 0; - uint32_t *even_head = 0, *even_tail = 0, eks = 0; - int i; + struct Crypto1State *statelist; + uint32_t *odd_head = 0, *odd_tail = 0, oks = 0; + uint32_t *even_head = 0, *even_tail = 0, eks = 0; + int i; - // split the keystream into an odd and even part - for (i = 31; i >= 0; i -= 2) - oks = oks << 1 | BEBIT(ks2, i); - for (i = 30; i >= 0; i -= 2) - eks = eks << 1 | BEBIT(ks2, i); + // split the keystream into an odd and even part + for (i = 31; i >= 0; i -= 2) + oks = oks << 1 | BEBIT(ks2, i); + for (i = 30; i >= 0; i -= 2) + eks = eks << 1 | BEBIT(ks2, i); - odd_head = odd_tail = malloc(sizeof(uint32_t) << 21); - even_head = even_tail = malloc(sizeof(uint32_t) << 21); - statelist = malloc(sizeof(struct Crypto1State) << 18); - if (!odd_tail-- || !even_tail-- || !statelist) { - free(statelist); - statelist = 0; - goto out; - } + odd_head = odd_tail = malloc(sizeof(uint32_t) << 21); + even_head = even_tail = malloc(sizeof(uint32_t) << 21); + statelist = malloc(sizeof(struct Crypto1State) << 18); + if (!odd_tail-- || !even_tail-- || !statelist) { + free(statelist); + statelist = 0; + goto out; + } - statelist->odd = statelist->even = 0; + statelist->odd = statelist->even = 0; - // allocate memory for out of place bucket_sort - bucket_array_t bucket; + // allocate memory for out of place bucket_sort + bucket_array_t bucket; - for (uint32_t i = 0; i < 2; i++) { - for (uint32_t j = 0; j <= 0xff; j++) { - bucket[i][j].head = malloc(sizeof(uint32_t) << 14); - if (!bucket[i][j].head) { - goto out; - } - } - } + for (uint32_t i = 0; i < 2; i++) { + for (uint32_t j = 0; j <= 0xff; j++) { + bucket[i][j].head = malloc(sizeof(uint32_t) << 14); + if (!bucket[i][j].head) { + goto out; + } + } + } - // initialize statelists: add all possible states which would result into the rightmost 2 bits of the keystream - for(i = 1 << 20; i >= 0; --i) { - if(filter(i) == (oks & 1)) - *++odd_tail = i; - if(filter(i) == (eks & 1)) - *++even_tail = i; - } + // initialize statelists: add all possible states which would result into the rightmost 2 bits of the keystream + for(i = 1 << 20; i >= 0; --i) { + if(filter(i) == (oks & 1)) + *++odd_tail = i; + if(filter(i) == (eks & 1)) + *++even_tail = i; + } - // extend the statelists. Look at the next 8 Bits of the keystream (4 Bit each odd and even): - for(i = 0; i < 4; i++) { - extend_table_simple(odd_head, &odd_tail, (oks >>= 1) & 1); - extend_table_simple(even_head, &even_tail, (eks >>= 1) & 1); - } + // extend the statelists. Look at the next 8 Bits of the keystream (4 Bit each odd and even): + for(i = 0; i < 4; i++) { + extend_table_simple(odd_head, &odd_tail, (oks >>= 1) & 1); + extend_table_simple(even_head, &even_tail, (eks >>= 1) & 1); + } - // the statelists now contain all states which could have generated the last 10 Bits of the keystream. - // 22 bits to go to recover 32 bits in total. From now on, we need to take the "in" - // parameter into account. - in = (in >> 16 & 0xff) | (in << 16) | (in & 0xff00); // Byte swapping - recover(odd_head, odd_tail, oks, even_head, even_tail, eks, 11, statelist, in << 1, bucket); + // the statelists now contain all states which could have generated the last 10 Bits of the keystream. + // 22 bits to go to recover 32 bits in total. From now on, we need to take the "in" + // parameter into account. + in = (in >> 16 & 0xff) | (in << 16) | (in & 0xff00); // Byte swapping + recover(odd_head, odd_tail, oks, even_head, even_tail, eks, 11, statelist, in << 1, bucket); out: - for (uint32_t i = 0; i < 2; i++) - for (uint32_t j = 0; j <= 0xff; j++) - free(bucket[i][j].head); - free(odd_head); - free(even_head); - return statelist; + for (uint32_t i = 0; i < 2; i++) + for (uint32_t j = 0; j <= 0xff; j++) + free(bucket[i][j].head); + free(odd_head); + free(even_head); + return statelist; } static const uint32_t S1[] = { 0x62141, 0x310A0, 0x18850, 0x0C428, 0x06214, - 0x0310A, 0x85E30, 0xC69AD, 0x634D6, 0xB5CDE, 0xDE8DA, 0x6F46D, 0xB3C83, - 0x59E41, 0xA8995, 0xD027F, 0x6813F, 0x3409F, 0x9E6FA}; + 0x0310A, 0x85E30, 0xC69AD, 0x634D6, 0xB5CDE, 0xDE8DA, 0x6F46D, 0xB3C83, + 0x59E41, 0xA8995, 0xD027F, 0x6813F, 0x3409F, 0x9E6FA}; static const uint32_t S2[] = { 0x3A557B00, 0x5D2ABD80, 0x2E955EC0, 0x174AAF60, - 0x0BA557B0, 0x05D2ABD8, 0x0449DE68, 0x048464B0, 0x42423258, 0x278192A8, - 0x156042D0, 0x0AB02168, 0x43F89B30, 0x61FC4D98, 0x765EAD48, 0x7D8FDD20, - 0x7EC7EE90, 0x7F63F748, 0x79117020}; + 0x0BA557B0, 0x05D2ABD8, 0x0449DE68, 0x048464B0, 0x42423258, 0x278192A8, + 0x156042D0, 0x0AB02168, 0x43F89B30, 0x61FC4D98, 0x765EAD48, 0x7D8FDD20, + 0x7EC7EE90, 0x7F63F748, 0x79117020}; static const uint32_t T1[] = { - 0x4F37D, 0x279BE, 0x97A6A, 0x4BD35, 0x25E9A, 0x12F4D, 0x097A6, 0x80D66, - 0xC4006, 0x62003, 0xB56B4, 0x5AB5A, 0xA9318, 0xD0F39, 0x6879C, 0xB057B, - 0x582BD, 0x2C15E, 0x160AF, 0x8F6E2, 0xC3DC4, 0xE5857, 0x72C2B, 0x39615, - 0x98DBF, 0xC806A, 0xE0680, 0x70340, 0x381A0, 0x98665, 0x4C332, 0xA272C}; + 0x4F37D, 0x279BE, 0x97A6A, 0x4BD35, 0x25E9A, 0x12F4D, 0x097A6, 0x80D66, + 0xC4006, 0x62003, 0xB56B4, 0x5AB5A, 0xA9318, 0xD0F39, 0x6879C, 0xB057B, + 0x582BD, 0x2C15E, 0x160AF, 0x8F6E2, 0xC3DC4, 0xE5857, 0x72C2B, 0x39615, + 0x98DBF, 0xC806A, 0xE0680, 0x70340, 0x381A0, 0x98665, 0x4C332, 0xA272C}; static const uint32_t T2[] = { 0x3C88B810, 0x5E445C08, 0x2982A580, 0x14C152C0, - 0x4A60A960, 0x253054B0, 0x52982A58, 0x2FEC9EA8, 0x1156C4D0, 0x08AB6268, - 0x42F53AB0, 0x217A9D58, 0x161DC528, 0x0DAE6910, 0x46D73488, 0x25CB11C0, - 0x52E588E0, 0x6972C470, 0x34B96238, 0x5CFC3A98, 0x28DE96C8, 0x12CFC0E0, - 0x4967E070, 0x64B3F038, 0x74F97398, 0x7CDC3248, 0x38CE92A0, 0x1C674950, - 0x0E33A4A8, 0x01B959D0, 0x40DCACE8, 0x26CEDDF0}; + 0x4A60A960, 0x253054B0, 0x52982A58, 0x2FEC9EA8, 0x1156C4D0, 0x08AB6268, + 0x42F53AB0, 0x217A9D58, 0x161DC528, 0x0DAE6910, 0x46D73488, 0x25CB11C0, + 0x52E588E0, 0x6972C470, 0x34B96238, 0x5CFC3A98, 0x28DE96C8, 0x12CFC0E0, + 0x4967E070, 0x64B3F038, 0x74F97398, 0x7CDC3248, 0x38CE92A0, 0x1C674950, + 0x0E33A4A8, 0x01B959D0, 0x40DCACE8, 0x26CEDDF0}; static const uint32_t C1[] = { 0x846B5, 0x4235A, 0x211AD}; static const uint32_t C2[] = { 0x1A822E0, 0x21A822E0, 0x21A822E0}; /** Reverse 64 bits of keystream into possible cipher states @@ -223,69 +223,69 @@ static const uint32_t C2[] = { 0x1A822E0, 0x21A822E0, 0x21A822E0}; */ struct Crypto1State* lfsr_recovery64(uint32_t ks2, uint32_t ks3) { - struct Crypto1State *statelist, *sl; - uint8_t oks[32], eks[32], hi[32]; - uint32_t low = 0, win = 0; - uint32_t *tail, table[1 << 16]; - int i, j; + struct Crypto1State *statelist, *sl; + uint8_t oks[32], eks[32], hi[32]; + uint32_t low = 0, win = 0; + uint32_t *tail, table[1 << 16]; + int i, j; - sl = statelist = malloc(sizeof(struct Crypto1State) << 4); - if(!sl) - return 0; - sl->odd = sl->even = 0; + sl = statelist = malloc(sizeof(struct Crypto1State) << 4); + if(!sl) + return 0; + sl->odd = sl->even = 0; - for(i = 30; i >= 0; i -= 2) { - oks[i >> 1] = BEBIT(ks2, i); - oks[16 + (i >> 1)] = BEBIT(ks3, i); - } - for(i = 31; i >= 0; i -= 2) { - eks[i >> 1] = BEBIT(ks2, i); - eks[16 + (i >> 1)] = BEBIT(ks3, i); - } + for(i = 30; i >= 0; i -= 2) { + oks[i >> 1] = BEBIT(ks2, i); + oks[16 + (i >> 1)] = BEBIT(ks3, i); + } + for(i = 31; i >= 0; i -= 2) { + eks[i >> 1] = BEBIT(ks2, i); + eks[16 + (i >> 1)] = BEBIT(ks3, i); + } - for(i = 0xfffff; i >= 0; --i) { - if (filter(i) != oks[0]) - continue; + for(i = 0xfffff; i >= 0; --i) { + if (filter(i) != oks[0]) + continue; - *(tail = table) = i; - for(j = 1; tail >= table && j < 29; ++j) - extend_table_simple(table, &tail, oks[j]); + *(tail = table) = i; + for(j = 1; tail >= table && j < 29; ++j) + extend_table_simple(table, &tail, oks[j]); - if(tail < table) - continue; + if(tail < table) + continue; - for(j = 0; j < 19; ++j) - low = low << 1 | evenparity32(i & S1[j]); - for(j = 0; j < 32; ++j) - hi[j] = evenparity32(i & T1[j]); + for(j = 0; j < 19; ++j) + low = low << 1 | evenparity32(i & S1[j]); + for(j = 0; j < 32; ++j) + hi[j] = evenparity32(i & T1[j]); - for(; tail >= table; --tail) { - for(j = 0; j < 3; ++j) { - *tail = *tail << 1; - *tail |= evenparity32((i & C1[j]) ^ (*tail & C2[j])); - if(filter(*tail) != oks[29 + j]) - goto continue2; - } + for(; tail >= table; --tail) { + for(j = 0; j < 3; ++j) { + *tail = *tail << 1; + *tail |= evenparity32((i & C1[j]) ^ (*tail & C2[j])); + if(filter(*tail) != oks[29 + j]) + goto continue2; + } - for(j = 0; j < 19; ++j) - win = win << 1 | evenparity32(*tail & S2[j]); + for(j = 0; j < 19; ++j) + win = win << 1 | evenparity32(*tail & S2[j]); - win ^= low; - for(j = 0; j < 32; ++j) { - win = win << 1 ^ hi[j] ^ evenparity32(*tail & T2[j]); - if(filter(win) != eks[j]) - goto continue2; - } + win ^= low; + for(j = 0; j < 32; ++j) { + win = win << 1 ^ hi[j] ^ evenparity32(*tail & T2[j]); + if(filter(win) != eks[j]) + goto continue2; + } - *tail = *tail << 1 | evenparity32(LF_POLY_EVEN & *tail); - sl->odd = *tail ^ evenparity32(LF_POLY_ODD & win); - sl->even = win; - ++sl; - sl->odd = sl->even = 0; - continue2:; - } - } - return statelist; + *tail = *tail << 1 | evenparity32(LF_POLY_EVEN & *tail); + sl->odd = *tail ^ evenparity32(LF_POLY_ODD & win); + sl->even = win; + ++sl; + sl->odd = sl->even = 0; + continue2:; + } + } + return statelist; } /** lfsr_rollback_bit @@ -293,37 +293,37 @@ struct Crypto1State* lfsr_recovery64(uint32_t ks2, uint32_t ks3) */ uint8_t lfsr_rollback_bit(struct Crypto1State *s, uint32_t in, int fb) { - int out; - uint8_t ret; - uint32_t t; + int out; + uint8_t ret; + uint32_t t; - s->odd &= 0xffffff; - t = s->odd, s->odd = s->even, s->even = t; + s->odd &= 0xffffff; + t = s->odd, s->odd = s->even, s->even = t; - out = s->even & 1; - out ^= LF_POLY_EVEN & (s->even >>= 1); - out ^= LF_POLY_ODD & s->odd; - out ^= !!in; - out ^= (ret = filter(s->odd)) & !!fb; + out = s->even & 1; + out ^= LF_POLY_EVEN & (s->even >>= 1); + out ^= LF_POLY_ODD & s->odd; + out ^= !!in; + out ^= (ret = filter(s->odd)) & !!fb; - s->even |= evenparity32(out) << 23; - return ret; + s->even |= evenparity32(out) << 23; + return ret; } /** lfsr_rollback_byte * Rollback the shift register in order to get previous states */ uint8_t lfsr_rollback_byte(struct Crypto1State *s, uint32_t in, int fb) { - uint8_t ret = 0; - ret |= lfsr_rollback_bit(s, BIT(in, 7), fb) << 7; - ret |= lfsr_rollback_bit(s, BIT(in, 6), fb) << 6; - ret |= lfsr_rollback_bit(s, BIT(in, 5), fb) << 5; - ret |= lfsr_rollback_bit(s, BIT(in, 4), fb) << 4; - ret |= lfsr_rollback_bit(s, BIT(in, 3), fb) << 3; - ret |= lfsr_rollback_bit(s, BIT(in, 2), fb) << 2; - ret |= lfsr_rollback_bit(s, BIT(in, 1), fb) << 1; - ret |= lfsr_rollback_bit(s, BIT(in, 0), fb) << 0; - return ret; + uint8_t ret = 0; + ret |= lfsr_rollback_bit(s, BIT(in, 7), fb) << 7; + ret |= lfsr_rollback_bit(s, BIT(in, 6), fb) << 6; + ret |= lfsr_rollback_bit(s, BIT(in, 5), fb) << 5; + ret |= lfsr_rollback_bit(s, BIT(in, 4), fb) << 4; + ret |= lfsr_rollback_bit(s, BIT(in, 3), fb) << 3; + ret |= lfsr_rollback_bit(s, BIT(in, 2), fb) << 2; + ret |= lfsr_rollback_bit(s, BIT(in, 1), fb) << 1; + ret |= lfsr_rollback_bit(s, BIT(in, 0), fb) << 0; + return ret; } /** lfsr_rollback_word * Rollback the shift register in order to get previous states @@ -331,43 +331,43 @@ uint8_t lfsr_rollback_byte(struct Crypto1State *s, uint32_t in, int fb) uint32_t lfsr_rollback_word(struct Crypto1State *s, uint32_t in, int fb) { - uint32_t ret = 0; - ret |= lfsr_rollback_bit(s, BEBIT(in, 31), fb) << (31 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 30), fb) << (30 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 29), fb) << (29 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 28), fb) << (28 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 27), fb) << (27 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 26), fb) << (26 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 25), fb) << (25 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 24), fb) << (24 ^ 24); + uint32_t ret = 0; + ret |= lfsr_rollback_bit(s, BEBIT(in, 31), fb) << (31 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 30), fb) << (30 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 29), fb) << (29 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 28), fb) << (28 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 27), fb) << (27 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 26), fb) << (26 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 25), fb) << (25 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 24), fb) << (24 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 23), fb) << (23 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 22), fb) << (22 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 21), fb) << (21 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 20), fb) << (20 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 19), fb) << (19 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 18), fb) << (18 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 17), fb) << (17 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 16), fb) << (16 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 23), fb) << (23 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 22), fb) << (22 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 21), fb) << (21 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 20), fb) << (20 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 19), fb) << (19 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 18), fb) << (18 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 17), fb) << (17 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 16), fb) << (16 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 15), fb) << (15 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 14), fb) << (14 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 13), fb) << (13 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 12), fb) << (12 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 11), fb) << (11 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 10), fb) << (10 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 9), fb) << (9 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 8), fb) << (8 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 15), fb) << (15 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 14), fb) << (14 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 13), fb) << (13 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 12), fb) << (12 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 11), fb) << (11 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 10), fb) << (10 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 9), fb) << (9 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 8), fb) << (8 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 7), fb) << (7 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 6), fb) << (6 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 5), fb) << (5 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 4), fb) << (4 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 3), fb) << (3 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 2), fb) << (2 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 1), fb) << (1 ^ 24); - ret |= lfsr_rollback_bit(s, BEBIT(in, 0), fb) << (0 ^ 24); - return ret; + ret |= lfsr_rollback_bit(s, BEBIT(in, 7), fb) << (7 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 6), fb) << (6 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 5), fb) << (5 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 4), fb) << (4 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 3), fb) << (3 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 2), fb) << (2 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 1), fb) << (1 ^ 24); + ret |= lfsr_rollback_bit(s, BEBIT(in, 0), fb) << (0 ^ 24); + return ret; } /** nonce_distance @@ -376,17 +376,17 @@ uint32_t lfsr_rollback_word(struct Crypto1State *s, uint32_t in, int fb) static uint16_t *dist = 0; int nonce_distance(uint32_t from, uint32_t to) { - uint16_t x, i; - if(!dist) { - dist = calloc(2 << 16, sizeof(uint8_t)); - if(!dist) - return -1; - for (x = i = 1; i; ++i) { - dist[(x & 0xff) << 8 | x >> 8] = i; - x = x >> 1 | (x ^ x >> 2 ^ x >> 3 ^ x >> 5) << 15; - } - } - return (65535 + dist[to >> 16] - dist[from >> 16]) % 65535; + uint16_t x, i; + if(!dist) { + dist = calloc(2 << 16, sizeof(uint8_t)); + if(!dist) + return -1; + for (x = i = 1; i; ++i) { + dist[(x & 0xff) << 8 | x >> 8] = i; + x = x >> 1 | (x ^ x >> 2 ^ x >> 3 ^ x >> 5) << 15; + } + } + return (65535 + dist[to >> 16] - dist[from >> 16]) % 65535; } /** validate_prng_nonce @@ -396,14 +396,14 @@ int nonce_distance(uint32_t from, uint32_t to) * false = hardend prng */ bool validate_prng_nonce(uint32_t nonce) { - // init prng table: - nonce_distance(nonce, nonce); - return ((65535 - dist[nonce >> 16] + dist[nonce & 0xffff]) % 65535) == 16; + // init prng table: + nonce_distance(nonce, nonce); + return ((65535 - dist[nonce >> 16] + dist[nonce & 0xffff]) % 65535) == 16; } static uint32_t fastfwd[2][8] = { - { 0, 0x4BC53, 0xECB1, 0x450E2, 0x25E29, 0x6E27A, 0x2B298, 0x60ECB}, - { 0, 0x1D962, 0x4BC53, 0x56531, 0xECB1, 0x135D3, 0x450E2, 0x58980}}; + { 0, 0x4BC53, 0xECB1, 0x450E2, 0x25E29, 0x6E27A, 0x2B298, 0x60ECB}, + { 0, 0x1D962, 0x4BC53, 0x56531, 0xECB1, 0x135D3, 0x450E2, 0x58980}}; /** lfsr_prefix_ks * @@ -416,25 +416,25 @@ static uint32_t fastfwd[2][8] = { */ uint32_t *lfsr_prefix_ks(uint8_t ks[8], int isodd) { - uint32_t *candidates = calloc(4 << 10, sizeof(uint8_t)); - if (!candidates) return 0; + uint32_t *candidates = calloc(4 << 10, sizeof(uint8_t)); + if (!candidates) return 0; - uint32_t c, entry; - int size = 0, i, good; + uint32_t c, entry; + int size = 0, i, good; - for (i = 0; i < 1 << 21; ++i) { - for (c = 0, good = 1; good && c < 8; ++c) { - entry = i ^ fastfwd[isodd][c]; - good &= (BIT(ks[c], isodd) == filter(entry >> 1)); - good &= (BIT(ks[c], isodd + 2) == filter(entry)); - } - if (good) - candidates[size++] = i; - } + for (i = 0; i < 1 << 21; ++i) { + for (c = 0, good = 1; good && c < 8; ++c) { + entry = i ^ fastfwd[isodd][c]; + good &= (BIT(ks[c], isodd) == filter(entry >> 1)); + good &= (BIT(ks[c], isodd + 2) == filter(entry)); + } + if (good) + candidates[size++] = i; + } - candidates[size] = -1; + candidates[size] = -1; - return candidates; + return candidates; } /** check_pfx_parity @@ -442,33 +442,33 @@ uint32_t *lfsr_prefix_ks(uint8_t ks[8], int isodd) */ static struct Crypto1State* check_pfx_parity(uint32_t prefix, uint32_t rresp, uint8_t parities[8][8], uint32_t odd, uint32_t even, struct Crypto1State* sl, uint32_t no_par) { - uint32_t ks1, nr, ks2, rr, ks3, c, good = 1; + uint32_t ks1, nr, ks2, rr, ks3, c, good = 1; - for(c = 0; good && c < 8; ++c) { - sl->odd = odd ^ fastfwd[1][c]; - sl->even = even ^ fastfwd[0][c]; + for(c = 0; good && c < 8; ++c) { + sl->odd = odd ^ fastfwd[1][c]; + sl->even = even ^ fastfwd[0][c]; - lfsr_rollback_bit(sl, 0, 0); - lfsr_rollback_bit(sl, 0, 0); + lfsr_rollback_bit(sl, 0, 0); + lfsr_rollback_bit(sl, 0, 0); - ks3 = lfsr_rollback_bit(sl, 0, 0); - ks2 = lfsr_rollback_word(sl, 0, 0); - ks1 = lfsr_rollback_word(sl, prefix | c << 5, 1); + ks3 = lfsr_rollback_bit(sl, 0, 0); + ks2 = lfsr_rollback_word(sl, 0, 0); + ks1 = lfsr_rollback_word(sl, prefix | c << 5, 1); - if (no_par) - break; + if (no_par) + break; - nr = ks1 ^ (prefix | c << 5); - rr = ks2 ^ rresp; + nr = ks1 ^ (prefix | c << 5); + rr = ks2 ^ rresp; - good &= evenparity32(nr & 0x000000ff) ^ parities[c][3] ^ BIT(ks2, 24); - good &= evenparity32(rr & 0xff000000) ^ parities[c][4] ^ BIT(ks2, 16); - good &= evenparity32(rr & 0x00ff0000) ^ parities[c][5] ^ BIT(ks2, 8); - good &= evenparity32(rr & 0x0000ff00) ^ parities[c][6] ^ BIT(ks2, 0); - good &= evenparity32(rr & 0x000000ff) ^ parities[c][7] ^ ks3; - } + good &= evenparity32(nr & 0x000000ff) ^ parities[c][3] ^ BIT(ks2, 24); + good &= evenparity32(rr & 0xff000000) ^ parities[c][4] ^ BIT(ks2, 16); + good &= evenparity32(rr & 0x00ff0000) ^ parities[c][5] ^ BIT(ks2, 8); + good &= evenparity32(rr & 0x0000ff00) ^ parities[c][6] ^ BIT(ks2, 0); + good &= evenparity32(rr & 0x000000ff) ^ parities[c][7] ^ ks3; + } - return sl + good; + return sl + good; } @@ -484,30 +484,30 @@ static struct Crypto1State* check_pfx_parity(uint32_t prefix, uint32_t rresp, ui struct Crypto1State* lfsr_common_prefix(uint32_t pfx, uint32_t rr, uint8_t ks[8], uint8_t par[8][8], uint32_t no_par) { - struct Crypto1State *statelist, *s; - uint32_t *odd, *even, *o, *e, top; + struct Crypto1State *statelist, *s; + uint32_t *odd, *even, *o, *e, top; - odd = lfsr_prefix_ks(ks, 1); - even = lfsr_prefix_ks(ks, 0); + odd = lfsr_prefix_ks(ks, 1); + even = lfsr_prefix_ks(ks, 0); - s = statelist = malloc((sizeof *statelist) << 24); // was << 20. Need more for no_par special attack. Enough??? - if (!s || !odd || !even) { - free(statelist); - statelist = 0; - goto out; - } + s = statelist = malloc((sizeof *statelist) << 24); // was << 20. Need more for no_par special attack. Enough??? + if (!s || !odd || !even) { + free(statelist); + statelist = 0; + goto out; + } - for (o = odd; *o + 1; ++o) - for (e = even; *e + 1; ++e) - for (top = 0; top < 64; ++top) { - *o += 1 << 21; - *e += (!(top & 7) + 1) << 21; - s = check_pfx_parity(pfx, rr, par, *o, *e, s, no_par); - } + for (o = odd; *o + 1; ++o) + for (e = even; *e + 1; ++e) + for (top = 0; top < 64; ++top) { + *o += 1 << 21; + *e += (!(top & 7) + 1) << 21; + s = check_pfx_parity(pfx, rr, par, *o, *e, s, no_par); + } - s->odd = s->even = 0; + s->odd = s->even = 0; out: - free(odd); - free(even); - return statelist; + free(odd); + free(even); + return statelist; } diff --git a/common/crapto1/crapto1.h b/common/crapto1/crapto1.h index 5abdaeb09..b51b2aa15 100644 --- a/common/crapto1/crapto1.h +++ b/common/crapto1/crapto1.h @@ -28,7 +28,7 @@ extern "C" { #endif struct Crypto1State {uint32_t odd, even;}; -#if defined(__arm__) && !defined(__linux__) && !defined(_WIN32) && !defined(__APPLE__) // bare metal ARM Proxmark lacks malloc()/free() +#if defined(__arm__) && !defined(__linux__) && !defined(_WIN32) && !defined(__APPLE__) // bare metal ARM Proxmark lacks malloc()/free() void crypto1_create(struct Crypto1State *s, uint64_t key); #else struct Crypto1State *crypto1_create(uint64_t key); @@ -53,15 +53,15 @@ uint32_t lfsr_rollback_word(struct Crypto1State* s, uint32_t in, int fb); int nonce_distance(uint32_t from, uint32_t to); extern bool validate_prng_nonce(uint32_t nonce); #define FOREACH_VALID_NONCE(N, FILTER, FSIZE)\ - uint32_t __n = 0,__M = 0, N = 0;\ - int __i;\ - for(; __n < 1 << 16; N = prng_successor(__M = ++__n, 16))\ - for(__i = FSIZE - 1; __i >= 0; __i--)\ - if(BIT(FILTER, __i) ^ evenparity32(__M & 0xFF01))\ - break;\ - else if(__i)\ - __M = prng_successor(__M, (__i == 7) ? 48 : 8);\ - else + uint32_t __n = 0,__M = 0, N = 0;\ + int __i;\ + for(; __n < 1 << 16; N = prng_successor(__M = ++__n, 16))\ + for(__i = FSIZE - 1; __i >= 0; __i--)\ + if(BIT(FILTER, __i) ^ evenparity32(__M & 0xFF01))\ + break;\ + else if(__i)\ + __M = prng_successor(__M, (__i == 7) ? 48 : 8);\ + else #define LF_POLY_ODD (0x29CE5C) #define LF_POLY_EVEN (0x870804) @@ -69,14 +69,14 @@ extern bool validate_prng_nonce(uint32_t nonce); #define BEBIT(x, n) BIT(x, (n) ^ 24) static inline int filter(uint32_t const x) { - uint32_t f; + uint32_t f; - f = 0xf22c0 >> (x & 0xf) & 16; - f |= 0x6c9c0 >> (x >> 4 & 0xf) & 8; - f |= 0x3c8b0 >> (x >> 8 & 0xf) & 4; - f |= 0x1e458 >> (x >> 12 & 0xf) & 2; - f |= 0x0d938 >> (x >> 16 & 0xf) & 1; - return BIT(0xEC57E80A, f); + f = 0xf22c0 >> (x & 0xf) & 16; + f |= 0x6c9c0 >> (x >> 4 & 0xf) & 8; + f |= 0x3c8b0 >> (x >> 8 & 0xf) & 4; + f |= 0x1e458 >> (x >> 12 & 0xf) & 2; + f |= 0x0d938 >> (x >> 16 & 0xf) & 1; + return BIT(0xEC57E80A, f); } #ifdef __cplusplus } diff --git a/common/crapto1/crypto1.c b/common/crapto1/crypto1.c index 034d92215..f4147bf4f 100644 --- a/common/crapto1/crypto1.c +++ b/common/crapto1/crypto1.c @@ -1,21 +1,21 @@ /* crypto1.c - This program is free software; you can redistribute it and/or - modify it under the terms of the GNU General Public License - as published by the Free Software Foundation; either version 2 - of the License, or (at your option) any later version. + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, - MA 02110-1301, US + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, + MA 02110-1301, US - Copyright (C) 2008-2008 bla + Copyright (C) 2008-2008 bla */ #include "crapto1.h" @@ -23,121 +23,121 @@ #include "parity.h" #define SWAPENDIAN(x)\ - (x = (x >> 8 & 0xff00ff) | (x & 0xff00ff) << 8, x = x >> 16 | x << 16) + (x = (x >> 8 & 0xff00ff) | (x & 0xff00ff) << 8, x = x >> 16 | x << 16) -#if defined(__arm__) && !defined(__linux__) && !defined(_WIN32) && !defined(__APPLE__) // bare metal ARM Proxmark lacks malloc()/free() +#if defined(__arm__) && !defined(__linux__) && !defined(_WIN32) && !defined(__APPLE__) // bare metal ARM Proxmark lacks malloc()/free() void crypto1_create(struct Crypto1State *s, uint64_t key) { - int i; + int i; - for(i = 47;s && i > 0; i -= 2) { - s->odd = s->odd << 1 | BIT(key, (i - 1) ^ 7); - s->even = s->even << 1 | BIT(key, i ^ 7); - } - return; + for(i = 47;s && i > 0; i -= 2) { + s->odd = s->odd << 1 | BIT(key, (i - 1) ^ 7); + s->even = s->even << 1 | BIT(key, i ^ 7); + } + return; } void crypto1_destroy(struct Crypto1State *state) { - state->odd = 0; - state->even = 0; + state->odd = 0; + state->even = 0; } #else struct Crypto1State * crypto1_create(uint64_t key) { - struct Crypto1State *s = malloc(sizeof(*s)); - if ( !s ) return NULL; + struct Crypto1State *s = malloc(sizeof(*s)); + if ( !s ) return NULL; - s->odd = s->even = 0; + s->odd = s->even = 0; - int i; - for(i = 47; i > 0; i -= 2) { - s->odd = s->odd << 1 | BIT(key, (i - 1) ^ 7); - s->even = s->even << 1 | BIT(key, i ^ 7); - } - return s; + int i; + for(i = 47; i > 0; i -= 2) { + s->odd = s->odd << 1 | BIT(key, (i - 1) ^ 7); + s->even = s->even << 1 | BIT(key, i ^ 7); + } + return s; } void crypto1_destroy(struct Crypto1State *state) { - free(state); + free(state); } #endif void crypto1_get_lfsr(struct Crypto1State *state, uint64_t *lfsr) { - int i; - for(*lfsr = 0, i = 23; i >= 0; --i) { - *lfsr = *lfsr << 1 | BIT(state->odd, i ^ 3); - *lfsr = *lfsr << 1 | BIT(state->even, i ^ 3); - } + int i; + for(*lfsr = 0, i = 23; i >= 0; --i) { + *lfsr = *lfsr << 1 | BIT(state->odd, i ^ 3); + *lfsr = *lfsr << 1 | BIT(state->even, i ^ 3); + } } uint8_t crypto1_bit(struct Crypto1State *s, uint8_t in, int is_encrypted) { - uint32_t feedin, t; - uint8_t ret = filter(s->odd); + uint32_t feedin, t; + uint8_t ret = filter(s->odd); - feedin = ret & !!is_encrypted; - feedin ^= !!in; - feedin ^= LF_POLY_ODD & s->odd; - feedin ^= LF_POLY_EVEN & s->even; - s->even = s->even << 1 | evenparity32(feedin); + feedin = ret & !!is_encrypted; + feedin ^= !!in; + feedin ^= LF_POLY_ODD & s->odd; + feedin ^= LF_POLY_EVEN & s->even; + s->even = s->even << 1 | evenparity32(feedin); - t = s->odd; - s->odd = s->even; - s->even = t; + t = s->odd; + s->odd = s->even; + s->even = t; - return ret; + return ret; } uint8_t crypto1_byte(struct Crypto1State *s, uint8_t in, int is_encrypted) { - uint8_t ret = 0; - ret |= crypto1_bit(s, BIT(in, 0), is_encrypted) << 0; - ret |= crypto1_bit(s, BIT(in, 1), is_encrypted) << 1; - ret |= crypto1_bit(s, BIT(in, 2), is_encrypted) << 2; - ret |= crypto1_bit(s, BIT(in, 3), is_encrypted) << 3; - ret |= crypto1_bit(s, BIT(in, 4), is_encrypted) << 4; - ret |= crypto1_bit(s, BIT(in, 5), is_encrypted) << 5; - ret |= crypto1_bit(s, BIT(in, 6), is_encrypted) << 6; - ret |= crypto1_bit(s, BIT(in, 7), is_encrypted) << 7; - return ret; + uint8_t ret = 0; + ret |= crypto1_bit(s, BIT(in, 0), is_encrypted) << 0; + ret |= crypto1_bit(s, BIT(in, 1), is_encrypted) << 1; + ret |= crypto1_bit(s, BIT(in, 2), is_encrypted) << 2; + ret |= crypto1_bit(s, BIT(in, 3), is_encrypted) << 3; + ret |= crypto1_bit(s, BIT(in, 4), is_encrypted) << 4; + ret |= crypto1_bit(s, BIT(in, 5), is_encrypted) << 5; + ret |= crypto1_bit(s, BIT(in, 6), is_encrypted) << 6; + ret |= crypto1_bit(s, BIT(in, 7), is_encrypted) << 7; + return ret; } uint32_t crypto1_word(struct Crypto1State *s, uint32_t in, int is_encrypted) { - uint32_t ret = 0; - ret |= crypto1_bit(s, BEBIT(in, 0), is_encrypted) << (0 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 1), is_encrypted) << (1 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 2), is_encrypted) << (2 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 3), is_encrypted) << (3 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 4), is_encrypted) << (4 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 5), is_encrypted) << (5 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 6), is_encrypted) << (6 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 7), is_encrypted) << (7 ^ 24); + uint32_t ret = 0; + ret |= crypto1_bit(s, BEBIT(in, 0), is_encrypted) << (0 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 1), is_encrypted) << (1 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 2), is_encrypted) << (2 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 3), is_encrypted) << (3 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 4), is_encrypted) << (4 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 5), is_encrypted) << (5 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 6), is_encrypted) << (6 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 7), is_encrypted) << (7 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 8), is_encrypted) << (8 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 9), is_encrypted) << (9 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 10), is_encrypted) << (10 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 11), is_encrypted) << (11 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 12), is_encrypted) << (12 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 13), is_encrypted) << (13 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 14), is_encrypted) << (14 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 15), is_encrypted) << (15 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 8), is_encrypted) << (8 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 9), is_encrypted) << (9 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 10), is_encrypted) << (10 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 11), is_encrypted) << (11 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 12), is_encrypted) << (12 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 13), is_encrypted) << (13 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 14), is_encrypted) << (14 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 15), is_encrypted) << (15 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 16), is_encrypted) << (16 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 17), is_encrypted) << (17 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 18), is_encrypted) << (18 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 19), is_encrypted) << (19 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 20), is_encrypted) << (20 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 21), is_encrypted) << (21 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 22), is_encrypted) << (22 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 23), is_encrypted) << (23 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 16), is_encrypted) << (16 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 17), is_encrypted) << (17 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 18), is_encrypted) << (18 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 19), is_encrypted) << (19 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 20), is_encrypted) << (20 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 21), is_encrypted) << (21 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 22), is_encrypted) << (22 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 23), is_encrypted) << (23 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 24), is_encrypted) << (24 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 25), is_encrypted) << (25 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 26), is_encrypted) << (26 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 27), is_encrypted) << (27 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 28), is_encrypted) << (28 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 29), is_encrypted) << (29 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 30), is_encrypted) << (30 ^ 24); - ret |= crypto1_bit(s, BEBIT(in, 31), is_encrypted) << (31 ^ 24); - return ret; + ret |= crypto1_bit(s, BEBIT(in, 24), is_encrypted) << (24 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 25), is_encrypted) << (25 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 26), is_encrypted) << (26 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 27), is_encrypted) << (27 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 28), is_encrypted) << (28 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 29), is_encrypted) << (29 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 30), is_encrypted) << (30 ^ 24); + ret |= crypto1_bit(s, BEBIT(in, 31), is_encrypted) << (31 ^ 24); + return ret; } /* prng_successor @@ -145,9 +145,9 @@ uint32_t crypto1_word(struct Crypto1State *s, uint32_t in, int is_encrypted) */ uint32_t prng_successor(uint32_t x, uint32_t n) { - SWAPENDIAN(x); - while(n--) - x = x >> 1 | (x >> 16 ^ x >> 18 ^ x >> 19 ^ x >> 21) << 31; + SWAPENDIAN(x); + while(n--) + x = x >> 1 | (x >> 16 ^ x >> 18 ^ x >> 19 ^ x >> 21) << 31; - return SWAPENDIAN(x); + return SWAPENDIAN(x); } diff --git a/common/crc.c b/common/crc.c index 4097fdd48..b91eea505 100644 --- a/common/crc.c +++ b/common/crc.c @@ -10,118 +10,118 @@ #include "crc.h" void crc_init_ref(crc_t *crc, int order, uint32_t polynom, uint32_t initial_value, uint32_t final_xor, bool refin, bool refout) { - crc_init(crc, order, polynom, initial_value, final_xor); - crc->refin = refin; - crc->refout = refout; - crc_clear(crc); + crc_init(crc, order, polynom, initial_value, final_xor); + crc->refin = refin; + crc->refout = refout; + crc_clear(crc); } void crc_init(crc_t *crc, int order, uint32_t polynom, uint32_t initial_value, uint32_t final_xor) { - crc->order = order; - crc->topbit = BITMASK( order-1 ); - crc->polynom = polynom; - crc->initial_value = initial_value; - crc->final_xor = final_xor; - crc->mask = (1L<refin = false; - crc->refout = false; - crc_clear(crc); + crc->order = order; + crc->topbit = BITMASK( order-1 ); + crc->polynom = polynom; + crc->initial_value = initial_value; + crc->final_xor = final_xor; + crc->mask = (1L<refin = false; + crc->refout = false; + crc_clear(crc); } void crc_clear(crc_t *crc) { - crc->state = crc->initial_value & crc->mask; - if (crc->refin) - crc->state = reflect(crc->state, crc->order); + crc->state = crc->initial_value & crc->mask; + if (crc->refin) + crc->state = reflect(crc->state, crc->order); } void crc_update2(crc_t *crc, uint32_t data, int data_width){ - if (crc->refin) - data = reflect(data, data_width); + if (crc->refin) + data = reflect(data, data_width); - // Bring the next byte into the remainder. - crc->state ^= data << (crc->order - data_width); + // Bring the next byte into the remainder. + crc->state ^= data << (crc->order - data_width); - for( uint8_t bit = data_width; bit > 0; --bit) { + for( uint8_t bit = data_width; bit > 0; --bit) { - if (crc->state & crc->topbit) - crc->state = (crc->state << 1) ^ crc->polynom; - else - crc->state = (crc->state << 1); - } + if (crc->state & crc->topbit) + crc->state = (crc->state << 1) ^ crc->polynom; + else + crc->state = (crc->state << 1); + } } void crc_update(crc_t *crc, uint32_t data, int data_width) { - if (crc->refin) - data = reflect(data, data_width); + if (crc->refin) + data = reflect(data, data_width); - int i; - for(i=0; istate; - crc->state = crc->state >> 1; - if( (oldstate^data) & 1 ) { - crc->state ^= crc->polynom; - } - data >>= 1; - } + int i; + for(i=0; istate; + crc->state = crc->state >> 1; + if( (oldstate^data) & 1 ) { + crc->state ^= crc->polynom; + } + data >>= 1; + } } uint32_t crc_finish(crc_t *crc) { - uint32_t val = crc->state; - if (crc->refout) - val = reflect(val, crc->order); - return ( val ^ crc->final_xor ) & crc->mask; + uint32_t val = crc->state; + if (crc->refout) + val = reflect(val, crc->order); + return ( val ^ crc->final_xor ) & crc->mask; } /* static void print_crc(crc_t *crc) { - printf(" Order %d\n Poly %x\n Init %x\n Final %x\n Mask %x\n topbit %x\n RefIn %s\n RefOut %s\n State %x\n", - crc->order, - crc->polynom, - crc->initial_value, - crc->final_xor, - crc->mask, - crc->topbit, - (crc->refin) ? "TRUE":"FALSE", - (crc->refout) ? "TRUE":"FALSE", - crc->state - ); + printf(" Order %d\n Poly %x\n Init %x\n Final %x\n Mask %x\n topbit %x\n RefIn %s\n RefOut %s\n State %x\n", + crc->order, + crc->polynom, + crc->initial_value, + crc->final_xor, + crc->mask, + crc->topbit, + (crc->refin) ? "TRUE":"FALSE", + (crc->refout) ? "TRUE":"FALSE", + crc->state + ); } */ // width=8 poly=0x31 init=0x00 refin=true refout=true xorout=0x00 check=0xA1 name="CRC-8/MAXIM" uint32_t CRC8Maxim(uint8_t *buff, size_t size) { - crc_t crc; - crc_init_ref(&crc, 8, 0x31, 0, 0, true, true); - for ( int i=0; i < size; ++i) - crc_update2(&crc, buff[i], 8); - return crc_finish(&crc); + crc_t crc; + crc_init_ref(&crc, 8, 0x31, 0, 0, true, true); + for ( int i=0; i < size; ++i) + crc_update2(&crc, buff[i], 8); + return crc_finish(&crc); } // width=8 poly=0x1d, init=0xc7 (0xe3 - WRONG! but it mentioned in MAD datasheet) refin=false refout=false xorout=0x00 name="CRC-8/MIFARE-MAD" uint32_t CRC8Mad(uint8_t *buff, size_t size) { - crc_t crc; - crc_init_ref(&crc, 8, 0x1d, 0xc7, 0, false, false); - for ( int i = 0; i < size; ++i) - crc_update2(&crc, buff[i], 8); - return crc_finish(&crc); + crc_t crc; + crc_init_ref(&crc, 8, 0x1d, 0xc7, 0, false, false); + for ( int i = 0; i < size; ++i) + crc_update2(&crc, buff[i], 8); + return crc_finish(&crc); } // width=4 poly=0xC, reversed poly=0x7 init=0x5 refin=true refout=true xorout=0x0000 check= name="CRC-4/LEGIC" uint32_t CRC4Legic(uint8_t *cmd, size_t size) { - crc_t crc; - crc_init_ref(&crc, 4, 0x19 >> 1, 0x5, 0, true, true); - crc_update2(&crc, 1, 1); /* CMD_READ */ - crc_update2(&crc, cmd[0], 8); - crc_update2(&crc, cmd[1], 8); - return reflect(crc_finish(&crc), 4); + crc_t crc; + crc_init_ref(&crc, 4, 0x19 >> 1, 0x5, 0, true, true); + crc_update2(&crc, 1, 1); /* CMD_READ */ + crc_update2(&crc, cmd[0], 8); + crc_update2(&crc, cmd[1], 8); + return reflect(crc_finish(&crc), 4); } // width=8 poly=0x63, reversed poly=0x8D init=0x55 refin=true refout=true xorout=0x0000 check=0xC6 name="CRC-8/LEGIC" // the CRC needs to be reversed before returned. uint32_t CRC8Legic(uint8_t *buff, size_t size) { - crc_t crc; - crc_init_ref(&crc, 8, 0x63, 0x55, 0, true, true); - for ( int i = 0; i < size; ++i) - crc_update2(&crc, buff[i], 8); - return reflect8(crc_finish(&crc)); + crc_t crc; + crc_init_ref(&crc, 8, 0x63, 0x55, 0, true, true); + for ( int i = 0; i < size; ++i) + crc_update2(&crc, buff[i], 8); + return reflect8(crc_finish(&crc)); } \ No newline at end of file diff --git a/common/crc.h b/common/crc.h index 015fbf60c..5a15563b7 100644 --- a/common/crc.h +++ b/common/crc.h @@ -9,19 +9,19 @@ #ifndef __CRC_H #define __CRC_H -#include "common.h" //stdint, stddef, stdbool -#include "util.h" // reflect, bswap_16 +#include "common.h" //stdint, stddef, stdbool +#include "util.h" // reflect, bswap_16 typedef struct crc { - uint32_t state; - int order; - uint32_t polynom; - uint32_t initial_value; - uint32_t final_xor; - uint32_t mask; - int topbit; - bool refin; /* Parameter: Reflect input bytes? */ - bool refout; /* Parameter: Reflect output CRC? */ + uint32_t state; + int order; + uint32_t polynom; + uint32_t initial_value; + uint32_t final_xor; + uint32_t mask; + int topbit; + bool refin; /* Parameter: Reflect input bytes? */ + bool refout; /* Parameter: Reflect output CRC? */ } crc_t; /* Initialize a crc structure. order is the order of the polynom, e.g. 32 for a CRC-32 @@ -64,14 +64,14 @@ uint32_t CRC8Legic(uint8_t *buff, size_t size); /* Static initialization of a crc structure */ #define CRC_INITIALIZER(_order, _polynom, _initial_value, _final_xor) { \ - .state = ((_initial_value) & ((1L<<(_order))-1)), \ - .order = (_order), \ - .polynom = (_polynom), \ - .initial_value = (_initial_value), \ - .final_xor = (_final_xor), \ - .mask = ((1L<<(_order))-1) \ - .refin = false, \ - .refout = false \ - } + .state = ((_initial_value) & ((1L<<(_order))-1)), \ + .order = (_order), \ + .polynom = (_polynom), \ + .initial_value = (_initial_value), \ + .final_xor = (_final_xor), \ + .mask = ((1L<<(_order))-1) \ + .refin = false, \ + .refout = false \ + } #endif /* __CRC_H */ diff --git a/common/crc16.c b/common/crc16.c index b6145013b..af3ca2676 100644 --- a/common/crc16.c +++ b/common/crc16.c @@ -13,125 +13,125 @@ static CrcType_t crc_type = CRC_NONE; void init_table(CrcType_t ct) { - // same crc algo, and initialised already - if ( ct == crc_type && crc_table_init) - return; + // same crc algo, and initialised already + if ( ct == crc_type && crc_table_init) + return; - // not the same crc algo. reset table. - if ( ct != crc_type) - reset_table(); + // not the same crc algo. reset table. + if ( ct != crc_type) + reset_table(); - crc_type = ct; + crc_type = ct; - switch (ct) { - case CRC_14443_A: - case CRC_14443_B: - case CRC_15693: - case CRC_ICLASS: generate_table(CRC16_POLY_CCITT, true); break; - case CRC_FELICA: generate_table(CRC16_POLY_CCITT, false); break; - case CRC_LEGIC: generate_table(CRC16_POLY_LEGIC, true); break; - case CRC_CCITT: generate_table(CRC16_POLY_CCITT, false); break; - case CRC_KERMIT: generate_table(CRC16_POLY_CCITT, true); break; - default: - crc_table_init = false; - crc_type = CRC_NONE; - break; - } + switch (ct) { + case CRC_14443_A: + case CRC_14443_B: + case CRC_15693: + case CRC_ICLASS: generate_table(CRC16_POLY_CCITT, true); break; + case CRC_FELICA: generate_table(CRC16_POLY_CCITT, false); break; + case CRC_LEGIC: generate_table(CRC16_POLY_LEGIC, true); break; + case CRC_CCITT: generate_table(CRC16_POLY_CCITT, false); break; + case CRC_KERMIT: generate_table(CRC16_POLY_CCITT, true); break; + default: + crc_table_init = false; + crc_type = CRC_NONE; + break; + } } void generate_table( uint16_t polynomial, bool refin) { - uint16_t i, j, crc, c; + uint16_t i, j, crc, c; - for (i = 0; i < 256; i++) { - crc = 0; - if (refin) - c = reflect8(i) << 8; - else - c = i << 8; + for (i = 0; i < 256; i++) { + crc = 0; + if (refin) + c = reflect8(i) << 8; + else + c = i << 8; for (j = 0; j < 8; j++) { if ( (crc ^ c) & 0x8000 ) - crc = ( crc << 1 ) ^ polynomial; + crc = ( crc << 1 ) ^ polynomial; else - crc = crc << 1; + crc = crc << 1; c = c << 1; } - if (refin) - crc = reflect16(crc); + if (refin) + crc = reflect16(crc); - crc_table[i] = crc; - } + crc_table[i] = crc; + } crc_table_init = true; } void reset_table(void) { - memset(crc_table, 0, sizeof(crc_table)); - crc_table_init = false; - crc_type = CRC_NONE; + memset(crc_table, 0, sizeof(crc_table)); + crc_table_init = false; + crc_type = CRC_NONE; } // table lookup LUT solution uint16_t crc16_fast(uint8_t const *d, size_t n, uint16_t initval, bool refin, bool refout) { - // fast lookup table algorithm without augmented zero bytes, e.g. used in pkzip. - // only usable with polynom orders of 8, 16, 24 or 32. - if (n == 0) + // fast lookup table algorithm without augmented zero bytes, e.g. used in pkzip. + // only usable with polynom orders of 8, 16, 24 or 32. + if (n == 0) return (~initval); - uint16_t crc = initval; + uint16_t crc = initval; - if (refin) - crc = reflect16(crc); + if (refin) + crc = reflect16(crc); - if (!refin) - while (n--) crc = (crc << 8) ^ crc_table[ ((crc >> 8) ^ *d++) & 0xFF ]; - else - while (n--) crc = (crc >> 8) ^ crc_table[ (crc & 0xFF) ^ *d++]; + if (!refin) + while (n--) crc = (crc << 8) ^ crc_table[ ((crc >> 8) ^ *d++) & 0xFF ]; + else + while (n--) crc = (crc >> 8) ^ crc_table[ (crc & 0xFF) ^ *d++]; - if (refout^refin) - crc = reflect16(crc); + if (refout^refin) + crc = reflect16(crc); - return crc; + return crc; } // bit looped solution TODO REMOVED uint16_t update_crc16_ex( uint16_t crc, uint8_t c, uint16_t polynomial ) { - uint16_t i, v, tmp = 0; + uint16_t i, v, tmp = 0; - v = (crc ^ c) & 0xff; + v = (crc ^ c) & 0xff; - for (i = 0; i < 8; i++) { + for (i = 0; i < 8; i++) { - if ( (tmp ^ v) & 1 ) - tmp = ( tmp >> 1 ) ^ polynomial; - else - tmp >>= 1; + if ( (tmp ^ v) & 1 ) + tmp = ( tmp >> 1 ) ^ polynomial; + else + tmp >>= 1; - v >>= 1; - } - return ((crc >> 8) ^ tmp) & 0xffff; + v >>= 1; + } + return ((crc >> 8) ^ tmp) & 0xffff; } uint16_t update_crc16( uint16_t crc, uint8_t c ) { - return update_crc16_ex( crc, c, CRC16_POLY_CCITT); + return update_crc16_ex( crc, c, CRC16_POLY_CCITT); } // two ways. msb or lsb loop. uint16_t crc16(uint8_t const *d, size_t length, uint16_t remainder, uint16_t polynomial, bool refin, bool refout) { - if (length == 0) + if (length == 0) return (~remainder); - uint8_t c; + uint8_t c; for (uint32_t i = 0; i < length; ++i) { - c = d[i]; - if (refin) c = reflect8(c); + c = d[i]; + if (refin) c = reflect8(c); - // xor in at msb + // xor in at msb remainder ^= (c << 8); - // 8 iteration loop + // 8 iteration loop for (uint8_t j = 8; j; --j) { if (remainder & 0x8000) { remainder = (remainder << 1) ^ polynomial; @@ -140,58 +140,58 @@ uint16_t crc16(uint8_t const *d, size_t length, uint16_t remainder, uint16_t pol } } } - if (refout) - remainder = reflect16(remainder); + if (refout) + remainder = reflect16(remainder); return remainder; } void compute_crc(CrcType_t ct, const uint8_t *d, size_t n, uint8_t *first, uint8_t *second) { - // can't calc a crc on less than 1 byte - if ( n == 0 ) return; + // can't calc a crc on less than 1 byte + if ( n == 0 ) return; - init_table(ct); + init_table(ct); - uint16_t crc = 0; - switch (ct) { - case CRC_14443_A: crc = crc16_a(d, n); break; - case CRC_14443_B: - case CRC_15693: crc = crc16_x25(d, n); break; - case CRC_ICLASS: crc = crc16_iclass(d, n); break; - case CRC_FELICA:crc = crc16_xmodem(d, n); break; - //case CRC_LEGIC: - case CRC_CCITT: crc = crc16_ccitt(d, n); break; - case CRC_KERMIT: crc = crc16_kermit(d, n); break; - default: break; - } - *first = (crc & 0xFF); + uint16_t crc = 0; + switch (ct) { + case CRC_14443_A: crc = crc16_a(d, n); break; + case CRC_14443_B: + case CRC_15693: crc = crc16_x25(d, n); break; + case CRC_ICLASS: crc = crc16_iclass(d, n); break; + case CRC_FELICA:crc = crc16_xmodem(d, n); break; + //case CRC_LEGIC: + case CRC_CCITT: crc = crc16_ccitt(d, n); break; + case CRC_KERMIT: crc = crc16_kermit(d, n); break; + default: break; + } + *first = (crc & 0xFF); *second = ((crc >> 8) & 0xFF); } uint16_t crc(CrcType_t ct, const uint8_t *d, size_t n) { - // can't calc a crc on less than 3 byte. (1byte + 2 crc bytes) - if ( n < 3 ) return 0; + // can't calc a crc on less than 3 byte. (1byte + 2 crc bytes) + if ( n < 3 ) return 0; - init_table(ct); - switch (ct) { - case CRC_14443_A: return crc16_a(d, n); - case CRC_14443_B: - case CRC_15693: return crc16_x25(d, n); - case CRC_ICLASS: return crc16_iclass(d, n); - case CRC_FELICA: return crc16_xmodem(d, n); - //case CRC_LEGIC: - case CRC_CCITT: return crc16_ccitt(d, n); - case CRC_KERMIT: return crc16_kermit(d, n); - default: break; - } - return 0; + init_table(ct); + switch (ct) { + case CRC_14443_A: return crc16_a(d, n); + case CRC_14443_B: + case CRC_15693: return crc16_x25(d, n); + case CRC_ICLASS: return crc16_iclass(d, n); + case CRC_FELICA: return crc16_xmodem(d, n); + //case CRC_LEGIC: + case CRC_CCITT: return crc16_ccitt(d, n); + case CRC_KERMIT: return crc16_kermit(d, n); + default: break; + } + return 0; } // check CRC // ct crc type -// d buffer with data -// n length (including crc) +// d buffer with data +// n length (including crc) // // This function uses the message + crc bytes in order to compare the "residue" afterwards. // crc16 algos like CRC-A become 0x000 @@ -199,39 +199,39 @@ uint16_t crc(CrcType_t ct, const uint8_t *d, size_t n) { // If calculated with crc bytes, the residue should be 0xF0B8 bool check_crc(CrcType_t ct, const uint8_t *d, size_t n) { - // can't calc a crc on less than 3 byte. (1byte + 2 crc bytes) - if ( n < 3 ) return false; + // can't calc a crc on less than 3 byte. (1byte + 2 crc bytes) + if ( n < 3 ) return false; - init_table(ct); + init_table(ct); - switch (ct) { - case CRC_14443_A: return (crc16_a(d, n) == 0); - case CRC_14443_B: return (crc16_x25(d, n) == X25_CRC_CHECK); - case CRC_15693: return (crc16_x25(d, n) == X25_CRC_CHECK); - case CRC_ICLASS: return (crc16_iclass(d, n) == 0); - case CRC_FELICA: return (crc16_xmodem(d, n) == 0); - //case CRC_LEGIC: - case CRC_CCITT: return (crc16_ccitt(d, n) == 0); - default: break; - } - return false; + switch (ct) { + case CRC_14443_A: return (crc16_a(d, n) == 0); + case CRC_14443_B: return (crc16_x25(d, n) == X25_CRC_CHECK); + case CRC_15693: return (crc16_x25(d, n) == X25_CRC_CHECK); + case CRC_ICLASS: return (crc16_iclass(d, n) == 0); + case CRC_FELICA: return (crc16_xmodem(d, n) == 0); + //case CRC_LEGIC: + case CRC_CCITT: return (crc16_ccitt(d, n) == 0); + default: break; + } + return false; } // poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1 residue=0x0000 name="CRC-16/CCITT-FALSE" uint16_t crc16_ccitt(uint8_t const *d, size_t n) { - return crc16_fast(d, n, 0xffff, false, false); + return crc16_fast(d, n, 0xffff, false, false); } // FDX-B ISO11784/85) uses KERMIT // poly=0x1021 init=0x0000 refin=true refout=true xorout=0x0000 name="KERMIT" uint16_t crc16_kermit(uint8_t const *d, size_t n) { - return crc16_fast(d, n, 0x0000, true, true); + return crc16_fast(d, n, 0x0000, true, true); } // FeliCa uses XMODEM // poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 name="XMODEM" uint16_t crc16_xmodem(uint8_t const *d, size_t n) { - return crc16_fast(d, n, 0x0000, false, false); + return crc16_fast(d, n, 0x0000, false, false); } // Following standards uses X-25 @@ -240,14 +240,14 @@ uint16_t crc16_xmodem(uint8_t const *d, size_t n) { // ISO/IEC 13239 (formerly ISO/IEC 3309) // poly=0x1021 init=0xffff refin=true refout=true xorout=0xffff name="X-25" uint16_t crc16_x25(uint8_t const *d, size_t n) { - uint16_t crc = crc16_fast(d, n, 0xffff, true, true); - crc = ~crc; - return crc; + uint16_t crc = crc16_fast(d, n, 0xffff, true, true); + crc = ~crc; + return crc; } // CRC-A (14443-3) // poly=0x1021 init=0xc6c6 refin=true refout=true xorout=0x0000 name="CRC-A" uint16_t crc16_a(uint8_t const *d, size_t n) { - return crc16_fast(d, n, 0xC6C6, true, true); + return crc16_fast(d, n, 0xC6C6, true, true); } // iClass crc @@ -255,12 +255,12 @@ uint16_t crc16_a(uint8_t const *d, size_t n) { // poly 0x1021 reflected 0x8408 // poly=0x1021 init=0x4807 refin=true refout=true xorout=0x0BC3 check=0xF0B8 name="CRC-16/ICLASS" uint16_t crc16_iclass(uint8_t const *d, size_t n) { - return crc16_fast(d, n, 0x4807, true, true); + return crc16_fast(d, n, 0x4807, true, true); } // This CRC-16 is used in Legic Advant systems. // poly=0xB400, init=depends refin=true refout=true xorout=0x0000 check= name="CRC-16/LEGIC" uint16_t crc16_legic(uint8_t const *d, size_t n, uint8_t uidcrc) { - uint16_t initial = uidcrc << 8 | uidcrc; - return crc16_fast(d, n, initial, true, true); + uint16_t initial = uidcrc << 8 | uidcrc; + return crc16_fast(d, n, initial, true, true); } diff --git a/common/crc16.h b/common/crc16.h index c11cf6351..6b3f1432b 100644 --- a/common/crc16.h +++ b/common/crc16.h @@ -14,20 +14,20 @@ #define CRC16_POLY_CCITT 0x1021 #define CRC16_POLY_LEGIC 0xc6c6 //0x6363 -#define CRC16_POLY_DNP 0x3d65 +#define CRC16_POLY_DNP 0x3d65 -#define X25_CRC_CHECK ((uint16_t)(~0xF0B8 & 0xFFFF)) // use this for checking of a correct crc +#define X25_CRC_CHECK ((uint16_t)(~0xF0B8 & 0xFFFF)) // use this for checking of a correct crc typedef enum { - CRC_NONE, - CRC_14443_A, - CRC_14443_B, - CRC_15693, - CRC_ICLASS, - CRC_FELICA, - CRC_LEGIC, - CRC_CCITT, - CRC_KERMIT, + CRC_NONE, + CRC_14443_A, + CRC_14443_B, + CRC_15693, + CRC_ICLASS, + CRC_FELICA, + CRC_LEGIC, + CRC_CCITT, + CRC_KERMIT, } CrcType_t; uint16_t update_crc16_ex( uint16_t crc, uint8_t c, uint16_t polynomial ); diff --git a/common/crc64.c b/common/crc64.c index adf62d97d..5dc779f8d 100644 --- a/common/crc64.c +++ b/common/crc64.c @@ -6,7 +6,7 @@ #define CRC64_ECMA_PRESET 0x0000000000000000 const uint64_t crc64_table[] = { - 0x0000000000000000, 0x42F0E1EBA9EA3693, 0x85E1C3D753D46D26, 0xC711223CFA3E5BB5, + 0x0000000000000000, 0x42F0E1EBA9EA3693, 0x85E1C3D753D46D26, 0xC711223CFA3E5BB5, 0x493366450E42ECDF, 0x0BC387AEA7A8DA4C, 0xCCD2A5925D9681F9, 0x8E224479F47CB76A, 0x9266CC8A1C85D9BE, 0xD0962D61B56FEF2D, 0x17870F5D4F51B498, 0x5577EEB6E6BB820B, 0xDB55AACF12C73561, 0x99A54B24BB2D03F2, 0x5EB4691841135847, 0x1C4488F3E8F96ED4, @@ -74,10 +74,10 @@ const uint64_t crc64_table[] = { void crc64 (const uint8_t *data, const size_t len, uint64_t *crc) { - for (size_t i = 0; i < len; i++) { - uint8_t tableIndex = (((uint8_t)(*crc >> 56)) ^ data[i]) & 0xff; - *crc = crc64_table[tableIndex] ^ (*crc << 8); - } + for (size_t i = 0; i < len; i++) { + uint8_t tableIndex = (((uint8_t)(*crc >> 56)) ^ data[i]) & 0xff; + *crc = crc64_table[tableIndex] ^ (*crc << 8); + } } -//suint8_t x = (c & 0xFF00000000000000 ) >> 56; \ No newline at end of file +//suint8_t x = (c & 0xFF00000000000000 ) >> 56; \ No newline at end of file diff --git a/common/default_version.c b/common/default_version.c index b143afd4c..af5bc59e2 100644 --- a/common/default_version.c +++ b/common/default_version.c @@ -1,9 +1,9 @@ #include "proxmark3.h" /* This is the default version.c file that Makefile.common falls back to if perl is not available */ const struct version_information __attribute__((section(".version_information"))) version_information = { - VERSION_INFORMATION_MAGIC, - 1, /* version 1 */ - 0, /* version information not present */ - 2, /* cleanliness couldn't be determined */ - /* Remaining fields: zero */ + VERSION_INFORMATION_MAGIC, + 1, /* version 1 */ + 0, /* version information not present */ + 2, /* cleanliness couldn't be determined */ + /* Remaining fields: zero */ }; diff --git a/common/desfire.h b/common/desfire.h index 81ca57b92..3cffd2967 100644 --- a/common/desfire.h +++ b/common/desfire.h @@ -55,15 +55,15 @@ typedef enum { enum DESFIRE_AUTH_SCHEME { - AS_LEGACY, - AS_NEW + AS_LEGACY, + AS_NEW }; enum DESFIRE_CRYPTOALGO { - T_DES = 0x00, - T_3DES = 0x01, - T_3K3DES = 0x02, - T_AES = 0x03 + T_DES = 0x00, + T_3DES = 0x01, + T_3K3DES = 0x02, + T_AES = 0x03 }; @@ -74,7 +74,7 @@ struct desfire_key { // DES_key_schedule ks1; // DES_key_schedule ks2; // DES_key_schedule ks3; - AesCtx aes_ks; + AesCtx aes_ks; uint8_t cmac_sk1[24]; uint8_t cmac_sk2[24]; uint8_t aes_version; @@ -89,10 +89,10 @@ struct desfire_tag { uint8_t last_internal_error; uint8_t last_pcd_error; desfirekey_t session_key; - enum DESFIRE_AUTH_SCHEME authentication_scheme; + enum DESFIRE_AUTH_SCHEME authentication_scheme; uint8_t authenticated_key_no; - uint8_t ivect[MAX_CRYPTO_BLOCK_SIZE]; + uint8_t ivect[MAX_CRYPTO_BLOCK_SIZE]; uint8_t cmac[16]; uint8_t *crypto_buffer; size_t crypto_buffer_size; @@ -111,66 +111,66 @@ enum DESFIRE_FILE_TYPES { }; enum DESFIRE_STATUS { - OPERATION_OK = 0x00, - NO_CHANGES = 0x0c, - OUT_OF_EEPROM_ERROR = 0x0e, - ILLEGAL_COMMAND_CODE = 0x1c, - INTEGRITY_ERROR = 0x1e, - NO_SUCH_KEY = 0x40, - LENGTH_ERROR = 0x7e, - PERMISSION_DENIED = 0x9d, - PARAMETER_ERROR = 0x9e, - APPLICATION_NOT_FOUND = 0xa0, - APPL_INTEGRITY_ERROR = 0xa1, - AUTHENTICATION_ERROR = 0xae, - ADDITIONAL_FRAME = 0xaf, - BOUNDARY_ERROR = 0xbe, - PICC_INTEGRITY_ERROR = 0xc1, - COMMAND_ABORTED = 0xca, - PICC_DISABLED_ERROR = 0xcd, - COUNT_ERROR = 0xce, - DUPLICATE_ERROR = 0xde, - EEPROM_ERROR = 0xee, - FILE_NOT_FOUND = 0xf0, - FILE_INTEGRITY_ERROR = 0xf1 + OPERATION_OK = 0x00, + NO_CHANGES = 0x0c, + OUT_OF_EEPROM_ERROR = 0x0e, + ILLEGAL_COMMAND_CODE = 0x1c, + INTEGRITY_ERROR = 0x1e, + NO_SUCH_KEY = 0x40, + LENGTH_ERROR = 0x7e, + PERMISSION_DENIED = 0x9d, + PARAMETER_ERROR = 0x9e, + APPLICATION_NOT_FOUND = 0xa0, + APPL_INTEGRITY_ERROR = 0xa1, + AUTHENTICATION_ERROR = 0xae, + ADDITIONAL_FRAME = 0xaf, + BOUNDARY_ERROR = 0xbe, + PICC_INTEGRITY_ERROR = 0xc1, + COMMAND_ABORTED = 0xca, + PICC_DISABLED_ERROR = 0xcd, + COUNT_ERROR = 0xce, + DUPLICATE_ERROR = 0xde, + EEPROM_ERROR = 0xee, + FILE_NOT_FOUND = 0xf0, + FILE_INTEGRITY_ERROR = 0xf1 }; enum DESFIRE_CMD { - CREATE_APPLICATION = 0xca, - DELETE_APPLICATION = 0xda, - GET_APPLICATION_IDS = 0x6a, - SELECT_APPLICATION = 0x5a, - FORMAT_PICC = 0xfc, - GET_VERSION = 0x60, - READ_DATA = 0xbd, - WRITE_DATA = 0x3d, - GET_VALUE = 0x6c, - CREDIT = 0x0c, - DEBIT = 0xdc, - LIMITED_CREDIT = 0x1c, - WRITE_RECORD = 0x3b, - READ_RECORDS = 0xbb, - CLEAR_RECORD_FILE = 0xeb, - COMMIT_TRANSACTION = 0xc7, - ABORT_TRANSACTION = 0xa7, - GET_FREE_MEMORY = 0x6e, - GET_FILE_IDS = 0x6f, - GET_FILE_SETTINGS = 0xf5, - CHANGE_FILE_SETTINGS = 0x5f, - CREATE_STD_DATA_FILE = 0xcd, - CREATE_BACKUP_DATA_FILE = 0xcb, - CREATE_VALUE_FILE = 0xcc, - CREATE_LINEAR_RECORD_FILE = 0xc1, - CREATE_CYCLIC_RECORD_FILE = 0xc0, - DELETE_FILE = 0xdf, - AUTHENTICATE = 0x0a, // AUTHENTICATE_NATIVE - AUTHENTICATE_ISO = 0x1a, // AUTHENTICATE_STANDARD - AUTHENTICATE_AES = 0xaa, - CHANGE_KEY_SETTINGS = 0x54, - GET_KEY_SETTINGS = 0x45, - CHANGE_KEY = 0xc4, - GET_KEY_VERSION = 0x64, - AUTHENTICATION_FRAME = 0xAF + CREATE_APPLICATION = 0xca, + DELETE_APPLICATION = 0xda, + GET_APPLICATION_IDS = 0x6a, + SELECT_APPLICATION = 0x5a, + FORMAT_PICC = 0xfc, + GET_VERSION = 0x60, + READ_DATA = 0xbd, + WRITE_DATA = 0x3d, + GET_VALUE = 0x6c, + CREDIT = 0x0c, + DEBIT = 0xdc, + LIMITED_CREDIT = 0x1c, + WRITE_RECORD = 0x3b, + READ_RECORDS = 0xbb, + CLEAR_RECORD_FILE = 0xeb, + COMMIT_TRANSACTION = 0xc7, + ABORT_TRANSACTION = 0xa7, + GET_FREE_MEMORY = 0x6e, + GET_FILE_IDS = 0x6f, + GET_FILE_SETTINGS = 0xf5, + CHANGE_FILE_SETTINGS = 0x5f, + CREATE_STD_DATA_FILE = 0xcd, + CREATE_BACKUP_DATA_FILE = 0xcb, + CREATE_VALUE_FILE = 0xcc, + CREATE_LINEAR_RECORD_FILE = 0xc1, + CREATE_CYCLIC_RECORD_FILE = 0xc0, + DELETE_FILE = 0xdf, + AUTHENTICATE = 0x0a, // AUTHENTICATE_NATIVE + AUTHENTICATE_ISO = 0x1a, // AUTHENTICATE_STANDARD + AUTHENTICATE_AES = 0xaa, + CHANGE_KEY_SETTINGS = 0x54, + GET_KEY_SETTINGS = 0x45, + CHANGE_KEY = 0xc4, + GET_KEY_VERSION = 0x64, + AUTHENTICATION_FRAME = 0xAF }; #endif diff --git a/common/i2c.c b/common/i2c.c index dff77a05a..041b3e7ec 100644 --- a/common/i2c.c +++ b/common/i2c.c @@ -10,797 +10,797 @@ //----------------------------------------------------------------------------- #include "i2c.h" -#define GPIO_RST AT91C_PIO_PA1 -#define GPIO_SCL AT91C_PIO_PA5 -#define GPIO_SDA AT91C_PIO_PA7 +#define GPIO_RST AT91C_PIO_PA1 +#define GPIO_SCL AT91C_PIO_PA5 +#define GPIO_SDA AT91C_PIO_PA7 -#define SCL_H HIGH(GPIO_SCL) -#define SCL_L LOW(GPIO_SCL) -#define SDA_H HIGH(GPIO_SDA) -#define SDA_L LOW(GPIO_SDA) +#define SCL_H HIGH(GPIO_SCL) +#define SCL_L LOW(GPIO_SCL) +#define SDA_H HIGH(GPIO_SDA) +#define SDA_L LOW(GPIO_SDA) -#define SCL_read (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SCL) -#define SDA_read (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SDA) +#define SCL_read (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SCL) +#define SDA_read (AT91C_BASE_PIOA->PIO_PDSR & GPIO_SDA) #define I2C_ERROR "I2C_WaitAck Error" volatile unsigned long c; -// Ö±½ÓʹÓÃÑ­»·À´ÑÓʱ£¬Ò»¸öÑ­»· 6 ÌõÖ¸Á48M£¬ Delay=1 ´ó¸ÅΪ 200kbps +// Ö±½ÓʹÓÃÑ­»·À´ÑÓʱ£¬Ò»¸öÑ­»· 6 ÌõÖ¸Á48M£¬ Delay=1 ´ó¸ÅΪ 200kbps // timer. // I2CSpinDelayClk(4) = 12.31us // I2CSpinDelayClk(1) = 3.07us void __attribute__((optimize("O0"))) I2CSpinDelayClk(uint16_t delay) { - for (c = delay * 2; c; c--) {}; + for (c = delay * 2; c; c--) {}; } -#define I2C_DELAY_1CLK I2CSpinDelayClk(1) -#define I2C_DELAY_2CLK I2CSpinDelayClk(2) -#define I2C_DELAY_XCLK(x) I2CSpinDelayClk((x)) +#define I2C_DELAY_1CLK I2CSpinDelayClk(1) +#define I2C_DELAY_2CLK I2CSpinDelayClk(2) +#define I2C_DELAY_XCLK(x) I2CSpinDelayClk((x)) #define ISO7618_MAX_FRAME 255 // try i2c bus recovery at 100kHz = 5uS high, 5uS low void I2C_recovery(void) { - DbpString("Performing i2c bus recovery"); + DbpString("Performing i2c bus recovery"); - // reset I2C - SDA_H; SCL_H; + // reset I2C + SDA_H; SCL_H; - //9nth cycle acts as NACK - for (int i = 0; i < 10; i++) { - SCL_H; WaitUS(5); - SCL_L; WaitUS(5); - } + //9nth cycle acts as NACK + for (int i = 0; i < 10; i++) { + SCL_H; WaitUS(5); + SCL_L; WaitUS(5); + } //a STOP signal (SDA from low to high while CLK is high) - SDA_L; WaitUS(5); - SCL_H; WaitUS(2); - SDA_H; WaitUS(2); + SDA_L; WaitUS(5); + SCL_H; WaitUS(2); + SDA_H; WaitUS(2); - bool isok = (SCL_read && SDA_read); + bool isok = (SCL_read && SDA_read); if (!SDA_read) - DbpString("I2C bus recovery error: SDA still LOW"); - if (!SCL_read) - DbpString("I2C bus recovery error: SCL still LOW"); - if (isok) - DbpString("I2C bus recovery complete"); + DbpString("I2C bus recovery error: SDA still LOW"); + if (!SCL_read) + DbpString("I2C bus recovery error: SCL still LOW"); + if (isok) + DbpString("I2C bus recovery complete"); } void I2C_init(void) { - // Configure reset pin, close up pull up, push-pull output, default high - AT91C_BASE_PIOA->PIO_PPUDR = GPIO_RST; - AT91C_BASE_PIOA->PIO_MDDR = GPIO_RST; + // Configure reset pin, close up pull up, push-pull output, default high + AT91C_BASE_PIOA->PIO_PPUDR = GPIO_RST; + AT91C_BASE_PIOA->PIO_MDDR = GPIO_RST; - // Configure I2C pin, open up, open leakage - AT91C_BASE_PIOA->PIO_PPUER |= (GPIO_SCL | GPIO_SDA); - AT91C_BASE_PIOA->PIO_MDER |= (GPIO_SCL | GPIO_SDA); + // Configure I2C pin, open up, open leakage + AT91C_BASE_PIOA->PIO_PPUER |= (GPIO_SCL | GPIO_SDA); + AT91C_BASE_PIOA->PIO_MDER |= (GPIO_SCL | GPIO_SDA); - // default three lines all pull up - AT91C_BASE_PIOA->PIO_SODR |= (GPIO_SCL | GPIO_SDA | GPIO_RST); + // default three lines all pull up + AT91C_BASE_PIOA->PIO_SODR |= (GPIO_SCL | GPIO_SDA | GPIO_RST); - AT91C_BASE_PIOA->PIO_OER |= (GPIO_SCL | GPIO_SDA | GPIO_RST); - AT91C_BASE_PIOA->PIO_PER |= (GPIO_SCL | GPIO_SDA | GPIO_RST); + AT91C_BASE_PIOA->PIO_OER |= (GPIO_SCL | GPIO_SDA | GPIO_RST); + AT91C_BASE_PIOA->PIO_PER |= (GPIO_SCL | GPIO_SDA | GPIO_RST); - bool isok = (SCL_read && SDA_read); - if ( !isok ) - I2C_recovery(); + bool isok = (SCL_read && SDA_read); + if ( !isok ) + I2C_recovery(); } // set the reset state void I2C_SetResetStatus(uint8_t LineRST, uint8_t LineSCK, uint8_t LineSDA) { - if (LineRST) - HIGH(GPIO_RST); - else - LOW(GPIO_RST); + if (LineRST) + HIGH(GPIO_RST); + else + LOW(GPIO_RST); - if (LineSCK) - HIGH(GPIO_SCL); - else - LOW(GPIO_SCL); + if (LineSCK) + HIGH(GPIO_SCL); + else + LOW(GPIO_SCL); - if (LineSDA) - HIGH(GPIO_SDA); - else - LOW(GPIO_SDA); + if (LineSDA) + HIGH(GPIO_SDA); + else + LOW(GPIO_SDA); } // Reset the SIM_Adapter, then enter the main program // Note: the SIM_Adapter will not enter the main program after power up. Please run this function before use SIM_Adapter. void I2C_Reset_EnterMainProgram(void) { - StartTicks(); - I2C_init(); - I2C_SetResetStatus(0, 0, 0); - WaitMS(30); - I2C_SetResetStatus(1, 0, 0); - WaitMS(30); - I2C_SetResetStatus(1, 1, 1); - WaitMS(10); + StartTicks(); + I2C_init(); + I2C_SetResetStatus(0, 0, 0); + WaitMS(30); + I2C_SetResetStatus(1, 0, 0); + WaitMS(30); + I2C_SetResetStatus(1, 1, 1); + WaitMS(10); } // Reset the SIM_Adapter, then enter the bootloader program // Reserve for firmware update. void I2C_Reset_EnterBootloader(void) { - StartTicks(); - I2C_init(); - I2C_SetResetStatus(0, 1, 1); - WaitMS(100); - I2C_SetResetStatus(1, 1, 1); - WaitMS(10); + StartTicks(); + I2C_init(); + I2C_SetResetStatus(0, 1, 1); + WaitMS(100); + I2C_SetResetStatus(1, 1, 1); + WaitMS(10); } // Wait for the clock to go High. bool WaitSCL_H_delay(uint32_t delay) { - while (delay--) { - if (SCL_read) { - return true; - } - I2C_DELAY_1CLK; - } - return false; + while (delay--) { + if (SCL_read) { + return true; + } + I2C_DELAY_1CLK; + } + return false; } // 5000 * 3.07us = 15350us. 15.35ms // 15000 * 3.07us = 46050us. 46.05ms bool WaitSCL_H(void) { - return WaitSCL_H_delay(15000); + return WaitSCL_H_delay(15000); } bool WaitSCL_L_delay(uint32_t delay) { - while (delay--) { - if (!SCL_read) { - return true; - } - I2C_DELAY_1CLK; - } - return false; + while (delay--) { + if (!SCL_read) { + return true; + } + I2C_DELAY_1CLK; + } + return false; } // 5000 * 3.07us = 15350us. 15.35ms bool WaitSCL_L(void) { - return WaitSCL_L_delay(15000); + return WaitSCL_L_delay(15000); } // Wait max 1800ms or until SCL goes LOW. // It timeout reading response from card // Which ever comes first bool WaitSCL_L_timeout(void){ - volatile uint16_t delay = 1800; - while ( delay-- ) { - // exit on SCL LOW - if (!SCL_read) - return true; + volatile uint16_t delay = 1800; + while ( delay-- ) { + // exit on SCL LOW + if (!SCL_read) + return true; - WaitMS(1); - } - return (delay == 0); + WaitMS(1); + } + return (delay == 0); } bool I2C_Start(void) { - I2C_DELAY_XCLK(4); - SDA_H; I2C_DELAY_1CLK; - SCL_H; - if (!WaitSCL_H()) return false; + I2C_DELAY_XCLK(4); + SDA_H; I2C_DELAY_1CLK; + SCL_H; + if (!WaitSCL_H()) return false; - I2C_DELAY_2CLK; + I2C_DELAY_2CLK; - if (!SCL_read) return false; - if (!SDA_read) return false; + if (!SCL_read) return false; + if (!SDA_read) return false; - SDA_L; I2C_DELAY_2CLK; - return true; + SDA_L; I2C_DELAY_2CLK; + return true; } bool I2C_WaitForSim() { - // wait for data from card - if (!WaitSCL_L_timeout()) - return false; + // wait for data from card + if (!WaitSCL_L_timeout()) + return false; - // 8051 speaks with smart card. - // 1000*50*3.07 = 153.5ms - // 1byte transfer == 1ms with max frame being 256bytes - if (!WaitSCL_H_delay(10 * 1000 * 50) ) - return false; + // 8051 speaks with smart card. + // 1000*50*3.07 = 153.5ms + // 1byte transfer == 1ms with max frame being 256bytes + if (!WaitSCL_H_delay(10 * 1000 * 50) ) + return false; - return true; + return true; } // send i2c STOP void I2C_Stop(void) { - SCL_L; I2C_DELAY_2CLK; - SDA_L; I2C_DELAY_2CLK; - SCL_H; I2C_DELAY_2CLK; - if (!WaitSCL_H()) return; - SDA_H; - I2C_DELAY_XCLK(8); + SCL_L; I2C_DELAY_2CLK; + SDA_L; I2C_DELAY_2CLK; + SCL_H; I2C_DELAY_2CLK; + if (!WaitSCL_H()) return; + SDA_H; + I2C_DELAY_XCLK(8); } // Send i2c ACK void I2C_Ack(void) { - SCL_L; I2C_DELAY_2CLK; - SDA_L; I2C_DELAY_2CLK; - SCL_H; I2C_DELAY_2CLK; - if (!WaitSCL_H()) return; - SCL_L; I2C_DELAY_2CLK; + SCL_L; I2C_DELAY_2CLK; + SDA_L; I2C_DELAY_2CLK; + SCL_H; I2C_DELAY_2CLK; + if (!WaitSCL_H()) return; + SCL_L; I2C_DELAY_2CLK; } // Send i2c NACK void I2C_NoAck(void) { - SCL_L; I2C_DELAY_2CLK; - SDA_H; I2C_DELAY_2CLK; - SCL_H; I2C_DELAY_2CLK; - if (!WaitSCL_H()) return; - SCL_L; I2C_DELAY_2CLK; + SCL_L; I2C_DELAY_2CLK; + SDA_H; I2C_DELAY_2CLK; + SCL_H; I2C_DELAY_2CLK; + if (!WaitSCL_H()) return; + SCL_L; I2C_DELAY_2CLK; } bool I2C_WaitAck(void) { - SCL_L; I2C_DELAY_1CLK; - SDA_H; I2C_DELAY_1CLK; - SCL_H; - if (!WaitSCL_H()) - return false; + SCL_L; I2C_DELAY_1CLK; + SDA_H; I2C_DELAY_1CLK; + SCL_H; + if (!WaitSCL_H()) + return false; - I2C_DELAY_2CLK; - I2C_DELAY_2CLK; - if (SDA_read) { - SCL_L; - return false; - } - SCL_L; - return true; + I2C_DELAY_2CLK; + I2C_DELAY_2CLK; + if (SDA_read) { + SCL_L; + return false; + } + SCL_L; + return true; } -void I2C_SendByte(uint8_t data) { - uint8_t bits = 8; +void I2C_SendByte(uint8_t data) { + uint8_t bits = 8; - while (bits--) { - SCL_L; + while (bits--) { + SCL_L; - I2C_DELAY_1CLK; + I2C_DELAY_1CLK; - if (data & 0x80) - SDA_H; - else - SDA_L; + if (data & 0x80) + SDA_H; + else + SDA_L; - data <<= 1; + data <<= 1; - I2C_DELAY_1CLK; + I2C_DELAY_1CLK; - SCL_H; - if (!WaitSCL_H()) - return; + SCL_H; + if (!WaitSCL_H()) + return; - I2C_DELAY_2CLK; - } - SCL_L; + I2C_DELAY_2CLK; + } + SCL_L; } int16_t I2C_ReadByte(void) { - uint8_t bits = 8, b = 0; + uint8_t bits = 8, b = 0; - SDA_H; - while (bits--) { - b <<= 1; - SCL_L; - if (!WaitSCL_L()) return -2; + SDA_H; + while (bits--) { + b <<= 1; + SCL_L; + if (!WaitSCL_L()) return -2; - I2C_DELAY_1CLK; + I2C_DELAY_1CLK; - SCL_H; - if (!WaitSCL_H()) return -1; + SCL_H; + if (!WaitSCL_H()) return -1; - I2C_DELAY_1CLK; - if (SDA_read) - b |= 0x01; - } - SCL_L; - return b; + I2C_DELAY_1CLK; + if (SDA_read) + b |= 0x01; + } + SCL_L; + return b; } // Sends one byte ( command to be written, SlaveDevice address) bool I2C_WriteCmd(uint8_t device_cmd, uint8_t device_address) { - bool bBreak = true; - do { - if (!I2C_Start()) - return false; + bool bBreak = true; + do { + if (!I2C_Start()) + return false; - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(device_cmd); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_cmd); + if (!I2C_WaitAck()) + break; - bBreak = false; - } while (false); + bBreak = false; + } while (false); - I2C_Stop(); - if (bBreak) { - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return false; - } - return true; + I2C_Stop(); + if (bBreak) { + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return false; + } + return true; } // Sends 1 byte data (Data to be written, command to be written , SlaveDevice address ). bool I2C_WriteByte(uint8_t data, uint8_t device_cmd, uint8_t device_address) { - bool bBreak = true; - do { - if (!I2C_Start()) - return false; + bool bBreak = true; + do { + if (!I2C_Start()) + return false; - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(device_cmd); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_cmd); + if (!I2C_WaitAck()) + break; - I2C_SendByte(data); - if (!I2C_WaitAck()) - break; + I2C_SendByte(data); + if (!I2C_WaitAck()) + break; - bBreak = false; - } while (false); + bBreak = false; + } while (false); - I2C_Stop(); - if (bBreak) { - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return false; - } - return true; + I2C_Stop(); + if (bBreak) { + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return false; + } + return true; } //Sends array of data (Array, length, command to be written , SlaveDevice address ). // len = uint8 (max buffer to write 256bytes) bool I2C_BufferWrite(uint8_t *data, uint8_t len, uint8_t device_cmd, uint8_t device_address) { - bool bBreak = true; - do { - if (!I2C_Start()) - return false; + bool bBreak = true; + do { + if (!I2C_Start()) + return false; - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(device_cmd); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_cmd); + if (!I2C_WaitAck()) + break; - while (len) { + while (len) { - I2C_SendByte(*data); - if (!I2C_WaitAck()) - break; + I2C_SendByte(*data); + if (!I2C_WaitAck()) + break; - len--; - data++; - } + len--; + data++; + } - if (len == 0) - bBreak = false; - } while (false); + if (len == 0) + bBreak = false; + } while (false); - I2C_Stop(); - if (bBreak) { - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return false; - } - return true; + I2C_Stop(); + if (bBreak) { + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return false; + } + return true; } // read one array of data (Data array, Readout length, command to be written , SlaveDevice address ). // len = uint8 (max buffer to read 256bytes) int16_t I2C_BufferRead(uint8_t *data, uint8_t len, uint8_t device_cmd, uint8_t device_address) { - if ( !data || len == 0 ) - return 0; + if ( !data || len == 0 ) + return 0; - // extra wait 500us (514us measured) - // 200us (xx measured) - WaitUS(600); - bool bBreak = true; - uint16_t readcount = 0; + // extra wait 500us (514us measured) + // 200us (xx measured) + WaitUS(600); + bool bBreak = true; + uint16_t readcount = 0; - do { - if (!I2C_Start()) - return 0; + do { + if (!I2C_Start()) + return 0; - // 0xB0 / 0xC0 == i2c write - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + // 0xB0 / 0xC0 == i2c write + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(device_cmd); - if (!I2C_WaitAck()) - break; + I2C_SendByte(device_cmd); + if (!I2C_WaitAck()) + break; - // 0xB1 / 0xC1 == i2c read - I2C_Start(); - I2C_SendByte(device_address | 1); - if (!I2C_WaitAck()) - break; + // 0xB1 / 0xC1 == i2c read + I2C_Start(); + I2C_SendByte(device_address | 1); + if (!I2C_WaitAck()) + break; - bBreak = false; - } while (false); + bBreak = false; + } while (false); - if (bBreak) { - I2C_Stop(); - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return 0; - } + if (bBreak) { + I2C_Stop(); + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return 0; + } - while (len) { + while (len) { - int16_t tmp = I2C_ReadByte(); - if ( tmp < 0 ) - return tmp; + int16_t tmp = I2C_ReadByte(); + if ( tmp < 0 ) + return tmp; - *data = (uint8_t)tmp & 0xFF; + *data = (uint8_t)tmp & 0xFF; - len--; + len--; - // ¶ÁÈ¡µÄµÚÒ»¸ö×Ö½ÚΪºóÐø³¤¶È - // The first byte in response is the message length - if (!readcount && (len > *data)) { - len = *data; - } else { - data++; - } - readcount++; + // ¶ÁÈ¡µÄµÚÒ»¸ö×Ö½ÚΪºóÐø³¤¶È + // The first byte in response is the message length + if (!readcount && (len > *data)) { + len = *data; + } else { + data++; + } + readcount++; - // acknowledgements. After last byte send NACK. - if (len == 0) - I2C_NoAck(); - else - I2C_Ack(); - } + // acknowledgements. After last byte send NACK. + if (len == 0) + I2C_NoAck(); + else + I2C_Ack(); + } - I2C_Stop(); + I2C_Stop(); - // return bytecount - first byte (which is length byte) - return --readcount; + // return bytecount - first byte (which is length byte) + return --readcount; } int16_t I2C_ReadFW(uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) { - //START, 0xB0, 0x00, 0x00, START, 0xB1, xx, yy, zz, ......, STOP - bool bBreak = true; - uint8_t readcount = 0; + //START, 0xB0, 0x00, 0x00, START, 0xB1, xx, yy, zz, ......, STOP + bool bBreak = true; + uint8_t readcount = 0; - // sending - do { - if (!I2C_Start()) - return 0; + // sending + do { + if (!I2C_Start()) + return 0; - // 0xB0 / 0xC0 i2c write - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + // 0xB0 / 0xC0 i2c write + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(msb); - if (!I2C_WaitAck()) - break; + I2C_SendByte(msb); + if (!I2C_WaitAck()) + break; - I2C_SendByte(lsb); - if (!I2C_WaitAck()) - break; + I2C_SendByte(lsb); + if (!I2C_WaitAck()) + break; - // 0xB1 / 0xC1 i2c read - I2C_Start(); - I2C_SendByte(device_address | 1); - if (!I2C_WaitAck()) - break; + // 0xB1 / 0xC1 i2c read + I2C_Start(); + I2C_SendByte(device_address | 1); + if (!I2C_WaitAck()) + break; - bBreak = false; - } while (false); + bBreak = false; + } while (false); - if (bBreak) { - I2C_Stop(); - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return 0; - } + if (bBreak) { + I2C_Stop(); + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return 0; + } - // reading - while (len) { + // reading + while (len) { - int16_t tmp = I2C_ReadByte(); - if ( tmp < 0 ) - return tmp; + int16_t tmp = I2C_ReadByte(); + if ( tmp < 0 ) + return tmp; - *data = (uint8_t)tmp & 0xFF; + *data = (uint8_t)tmp & 0xFF; - data++; - readcount++; - len--; + data++; + readcount++; + len--; - // acknowledgements. After last byte send NACK. - if (len == 0) - I2C_NoAck(); - else - I2C_Ack(); - } + // acknowledgements. After last byte send NACK. + if (len == 0) + I2C_NoAck(); + else + I2C_Ack(); + } - I2C_Stop(); - return readcount; + I2C_Stop(); + return readcount; } bool I2C_WriteFW(uint8_t *data, uint8_t len, uint8_t msb, uint8_t lsb, uint8_t device_address) { - //START, 0xB0, 0x00, 0x00, xx, yy, zz, ......, STOP - bool bBreak = true; + //START, 0xB0, 0x00, 0x00, xx, yy, zz, ......, STOP + bool bBreak = true; - do { - if (!I2C_Start()) - return false; + do { + if (!I2C_Start()) + return false; - // 0xB0 == i2c write - I2C_SendByte(device_address & 0xFE); - if (!I2C_WaitAck()) - break; + // 0xB0 == i2c write + I2C_SendByte(device_address & 0xFE); + if (!I2C_WaitAck()) + break; - I2C_SendByte(msb); - if (!I2C_WaitAck()) - break; + I2C_SendByte(msb); + if (!I2C_WaitAck()) + break; - I2C_SendByte(lsb); - if (!I2C_WaitAck()) - break; + I2C_SendByte(lsb); + if (!I2C_WaitAck()) + break; - while (len) { - I2C_SendByte(*data); - if (!I2C_WaitAck()) - break; + while (len) { + I2C_SendByte(*data); + if (!I2C_WaitAck()) + break; - len--; - data++; - } + len--; + data++; + } - if (len == 0) - bBreak = false; - } while (false); + if (len == 0) + bBreak = false; + } while (false); - I2C_Stop(); - if (bBreak) { - if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - return false; - } - return true; + I2C_Stop(); + if (bBreak) { + if ( MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + return false; + } + return true; } void I2C_print_status(void) { - DbpString("Smart card module (ISO 7816)"); - uint8_t resp[] = {0,0,0,0}; - I2C_Reset_EnterMainProgram(); - uint8_t len = I2C_BufferRead(resp, sizeof(resp), I2C_DEVICE_CMD_GETVERSION, I2C_DEVICE_ADDRESS_MAIN); - if ( len > 0 ) - Dbprintf(" version.................v%x.%02d", resp[0], resp[1]); - else - DbpString(" version.................FAILED"); + DbpString("Smart card module (ISO 7816)"); + uint8_t resp[] = {0,0,0,0}; + I2C_Reset_EnterMainProgram(); + uint8_t len = I2C_BufferRead(resp, sizeof(resp), I2C_DEVICE_CMD_GETVERSION, I2C_DEVICE_ADDRESS_MAIN); + if ( len > 0 ) + Dbprintf(" version.................v%x.%02d", resp[0], resp[1]); + else + DbpString(" version.................FAILED"); } // Will read response from smart card module, retries 3 times to get the data. bool sc_rx_bytes(uint8_t* dest, uint8_t *destlen) { - uint8_t i = 3; - int16_t len = 0; - while (i--) { + uint8_t i = 3; + int16_t len = 0; + while (i--) { - I2C_WaitForSim(); + I2C_WaitForSim(); - len = I2C_BufferRead(dest, *destlen, I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN); + len = I2C_BufferRead(dest, *destlen, I2C_DEVICE_CMD_READ, I2C_DEVICE_ADDRESS_MAIN); - if ( len > 1 ){ - break; - } else if ( len == 1 ) { - continue; - } else if ( len <= 0 ) { - return false; - } - } + if ( len > 1 ){ + break; + } else if ( len == 1 ) { + continue; + } else if ( len <= 0 ) { + return false; + } + } - // after three - if ( len <= 1 ) - return false; + // after three + if ( len <= 1 ) + return false; - *destlen = (uint8_t)len & 0xFF; - return true; + *destlen = (uint8_t)len & 0xFF; + return true; } bool GetATR(smart_card_atr_t *card_ptr) { - if ( !card_ptr ) - return false; + if ( !card_ptr ) + return false; - card_ptr->atr_len = 0; - memset(card_ptr->atr, 0, sizeof(card_ptr->atr)); + card_ptr->atr_len = 0; + memset(card_ptr->atr, 0, sizeof(card_ptr->atr)); - // Send ATR - // start [C0 01] stop start C1 len aa bb cc stop] - I2C_WriteCmd(I2C_DEVICE_CMD_GENERATE_ATR, I2C_DEVICE_ADDRESS_MAIN); + // Send ATR + // start [C0 01] stop start C1 len aa bb cc stop] + I2C_WriteCmd(I2C_DEVICE_CMD_GENERATE_ATR, I2C_DEVICE_ADDRESS_MAIN); - //wait for sim card to answer. - // 1byte = 1ms , max frame 256bytes. SHould wait 256ms atleast just in case. - if (!I2C_WaitForSim()) - return false; + //wait for sim card to answer. + // 1byte = 1ms , max frame 256bytes. SHould wait 256ms atleast just in case. + if (!I2C_WaitForSim()) + return false; - // read bytes from module - uint8_t len = sizeof(card_ptr->atr); - if ( !sc_rx_bytes(card_ptr->atr, &len) ) - return false; + // read bytes from module + uint8_t len = sizeof(card_ptr->atr); + if ( !sc_rx_bytes(card_ptr->atr, &len) ) + return false; - uint8_t pos_td = 1; - if ( (card_ptr->atr[1] & 0x10) == 0x10) pos_td++; - if ( (card_ptr->atr[1] & 0x20) == 0x20) pos_td++; - if ( (card_ptr->atr[1] & 0x40) == 0x40) pos_td++; + uint8_t pos_td = 1; + if ( (card_ptr->atr[1] & 0x10) == 0x10) pos_td++; + if ( (card_ptr->atr[1] & 0x20) == 0x20) pos_td++; + if ( (card_ptr->atr[1] & 0x40) == 0x40) pos_td++; - // T0 indicate presence T=0 vs T=1. T=1 has checksum TCK - if ( (card_ptr->atr[1] & 0x80) == 0x80) { + // T0 indicate presence T=0 vs T=1. T=1 has checksum TCK + if ( (card_ptr->atr[1] & 0x80) == 0x80) { - pos_td++; + pos_td++; - // 1 == T1 , presence of checksum TCK - if ( (card_ptr->atr[pos_td] & 0x01) == 0x01) { + // 1 == T1 , presence of checksum TCK + if ( (card_ptr->atr[pos_td] & 0x01) == 0x01) { - uint8_t chksum = 0; - // xor property. will be zero when xored with chksum. - for (uint8_t i = 1; i < len; ++i) - chksum ^= card_ptr->atr[i]; + uint8_t chksum = 0; + // xor property. will be zero when xored with chksum. + for (uint8_t i = 1; i < len; ++i) + chksum ^= card_ptr->atr[i]; - if ( chksum ) { - if ( MF_DBGLEVEL > 2) DbpString("Wrong ATR checksum"); - } - } - } + if ( chksum ) { + if ( MF_DBGLEVEL > 2) DbpString("Wrong ATR checksum"); + } + } + } - card_ptr->atr_len = len; - LogTrace(card_ptr->atr, card_ptr->atr_len, 0, 0, NULL, false); - return true; + card_ptr->atr_len = len; + LogTrace(card_ptr->atr, card_ptr->atr_len, 0, 0, NULL, false); + return true; } void SmartCardAtr(void) { - smart_card_atr_t card; - LED_D_ON(); - clear_trace(); - set_tracing(true); - I2C_Reset_EnterMainProgram(); - bool isOK = GetATR( &card ); - cmd_send(CMD_ACK, isOK, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t)); - set_tracing(false); - LEDsoff(); + smart_card_atr_t card; + LED_D_ON(); + clear_trace(); + set_tracing(true); + I2C_Reset_EnterMainProgram(); + bool isOK = GetATR( &card ); + cmd_send(CMD_ACK, isOK, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t)); + set_tracing(false); + LEDsoff(); } void SmartCardRaw( uint64_t arg0, uint64_t arg1, uint8_t *data ) { - LED_D_ON(); + LED_D_ON(); - uint8_t len = 0; - uint8_t *resp = BigBuf_malloc(ISO7618_MAX_FRAME); - smartcard_command_t flags = arg0; + uint8_t len = 0; + uint8_t *resp = BigBuf_malloc(ISO7618_MAX_FRAME); + smartcard_command_t flags = arg0; - if ((flags & SC_CONNECT)) - clear_trace(); + if ((flags & SC_CONNECT)) + clear_trace(); - set_tracing(true); + set_tracing(true); - if ((flags & SC_CONNECT)) { + if ((flags & SC_CONNECT)) { - I2C_Reset_EnterMainProgram(); + I2C_Reset_EnterMainProgram(); - if ((flags & SC_SELECT)) { - smart_card_atr_t card; - bool gotATR = GetATR( &card ); - //cmd_send(CMD_ACK, gotATR, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t)); - if ( !gotATR ) - goto OUT; - } - } + if ((flags & SC_SELECT)) { + smart_card_atr_t card; + bool gotATR = GetATR( &card ); + //cmd_send(CMD_ACK, gotATR, sizeof(smart_card_atr_t), 0, &card, sizeof(smart_card_atr_t)); + if ( !gotATR ) + goto OUT; + } + } - if ((flags & SC_RAW) || (flags & SC_RAW_T0)) { + if ((flags & SC_RAW) || (flags & SC_RAW_T0)) { - LogTrace(data, arg1, 0, 0, NULL, true); + LogTrace(data, arg1, 0, 0, NULL, true); - // Send raw bytes - // asBytes = A0 A4 00 00 02 - // arg1 = len 5 - bool res = I2C_BufferWrite(data, arg1, ((flags & SC_RAW_T0) ? I2C_DEVICE_CMD_SEND_T0 : I2C_DEVICE_CMD_SEND), I2C_DEVICE_ADDRESS_MAIN); - if ( !res && MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); + // Send raw bytes + // asBytes = A0 A4 00 00 02 + // arg1 = len 5 + bool res = I2C_BufferWrite(data, arg1, ((flags & SC_RAW_T0) ? I2C_DEVICE_CMD_SEND_T0 : I2C_DEVICE_CMD_SEND), I2C_DEVICE_ADDRESS_MAIN); + if ( !res && MF_DBGLEVEL > 3 ) DbpString(I2C_ERROR); - // read bytes from module - len = ISO7618_MAX_FRAME; - res = sc_rx_bytes(resp, &len); - if ( res ) { - LogTrace(resp, len, 0, 0, NULL, false); - } else { - len = 0; - } - } + // read bytes from module + len = ISO7618_MAX_FRAME; + res = sc_rx_bytes(resp, &len); + if ( res ) { + LogTrace(resp, len, 0, 0, NULL, false); + } else { + len = 0; + } + } OUT: - cmd_send(CMD_ACK, len, 0, 0, resp, len); - BigBuf_free(); - set_tracing(false); - LEDsoff(); + cmd_send(CMD_ACK, len, 0, 0, resp, len); + BigBuf_free(); + set_tracing(false); + LEDsoff(); } void SmartCardUpgrade(uint64_t arg0) { - LED_C_ON(); + LED_C_ON(); - #define I2C_BLOCK_SIZE 128 - // write. Sector0, with 11,22,33,44 - // erase is 128bytes, and takes 50ms to execute + #define I2C_BLOCK_SIZE 128 + // write. Sector0, with 11,22,33,44 + // erase is 128bytes, and takes 50ms to execute - I2C_Reset_EnterBootloader(); + I2C_Reset_EnterBootloader(); - bool isOK = true; - int16_t res = 0; - uint16_t length = arg0; - uint16_t pos = 0; - uint8_t *fwdata = BigBuf_get_addr(); - uint8_t *verfiydata = BigBuf_malloc(I2C_BLOCK_SIZE); + bool isOK = true; + int16_t res = 0; + uint16_t length = arg0; + uint16_t pos = 0; + uint8_t *fwdata = BigBuf_get_addr(); + uint8_t *verfiydata = BigBuf_malloc(I2C_BLOCK_SIZE); - while (length) { + while (length) { - uint8_t msb = (pos >> 8) & 0xFF; - uint8_t lsb = pos & 0xFF; + uint8_t msb = (pos >> 8) & 0xFF; + uint8_t lsb = pos & 0xFF; - Dbprintf("FW %02X%02X", msb, lsb); + Dbprintf("FW %02X%02X", msb, lsb); - size_t size = MIN(I2C_BLOCK_SIZE, length); + size_t size = MIN(I2C_BLOCK_SIZE, length); - // write - res = I2C_WriteFW(fwdata+pos, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT); - if ( !res ) { - DbpString("Writing failed"); - isOK = false; - break; - } + // write + res = I2C_WriteFW(fwdata+pos, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT); + if ( !res ) { + DbpString("Writing failed"); + isOK = false; + break; + } - // writing takes time. - WaitMS(50); + // writing takes time. + WaitMS(50); - // read - res = I2C_ReadFW(verfiydata, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT); - if ( res <= 0) { - DbpString("Reading back failed"); - isOK = false; - break; - } + // read + res = I2C_ReadFW(verfiydata, size, msb, lsb, I2C_DEVICE_ADDRESS_BOOT); + if ( res <= 0) { + DbpString("Reading back failed"); + isOK = false; + break; + } - // cmp - if ( 0 != memcmp(fwdata+pos, verfiydata, size)) { - DbpString("not equal data"); - isOK = false; - break; - } + // cmp + if ( 0 != memcmp(fwdata+pos, verfiydata, size)) { + DbpString("not equal data"); + isOK = false; + break; + } - length -= size; - pos += size; - } - cmd_send(CMD_ACK, isOK, pos, 0, 0, 0); - LED_C_OFF(); - BigBuf_free(); + length -= size; + pos += size; + } + cmd_send(CMD_ACK, isOK, pos, 0, 0, 0); + LED_C_OFF(); + BigBuf_free(); } void SmartCardSetBaud(uint64_t arg0) { } void SmartCardSetClock(uint64_t arg0) { - LED_D_ON(); - set_tracing(true); - I2C_Reset_EnterMainProgram(); + LED_D_ON(); + set_tracing(true); + I2C_Reset_EnterMainProgram(); - // Send SIM CLC - // start [C0 05 xx] stop - I2C_WriteByte(arg0, I2C_DEVICE_CMD_SIM_CLC, I2C_DEVICE_ADDRESS_MAIN); + // Send SIM CLC + // start [C0 05 xx] stop + I2C_WriteByte(arg0, I2C_DEVICE_CMD_SIM_CLC, I2C_DEVICE_ADDRESS_MAIN); - cmd_send(CMD_ACK, 1, 0, 0, 0, 0); - set_tracing(false); - LEDsoff(); + cmd_send(CMD_ACK, 1, 0, 0, 0, 0); + set_tracing(false); + LEDsoff(); } diff --git a/common/i2c.h b/common/i2c.h index e5ff26e4d..4254a73c3 100644 --- a/common/i2c.h +++ b/common/i2c.h @@ -8,16 +8,16 @@ #include "BigBuf.h" #include "mifare.h" -#define I2C_DEVICE_ADDRESS_BOOT 0xB0 -#define I2C_DEVICE_ADDRESS_MAIN 0xC0 +#define I2C_DEVICE_ADDRESS_BOOT 0xB0 +#define I2C_DEVICE_ADDRESS_MAIN 0xC0 -#define I2C_DEVICE_CMD_GENERATE_ATR 0x01 -#define I2C_DEVICE_CMD_SEND 0x02 -#define I2C_DEVICE_CMD_READ 0x03 -#define I2C_DEVICE_CMD_SETBAUD 0x04 -#define I2C_DEVICE_CMD_SIM_CLC 0x05 -#define I2C_DEVICE_CMD_GETVERSION 0x06 -#define I2C_DEVICE_CMD_SEND_T0 0x07 +#define I2C_DEVICE_CMD_GENERATE_ATR 0x01 +#define I2C_DEVICE_CMD_SEND 0x02 +#define I2C_DEVICE_CMD_READ 0x03 +#define I2C_DEVICE_CMD_SETBAUD 0x04 +#define I2C_DEVICE_CMD_SIM_CLC 0x05 +#define I2C_DEVICE_CMD_GETVERSION 0x06 +#define I2C_DEVICE_CMD_SEND_T0 0x07 void I2C_recovery(void); diff --git a/common/iso14443crc.c b/common/iso14443crc.c index ceac3c4df..891eb9b14 100644 --- a/common/iso14443crc.c +++ b/common/iso14443crc.c @@ -22,7 +22,7 @@ void ComputeCrc14443(uint16_t CrcType, const uint8_t *data, int length, uint8_t b; uint16_t crc = CrcType; - do { + do { b = *data++; UpdateCrc14443(b, &crc); } while (--length); @@ -36,10 +36,10 @@ void ComputeCrc14443(uint16_t CrcType, const uint8_t *data, int length, } bool CheckCrc14443(uint16_t CrcType, const uint8_t *data, int length) { - if (length < 3) return false; - uint8_t b1, b2; - ComputeCrc14443(CrcType, data, length - 2, &b1, &b2); - if ((b1 == data[length - 2]) && (b2 == data[length - 1])) - return true; - return false; + if (length < 3) return false; + uint8_t b1, b2; + ComputeCrc14443(CrcType, data, length - 2, &b1, &b2); + if ((b1 == data[length - 2]) && (b2 == data[length - 1])) + return true; + return false; } diff --git a/common/iso15693tools.c b/common/iso15693tools.c index fbd79da52..e821feb1f 100644 --- a/common/iso15693tools.c +++ b/common/iso15693tools.c @@ -9,17 +9,17 @@ // returns a string representation of the UID // UID is transmitted and stored LSB first, displayed MSB first -// target char* buffer, where to put the UID, if NULL a static buffer is returned -// uid[] the UID in transmission order -// return: ptr to string +// target char* buffer, where to put the UID, if NULL a static buffer is returned +// uid[] the UID in transmission order +// return: ptr to string char* Iso15693sprintUID(char *target, uint8_t *uid) { - static char tempbuf[2*8+1] = {0}; - if (target == NULL) - target = tempbuf; - sprintf(target, "%02X %02X %02X %02X %02X %02X %02X %02X", - uid[7], uid[6], uid[5], uid[4], - uid[3], uid[2], uid[1], uid[0] - ); - return target; + static char tempbuf[2*8+1] = {0}; + if (target == NULL) + target = tempbuf; + sprintf(target, "%02X %02X %02X %02X %02X %02X %02X %02X", + uid[7], uid[6], uid[5], uid[4], + uid[3], uid[2], uid[1], uid[0] + ); + return target; } \ No newline at end of file diff --git a/common/iso15693tools.h b/common/iso15693tools.h index 0dd4d4444..1937ec84e 100644 --- a/common/iso15693tools.h +++ b/common/iso15693tools.h @@ -17,58 +17,58 @@ #include "crc16.h" // REQUEST FLAGS -#define ISO15_REQ_SUBCARRIER_SINGLE 0x00 // Tag should respond using one subcarrier (ASK) -#define ISO15_REQ_SUBCARRIER_TWO 0x01 // Tag should respond using two subcarriers (FSK) -#define ISO15_REQ_DATARATE_LOW 0x00 // Tag should respond using low data rate -#define ISO15_REQ_DATARATE_HIGH 0x02 // Tag should respond using high data rate -#define ISO15_REQ_NONINVENTORY 0x00 -#define ISO15_REQ_INVENTORY 0x04 // This is an inventory request - see inventory flags -#define ISO15_REQ_PROTOCOL_NONEXT 0x00 -#define ISO15_REQ_PROTOCOL_EXT 0x08 // RFU +#define ISO15_REQ_SUBCARRIER_SINGLE 0x00 // Tag should respond using one subcarrier (ASK) +#define ISO15_REQ_SUBCARRIER_TWO 0x01 // Tag should respond using two subcarriers (FSK) +#define ISO15_REQ_DATARATE_LOW 0x00 // Tag should respond using low data rate +#define ISO15_REQ_DATARATE_HIGH 0x02 // Tag should respond using high data rate +#define ISO15_REQ_NONINVENTORY 0x00 +#define ISO15_REQ_INVENTORY 0x04 // This is an inventory request - see inventory flags +#define ISO15_REQ_PROTOCOL_NONEXT 0x00 +#define ISO15_REQ_PROTOCOL_EXT 0x08 // RFU // REQUEST FLAGS when INVENTORY is not set -#define ISO15_REQ_SELECT 0x10 // only selected cards response -#define ISO15_REQ_ADDRESS 0x20 // this req contains an address -#define ISO15_REQ_OPTION 0x40 // Command specific option selector +#define ISO15_REQ_SELECT 0x10 // only selected cards response +#define ISO15_REQ_ADDRESS 0x20 // this req contains an address +#define ISO15_REQ_OPTION 0x40 // Command specific option selector //REQUEST FLAGS when INVENTORY is set -#define ISO15_REQINV_AFI 0x10 // AFI Field is present -#define ISO15_REQINV_SLOT1 0x20 // 1 Slot -#define ISO15_REQINV_SLOT16 0x00 // 16 Slots -#define ISO15_REQINV_OPTION 0x40 // Command specific option selector +#define ISO15_REQINV_AFI 0x10 // AFI Field is present +#define ISO15_REQINV_SLOT1 0x20 // 1 Slot +#define ISO15_REQINV_SLOT16 0x00 // 16 Slots +#define ISO15_REQINV_OPTION 0x40 // Command specific option selector //RESPONSE FLAGS -#define ISO15_RES_ERROR 0x01 -#define ISO15_RES_EXT 0x08 // Protocol Extention +#define ISO15_RES_ERROR 0x01 +#define ISO15_RES_EXT 0x08 // Protocol Extention // RESPONSE ERROR CODES -#define ISO15_NOERROR 0x00 -#define ISO15_ERROR_CMD_NOT_SUP 0x01 // Command not supported -#define ISO15_ERROR_CMD_NOT_REC 0x02 // Command not recognized (eg. parameter error) -#define ISO15_ERROR_CMD_OPTION 0x03 // Command option not supported -#define ISO15_ERROR_GENERIC 0x0F // No additional Info about this error -#define ISO15_ERROR_BLOCK_UNAVAILABLE 0x10 -#define ISO15_ERROR_BLOCK_LOCKED_ALREADY 0x11 // cannot lock again -#define ISO15_ERROR_BLOCK_LOCKED 0x12 // cannot be changed -#define ISO15_ERROR_BLOCK_WRITE 0x13 // Writing was unsuccessful -#define ISO15_ERROR_BLOCL_WRITELOCK 0x14 // Locking was unsuccessful +#define ISO15_NOERROR 0x00 +#define ISO15_ERROR_CMD_NOT_SUP 0x01 // Command not supported +#define ISO15_ERROR_CMD_NOT_REC 0x02 // Command not recognized (eg. parameter error) +#define ISO15_ERROR_CMD_OPTION 0x03 // Command option not supported +#define ISO15_ERROR_GENERIC 0x0F // No additional Info about this error +#define ISO15_ERROR_BLOCK_UNAVAILABLE 0x10 +#define ISO15_ERROR_BLOCK_LOCKED_ALREADY 0x11 // cannot lock again +#define ISO15_ERROR_BLOCK_LOCKED 0x12 // cannot be changed +#define ISO15_ERROR_BLOCK_WRITE 0x13 // Writing was unsuccessful +#define ISO15_ERROR_BLOCL_WRITELOCK 0x14 // Locking was unsuccessful // COMMAND CODES -#define ISO15_CMD_INVENTORY 0x01 -#define ISO15_CMD_STAYQUIET 0x02 -#define ISO15_CMD_READ 0x20 -#define ISO15_CMD_WRITE 0x21 -#define ISO15_CMD_LOCK 0x22 -#define ISO15_CMD_READMULTI 0x23 -#define ISO15_CMD_WRITEMULTI 0x24 -#define ISO15_CMD_SELECT 0x25 -#define ISO15_CMD_RESET 0x26 -#define ISO15_CMD_WRITEAFI 0x27 -#define ISO15_CMD_LOCKAFI 0x28 -#define ISO15_CMD_WRITEDSFID 0x29 -#define ISO15_CMD_LOCKDSFID 0x2A -#define ISO15_CMD_SYSINFO 0x2B -#define ISO15_CMD_SECSTATUS 0x2C +#define ISO15_CMD_INVENTORY 0x01 +#define ISO15_CMD_STAYQUIET 0x02 +#define ISO15_CMD_READ 0x20 +#define ISO15_CMD_WRITE 0x21 +#define ISO15_CMD_LOCK 0x22 +#define ISO15_CMD_READMULTI 0x23 +#define ISO15_CMD_WRITEMULTI 0x24 +#define ISO15_CMD_SELECT 0x25 +#define ISO15_CMD_RESET 0x26 +#define ISO15_CMD_WRITEAFI 0x27 +#define ISO15_CMD_LOCKAFI 0x28 +#define ISO15_CMD_WRITEDSFID 0x29 +#define ISO15_CMD_LOCKDSFID 0x2A +#define ISO15_CMD_SYSINFO 0x2B +#define ISO15_CMD_SECSTATUS 0x2C char* Iso15693sprintUID(char *target, uint8_t *uid); @@ -78,49 +78,49 @@ char* Iso15693sprintUID(char *target, uint8_t *uid); // Mode: highspeed && one subcarrier (ASK) //----------------------------------------------------------------------------- - // The sampling rate is 106.353 ksps/s, for T = 18.8 us + // The sampling rate is 106.353 ksps/s, for T = 18.8 us - // SOF defined as - // 1) Unmodulated time of 56.64us - // 2) 24 pulses of 423.75khz - // 3) logic '1' (unmodulated for 18.88us followed by 8 pulses of 423.75khz) + // SOF defined as + // 1) Unmodulated time of 56.64us + // 2) 24 pulses of 423.75khz + // 3) logic '1' (unmodulated for 18.88us followed by 8 pulses of 423.75khz) - static const int Iso15693FrameSOF[] = { - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - -1, -1, -1, -1, - -1, -1, -1, -1, - 1, 1, 1, 1, - 1, 1, 1, 1 - }; - static const int Iso15693Logic0[] = { - 1, 1, 1, 1, - 1, 1, 1, 1, - -1, -1, -1, -1, - -1, -1, -1, -1 - }; - static const int Iso15693Logic1[] = { - -1, -1, -1, -1, - -1, -1, -1, -1, - 1, 1, 1, 1, - 1, 1, 1, 1 - }; + static const int Iso15693FrameSOF[] = { + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + -1, -1, -1, -1, + -1, -1, -1, -1, + 1, 1, 1, 1, + 1, 1, 1, 1 + }; + static const int Iso15693Logic0[] = { + 1, 1, 1, 1, + 1, 1, 1, 1, + -1, -1, -1, -1, + -1, -1, -1, -1 + }; + static const int Iso15693Logic1[] = { + -1, -1, -1, -1, + -1, -1, -1, -1, + 1, 1, 1, 1, + 1, 1, 1, 1 + }; - // EOF defined as - // 1) logic '0' (8 pulses of 423.75khz followed by unmodulated for 18.88us) - // 2) 24 pulses of 423.75khz - // 3) Unmodulated time of 56.64us - static const int Iso15693FrameEOF[] = { - 1, 1, 1, 1, - 1, 1, 1, 1, - -1, -1, -1, -1, - -1, -1, -1, -1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, - -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 - }; + // EOF defined as + // 1) logic '0' (8 pulses of 423.75khz followed by unmodulated for 18.88us) + // 2) 24 pulses of 423.75khz + // 3) Unmodulated time of 56.64us + static const int Iso15693FrameEOF[] = { + 1, 1, 1, 1, + 1, 1, 1, 1, + -1, -1, -1, -1, + -1, -1, -1, -1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, + -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 + }; #endif \ No newline at end of file diff --git a/common/ldscript.common b/common/ldscript.common index c0134ffd0..5b2f1e3c4 100644 --- a/common/ldscript.common +++ b/common/ldscript.common @@ -11,13 +11,13 @@ ms of the GNU GPL, version 2 or, /* AT91SAM7S256 has 256k Flash and 64k RAM */ /* AT91SAM7S512 has 512k Flash and 64k RAM */ -MEMORY +MEMORY { - bootphase1 : ORIGIN = 0x00100000, LENGTH = 0x200 /* Phase 1 bootloader: Copies real bootloader to RAM */ - bootphase2 : ORIGIN = 0x00100200, LENGTH = 0x2000 - 0x200 /* Main bootloader code, stored in Flash, executed from RAM */ - osimage : ORIGIN = 0x00102000, LENGTH = 512K - 0x2000 /* Place where the main OS will end up */ - ram : ORIGIN = 0x00200000, LENGTH = 64K - 0x20 /* RAM, minus small common area */ - commonarea : ORIGIN = 0x00200000 + 64K - 0x20, LENGTH = 0x20 /* Communication between bootloader and main OS */ + bootphase1 : ORIGIN = 0x00100000, LENGTH = 0x200 /* Phase 1 bootloader: Copies real bootloader to RAM */ + bootphase2 : ORIGIN = 0x00100200, LENGTH = 0x2000 - 0x200 /* Main bootloader code, stored in Flash, executed from RAM */ + osimage : ORIGIN = 0x00102000, LENGTH = 512K - 0x2000 /* Place where the main OS will end up */ + ram : ORIGIN = 0x00200000, LENGTH = 64K - 0x20 /* RAM, minus small common area */ + commonarea : ORIGIN = 0x00200000 + 64K - 0x20, LENGTH = 0x20 /* Communication between bootloader and main OS */ } /* Export some information that can be used from within the firmware */ diff --git a/common/legic_prng.c b/common/legic_prng.c index c4927b33b..75e04d8b6 100644 --- a/common/legic_prng.c +++ b/common/legic_prng.c @@ -13,9 +13,9 @@ // c keeps track on which step the prng is. // legic_prng_get_bit() = gets a bit muxed from a and b. struct lfsr { - uint8_t a; - uint8_t b; - uint32_t c; + uint8_t a; + uint8_t b; + uint32_t c; } lfsr; // Normal init is set following variables with a random value IV @@ -28,38 +28,38 @@ struct lfsr { // which is used in the initialisation phase sending the IV // void legic_prng_init(uint8_t iv) { - lfsr.a = iv; - lfsr.b = 0; // hack to get a always 0 keystream - lfsr.c = 0; - if(iv) - lfsr.b = (iv << 1) | 1; + lfsr.a = iv; + lfsr.b = 0; // hack to get a always 0 keystream + lfsr.c = 0; + if(iv) + lfsr.b = (iv << 1) | 1; } void legic_prng_forward(int count) { - if (count == 0) return; + if (count == 0) return; - lfsr.c += count; - while(count--) { - // According: http://www.proxmark.org/forum/viewtopic.php?pid=5437#p5437 - lfsr.a = (lfsr.a >> 1 | (lfsr.a ^ lfsr.a >> 6) << 6) & 0x7F; - lfsr.b = lfsr.b >> 1 | (lfsr.b ^ lfsr.b >> 2 ^ lfsr.b >> 3 ^ lfsr.b >> 7) << 7; - } + lfsr.c += count; + while(count--) { + // According: http://www.proxmark.org/forum/viewtopic.php?pid=5437#p5437 + lfsr.a = (lfsr.a >> 1 | (lfsr.a ^ lfsr.a >> 6) << 6) & 0x7F; + lfsr.b = lfsr.b >> 1 | (lfsr.b ^ lfsr.b >> 2 ^ lfsr.b >> 3 ^ lfsr.b >> 7) << 7; + } } uint32_t legic_prng_count() { - return lfsr.c; + return lfsr.c; } uint8_t legic_prng_get_bit() { - uint8_t idx = 7 - ( (lfsr.a & 4) | (lfsr.a >> 2 & 2) | (lfsr.a >> 4 & 1) ); - return lfsr.b >> idx & 1; + uint8_t idx = 7 - ( (lfsr.a & 4) | (lfsr.a >> 2 & 2) | (lfsr.a >> 4 & 1) ); + return lfsr.b >> idx & 1; } uint32_t legic_prng_get_bits(uint8_t len){ - uint32_t a = 0; - for(uint8_t i = 0; i < len; ++i) { - a |= legic_prng_get_bit() << i; - legic_prng_forward(1); - } - return a; + uint32_t a = 0; + for(uint8_t i = 0; i < len; ++i) { + a |= legic_prng_get_bit() << i; + legic_prng_forward(1); + } + return a; } \ No newline at end of file diff --git a/common/lfdemod.c b/common/lfdemod.c index 65999ae77..13c2a3f9f 100644 --- a/common/lfdemod.c +++ b/common/lfdemod.c @@ -65,124 +65,124 @@ extern void Dbprintf(const char *fmt, ...); signal_t signalprop = { 255, -255, 0, 0, true }; signal_t* getSignalProperties(void) { - return &signalprop; + return &signalprop; } static void resetSignal(void) { - signalprop.low = 255; - signalprop.high = -255; - signalprop.mean = 0; - signalprop.amplitude = 0; - signalprop.isnoise = true; + signalprop.low = 255; + signalprop.high = -255; + signalprop.mean = 0; + signalprop.amplitude = 0; + signalprop.isnoise = true; } static void printSignal(void) { - prnt("LF signal properties:"); - prnt(" high..........%d", signalprop.high); - prnt(" low...........%d", signalprop.low); - prnt(" mean..........%d", signalprop.mean); - prnt(" amplitude.....%d", signalprop.amplitude); - prnt(" is Noise......%s", (signalprop.isnoise) ? "Yes" : "No"); - prnt(" THRESHOLD noise amplitude......%d" , NOISE_AMPLITUDE_THRESHOLD); + prnt("LF signal properties:"); + prnt(" high..........%d", signalprop.high); + prnt(" low...........%d", signalprop.low); + prnt(" mean..........%d", signalprop.mean); + prnt(" amplitude.....%d", signalprop.amplitude); + prnt(" is Noise......%s", (signalprop.isnoise) ? "Yes" : "No"); + prnt(" THRESHOLD noise amplitude......%d" , NOISE_AMPLITUDE_THRESHOLD); } void computeSignalProperties(uint8_t *samples, uint32_t size) { - resetSignal(); + resetSignal(); - uint32_t sum = 0; - for ( uint32_t i = 0; i < size; i++) { - if ( samples[i] < signalprop.low ) signalprop.low = samples[i]; - if ( samples[i] > signalprop.high ) signalprop.high = samples[i]; - sum += samples[i]; - } + uint32_t sum = 0; + for ( uint32_t i = 0; i < size; i++) { + if ( samples[i] < signalprop.low ) signalprop.low = samples[i]; + if ( samples[i] > signalprop.high ) signalprop.high = samples[i]; + sum += samples[i]; + } - // measure amplitude of signal - signalprop.mean = sum / size; - signalprop.amplitude = signalprop.high - signalprop.mean; - // By measuring mean and look at amplitude of signal from HIGH / LOW, - // we can detect noise - signalprop.isnoise = signalprop.amplitude < NOISE_AMPLITUDE_THRESHOLD; + // measure amplitude of signal + signalprop.mean = sum / size; + signalprop.amplitude = signalprop.high - signalprop.mean; + // By measuring mean and look at amplitude of signal from HIGH / LOW, + // we can detect noise + signalprop.isnoise = signalprop.amplitude < NOISE_AMPLITUDE_THRESHOLD; - if (g_debugMode) - printSignal(); + if (g_debugMode) + printSignal(); } void removeSignalOffset(uint8_t *samples, uint32_t size) { - if ( samples == NULL || size < SIGNAL_MIN_SAMPLES ) return; + if ( samples == NULL || size < SIGNAL_MIN_SAMPLES ) return; - int acc_off = 0; - for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) - acc_off += samples[i] - 128; - acc_off /= (int)(size - SIGNAL_IGNORE_FIRST_SAMPLES); + int acc_off = 0; + for (uint32_t i = SIGNAL_IGNORE_FIRST_SAMPLES; i < size; i++) + acc_off += samples[i] - 128; + acc_off /= (int)(size - SIGNAL_IGNORE_FIRST_SAMPLES); - // shift and saturate samples to center the mean - for ( uint32_t i = 0; i < size; i++) { - if (acc_off > 0) { - samples[i] = (samples[i] >= acc_off)? samples[i] - acc_off : 0; - } - if (acc_off < 0) { - samples[i] = (255 - samples[i] >= -acc_off)? samples[i] - acc_off : 255; - } - } + // shift and saturate samples to center the mean + for ( uint32_t i = 0; i < size; i++) { + if (acc_off > 0) { + samples[i] = (samples[i] >= acc_off)? samples[i] - acc_off : 0; + } + if (acc_off < 0) { + samples[i] = (255 - samples[i] >= -acc_off)? samples[i] - acc_off : 255; + } + } } //by marshmellow //get high and low values of a wave with passed in fuzz factor. also return noise test = 1 for passed or 0 for only noise //void getHiLo(uint8_t *bits, size_t size, int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) { void getHiLo(int *high, int *low, uint8_t fuzzHi, uint8_t fuzzLo) { - // add fuzz. - *high = (signalprop.high * fuzzHi) / 100; - if ( signalprop.low < 0 ) { - *low = (signalprop.low * fuzzLo) / 100; - } else { - uint8_t range = signalprop.high - signalprop.low; + // add fuzz. + *high = (signalprop.high * fuzzHi) / 100; + if ( signalprop.low < 0 ) { + *low = (signalprop.low * fuzzLo) / 100; + } else { + uint8_t range = signalprop.high - signalprop.low; - *low = signalprop.low + ((range * (100-fuzzLo))/100); - } + *low = signalprop.low + ((range * (100-fuzzLo))/100); + } - // if fuzzing to great and overlap - if ( *high < *low ) { - *high = signalprop.high; - *low = signalprop.low; - } + // if fuzzing to great and overlap + if ( *high < *low ) { + *high = signalprop.high; + *low = signalprop.low; + } - if (g_debugMode) - prnt("getHiLo fuzzed: High %d | Low %d", *high, *low); + if (g_debugMode) + prnt("getHiLo fuzzed: High %d | Low %d", *high, *low); } // by marshmellow // pass bits to be tested in bits, length bits passed in bitLen, and parity type (even=0 | odd=1) in pType // returns 1 if passed bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType) { - return oddparity32(bits) ^ pType; + return oddparity32(bits) ^ pType; } //by marshmellow // takes a array of binary values, start position, length of bits per parity (includes parity bit - MAX 32), // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) size_t removeParity(uint8_t *bits, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) { - uint32_t parityWd = 0; - size_t bitCnt = 0; - for (int word = 0; word < (bLen); word += pLen){ - for (int bit=0; bit < pLen; bit++){ - if (word+bit >= bLen) break; - parityWd = (parityWd << 1) | bits[startIdx+word+bit]; - bits[bitCnt++] = (bits[startIdx+word+bit]); - } - if (word+pLen > bLen) break; + uint32_t parityWd = 0; + size_t bitCnt = 0; + for (int word = 0; word < (bLen); word += pLen){ + for (int bit=0; bit < pLen; bit++){ + if (word+bit >= bLen) break; + parityWd = (parityWd << 1) | bits[startIdx+word+bit]; + bits[bitCnt++] = (bits[startIdx+word+bit]); + } + if (word+pLen > bLen) break; - bitCnt--; // overwrite parity with next data - // if parity fails then return 0 - switch (pType) { - case 3: if (bits[bitCnt]==1) {return 0;} break; //should be 0 spacer bit - case 2: if (bits[bitCnt]==0) {return 0;} break; //should be 1 spacer bit - default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; //test parity - } - parityWd = 0; - } - // if we got here then all the parities passed - //return size - return bitCnt; + bitCnt--; // overwrite parity with next data + // if parity fails then return 0 + switch (pType) { + case 3: if (bits[bitCnt]==1) {return 0;} break; //should be 0 spacer bit + case 2: if (bits[bitCnt]==0) {return 0;} break; //should be 1 spacer bit + default: if (parityTest(parityWd, pLen, pType) == 0) { return 0; } break; //test parity + } + parityWd = 0; + } + // if we got here then all the parities passed + //return size + return bitCnt; } // by marshmellow @@ -190,61 +190,61 @@ size_t removeParity(uint8_t *bits, size_t startIdx, uint8_t pLen, uint8_t pType, // Parity Type (1 for odd; 0 for even; 2 Always 1's; 3 Always 0's), and binary Length (length to run) // Make sure *dest is long enough to store original sourceLen + #_of_parities_to_be_added size_t addParity(uint8_t *src, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType) { - uint32_t parityWd = 0; - size_t j = 0, bitCnt = 0; - for (int word = 0; word < sourceLen; word += pLen-1) { - for (int bit=0; bit < pLen-1; bit++){ - parityWd = (parityWd << 1) | src[word+bit]; - dest[j++] = (src[word+bit]); - } - // if parity fails then return 0 - switch (pType) { - case 3: dest[j++]=0; break; // marker bit which should be a 0 - case 2: dest[j++]=1; break; // marker bit which should be a 1 - default: - dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1; - break; - } - bitCnt += pLen; - parityWd = 0; - } - // if we got here then all the parities passed - //return ID start index and size - return bitCnt; + uint32_t parityWd = 0; + size_t j = 0, bitCnt = 0; + for (int word = 0; word < sourceLen; word += pLen-1) { + for (int bit=0; bit < pLen-1; bit++){ + parityWd = (parityWd << 1) | src[word+bit]; + dest[j++] = (src[word+bit]); + } + // if parity fails then return 0 + switch (pType) { + case 3: dest[j++]=0; break; // marker bit which should be a 0 + case 2: dest[j++]=1; break; // marker bit which should be a 1 + default: + dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1; + break; + } + bitCnt += pLen; + parityWd = 0; + } + // if we got here then all the parities passed + //return ID start index and size + return bitCnt; } // array must be size dividable with 8 uint8_t bits_to_array(const uint8_t *bits, size_t size, uint8_t *dest) { - if ( (size == 0) || (size % 8) != 0) return 0; + if ( (size == 0) || (size % 8) != 0) return 0; - for(uint32_t i = 0; i < (size / 8); i++) - dest[i] = bytebits_to_byte((uint8_t *) bits + (i * 8), 8); + for(uint32_t i = 0; i < (size / 8); i++) + dest[i] = bytebits_to_byte((uint8_t *) bits + (i * 8), 8); - return 0; + return 0; } uint32_t bytebits_to_byte(uint8_t *src, size_t numbits) { - uint32_t num = 0; - for(int i = 0 ; i < numbits ; i++) { - num = (num << 1) | (*src); - src++; - } - return num; + uint32_t num = 0; + for(int i = 0 ; i < numbits ; i++) { + num = (num << 1) | (*src); + src++; + } + return num; } //least significant bit first uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits) { - uint32_t num = 0; - for(int i = 0 ; i < numbits ; i++) { - num = (num << 1) | *(src + (numbits-(i+1))); - } - return num; + uint32_t num = 0; + for(int i = 0 ; i < numbits ; i++) { + num = (num << 1) | *(src + (numbits-(i+1))); + } + return num; } //by marshmellow //search for given preamble in given BitStream and return success = TRUE or fail = FALSE and startIndex and length bool preambleSearch(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx){ - return preambleSearchEx(bits, preamble, pLen, size, startIdx, false); + return preambleSearchEx(bits, preamble, pLen, size, startIdx, false); } //by marshmellow // search for given preamble in given BitStream and return success=1 or fail=0 and startIndex (where it was found) and length if not fineone @@ -252,210 +252,210 @@ bool preambleSearch(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, //(iceman) FINDONE, only finds start index. NOT SIZE!. I see Em410xDecode (lfdemod.c) uses SIZE to determine success bool preambleSearchEx(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone) { - // Sanity check. If preamble length is bigger than bits length. - if ( *size <= pLen ) return false; + // Sanity check. If preamble length is bigger than bits length. + if ( *size <= pLen ) return false; - uint8_t foundCnt = 0; - for (size_t idx = 0; idx < *size - pLen; idx++) { - if (memcmp(bits+idx, preamble, pLen) == 0){ - //first index found - foundCnt++; - if (foundCnt == 1){ - if (g_debugMode) prnt("DEBUG: (preambleSearchEx) preamble found at %i", idx); - *startIdx = idx; - if (findone) return true; - } - if (foundCnt == 2){ - if (g_debugMode) prnt("DEBUG: (preambleSearchEx) preamble 2 found at %i", idx); - *size = idx - *startIdx; - return true; - } - } - } - return (foundCnt > 0); + uint8_t foundCnt = 0; + for (size_t idx = 0; idx < *size - pLen; idx++) { + if (memcmp(bits+idx, preamble, pLen) == 0){ + //first index found + foundCnt++; + if (foundCnt == 1){ + if (g_debugMode) prnt("DEBUG: (preambleSearchEx) preamble found at %i", idx); + *startIdx = idx; + if (findone) return true; + } + if (foundCnt == 2){ + if (g_debugMode) prnt("DEBUG: (preambleSearchEx) preamble 2 found at %i", idx); + *size = idx - *startIdx; + return true; + } + } + } + return (foundCnt > 0); } // find start of modulating data (for fsk and psk) in case of beginning noise or slow chip startup. size_t findModStart(uint8_t *src, size_t size, uint8_t expWaveSize) { - size_t i = 0; - size_t waveSizeCnt = 0; - uint8_t thresholdCnt = 0; - bool isAboveThreshold = src[i++] >= signalprop.mean; //FSK_PSK_THRESHOLD; - for (; i < size-20; i++ ) { - if (src[i] < signalprop.mean && isAboveThreshold) { - thresholdCnt++; - if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; - isAboveThreshold = false; - waveSizeCnt = 0; - } else if (src[i] >= signalprop.mean && !isAboveThreshold) { - thresholdCnt++; - if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; - isAboveThreshold = true; - waveSizeCnt = 0; - } else { - waveSizeCnt++; - } - if (thresholdCnt > 10) break; - } - if (g_debugMode == 2) prnt("DEBUG: threshold Count reached at index %u, count: %u", i, thresholdCnt); - return i; + size_t i = 0; + size_t waveSizeCnt = 0; + uint8_t thresholdCnt = 0; + bool isAboveThreshold = src[i++] >= signalprop.mean; //FSK_PSK_THRESHOLD; + for (; i < size-20; i++ ) { + if (src[i] < signalprop.mean && isAboveThreshold) { + thresholdCnt++; + if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; + isAboveThreshold = false; + waveSizeCnt = 0; + } else if (src[i] >= signalprop.mean && !isAboveThreshold) { + thresholdCnt++; + if (thresholdCnt > 2 && waveSizeCnt < expWaveSize+1) break; + isAboveThreshold = true; + waveSizeCnt = 0; + } else { + waveSizeCnt++; + } + if (thresholdCnt > 10) break; + } + if (g_debugMode == 2) prnt("DEBUG: threshold Count reached at index %u, count: %u", i, thresholdCnt); + return i; } int getClosestClock(int testclk) { - uint16_t clocks[] = {8, 16, 32, 40, 50, 64, 128, 256, 384}; - uint8_t limit[] = {1, 2, 4, 4, 5, 8, 8, 8, 8}; + uint16_t clocks[] = {8, 16, 32, 40, 50, 64, 128, 256, 384}; + uint8_t limit[] = {1, 2, 4, 4, 5, 8, 8, 8, 8}; - for (uint8_t i = 0; i < 9; i++) - if ( testclk >= clocks[i]-limit[i] && testclk <= clocks[i]+limit[i] ) - return clocks[i]; + for (uint8_t i = 0; i < 9; i++) + if ( testclk >= clocks[i]-limit[i] && testclk <= clocks[i]+limit[i] ) + return clocks[i]; - return 0; + return 0; } void getNextLow(uint8_t *samples, size_t size, int low, size_t *i) { - while ((samples[*i] > low) && (*i < size)) - *i += 1; + while ((samples[*i] > low) && (*i < size)) + *i += 1; } void getNextHigh(uint8_t *samples, size_t size, int high, size_t *i) { - while ((samples[*i] < high) && (*i < size)) - *i += 1; + while ((samples[*i] < high) && (*i < size)) + *i += 1; } // load wave counters bool loadWaveCounters(uint8_t *samples, size_t size, int lowToLowWaveLen[], int highToLowWaveLen[], int *waveCnt, int *skip, int *minClk, int *high, int *low) { - size_t i = 0, firstLow, firstHigh; - //size_t testsize = (size < 512) ? size : 512; + size_t i = 0, firstLow, firstHigh; + //size_t testsize = (size < 512) ? size : 512; - // just noise - no super good detection. good enough - if (signalprop.isnoise) { - if (g_debugMode == 2) prnt("DEBUG STT: just noise detected - quitting"); - return false; - } + // just noise - no super good detection. good enough + if (signalprop.isnoise) { + if (g_debugMode == 2) prnt("DEBUG STT: just noise detected - quitting"); + return false; + } - getHiLo(high, low, 80, 80); + getHiLo(high, low, 80, 80); - // get to first full low to prime loop and skip incomplete first pulse - getNextHigh(samples, size, *high, &i); - getNextLow(samples, size, *low, &i); - *skip = i; + // get to first full low to prime loop and skip incomplete first pulse + getNextHigh(samples, size, *high, &i); + getNextLow(samples, size, *low, &i); + *skip = i; - // populate tmpbuff buffer with pulse lengths - while (i < size) { - // measure from low to low - firstLow = i; - //find first high point for this wave - getNextHigh(samples, size, *high, &i); - firstHigh = i; + // populate tmpbuff buffer with pulse lengths + while (i < size) { + // measure from low to low + firstLow = i; + //find first high point for this wave + getNextHigh(samples, size, *high, &i); + firstHigh = i; - getNextLow(samples, size, *low, &i); + getNextLow(samples, size, *low, &i); - if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK)) - break; + if (*waveCnt >= (size/LOWEST_DEFAULT_CLOCK)) + break; - highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low - lowToLowWaveLen[*waveCnt] = i - firstLow; - *waveCnt += 1; - if (i-firstLow < *minClk && i < size) { - *minClk = i - firstLow; - } - } - return true; + highToLowWaveLen[*waveCnt] = i - firstHigh; //first high to first low + lowToLowWaveLen[*waveCnt] = i - firstLow; + *waveCnt += 1; + if (i-firstLow < *minClk && i < size) { + *minClk = i - firstLow; + } + } + return true; } size_t pskFindFirstPhaseShift(uint8_t *samples, size_t size, uint8_t *curPhase, size_t waveStart, uint16_t fc, uint16_t *fullWaveLen) { - uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array... + uint16_t loopCnt = (size+3 < 4096) ? size : 4096; //don't need to loop through entire array... - uint16_t avgWaveVal=0, lastAvgWaveVal=0; - size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave; - for (; i= samples[i+2]){ - waveEnd = i+1; - if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart); - waveLenCnt = waveEnd-waveStart; - if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack - lastAvgWaveVal = avgWaveVal/(waveLenCnt); - firstFullWave = waveStart; - *fullWaveLen = waveLenCnt; - //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting) - if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1; - return firstFullWave; - } - waveStart = i+1; - avgWaveVal = 0; - } - avgWaveVal += samples[i+2]; - } - return 0; + uint16_t avgWaveVal=0, lastAvgWaveVal=0; + size_t i = waveStart, waveEnd, waveLenCnt, firstFullWave; + for (; i= samples[i+2]){ + waveEnd = i+1; + if (g_debugMode == 2) prnt("DEBUG PSK: waveEnd: %u, waveStart: %u", waveEnd, waveStart); + waveLenCnt = waveEnd-waveStart; + if (waveLenCnt > fc && waveStart > fc && !(waveLenCnt > fc+8)){ //not first peak and is a large wave but not out of whack + lastAvgWaveVal = avgWaveVal/(waveLenCnt); + firstFullWave = waveStart; + *fullWaveLen = waveLenCnt; + //if average wave value is > graph 0 then it is an up wave or a 1 (could cause inverting) + if (lastAvgWaveVal > FSK_PSK_THRESHOLD) *curPhase ^= 1; + return firstFullWave; + } + waveStart = i+1; + avgWaveVal = 0; + } + avgWaveVal += samples[i+2]; + } + return 0; } //by marshmellow //amplify based on ask edge detection - not accurate enough to use all the time void askAmp(uint8_t *bits, size_t size) { - uint8_t last = 128; - for(size_t i = 1; i < size; ++i){ - if ( bits[i] - bits[i-1] >= 30) //large jump up - last = 255; - else if ( bits[i-1] - bits[i] >= 20) //large jump down - last = 0; + uint8_t last = 128; + for(size_t i = 1; i < size; ++i){ + if ( bits[i] - bits[i-1] >= 30) //large jump up + last = 255; + else if ( bits[i-1] - bits[i] >= 20) //large jump down + last = 0; - bits[i] = last; - } + bits[i] = last; + } } // iceman, simplify this uint32_t manchesterEncode2Bytes(uint16_t datain) { - uint32_t output = 0; - uint8_t curBit = 0; - for (uint8_t i = 0; i < 16; i++) { - curBit = (datain >> (15-i) & 1); - output |= ( 1 << ( ( (15-i) * 2 ) + curBit)); - } - return output; + uint32_t output = 0; + uint8_t curBit = 0; + for (uint8_t i = 0; i < 16; i++) { + curBit = (datain >> (15-i) & 1); + output |= ( 1 << ( ( (15-i) * 2 ) + curBit)); + } + return output; } //by marshmellow //encode binary data into binary manchester //NOTE: bitstream must have triple the size of "size" available in memory to do the swap int ManchesterEncode(uint8_t *bits, size_t size) { - //allow up to 4096b out (means bits must be at least 2048+4096 to handle the swap) - size = (size > 2048) ? 2048 : size; - size_t modIdx = size; - size_t i; - for (size_t idx=0; idx < size; idx++){ - bits[idx+modIdx++] = bits[idx]; - bits[idx+modIdx++] = bits[idx]^1; - } - for (i=0; i < (size*2); i++){ - bits[i] = bits[i+size]; - } - return i; + //allow up to 4096b out (means bits must be at least 2048+4096 to handle the swap) + size = (size > 2048) ? 2048 : size; + size_t modIdx = size; + size_t i; + for (size_t idx=0; idx < size; idx++){ + bits[idx+modIdx++] = bits[idx]; + bits[idx+modIdx++] = bits[idx]^1; + } + for (i=0; i < (size*2); i++){ + bits[i] = bits[i+size]; + } + return i; } // by marshmellow // to detect a wave that has heavily clipped (clean) samples // loop 512 samples, if 250 of them is deemed maxed out, we assume the wave is clipped. bool DetectCleanAskWave(uint8_t *dest, size_t size, uint8_t high, uint8_t low) { - bool allArePeaks = true; - uint16_t cntPeaks = 0; - size_t loopEnd = 512 + 160; + bool allArePeaks = true; + uint16_t cntPeaks = 0; + size_t loopEnd = 512 + 160; - // sanity check - if (loopEnd > size) loopEnd = size; + // sanity check + if (loopEnd > size) loopEnd = size; - for (size_t i = 160; i < loopEnd; i++){ + for (size_t i = 160; i < loopEnd; i++){ - if (dest[i] > low && dest[i] < high) - allArePeaks = false; - else - cntPeaks++; - } + if (dest[i] > low && dest[i] < high) + allArePeaks = false; + else + cntPeaks++; + } - if (!allArePeaks){ - if (cntPeaks > 250) return true; - } - return allArePeaks; + if (!allArePeaks){ + if (cntPeaks > 250) return true; + } + return allArePeaks; } @@ -468,38 +468,38 @@ bool DetectCleanAskWave(uint8_t *dest, size_t size, uint8_t high, uint8_t low) { // to help detect clocks on heavily clipped samples // based on count of low to low int DetectStrongAskClock(uint8_t *dest, size_t size, int high, int low, int *clock) { - size_t startwave; - size_t i = 100; - size_t minClk = 512; - int shortestWaveIdx = 0; + size_t startwave; + size_t i = 100; + size_t minClk = 512; + int shortestWaveIdx = 0; - // get to first full low to prime loop and skip incomplete first pulse - getNextHigh(dest, size, high, &i); - getNextLow(dest, size, low, &i); + // get to first full low to prime loop and skip incomplete first pulse + getNextHigh(dest, size, high, &i); + getNextLow(dest, size, low, &i); - // loop through all samples (well, we don't want to go out-of-bounds) - while (i < size-512) { - // measure from low to low - startwave = i; + // loop through all samples (well, we don't want to go out-of-bounds) + while (i < size-512) { + // measure from low to low + startwave = i; - getNextHigh(dest, size, high, &i); - getNextLow(dest, size, low, &i); + getNextHigh(dest, size, high, &i); + getNextLow(dest, size, low, &i); - //get minimum measured distance - if (i-startwave < minClk && i < size) { - minClk = i - startwave; - shortestWaveIdx = startwave; - } - } + //get minimum measured distance + if (i-startwave < minClk && i < size) { + minClk = i - startwave; + shortestWaveIdx = startwave; + } + } - // set clock - if (g_debugMode == 2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d", minClk); - *clock = getClosestClock(minClk); + // set clock + if (g_debugMode == 2) prnt("DEBUG ASK: DetectStrongAskClock smallest wave: %d", minClk); + *clock = getClosestClock(minClk); - if (*clock == 0) - return -1; + if (*clock == 0) + return -1; - return shortestWaveIdx; + return shortestWaveIdx; } // by marshmellow @@ -508,312 +508,312 @@ int DetectStrongAskClock(uint8_t *dest, size_t size, int high, int low, int *clo // return start index of best starting position for that clock and return clock (by reference) int DetectASKClock(uint8_t *dest, size_t size, int *clock, int maxErr) { - //don't need to loop through entire array. (cotag has clock of 384) - uint16_t loopCnt = 1500; + //don't need to loop through entire array. (cotag has clock of 384) + uint16_t loopCnt = 1500; - // not enough samples - if (size <= loopCnt + 60){ - if (g_debugMode == 2) prnt("DEBUG DetectASKClock: not enough samples - aborting"); - return -1; - } + // not enough samples + if (size <= loopCnt + 60){ + if (g_debugMode == 2) prnt("DEBUG DetectASKClock: not enough samples - aborting"); + return -1; + } - // just noise - no super good detection. good enough - if (signalprop.isnoise) { - if (g_debugMode == 2) prnt("DEBUG DetectASKClock: just noise detected - aborting"); - return -2; - } + // just noise - no super good detection. good enough + if (signalprop.isnoise) { + if (g_debugMode == 2) prnt("DEBUG DetectASKClock: just noise detected - aborting"); + return -2; + } - size_t i = 1; - uint16_t num_clks = 9; - // first 255 value pos0 is placeholder for user inputed clock. - uint16_t clk[] = {255, 8, 16, 32, 40, 50, 64, 100, 128, 255}; + size_t i = 1; + uint16_t num_clks = 9; + // first 255 value pos0 is placeholder for user inputed clock. + uint16_t clk[] = {255, 8, 16, 32, 40, 50, 64, 100, 128, 255}; - // sometimes there is a strange end wave - filter out this - size -= 60; + // sometimes there is a strange end wave - filter out this + size -= 60; - // What is purpose? - // already have a valid clock? - uint8_t found_clk = 0; - for (; i < num_clks; ++i) { - if (clk[i] == *clock) { - found_clk = i; - } - } + // What is purpose? + // already have a valid clock? + uint8_t found_clk = 0; + for (; i < num_clks; ++i) { + if (clk[i] == *clock) { + found_clk = i; + } + } - // threshold 75% of high, low peak - int peak_hi, peak_low; - getHiLo(&peak_hi, &peak_low, 75, 75); + // threshold 75% of high, low peak + int peak_hi, peak_low; + getHiLo(&peak_hi, &peak_low, 75, 75); - // test for large clean, STRONG, CLIPPED peaks + // test for large clean, STRONG, CLIPPED peaks - if (!found_clk) { + if (!found_clk) { - if (DetectCleanAskWave(dest, size, peak_hi, peak_low)){ + if (DetectCleanAskWave(dest, size, peak_hi, peak_low)){ - int idx = DetectStrongAskClock(dest, size, peak_hi, peak_low, clock); - if (g_debugMode == 2) - prnt("DEBUG ASK: DetectASKClock Clean ASK Wave detected: clk %i, Best Starting Position: %i", *clock, idx); + int idx = DetectStrongAskClock(dest, size, peak_hi, peak_low, clock); + if (g_debugMode == 2) + prnt("DEBUG ASK: DetectASKClock Clean ASK Wave detected: clk %i, Best Starting Position: %i", *clock, idx); - // return shortest wave start position - if (idx > -1) - return idx; - } - } - // test for weak peaks + // return shortest wave start position + if (idx > -1) + return idx; + } + } + // test for weak peaks - // test clock if given as cmd parameter - if ( *clock > 0 ) - clk[0] = *clock; + // test clock if given as cmd parameter + if ( *clock > 0 ) + clk[0] = *clock; - uint8_t clkCnt, tol = 0; - size_t j = 0; - uint16_t bestErr[] = {1000,1000,1000,1000,1000,1000,1000,1000,1000}; - uint8_t bestStart[] = {0,0,0,0,0,0,0,0,0}; - size_t errCnt = 0, arrLoc, loopEnd; + uint8_t clkCnt, tol = 0; + size_t j = 0; + uint16_t bestErr[] = {1000,1000,1000,1000,1000,1000,1000,1000,1000}; + uint8_t bestStart[] = {0,0,0,0,0,0,0,0,0}; + size_t errCnt = 0, arrLoc, loopEnd; - if (found_clk) { - clkCnt = found_clk; - num_clks = found_clk + 1; - } else { - clkCnt = 1; - } + if (found_clk) { + clkCnt = found_clk; + num_clks = found_clk + 1; + } else { + clkCnt = 1; + } - //test each valid clock from smallest to greatest to see which lines up - for (; clkCnt < num_clks; clkCnt++) { - if (clk[clkCnt] <= 32) { - tol = 1; - } else { - tol = 0; - } - //if no errors allowed - keep start within the first clock - if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt] < 128) - loopCnt = clk[clkCnt] * 2; + //test each valid clock from smallest to greatest to see which lines up + for (; clkCnt < num_clks; clkCnt++) { + if (clk[clkCnt] <= 32) { + tol = 1; + } else { + tol = 0; + } + //if no errors allowed - keep start within the first clock + if (!maxErr && size > clk[clkCnt]*2 + tol && clk[clkCnt] < 128) + loopCnt = clk[clkCnt] * 2; - bestErr[clkCnt] = 1000; + bestErr[clkCnt] = 1000; - //try lining up the peaks by moving starting point (try first few clocks) + //try lining up the peaks by moving starting point (try first few clocks) - // get to first full low to prime loop and skip incomplete first pulse - getNextHigh(dest, size, peak_hi, &j); - getNextLow(dest, size, peak_low, &j); + // get to first full low to prime loop and skip incomplete first pulse + getNextHigh(dest, size, peak_hi, &j); + getNextLow(dest, size, peak_low, &j); - for (; j < loopCnt; j++){ - errCnt = 0; - // now that we have the first one lined up test rest of wave array - loopEnd = ((size-j-tol) / clk[clkCnt]) - 1; - for (i=0; i < loopEnd; ++i){ - arrLoc = j + (i * clk[clkCnt]); - if (dest[arrLoc] >= peak_hi || dest[arrLoc] <= peak_low){ - } else if (dest[arrLoc-tol] >= peak_hi || dest[arrLoc-tol] <= peak_low){ - } else if (dest[arrLoc+tol] >= peak_hi || dest[arrLoc+tol] <= peak_low){ - } else { //error no peak detected - errCnt++; - } - } - // if we found no errors then we can stop here and a low clock (common clocks) - // this is correct one - return this clock - // if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d", clk[clkCnt], errCnt, j, i); - if (errCnt == 0 && clkCnt < 7) { - if (!found_clk) - *clock = clk[clkCnt]; - return j; - } - // if we found errors see if it is lowest so far and save it as best run - if (errCnt < bestErr[clkCnt]) { - bestErr[clkCnt] = errCnt; - bestStart[clkCnt] = j; - } - } - } + for (; j < loopCnt; j++){ + errCnt = 0; + // now that we have the first one lined up test rest of wave array + loopEnd = ((size-j-tol) / clk[clkCnt]) - 1; + for (i=0; i < loopEnd; ++i){ + arrLoc = j + (i * clk[clkCnt]); + if (dest[arrLoc] >= peak_hi || dest[arrLoc] <= peak_low){ + } else if (dest[arrLoc-tol] >= peak_hi || dest[arrLoc-tol] <= peak_low){ + } else if (dest[arrLoc+tol] >= peak_hi || dest[arrLoc+tol] <= peak_low){ + } else { //error no peak detected + errCnt++; + } + } + // if we found no errors then we can stop here and a low clock (common clocks) + // this is correct one - return this clock + // if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, err %d, startpos %d, endpos %d", clk[clkCnt], errCnt, j, i); + if (errCnt == 0 && clkCnt < 7) { + if (!found_clk) + *clock = clk[clkCnt]; + return j; + } + // if we found errors see if it is lowest so far and save it as best run + if (errCnt < bestErr[clkCnt]) { + bestErr[clkCnt] = errCnt; + bestStart[clkCnt] = j; + } + } + } - uint8_t k, best = 0; + uint8_t k, best = 0; - for (k=1; k < num_clks; ++k){ - if (bestErr[k] < bestErr[best]){ - if (bestErr[k] == 0) bestErr[k] = 1; - // current best bit to error ratio vs new bit to error ratio - if ( (size/clk[best])/bestErr[best] < (size/clk[k])/bestErr[k] ){ - best = k; - } - } - //if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d", clk[k], bestErr[k], clk[best], bestStart[best]); - } + for (k=1; k < num_clks; ++k){ + if (bestErr[k] < bestErr[best]){ + if (bestErr[k] == 0) bestErr[k] = 1; + // current best bit to error ratio vs new bit to error ratio + if ( (size/clk[best])/bestErr[best] < (size/clk[k])/bestErr[k] ){ + best = k; + } + } + //if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, # Errors %d, Current Best Clk %d, bestStart %d", clk[k], bestErr[k], clk[best], bestStart[best]); + } - if (!found_clk) - *clock = clk[best]; + if (!found_clk) + *clock = clk[best]; - return bestStart[best]; + return bestStart[best]; } int DetectStrongNRZClk(uint8_t *dest, size_t size, int peak, int low, bool *strong) { - //find shortest transition from high to low - *strong = false; - size_t i = 0; - size_t transition1 = 0; - int lowestTransition = 255; - bool lastWasHigh = false; - size_t transitionSampleCount = 0; - //find first valid beginning of a high or low wave - while ((dest[i] >= peak || dest[i] <= low) && (i < size)) - ++i; - while ((dest[i] < peak && dest[i] > low) && (i < size)) - ++i; + //find shortest transition from high to low + *strong = false; + size_t i = 0; + size_t transition1 = 0; + int lowestTransition = 255; + bool lastWasHigh = false; + size_t transitionSampleCount = 0; + //find first valid beginning of a high or low wave + while ((dest[i] >= peak || dest[i] <= low) && (i < size)) + ++i; + while ((dest[i] < peak && dest[i] > low) && (i < size)) + ++i; - lastWasHigh = (dest[i] >= peak); + lastWasHigh = (dest[i] >= peak); - if (i == size) - return 0; + if (i == size) + return 0; - transition1 = i; + transition1 = i; - for (;i < size; i++) { - if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) { - lastWasHigh = (dest[i] >= peak); - if (i-transition1 < lowestTransition) - lowestTransition = i-transition1; - transition1 = i; - } else if (dest[i] < peak && dest[i] > low) { - transitionSampleCount++; - } - } - if (lowestTransition == 255) - lowestTransition = 0; + for (;i < size; i++) { + if ((dest[i] >= peak && !lastWasHigh) || (dest[i] <= low && lastWasHigh)) { + lastWasHigh = (dest[i] >= peak); + if (i-transition1 < lowestTransition) + lowestTransition = i-transition1; + transition1 = i; + } else if (dest[i] < peak && dest[i] > low) { + transitionSampleCount++; + } + } + if (lowestTransition == 255) + lowestTransition = 0; - if (g_debugMode == 2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d", lowestTransition); - // if less than 10% of the samples were not peaks (or 90% were peaks) then we have a strong wave - if (transitionSampleCount / size < 10) { - *strong = true; - lowestTransition = getClosestClock(lowestTransition); - } - return lowestTransition; + if (g_debugMode == 2) prnt("DEBUG NRZ: detectstrongNRZclk smallest wave: %d", lowestTransition); + // if less than 10% of the samples were not peaks (or 90% were peaks) then we have a strong wave + if (transitionSampleCount / size < 10) { + *strong = true; + lowestTransition = getClosestClock(lowestTransition); + } + return lowestTransition; } //by marshmellow //detect nrz clock by reading #peaks vs no peaks(or errors) int DetectNRZClock(uint8_t *dest, size_t size, int clock, size_t *clockStartIdx) { - size_t i = 0; - uint8_t clk[] = {8,16,32,40,50,64,100,128,255}; - size_t loopCnt = 4096; //don't need to loop through entire array... + size_t i = 0; + uint8_t clk[] = {8,16,32,40,50,64,100,128,255}; + size_t loopCnt = 4096; //don't need to loop through entire array... - //if we already have a valid clock quit - for (; i < 8; ++i) - if (clk[i] == clock) return clock; + //if we already have a valid clock quit + for (; i < 8; ++i) + if (clk[i] == clock) return clock; - if (size < 20) return 0; - // size must be larger than 20 here - if (size < loopCnt) loopCnt = size-20; + if (size < 20) return 0; + // size must be larger than 20 here + if (size < loopCnt) loopCnt = size-20; - // just noise - no super good detection. good enough - if (signalprop.isnoise) { - if (g_debugMode == 2) prnt("DEBUG DetectNZRClock: just noise detected - quitting"); - return 0; - } + // just noise - no super good detection. good enough + if (signalprop.isnoise) { + if (g_debugMode == 2) prnt("DEBUG DetectNZRClock: just noise detected - quitting"); + return 0; + } - //get high and low peak - int peak, low; - //getHiLo(dest, loopCnt, &peak, &low, 90, 90); - getHiLo(&peak, &low, 90, 90); + //get high and low peak + int peak, low; + //getHiLo(dest, loopCnt, &peak, &low, 90, 90); + getHiLo(&peak, &low, 90, 90); - bool strong = false; - int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low, &strong); - if (strong) return lowestTransition; - size_t ii; - uint8_t clkCnt; - uint8_t tol = 0; - uint16_t smplCnt = 0; - int16_t peakcnt = 0; - int16_t peaksdet[] = {0,0,0,0,0,0,0,0}; - uint16_t minPeak = 255; - bool firstpeak = true; - //test for large clipped waves - ignore first peak - for (i=0; i= peak || dest[i] <= low){ - if (firstpeak) continue; - smplCnt++; - } else { - firstpeak = false; - if (smplCnt > 0) { - if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt; - peakcnt++; - if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d",minPeak,smplCnt,peakcnt); - smplCnt = 0; - } - } - } - if (minPeak < 8) return 0; + bool strong = false; + int lowestTransition = DetectStrongNRZClk(dest, size-20, peak, low, &strong); + if (strong) return lowestTransition; + size_t ii; + uint8_t clkCnt; + uint8_t tol = 0; + uint16_t smplCnt = 0; + int16_t peakcnt = 0; + int16_t peaksdet[] = {0,0,0,0,0,0,0,0}; + uint16_t minPeak = 255; + bool firstpeak = true; + //test for large clipped waves - ignore first peak + for (i=0; i= peak || dest[i] <= low){ + if (firstpeak) continue; + smplCnt++; + } else { + firstpeak = false; + if (smplCnt > 0) { + if (minPeak > smplCnt && smplCnt > 7) minPeak = smplCnt; + peakcnt++; + if (g_debugMode == 2) prnt("DEBUG NRZ: minPeak: %d, smplCnt: %d, peakcnt: %d",minPeak,smplCnt,peakcnt); + smplCnt = 0; + } + } + } + if (minPeak < 8) return 0; - bool errBitHigh = 0, bitHigh = 0, lastPeakHigh = 0; - uint8_t ignoreCnt = 0, ignoreWindow = 4; - int lastBit = 0; - size_t bestStart[] = {0,0,0,0,0,0,0,0,0}; - peakcnt = 0; - //test each valid clock from smallest to greatest to see which lines up - for (clkCnt=0; clkCnt < 8; ++clkCnt){ - //ignore clocks smaller than smallest peak - if (clk[clkCnt] < minPeak - (clk[clkCnt]/4)) continue; - //try lining up the peaks by moving starting point (try first 256) - for (ii=20; ii < loopCnt; ++ii){ - if ((dest[ii] >= peak) || (dest[ii] <= low)){ - peakcnt=0; - bitHigh = false; - ignoreCnt = 0; - lastBit = ii-clk[clkCnt]; - //loop through to see if this start location works - for (i = ii; i < size-20; ++i) { - //if we are at a clock bit - if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) { - //test high/low - if (dest[i] >= peak || dest[i] <= low) { - //if same peak don't count it - if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) { - peakcnt++; - } - lastPeakHigh = (dest[i] >= peak); - bitHigh = true; - errBitHigh = false; - ignoreCnt = ignoreWindow; - lastBit += clk[clkCnt]; - } else if (i == lastBit + clk[clkCnt] + tol) { - lastBit += clk[clkCnt]; - } - //else if not a clock bit and no peaks - } else if (dest[i] < peak && dest[i] > low){ - if (ignoreCnt == 0){ - bitHigh = false; - if (errBitHigh == true) - peakcnt--; - errBitHigh = false; - } else { - ignoreCnt--; - } - // else if not a clock bit but we have a peak - } else if ((dest[i] >= peak || dest[i] <= low) && (!bitHigh)) { - //error bar found no clock... - errBitHigh = true; - } - } - if (peakcnt > peaksdet[clkCnt]) { - bestStart[clkCnt] = ii; - peaksdet[clkCnt] = peakcnt; - } - } - } - } + bool errBitHigh = 0, bitHigh = 0, lastPeakHigh = 0; + uint8_t ignoreCnt = 0, ignoreWindow = 4; + int lastBit = 0; + size_t bestStart[] = {0,0,0,0,0,0,0,0,0}; + peakcnt = 0; + //test each valid clock from smallest to greatest to see which lines up + for (clkCnt=0; clkCnt < 8; ++clkCnt){ + //ignore clocks smaller than smallest peak + if (clk[clkCnt] < minPeak - (clk[clkCnt]/4)) continue; + //try lining up the peaks by moving starting point (try first 256) + for (ii=20; ii < loopCnt; ++ii){ + if ((dest[ii] >= peak) || (dest[ii] <= low)){ + peakcnt=0; + bitHigh = false; + ignoreCnt = 0; + lastBit = ii-clk[clkCnt]; + //loop through to see if this start location works + for (i = ii; i < size-20; ++i) { + //if we are at a clock bit + if ((i >= lastBit + clk[clkCnt] - tol) && (i <= lastBit + clk[clkCnt] + tol)) { + //test high/low + if (dest[i] >= peak || dest[i] <= low) { + //if same peak don't count it + if ((dest[i] >= peak && !lastPeakHigh) || (dest[i] <= low && lastPeakHigh)) { + peakcnt++; + } + lastPeakHigh = (dest[i] >= peak); + bitHigh = true; + errBitHigh = false; + ignoreCnt = ignoreWindow; + lastBit += clk[clkCnt]; + } else if (i == lastBit + clk[clkCnt] + tol) { + lastBit += clk[clkCnt]; + } + //else if not a clock bit and no peaks + } else if (dest[i] < peak && dest[i] > low){ + if (ignoreCnt == 0){ + bitHigh = false; + if (errBitHigh == true) + peakcnt--; + errBitHigh = false; + } else { + ignoreCnt--; + } + // else if not a clock bit but we have a peak + } else if ((dest[i] >= peak || dest[i] <= low) && (!bitHigh)) { + //error bar found no clock... + errBitHigh = true; + } + } + if (peakcnt > peaksdet[clkCnt]) { + bestStart[clkCnt] = ii; + peaksdet[clkCnt] = peakcnt; + } + } + } + } - uint8_t best = 0; - for (int m = 7; m > 0; m--){ - if ((peaksdet[m] >= (peaksdet[best]-1)) && (peaksdet[m] <= peaksdet[best]+1) && lowestTransition) { - if (clk[m] > (lowestTransition - (clk[m]/8)) && clk[m] < (lowestTransition + (clk[m]/8))) { - best = m; - } - } else if (peaksdet[m] > peaksdet[best]){ - best = m; - } - if (g_debugMode == 2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d", clk[m], peaksdet[m], minPeak, clk[best], lowestTransition); - } - *clockStartIdx = bestStart[best]; - return clk[best]; + uint8_t best = 0; + for (int m = 7; m > 0; m--){ + if ((peaksdet[m] >= (peaksdet[best]-1)) && (peaksdet[m] <= peaksdet[best]+1) && lowestTransition) { + if (clk[m] > (lowestTransition - (clk[m]/8)) && clk[m] < (lowestTransition + (clk[m]/8))) { + best = m; + } + } else if (peaksdet[m] > peaksdet[best]){ + best = m; + } + if (g_debugMode == 2) prnt("DEBUG NRZ: Clk: %d, peaks: %d, minPeak: %d, bestClk: %d, lowestTrs: %d", clk[m], peaksdet[m], minPeak, clk[best], lowestTransition); + } + *clockStartIdx = bestStart[best]; + return clk[best]; } //by marshmellow @@ -821,299 +821,299 @@ int DetectNRZClock(uint8_t *dest, size_t size, int clock, size_t *clockStartIdx) //counts and returns the 2 most common wave lengths //mainly used for FSK field clock detection uint16_t countFC(uint8_t *bits, size_t size, bool fskAdj) { - uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - uint8_t fcLensFnd = 0; - uint8_t lastFCcnt = 0; - uint8_t fcCounter = 0; - size_t i; - if (size < 180) return 0; + uint8_t fcLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint16_t fcCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t fcLensFnd = 0; + uint8_t lastFCcnt = 0; + uint8_t fcCounter = 0; + size_t i; + if (size < 180) return 0; - // prime i to first up transition - for (i = 160; i < size-20; i++) - if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]) - break; + // prime i to first up transition + for (i = 160; i < size-20; i++) + if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]) + break; - for (; i < size-20; i++){ - if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]){ - // new up transition - fcCounter++; - if (fskAdj){ - //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) - if (lastFCcnt == 5 && fcCounter == 9) fcCounter--; + for (; i < size-20; i++){ + if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]){ + // new up transition + fcCounter++; + if (fskAdj){ + //if we had 5 and now have 9 then go back to 8 (for when we get a fc 9 instead of an 8) + if (lastFCcnt == 5 && fcCounter == 9) fcCounter--; - //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5) - if ((fcCounter == 9) || fcCounter == 4) fcCounter++; - // save last field clock count (fc/xx) - lastFCcnt = fcCounter; - } - // find which fcLens to save it to: - for (int m=0; m<15; m++){ - if (fcLens[m] == fcCounter){ - fcCnts[m]++; - fcCounter = 0; - break; - } - } - if (fcCounter>0 && fcLensFnd<15){ - //add new fc length - fcCnts[fcLensFnd]++; - fcLens[fcLensFnd++] = fcCounter; - } - fcCounter=0; - } else { - // count sample - fcCounter++; - } - } + //if fc=9 or 4 add one (for when we get a fc 9 instead of 10 or a 4 instead of a 5) + if ((fcCounter == 9) || fcCounter == 4) fcCounter++; + // save last field clock count (fc/xx) + lastFCcnt = fcCounter; + } + // find which fcLens to save it to: + for (int m=0; m<15; m++){ + if (fcLens[m] == fcCounter){ + fcCnts[m]++; + fcCounter = 0; + break; + } + } + if (fcCounter>0 && fcLensFnd<15){ + //add new fc length + fcCnts[fcLensFnd]++; + fcLens[fcLensFnd++] = fcCounter; + } + fcCounter=0; + } else { + // count sample + fcCounter++; + } + } - uint8_t best1 = 14, best2 = 14, best3 = 14; - uint16_t maxCnt1 = 0; - // go through fclens and find which ones are bigest 2 - for (i=0; i<15; i++){ - // get the 3 best FC values - if (fcCnts[i] > maxCnt1) { - best3 = best2; - best2 = best1; - maxCnt1 = fcCnts[i]; - best1 = i; - } else if(fcCnts[i] > fcCnts[best2]){ - best3 = best2; - best2 = i; - } else if(fcCnts[i] > fcCnts[best3]){ - best3 = i; - } - if (g_debugMode == 2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u", fcLens[i], fcCnts[i], fcLens[best1], fcLens[best2]); - if (fcLens[i] == 0) break; - } + uint8_t best1 = 14, best2 = 14, best3 = 14; + uint16_t maxCnt1 = 0; + // go through fclens and find which ones are bigest 2 + for (i=0; i<15; i++){ + // get the 3 best FC values + if (fcCnts[i] > maxCnt1) { + best3 = best2; + best2 = best1; + maxCnt1 = fcCnts[i]; + best1 = i; + } else if(fcCnts[i] > fcCnts[best2]){ + best3 = best2; + best2 = i; + } else if(fcCnts[i] > fcCnts[best3]){ + best3 = i; + } + if (g_debugMode == 2) prnt("DEBUG countfc: FC %u, Cnt %u, best fc: %u, best2 fc: %u", fcLens[i], fcCnts[i], fcLens[best1], fcLens[best2]); + if (fcLens[i] == 0) break; + } - if (fcLens[best1] == 0) return 0; - uint8_t fcH = 0, fcL = 0; - if (fcLens[best1] > fcLens[best2]){ - fcH = fcLens[best1]; - fcL = fcLens[best2]; - } else{ - fcH = fcLens[best2]; - fcL = fcLens[best1]; - } - if ((size-180)/fcH/3 > fcCnts[best1] + fcCnts[best2]) { - if (g_debugMode == 2) prnt("DEBUG countfc: fc is too large: %u > %u. Not psk or fsk", (size-180)/fcH/3, fcCnts[best1] + fcCnts[best2]); - return 0; //lots of waves not psk or fsk - } - // TODO: take top 3 answers and compare to known Field clocks to get top 2 + if (fcLens[best1] == 0) return 0; + uint8_t fcH = 0, fcL = 0; + if (fcLens[best1] > fcLens[best2]){ + fcH = fcLens[best1]; + fcL = fcLens[best2]; + } else{ + fcH = fcLens[best2]; + fcL = fcLens[best1]; + } + if ((size-180)/fcH/3 > fcCnts[best1] + fcCnts[best2]) { + if (g_debugMode == 2) prnt("DEBUG countfc: fc is too large: %u > %u. Not psk or fsk", (size-180)/fcH/3, fcCnts[best1] + fcCnts[best2]); + return 0; //lots of waves not psk or fsk + } + // TODO: take top 3 answers and compare to known Field clocks to get top 2 - uint16_t fcs = (((uint16_t)fcH)<<8) | fcL; - if (fskAdj) return fcs; - return (uint16_t)fcLens[best2] << 8 | fcLens[best1]; + uint16_t fcs = (((uint16_t)fcH)<<8) | fcL; + if (fskAdj) return fcs; + return (uint16_t)fcLens[best2] << 8 | fcLens[best1]; } //by marshmellow //detect psk clock by reading each phase shift // a phase shift is determined by measuring the sample length of each wave int DetectPSKClock(uint8_t *dest, size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc) { - uint8_t clk[] = {255,16,32,40,50,64,100,128,255}; //255 is not a valid clock - uint16_t loopCnt = 4096; //don't need to loop through entire array... + uint8_t clk[] = {255,16,32,40,50,64,100,128,255}; //255 is not a valid clock + uint16_t loopCnt = 4096; //don't need to loop through entire array... - //if we already have a valid clock quit - size_t i = 1; - for (; i < 8; ++i) - if (clk[i] == clock) return clock; + //if we already have a valid clock quit + size_t i = 1; + for (; i < 8; ++i) + if (clk[i] == clock) return clock; - if (size < 160+20) return 0; - // size must be larger than 20 here, and 160 later on. - if (size < loopCnt) loopCnt = size-20; + if (size < 160+20) return 0; + // size must be larger than 20 here, and 160 later on. + if (size < loopCnt) loopCnt = size-20; - uint16_t fcs = countFC(dest, size, 0); + uint16_t fcs = countFC(dest, size, 0); - *fc = fcs & 0xFF; + *fc = fcs & 0xFF; - if (g_debugMode == 2) prnt("DEBUG PSK: FC: %d, FC2: %d",*fc, fcs>>8); + if (g_debugMode == 2) prnt("DEBUG PSK: FC: %d, FC2: %d",*fc, fcs>>8); - if ((fcs >> 8) == 10 && *fc == 8) return 0; + if ((fcs >> 8) == 10 && *fc == 8) return 0; - if (*fc != 2 && *fc != 4 && *fc != 8) return 0; + if (*fc != 2 && *fc != 4 && *fc != 8) return 0; - size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0; + size_t waveStart=0, waveEnd=0, firstFullWave=0, lastClkBit=0; - uint8_t clkCnt, tol=1; - uint16_t peakcnt=0, errCnt=0, waveLenCnt=0, fullWaveLen=0; - uint16_t bestErr[] = {1000,1000,1000,1000,1000,1000,1000,1000,1000}; - uint16_t peaksdet[] = {0,0,0,0,0,0,0,0,0}; + uint8_t clkCnt, tol=1; + uint16_t peakcnt=0, errCnt=0, waveLenCnt=0, fullWaveLen=0; + uint16_t bestErr[] = {1000,1000,1000,1000,1000,1000,1000,1000,1000}; + uint16_t peaksdet[] = {0,0,0,0,0,0,0,0,0}; - //find start of modulating data in trace - i = findModStart(dest, size, *fc); + //find start of modulating data in trace + i = findModStart(dest, size, *fc); - firstFullWave = pskFindFirstPhaseShift(dest, size, curPhase, i, *fc, &fullWaveLen); - if (firstFullWave == 0) { - // no phase shift detected - could be all 1's or 0's - doesn't matter where we start - // so skip a little to ensure we are past any Start Signal - firstFullWave = 160; - fullWaveLen = 0; - } + firstFullWave = pskFindFirstPhaseShift(dest, size, curPhase, i, *fc, &fullWaveLen); + if (firstFullWave == 0) { + // no phase shift detected - could be all 1's or 0's - doesn't matter where we start + // so skip a little to ensure we are past any Start Signal + firstFullWave = 160; + fullWaveLen = 0; + } - *firstPhaseShift = firstFullWave; - if (g_debugMode == 2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); + *firstPhaseShift = firstFullWave; + if (g_debugMode == 2) prnt("DEBUG PSK: firstFullWave: %d, waveLen: %d",firstFullWave,fullWaveLen); - //test each valid clock from greatest to smallest to see which lines up - for (clkCnt=7; clkCnt >= 1 ; clkCnt--){ - tol = *fc/2; - lastClkBit = firstFullWave; //set end of wave as clock align - waveStart = 0; - errCnt = 0; - peakcnt = 0; - if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d", clk[clkCnt], lastClkBit); + //test each valid clock from greatest to smallest to see which lines up + for (clkCnt=7; clkCnt >= 1 ; clkCnt--){ + tol = *fc/2; + lastClkBit = firstFullWave; //set end of wave as clock align + waveStart = 0; + errCnt = 0; + peakcnt = 0; + if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %d", clk[clkCnt], lastClkBit); - for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){ - //top edge of wave = start of new wave - if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){ - if (waveStart == 0) { - waveStart = i+1; - waveLenCnt = 0; - } else { //waveEnd - waveEnd = i+1; - waveLenCnt = waveEnd-waveStart; - if (waveLenCnt > *fc){ - //if this wave is a phase shift - if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d", waveStart, waveLenCnt, lastClkBit + clk[clkCnt] - tol, i+1, *fc); - if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit - peakcnt++; - lastClkBit += clk[clkCnt]; - } else if (i < lastClkBit+8){ - //noise after a phase shift - ignore - } else { //phase shift before supposed to based on clock - errCnt++; - } - } else if (i+1 > lastClkBit + clk[clkCnt] + tol + *fc){ - lastClkBit+=clk[clkCnt]; //no phase shift but clock bit - } - waveStart = i+1; - } - } - } - if (errCnt == 0) return clk[clkCnt]; - if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt] = errCnt; - if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt] = peakcnt; - } - //all tested with errors - //return the highest clk with the most peaks found - uint8_t best = 7; - for (i=7; i >= 1; i--){ - if (peaksdet[i] > peaksdet[best]) - best = i; + for (i = firstFullWave+fullWaveLen-1; i < loopCnt-2; i++){ + //top edge of wave = start of new wave + if (dest[i] < dest[i+1] && dest[i+1] >= dest[i+2]){ + if (waveStart == 0) { + waveStart = i+1; + waveLenCnt = 0; + } else { //waveEnd + waveEnd = i+1; + waveLenCnt = waveEnd-waveStart; + if (waveLenCnt > *fc){ + //if this wave is a phase shift + if (g_debugMode == 2) prnt("DEBUG PSK: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d", waveStart, waveLenCnt, lastClkBit + clk[clkCnt] - tol, i+1, *fc); + if (i+1 >= lastClkBit + clk[clkCnt] - tol){ //should be a clock bit + peakcnt++; + lastClkBit += clk[clkCnt]; + } else if (i < lastClkBit+8){ + //noise after a phase shift - ignore + } else { //phase shift before supposed to based on clock + errCnt++; + } + } else if (i+1 > lastClkBit + clk[clkCnt] + tol + *fc){ + lastClkBit+=clk[clkCnt]; //no phase shift but clock bit + } + waveStart = i+1; + } + } + } + if (errCnt == 0) return clk[clkCnt]; + if (errCnt <= bestErr[clkCnt]) bestErr[clkCnt] = errCnt; + if (peakcnt > peaksdet[clkCnt]) peaksdet[clkCnt] = peakcnt; + } + //all tested with errors + //return the highest clk with the most peaks found + uint8_t best = 7; + for (i=7; i >= 1; i--){ + if (peaksdet[i] > peaksdet[best]) + best = i; - if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[i],peaksdet[i],bestErr[i],clk[best]); - } - return clk[best]; + if (g_debugMode == 2) prnt("DEBUG PSK: Clk: %d, peaks: %d, errs: %d, bestClk: %d",clk[i],peaksdet[i],bestErr[i],clk[best]); + } + return clk[best]; } //by marshmellow //detects the bit clock for FSK given the high and low Field Clocks uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge) { - if (size == 0) - return 0; + if (size == 0) + return 0; - uint8_t clk[] = {8,16,32,40,50,64,100,128,0}; - uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; - uint8_t rfLensFnd = 0; - uint8_t lastFCcnt = 0; - uint16_t fcCounter = 0; - uint16_t rfCounter = 0; - uint8_t firstBitFnd = 0; - size_t i; - uint8_t fcTol = ((fcHigh * 100 - fcLow * 100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2); + uint8_t clk[] = {8,16,32,40,50,64,100,128,0}; + uint16_t rfLens[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t rfCnts[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + uint8_t rfLensFnd = 0; + uint8_t lastFCcnt = 0; + uint16_t fcCounter = 0; + uint16_t rfCounter = 0; + uint8_t firstBitFnd = 0; + size_t i; + uint8_t fcTol = ((fcHigh * 100 - fcLow * 100)/2 + 50)/100; //(uint8_t)(0.5+(float)(fcHigh-fcLow)/2); - // prime i to first peak / up transition - for (i = 160; i < size-20; i++) - if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]) - break; + // prime i to first peak / up transition + for (i = 160; i < size-20; i++) + if (bits[i] > bits[i-1] && bits[i] >= bits[i+1]) + break; - for (; i < size-20; i++){ - fcCounter++; - rfCounter++; + for (; i < size-20; i++){ + fcCounter++; + rfCounter++; - if (bits[i] <= bits[i-1] || bits[i] < bits[i+1]) - continue; - // else new peak - // if we got less than the small fc + tolerance then set it to the small fc - // if it is inbetween set it to the last counter - if (fcCounter < fcHigh && fcCounter > fcLow) - fcCounter = lastFCcnt; - else if (fcCounter < fcLow+fcTol) - fcCounter = fcLow; - else //set it to the large fc - fcCounter = fcHigh; + if (bits[i] <= bits[i-1] || bits[i] < bits[i+1]) + continue; + // else new peak + // if we got less than the small fc + tolerance then set it to the small fc + // if it is inbetween set it to the last counter + if (fcCounter < fcHigh && fcCounter > fcLow) + fcCounter = lastFCcnt; + else if (fcCounter < fcLow+fcTol) + fcCounter = fcLow; + else //set it to the large fc + fcCounter = fcHigh; - //look for bit clock (rf/xx) - if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){ - //not the same size as the last wave - start of new bit sequence - if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit - for (int ii=0; ii<15; ii++){ - if (rfLens[ii] >= (rfCounter-4) && rfLens[ii] <= (rfCounter+4)){ - rfCnts[ii]++; - rfCounter = 0; - break; - } - } - if (rfCounter > 0 && rfLensFnd < 15){ - //prnt("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); - rfCnts[rfLensFnd]++; - rfLens[rfLensFnd++] = rfCounter; - } - } else { - *firstClockEdge = i; - firstBitFnd++; - } - rfCounter = 0; - lastFCcnt = fcCounter; - } - fcCounter = 0; - } - uint8_t rfHighest = 15, rfHighest2 = 15, rfHighest3 = 15; + //look for bit clock (rf/xx) + if ((fcCounter < lastFCcnt || fcCounter > lastFCcnt)){ + //not the same size as the last wave - start of new bit sequence + if (firstBitFnd > 1){ //skip first wave change - probably not a complete bit + for (int ii=0; ii<15; ii++){ + if (rfLens[ii] >= (rfCounter-4) && rfLens[ii] <= (rfCounter+4)){ + rfCnts[ii]++; + rfCounter = 0; + break; + } + } + if (rfCounter > 0 && rfLensFnd < 15){ + //prnt("DEBUG: rfCntr %d, fcCntr %d",rfCounter,fcCounter); + rfCnts[rfLensFnd]++; + rfLens[rfLensFnd++] = rfCounter; + } + } else { + *firstClockEdge = i; + firstBitFnd++; + } + rfCounter = 0; + lastFCcnt = fcCounter; + } + fcCounter = 0; + } + uint8_t rfHighest = 15, rfHighest2 = 15, rfHighest3 = 15; - for (i=0; i<15; i++){ - //get highest 2 RF values (might need to get more values to compare or compare all?) - if (rfCnts[i] > rfCnts[rfHighest]){ - rfHighest3 = rfHighest2; - rfHighest2 = rfHighest; - rfHighest = i; - } else if(rfCnts[i] > rfCnts[rfHighest2]){ - rfHighest3 = rfHighest2; - rfHighest2 = i; - } else if(rfCnts[i] > rfCnts[rfHighest3]){ - rfHighest3 = i; - } - if (g_debugMode == 2) - prnt("DEBUG FSK: RF %d, cnts %d", rfLens[i], rfCnts[i]); - } - // set allowed clock remainder tolerance to be 1 large field clock length+1 - // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off - uint8_t tol1 = fcHigh+1; + for (i=0; i<15; i++){ + //get highest 2 RF values (might need to get more values to compare or compare all?) + if (rfCnts[i] > rfCnts[rfHighest]){ + rfHighest3 = rfHighest2; + rfHighest2 = rfHighest; + rfHighest = i; + } else if(rfCnts[i] > rfCnts[rfHighest2]){ + rfHighest3 = rfHighest2; + rfHighest2 = i; + } else if(rfCnts[i] > rfCnts[rfHighest3]){ + rfHighest3 = i; + } + if (g_debugMode == 2) + prnt("DEBUG FSK: RF %d, cnts %d", rfLens[i], rfCnts[i]); + } + // set allowed clock remainder tolerance to be 1 large field clock length+1 + // we could have mistakenly made a 9 a 10 instead of an 8 or visa versa so rfLens could be 1 FC off + uint8_t tol1 = fcHigh+1; - if (g_debugMode == 2) - prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d", rfLens[rfHighest], rfLens[rfHighest2], rfLens[rfHighest3]); + if (g_debugMode == 2) + prnt("DEBUG FSK: most counted rf values: 1 %d, 2 %d, 3 %d", rfLens[rfHighest], rfLens[rfHighest2], rfLens[rfHighest3]); - // loop to find the highest clock that has a remainder less than the tolerance - // compare samples counted divided by - // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less) - int m = 7; - for (; m >= 2; m--){ - if (rfLens[rfHighest] % clk[m] < tol1 || rfLens[rfHighest] % clk[m] > clk[m]-tol1){ - if (rfLens[rfHighest2] % clk[m] < tol1 || rfLens[rfHighest2] % clk[m] > clk[m]-tol1){ - if (rfLens[rfHighest3] % clk[m] < tol1 || rfLens[rfHighest3] % clk[m] > clk[m]-tol1){ - if (g_debugMode == 2) - prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance", clk[m]); - break; - } - } - } - } + // loop to find the highest clock that has a remainder less than the tolerance + // compare samples counted divided by + // test 128 down to 32 (shouldn't be possible to have fc/10 & fc/8 and rf/16 or less) + int m = 7; + for (; m >= 2; m--){ + if (rfLens[rfHighest] % clk[m] < tol1 || rfLens[rfHighest] % clk[m] > clk[m]-tol1){ + if (rfLens[rfHighest2] % clk[m] < tol1 || rfLens[rfHighest2] % clk[m] > clk[m]-tol1){ + if (rfLens[rfHighest3] % clk[m] < tol1 || rfLens[rfHighest3] % clk[m] > clk[m]-tol1){ + if (g_debugMode == 2) + prnt("DEBUG FSK: clk %d divides into the 3 most rf values within tolerance", clk[m]); + break; + } + } + } + } - if (m < 2) return 0; // oops we went too far + if (m < 2) return 0; // oops we went too far - return clk[m]; + return clk[m]; } @@ -1124,145 +1124,145 @@ uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, // look for Sequence Terminator - should be pulses of clk*(1 or 2), clk*2, clk*(1.5 or 2), by idx we mean graph position index... bool findST(int *stStopLoc, int *stStartIdx, int lowToLowWaveLen[], int highToLowWaveLen[], int clk, int tol, int buffSize, size_t *i) { - if (buffSize < *i+4) return false; + if (buffSize < *i+4) return false; - for (; *i < buffSize - 4; *i+=1) { - *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now... - if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior - if (lowToLowWaveLen[*i+1] >= clk*2-tol && lowToLowWaveLen[*i+1] <= clk*2+tol && highToLowWaveLen[*i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2 - if (lowToLowWaveLen[*i+2] >= (clk*3)/2-tol && lowToLowWaveLen[*i+2] <= clk*2+tol && highToLowWaveLen[*i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave - if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit - *stStopLoc = *i + 3; - return true; - } - } - } - } - } - return false; + for (; *i < buffSize - 4; *i+=1) { + *stStartIdx += lowToLowWaveLen[*i]; //caution part of this wave may be data and part may be ST.... to be accounted for in main function for now... + if (lowToLowWaveLen[*i] >= clk*1-tol && lowToLowWaveLen[*i] <= (clk*2)+tol && highToLowWaveLen[*i] < clk+tol) { //1 to 2 clocks depending on 2 bits prior + if (lowToLowWaveLen[*i+1] >= clk*2-tol && lowToLowWaveLen[*i+1] <= clk*2+tol && highToLowWaveLen[*i+1] > clk*3/2-tol) { //2 clocks and wave size is 1 1/2 + if (lowToLowWaveLen[*i+2] >= (clk*3)/2-tol && lowToLowWaveLen[*i+2] <= clk*2+tol && highToLowWaveLen[*i+2] > clk-tol) { //1 1/2 to 2 clocks and at least one full clock wave + if (lowToLowWaveLen[*i+3] >= clk*1-tol && lowToLowWaveLen[*i+3] <= clk*2+tol) { //1 to 2 clocks for end of ST + first bit + *stStopLoc = *i + 3; + return true; + } + } + } + } + } + return false; } //by marshmellow //attempt to identify a Sequence Terminator in ASK modulated raw wave bool DetectST(uint8_t *buffer, size_t *size, int *foundclock, size_t *ststart, size_t *stend) { - size_t bufsize = *size; - //need to loop through all samples and identify our clock, look for the ST pattern - int clk = 0; - int tol = 0; - int j=0, high, low, skip=0, start=0, end=0, minClk=255; - size_t i = 0; - //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow] - int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured - int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed... - //size_t testsize = (bufsize < 512) ? bufsize : 512; - int phaseoff = 0; - high = low = 128; - memset(tmpbuff, 0, sizeof(tmpbuff)); - memset(waveLen, 0, sizeof(waveLen)); + size_t bufsize = *size; + //need to loop through all samples and identify our clock, look for the ST pattern + int clk = 0; + int tol = 0; + int j=0, high, low, skip=0, start=0, end=0, minClk=255; + size_t i = 0; + //probably should malloc... || test if memory is available ... handle device side? memory danger!!! [marshmellow] + int tmpbuff[bufsize / LOWEST_DEFAULT_CLOCK]; // low to low wave count //guess rf/32 clock, if click is smaller we will only have room for a fraction of the samples captured + int waveLen[bufsize / LOWEST_DEFAULT_CLOCK]; // high to low wave count //if clock is larger then we waste memory in array size that is not needed... + //size_t testsize = (bufsize < 512) ? bufsize : 512; + int phaseoff = 0; + high = low = 128; + memset(tmpbuff, 0, sizeof(tmpbuff)); + memset(waveLen, 0, sizeof(waveLen)); - if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false; - // set clock - might be able to get this externally and remove this work... - clk = getClosestClock(minClk); - // clock not found - ERROR - if (!clk) { - if (g_debugMode == 2) prnt("DEBUG STT: clock not found - quitting"); - return false; - } - *foundclock = clk; + if (!loadWaveCounters(buffer, bufsize, tmpbuff, waveLen, &j, &skip, &minClk, &high, &low)) return false; + // set clock - might be able to get this externally and remove this work... + clk = getClosestClock(minClk); + // clock not found - ERROR + if (!clk) { + if (g_debugMode == 2) prnt("DEBUG STT: clock not found - quitting"); + return false; + } + *foundclock = clk; - tol = clk/8; - if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) { - // first ST not found - ERROR - if (g_debugMode == 2) prnt("DEBUG STT: first STT not found - quitting"); - return false; - } else { - if (g_debugMode == 2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j); - } - if (waveLen[i+2] > clk*1+tol) - phaseoff = 0; - else - phaseoff = clk/2; + tol = clk/8; + if (!findST(&start, &skip, tmpbuff, waveLen, clk, tol, j, &i)) { + // first ST not found - ERROR + if (g_debugMode == 2) prnt("DEBUG STT: first STT not found - quitting"); + return false; + } else { + if (g_debugMode == 2) prnt("DEBUG STT: first STT found at wave: %i, skip: %i, j=%i", start, skip, j); + } + if (waveLen[i+2] > clk*1+tol) + phaseoff = 0; + else + phaseoff = clk/2; - // skip over the remainder of ST - skip += clk*7/2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point + // skip over the remainder of ST + skip += clk*7/2; //3.5 clocks from tmpbuff[i] = end of st - also aligns for ending point - // now do it again to find the end - int dummy1 = 0; - end = skip; - i += 3; - if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) { - //didn't find second ST - ERROR - if (g_debugMode == 2) prnt("DEBUG STT: second STT not found - quitting"); - return false; - } - end -= phaseoff; - if (g_debugMode == 2) prnt("DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d", skip, end, end-skip, clk, (end-skip)/clk, phaseoff); - //now begin to trim out ST so we can use normal demod cmds - start = skip; - size_t datalen = end - start; - // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock - if ( clk - (datalen % clk) <= clk/8) { - // padd the amount off - could be problematic... but shouldn't happen often - datalen += clk - (datalen % clk); - } else if ( (datalen % clk) <= clk/8 ) { - // padd the amount off - could be problematic... but shouldn't happen often - datalen -= datalen % clk; - } else { - if (g_debugMode == 2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk); - return false; - } - // if datalen is less than one t55xx block - ERROR - if (datalen/clk < 8*4) { - if (g_debugMode == 2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting"); - return false; - } - size_t dataloc = start; - if (buffer[dataloc-(clk*4)-(clk/4)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) { - //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start - for ( i=0; i <= (clk/4); ++i ) { - if ( buffer[dataloc - (clk*4) - i] <= low ) { - dataloc -= i; - break; - } - } - } + // now do it again to find the end + int dummy1 = 0; + end = skip; + i += 3; + if (!findST(&dummy1, &end, tmpbuff, waveLen, clk, tol, j, &i)) { + //didn't find second ST - ERROR + if (g_debugMode == 2) prnt("DEBUG STT: second STT not found - quitting"); + return false; + } + end -= phaseoff; + if (g_debugMode == 2) prnt("DEBUG STT: start of data: %d end of data: %d, datalen: %d, clk: %d, bits: %d, phaseoff: %d", skip, end, end-skip, clk, (end-skip)/clk, phaseoff); + //now begin to trim out ST so we can use normal demod cmds + start = skip; + size_t datalen = end - start; + // check validity of datalen (should be even clock increments) - use a tolerance of up to 1/8th a clock + if ( clk - (datalen % clk) <= clk/8) { + // padd the amount off - could be problematic... but shouldn't happen often + datalen += clk - (datalen % clk); + } else if ( (datalen % clk) <= clk/8 ) { + // padd the amount off - could be problematic... but shouldn't happen often + datalen -= datalen % clk; + } else { + if (g_debugMode == 2) prnt("DEBUG STT: datalen not divisible by clk: %u %% %d = %d - quitting", datalen, clk, datalen % clk); + return false; + } + // if datalen is less than one t55xx block - ERROR + if (datalen/clk < 8*4) { + if (g_debugMode == 2) prnt("DEBUG STT: datalen is less than 1 full t55xx block - quitting"); + return false; + } + size_t dataloc = start; + if (buffer[dataloc-(clk*4)-(clk/4)] <= low && buffer[dataloc] <= low && buffer[dataloc-(clk*4)] >= high) { + //we have low drift (and a low just before the ST and a low just after the ST) - compensate by backing up the start + for ( i=0; i <= (clk/4); ++i ) { + if ( buffer[dataloc - (clk*4) - i] <= low ) { + dataloc -= i; + break; + } + } + } - size_t newloc = 0; - i=0; - if (g_debugMode == 2) prnt("DEBUG STT: Starting STT trim - start: %d, datalen: %d ",dataloc, datalen); - bool firstrun = true; - // warning - overwriting buffer given with raw wave data with ST removed... - while ( dataloc < bufsize-(clk/2) ) { - //compensate for long high at end of ST not being high due to signal loss... (and we cut out the start of wave high part) - if (buffer[dataloc]low && buffer[dataloc+clk/4]low) { - for(i=0; i < clk/2-tol; ++i) { - buffer[dataloc+i] = high+5; - } - } //test for small spike outlier (high between two lows) in the case of very strong waves - if (buffer[dataloc] > low && buffer[dataloc+clk/4] <= low) { - for(i=0; i < clk/4; ++i) { - buffer[dataloc+i] = buffer[dataloc+clk/4]; - } - } - if (firstrun) { - *stend = dataloc; - *ststart = dataloc-(clk*4); - firstrun=false; - } - for (i=0; ilow && buffer[dataloc+clk/4]low) { + for(i=0; i < clk/2-tol; ++i) { + buffer[dataloc+i] = high+5; + } + } //test for small spike outlier (high between two lows) in the case of very strong waves + if (buffer[dataloc] > low && buffer[dataloc+clk/4] <= low) { + for(i=0; i < clk/4; ++i) { + buffer[dataloc+i] = buffer[dataloc+clk/4]; + } + } + if (firstrun) { + *stend = dataloc; + *ststart = dataloc-(clk*4); + firstrun=false; + } + for (i=0; i 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) { - errCnt++; - bits[bitCnt++] = 7; - continue; - } - bits[bitCnt++] = bits[i] ^ bits[i+1] ^ invert; + //find alignment, needs 4 1s or 0s to properly align + for (i=1; i < *size-1; i++) { + alignCnt = (bits[i] == curBit) ? alignCnt+1 : 0; + curBit = bits[i]; + if (alignCnt == 4) break; + } + // for now error if alignment not found. later add option to run it with multiple offsets... + if (alignCnt != 4) { + if (g_debugMode) prnt("ERROR MillerDecode: alignment not found so either your bits is not miller or your data does not have a 101 in it"); + return -1; + } + alignedIdx = (i-1) % 2; + for (i = alignedIdx; i < *size-3; i += 2) { + halfClkErr = (uint8_t)((halfClkErr << 1 | bits[i]) & 0xFF); + if ( (halfClkErr & 0x7) == 5 || (halfClkErr & 0x7) == 2 || (i > 2 && (halfClkErr & 0x7) == 0) || (halfClkErr & 0x1F) == 0x1F) { + errCnt++; + bits[bitCnt++] = 7; + continue; + } + bits[bitCnt++] = bits[i] ^ bits[i+1] ^ invert; - if (bitCnt > MaxBits) break; - } - *size = bitCnt; - return errCnt; + if (bitCnt > MaxBits) break; + } + *size = bitCnt; + return errCnt; } //by marshmellow @@ -1309,40 +1309,40 @@ int millerRawDecode(uint8_t *bits, size_t *size, int invert) { //check for phase errors - should never have 111 or 000 should be 01001011 or 10110100 for 1010 //decodes biphase or if inverted it is AKA conditional dephase encoding AKA differential manchester encoding int BiphaseRawDecode(uint8_t *bits, size_t *size, int *offset, int invert) { - //sanity check - if (*size < 51) return -1; + //sanity check + if (*size < 51) return -1; - uint16_t bitnum = 0; - uint16_t errCnt = 0; - size_t i = *offset; - uint16_t maxbits = 512; + uint16_t bitnum = 0; + uint16_t errCnt = 0; + size_t i = *offset; + uint16_t maxbits = 512; - //check for phase change faults - skip one sample if faulty - bool offsetA = true, offsetB = true; - for (; i < *offset+48; i += 2){ - if (bits[i+1] == bits[i+2]) offsetA = false; - if (bits[i+2] == bits[i+3]) offsetB = false; - } - if (!offsetA && offsetB) ++*offset; + //check for phase change faults - skip one sample if faulty + bool offsetA = true, offsetB = true; + for (; i < *offset+48; i += 2){ + if (bits[i+1] == bits[i+2]) offsetA = false; + if (bits[i+2] == bits[i+3]) offsetB = false; + } + if (!offsetA && offsetB) ++*offset; - for (i = *offset; i < *size-3; i += 2){ - //check for phase error - if (bits[i+1] == bits[i+2]) { - bits[bitnum++] = 7; - errCnt++; - } - if((bits[i]==1 && bits[i+1]==0) || (bits[i]==0 && bits[i+1]==1)){ - bits[bitnum++] = 1 ^ invert; - } else if((bits[i]==0 && bits[i+1]==0) || (bits[i]==1 && bits[i+1]==1)){ - bits[bitnum++] = invert; - } else { - bits[bitnum++] = 7; - errCnt++; - } - if (bitnum > maxbits) break; - } - *size = bitnum; - return errCnt; + for (i = *offset; i < *size-3; i += 2){ + //check for phase error + if (bits[i+1] == bits[i+2]) { + bits[bitnum++] = 7; + errCnt++; + } + if((bits[i]==1 && bits[i+1]==0) || (bits[i]==0 && bits[i+1]==1)){ + bits[bitnum++] = 1 ^ invert; + } else if((bits[i]==0 && bits[i+1]==0) || (bits[i]==1 && bits[i+1]==1)){ + bits[bitnum++] = invert; + } else { + bits[bitnum++] = 7; + errCnt++; + } + if (bitnum > maxbits) break; + } + *size = bitnum; + return errCnt; } //by marshmellow @@ -1350,433 +1350,433 @@ int BiphaseRawDecode(uint8_t *bits, size_t *size, int *offset, int invert) { //run through 2 times and take least errCnt int manrawdecode(uint8_t *bits, size_t *size, uint8_t invert, uint8_t *alignPos){ - // sanity check - if (*size < 16) return -1; + // sanity check + if (*size < 16) return -1; - int errCnt = 0, bestErr = 1000; - uint16_t bitnum = 0, maxBits = 512, bestRun = 0; - size_t i, k; + int errCnt = 0, bestErr = 1000; + uint16_t bitnum = 0, maxBits = 512, bestRun = 0; + size_t i, k; - //find correct start position [alignment] - for (k = 0; k < 2; ++k){ - for (i = k; i < *size-3; i += 2) { - if (bits[i] == bits[i+1]) - errCnt++; - } - if (bestErr > errCnt){ - bestErr = errCnt; - bestRun = k; - } - errCnt = 0; - } - *alignPos = bestRun; - //decode - for (i = bestRun; i < *size-3; i += 2){ - if (bits[i] == 1 && (bits[i+1] == 0)){ - bits[bitnum++] = invert; - } else if ((bits[i] == 0) && bits[i+1] == 1){ - bits[bitnum++] = invert^1; - } else { - bits[bitnum++] = 7; - } - if (bitnum > maxBits) break; - } - *size = bitnum; - return bestErr; + //find correct start position [alignment] + for (k = 0; k < 2; ++k){ + for (i = k; i < *size-3; i += 2) { + if (bits[i] == bits[i+1]) + errCnt++; + } + if (bestErr > errCnt){ + bestErr = errCnt; + bestRun = k; + } + errCnt = 0; + } + *alignPos = bestRun; + //decode + for (i = bestRun; i < *size-3; i += 2){ + if (bits[i] == 1 && (bits[i+1] == 0)){ + bits[bitnum++] = invert; + } else if ((bits[i] == 0) && bits[i+1] == 1){ + bits[bitnum++] = invert^1; + } else { + bits[bitnum++] = 7; + } + if (bitnum > maxBits) break; + } + *size = bitnum; + return bestErr; } //by marshmellow //demodulates strong heavily clipped samples //RETURN: num of errors. if 0, is ok. int cleanAskRawDemod(uint8_t *bits, size_t *size, int clk, int invert, int high, int low, int *startIdx) { - *startIdx = 0; - size_t bitCnt = 0, smplCnt = 1, errCnt = 0, pos = 0; - uint8_t cl_4 = clk / 4; - uint8_t cl_2 = clk / 2; - bool waveHigh = true; + *startIdx = 0; + size_t bitCnt = 0, smplCnt = 1, errCnt = 0, pos = 0; + uint8_t cl_4 = clk / 4; + uint8_t cl_2 = clk / 2; + bool waveHigh = true; - getNextHigh(bits, *size, high, &pos); + getNextHigh(bits, *size, high, &pos); - // sample counts, like clock = 32.. it tries to find 32/4 = 8, 32/2 = 16 - for (size_t i = pos; i < *size; i++){ - if (bits[i] >= high && waveHigh){ - smplCnt++; - } else if (bits[i] <= low && !waveHigh){ - smplCnt++; - } else { //transition - if ((bits[i] >= high && !waveHigh) || (bits[i] <= low && waveHigh)){ + // sample counts, like clock = 32.. it tries to find 32/4 = 8, 32/2 = 16 + for (size_t i = pos; i < *size; i++){ + if (bits[i] >= high && waveHigh){ + smplCnt++; + } else if (bits[i] <= low && !waveHigh){ + smplCnt++; + } else { //transition + if ((bits[i] >= high && !waveHigh) || (bits[i] <= low && waveHigh)){ - // 32-8-1 = 23 - // 32+8+1 = 41 - if (smplCnt > clk - cl_4 - 1) { //full clock + // 32-8-1 = 23 + // 32+8+1 = 41 + if (smplCnt > clk - cl_4 - 1) { //full clock - if (smplCnt > clk + cl_4 + 1) { //too many samples - errCnt++; - if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error FULL at: %u [%u]", i, smplCnt); - bits[bitCnt++] = 7; - } else if (waveHigh) { - bits[bitCnt++] = invert; - bits[bitCnt++] = invert; - } else if (!waveHigh) { - bits[bitCnt++] = invert ^ 1; - bits[bitCnt++] = invert ^ 1; - } - if (*startIdx == 0) - *startIdx = i - clk; - waveHigh = !waveHigh; - smplCnt = 0; + if (smplCnt > clk + cl_4 + 1) { //too many samples + errCnt++; + if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error FULL at: %u [%u]", i, smplCnt); + bits[bitCnt++] = 7; + } else if (waveHigh) { + bits[bitCnt++] = invert; + bits[bitCnt++] = invert; + } else if (!waveHigh) { + bits[bitCnt++] = invert ^ 1; + bits[bitCnt++] = invert ^ 1; + } + if (*startIdx == 0) + *startIdx = i - clk; + waveHigh = !waveHigh; + smplCnt = 0; - // 16-8-1 = 7 - } else if (smplCnt > cl_2 - cl_4 - 1) { //half clock + // 16-8-1 = 7 + } else if (smplCnt > cl_2 - cl_4 - 1) { //half clock - if (smplCnt > cl_2 + cl_4 + 1) { //too many samples - errCnt++; - if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error HALF at: %u [%u]", i, smplCnt); - bits[bitCnt++] = 7; - } + if (smplCnt > cl_2 + cl_4 + 1) { //too many samples + errCnt++; + if (g_debugMode == 2) prnt("DEBUG ASK: cleanAskRawDemod ASK Modulation Error HALF at: %u [%u]", i, smplCnt); + bits[bitCnt++] = 7; + } - if (waveHigh) { - bits[bitCnt++] = invert; - } else if (!waveHigh) { - bits[bitCnt++] = invert ^ 1; - } - if (*startIdx == 0) - *startIdx = i - cl_2; - waveHigh = !waveHigh; - smplCnt = 0; - } else { - smplCnt++; - //transition bit oops - } - } else { //haven't hit new high or new low yet - smplCnt++; - } - } - } - *size = bitCnt; - return errCnt; + if (waveHigh) { + bits[bitCnt++] = invert; + } else if (!waveHigh) { + bits[bitCnt++] = invert ^ 1; + } + if (*startIdx == 0) + *startIdx = i - cl_2; + waveHigh = !waveHigh; + smplCnt = 0; + } else { + smplCnt++; + //transition bit oops + } + } else { //haven't hit new high or new low yet + smplCnt++; + } + } + } + *size = bitCnt; + return errCnt; } //by marshmellow //attempts to demodulate ask modulations, askType == 0 for ask/raw, askType==1 for ask/manchester int askdemod_ext(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType, int *startIdx) { - if (*size == 0) return -1; + if (*size == 0) return -1; - int start = DetectASKClock(bits, *size, clk, maxErr); - if (*clk == 0 || start < 0) return -3; + int start = DetectASKClock(bits, *size, clk, maxErr); + if (*clk == 0 || start < 0) return -3; - if (*invert != 1) *invert = 0; + if (*invert != 1) *invert = 0; - // amplify signal data. - // ICEMAN todo, - if (amp == 1) askAmp(bits, *size); + // amplify signal data. + // ICEMAN todo, + if (amp == 1) askAmp(bits, *size); - if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, beststart %d, amp %d", *clk, start, amp); + if (g_debugMode == 2) prnt("DEBUG ASK: clk %d, beststart %d, amp %d", *clk, start, amp); - //start pos from detect ask clock is 1/2 clock offset - // NOTE: can be negative (demod assumes rest of wave was there) - *startIdx = start - (*clk/2); - uint16_t initLoopMax = 1024; - if (initLoopMax > *size) initLoopMax = *size; + //start pos from detect ask clock is 1/2 clock offset + // NOTE: can be negative (demod assumes rest of wave was there) + *startIdx = start - (*clk/2); + uint16_t initLoopMax = 1024; + if (initLoopMax > *size) initLoopMax = *size; - // just noise - no super good detection. good enough - if (signalprop.isnoise) { - if (g_debugMode == 2) prnt("DEBUG askdemod_ext: just noise detected - aborting"); - return -2; - } + // just noise - no super good detection. good enough + if (signalprop.isnoise) { + if (g_debugMode == 2) prnt("DEBUG askdemod_ext: just noise detected - aborting"); + return -2; + } - // Detect high and lows - //25% clip in case highs and lows aren't clipped [marshmellow] - int high, low; - //getHiLo(bits, initLoopMax, &high, &low, 75, 75); - getHiLo(&high, &low, 75, 75); + // Detect high and lows + //25% clip in case highs and lows aren't clipped [marshmellow] + int high, low; + //getHiLo(bits, initLoopMax, &high, &low, 75, 75); + getHiLo(&high, &low, 75, 75); - size_t errCnt = 0; - // if clean clipped waves detected run alternate demod - if (DetectCleanAskWave(bits, *size, high, low)) { + size_t errCnt = 0; + // if clean clipped waves detected run alternate demod + if (DetectCleanAskWave(bits, *size, high, low)) { - if (g_debugMode == 2) prnt("DEBUG ASK: Clean Wave Detected - using clean wave demod"); + if (g_debugMode == 2) prnt("DEBUG ASK: Clean Wave Detected - using clean wave demod"); - errCnt = cleanAskRawDemod(bits, size, *clk, *invert, high, low, startIdx); + errCnt = cleanAskRawDemod(bits, size, *clk, *invert, high, low, startIdx); - if (askType) { //ask/manchester - uint8_t alignPos = 0; - errCnt = manrawdecode(bits, size, 0, &alignPos); - *startIdx += *clk/2 * alignPos; + if (askType) { //ask/manchester + uint8_t alignPos = 0; + errCnt = manrawdecode(bits, size, 0, &alignPos); + *startIdx += *clk/2 * alignPos; - if (g_debugMode) - prnt("DEBUG: (askdemod_ext) CLEAN: startIdx %i, alignPos %u", *startIdx, alignPos); - } - return errCnt; - } + if (g_debugMode) + prnt("DEBUG: (askdemod_ext) CLEAN: startIdx %i, alignPos %u", *startIdx, alignPos); + } + return errCnt; + } - if (g_debugMode) prnt("DEBUG: (askdemod_ext) Weak wave detected: startIdx %i", *startIdx); + if (g_debugMode) prnt("DEBUG: (askdemod_ext) Weak wave detected: startIdx %i", *startIdx); - int lastBit; //set first clock check - can go negative - size_t i, bitnum = 0; //output counter - uint8_t midBit = 0; - uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave - if (*clk <= 32) tol = 1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely - size_t MaxBits = 3072; //max bits to collect - lastBit = start - *clk; + int lastBit; //set first clock check - can go negative + size_t i, bitnum = 0; //output counter + uint8_t midBit = 0; + uint8_t tol = 0; //clock tolerance adjust - waves will be accepted as within the clock if they fall + or - this value + clock from last valid wave + if (*clk <= 32) tol = 1; //clock tolerance may not be needed anymore currently set to + or - 1 but could be increased for poor waves or removed entirely + size_t MaxBits = 3072; //max bits to collect + lastBit = start - *clk; - for (i = start; i < *size; ++i) { - if (i - lastBit >= *clk - tol){ - if (bits[i] >= high) { - bits[bitnum++] = *invert; - } else if (bits[i] <= low) { - bits[bitnum++] = *invert ^ 1; - } else if (i-lastBit >= *clk + tol) { - if (bitnum > 0) { - if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) Modulation Error at: %u", i); - bits[bitnum++] = 7; - errCnt++; - } - } else { //in tolerance - looking for peak - continue; - } - midBit = 0; - lastBit += *clk; - } else if (i-lastBit >= (*clk/2 - tol) && !midBit && !askType){ - if (bits[i] >= high) { - bits[bitnum++] = *invert; - } else if (bits[i] <= low) { - bits[bitnum++] = *invert ^ 1; - } else if (i-lastBit >= *clk/2 + tol) { - bits[bitnum] = bits[bitnum-1]; - bitnum++; - } else { //in tolerance - looking for peak - continue; - } - midBit = 1; - } - if (bitnum >= MaxBits) break; - } - *size = bitnum; - return errCnt; + for (i = start; i < *size; ++i) { + if (i - lastBit >= *clk - tol){ + if (bits[i] >= high) { + bits[bitnum++] = *invert; + } else if (bits[i] <= low) { + bits[bitnum++] = *invert ^ 1; + } else if (i-lastBit >= *clk + tol) { + if (bitnum > 0) { + if (g_debugMode == 2) prnt("DEBUG: (askdemod_ext) Modulation Error at: %u", i); + bits[bitnum++] = 7; + errCnt++; + } + } else { //in tolerance - looking for peak + continue; + } + midBit = 0; + lastBit += *clk; + } else if (i-lastBit >= (*clk/2 - tol) && !midBit && !askType){ + if (bits[i] >= high) { + bits[bitnum++] = *invert; + } else if (bits[i] <= low) { + bits[bitnum++] = *invert ^ 1; + } else if (i-lastBit >= *clk/2 + tol) { + bits[bitnum] = bits[bitnum-1]; + bitnum++; + } else { //in tolerance - looking for peak + continue; + } + midBit = 1; + } + if (bitnum >= MaxBits) break; + } + *size = bitnum; + return errCnt; } int askdemod(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType) { - int start = 0; - return askdemod_ext(bits, size, clk, invert, maxErr, amp, askType, &start); + int start = 0; + return askdemod_ext(bits, size, clk, invert, maxErr, amp, askType, &start); } // by marshmellow - demodulate NRZ wave - requires a read with strong signal // peaks invert bit (high=1 low=0) each clock cycle = 1 bit determined by last peak int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx) { - if (signalprop.isnoise) return -1; + if (signalprop.isnoise) return -1; - size_t clkStartIdx = 0; - *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx); - if (*clk == 0) return -2; + size_t clkStartIdx = 0; + *clk = DetectNRZClock(dest, *size, *clk, &clkStartIdx); + if (*clk == 0) return -2; - size_t i, gLen = 4096; - if (gLen > *size) - gLen = *size-20; + size_t i, gLen = 4096; + if (gLen > *size) + gLen = *size-20; - // just noise - no super good detection. good enough - if (signalprop.isnoise) { - if (g_debugMode == 2) prnt("DEBUG nrzRawDemod: just noise detected - quitting"); - return -3; - } + // just noise - no super good detection. good enough + if (signalprop.isnoise) { + if (g_debugMode == 2) prnt("DEBUG nrzRawDemod: just noise detected - quitting"); + return -3; + } - int high, low; - //getHiLo(dest, gLen, &high, &low, 75, 75); - getHiLo(&high, &low, 75, 75); - getHiLo(&high, &low, 75, 75); + int high, low; + //getHiLo(dest, gLen, &high, &low, 75, 75); + getHiLo(&high, &low, 75, 75); + getHiLo(&high, &low, 75, 75); - uint8_t bit=0; - //convert wave samples to 1's and 0's - for(i=20; i < *size-20; i++){ - if (dest[i] >= high) bit = 1; - if (dest[i] <= low) bit = 0; - dest[i] = bit; - } - //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit) - size_t lastBit = 0; - size_t numBits = 0; - for(i=21; i < *size-20; i++) { - //if transition detected or large number of same bits - store the passed bits - if (dest[i] != dest[i-1] || (i-lastBit) == (10 * *clk)) { - memset(dest+numBits, dest[i-1] ^ *invert, (i - lastBit + (*clk/4)) / *clk); - numBits += (i - lastBit + (*clk/4)) / *clk; - if (lastBit == 0) { - *startIdx = i - (numBits * *clk); - if (g_debugMode == 2) prnt("DEBUG NRZ: startIdx %i", *startIdx); - } - lastBit = i-1; - } - } - *size = numBits; - return 0; + uint8_t bit=0; + //convert wave samples to 1's and 0's + for(i=20; i < *size-20; i++){ + if (dest[i] >= high) bit = 1; + if (dest[i] <= low) bit = 0; + dest[i] = bit; + } + //now demod based on clock (rf/32 = 32 1's for one 1 bit, 32 0's for one 0 bit) + size_t lastBit = 0; + size_t numBits = 0; + for(i=21; i < *size-20; i++) { + //if transition detected or large number of same bits - store the passed bits + if (dest[i] != dest[i-1] || (i-lastBit) == (10 * *clk)) { + memset(dest+numBits, dest[i-1] ^ *invert, (i - lastBit + (*clk/4)) / *clk); + numBits += (i - lastBit + (*clk/4)) / *clk; + if (lastBit == 0) { + *startIdx = i - (numBits * *clk); + if (g_debugMode == 2) prnt("DEBUG NRZ: startIdx %i", *startIdx); + } + lastBit = i-1; + } + } + *size = numBits; + return 0; } //translate wave to 11111100000 (1 for each short wave [higher freq] 0 for each long wave [lower freq]) size_t fsk_wave_demod(uint8_t *dest, size_t size, uint8_t fchigh, uint8_t fclow, int *startIdx) { - if ( size < 1024 ) return 0; // not enough samples + if ( size < 1024 ) return 0; // not enough samples - if (fchigh == 0) fchigh = 10; - if (fclow == 0) fclow = 8; + if (fchigh == 0) fchigh = 10; + if (fclow == 0) fclow = 8; - //set the threshold close to 0 (graph) or 128 std to avoid static - size_t preLastSample = 0; - size_t LastSample = 0; - size_t currSample = 0; - size_t last_transition = 0; - size_t idx = 1; - size_t numBits = 0; + //set the threshold close to 0 (graph) or 128 std to avoid static + size_t preLastSample = 0; + size_t LastSample = 0; + size_t currSample = 0; + size_t last_transition = 0; + size_t idx = 1; + size_t numBits = 0; - //find start of modulating data in trace - idx = findModStart(dest, size, fchigh); - // Need to threshold first sample - dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; + //find start of modulating data in trace + idx = findModStart(dest, size, fchigh); + // Need to threshold first sample + dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; - last_transition = idx; - idx++; + last_transition = idx; + idx++; - // Definition: cycles between consecutive lo-hi transitions - // Lets define some expected lengths. FSK1 is easier since it has bigger differences between. - // FSK1 8/5 - // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 - // 50/5 = 10 | 40/5 = 8 | 64/5 = 12 + // Definition: cycles between consecutive lo-hi transitions + // Lets define some expected lengths. FSK1 is easier since it has bigger differences between. + // FSK1 8/5 + // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 + // 50/5 = 10 | 40/5 = 8 | 64/5 = 12 - // FSK2 10/8 - // 50/10 = 5 | 40/10 = 4 | 64/10 = 6 - // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 + // FSK2 10/8 + // 50/10 = 5 | 40/10 = 4 | 64/10 = 6 + // 50/8 = 6 | 40/8 = 5 | 64/8 = 8 - // count cycles between consecutive lo-hi transitions, - // in practice due to noise etc we may end up with anywhere - // To allow fuzz would mean +-1 on expected cycle width. - // FSK1 8/5 - // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) - // 50/5 = 10 (9-11) | 40/5 = 8 (7-9) | 64/5 = 12 (11-13) + // count cycles between consecutive lo-hi transitions, + // in practice due to noise etc we may end up with anywhere + // To allow fuzz would mean +-1 on expected cycle width. + // FSK1 8/5 + // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) + // 50/5 = 10 (9-11) | 40/5 = 8 (7-9) | 64/5 = 12 (11-13) - // FSK2 10/8 - // 50/10 = 5 (4-6) | 40/10 = 4 (3-5) | 64/10 = 6 (5-7) - // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) - // - // It easy to see to the overgaping, but luckily we the group value also, like 1111000001111 - // to separate between which bit to demodulate to. + // FSK2 10/8 + // 50/10 = 5 (4-6) | 40/10 = 4 (3-5) | 64/10 = 6 (5-7) + // 50/8 = 6 (5-7) | 40/8 = 5 (4-6) | 64/8 = 8 (7-9) + // + // It easy to see to the overgaping, but luckily we the group value also, like 1111000001111 + // to separate between which bit to demodulate to. - // process: - // count width from 0-1 transition to 1-0. - // determine the width is withing FUZZ_min and FUZZ_max tolerances - // width should be divided with exp_one. i:e 6+7+6+2=21, 21/5 = 4, - // the 1-0 to 0-1 width should be divided with exp_zero. Ie: 3+5+6+7 = 21/6 = 3 + // process: + // count width from 0-1 transition to 1-0. + // determine the width is withing FUZZ_min and FUZZ_max tolerances + // width should be divided with exp_one. i:e 6+7+6+2=21, 21/5 = 4, + // the 1-0 to 0-1 width should be divided with exp_zero. Ie: 3+5+6+7 = 21/6 = 3 - for(; idx < size-20; idx++) { + for(; idx < size-20; idx++) { - // threshold current value - dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; + // threshold current value + dest[idx] = (dest[idx] < signalprop.mean) ? 0 : 1; - // Check for 0->1 transition - if (dest[idx-1] < dest[idx]) { - preLastSample = LastSample; - LastSample = currSample; - currSample = idx-last_transition; - if (currSample < (fclow-2)){ //0-5 = garbage noise (or 0-3) - //do nothing with extra garbage - } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves (or 3-6 = 5) - //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5) - if (numBits > 1 && LastSample > (fchigh-2) && (preLastSample < (fchigh-1))){ - dest[numBits-1]=1; - } - dest[numBits++]=1; + // Check for 0->1 transition + if (dest[idx-1] < dest[idx]) { + preLastSample = LastSample; + LastSample = currSample; + currSample = idx-last_transition; + if (currSample < (fclow-2)){ //0-5 = garbage noise (or 0-3) + //do nothing with extra garbage + } else if (currSample < (fchigh-1)) { //6-8 = 8 sample waves (or 3-6 = 5) + //correct previous 9 wave surrounded by 8 waves (or 6 surrounded by 5) + if (numBits > 1 && LastSample > (fchigh-2) && (preLastSample < (fchigh-1))){ + dest[numBits-1]=1; + } + dest[numBits++]=1; - if (numBits > 0 && *startIdx == 0) - *startIdx = idx - fclow; + if (numBits > 0 && *startIdx == 0) + *startIdx = idx - fclow; - } else if (currSample > (fchigh+1) && numBits < 3) { //12 + and first two bit = unusable garbage - //do nothing with beginning garbage and reset.. should be rare.. - numBits = 0; - } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's) - dest[numBits++]=1; - if (numBits > 0 && *startIdx == 0) { - *startIdx = idx - fclow; - } - } else { //9+ = 10 sample waves (or 6+ = 7) - dest[numBits++]=0; - if (numBits > 0 && *startIdx == 0) { - *startIdx = idx - fchigh; - } - } - last_transition = idx; - } - } - return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0 + } else if (currSample > (fchigh+1) && numBits < 3) { //12 + and first two bit = unusable garbage + //do nothing with beginning garbage and reset.. should be rare.. + numBits = 0; + } else if (currSample == (fclow+1) && LastSample == (fclow-1)) { // had a 7 then a 9 should be two 8's (or 4 then a 6 should be two 5's) + dest[numBits++]=1; + if (numBits > 0 && *startIdx == 0) { + *startIdx = idx - fclow; + } + } else { //9+ = 10 sample waves (or 6+ = 7) + dest[numBits++]=0; + if (numBits > 0 && *startIdx == 0) { + *startIdx = idx - fchigh; + } + } + last_transition = idx; + } + } + return numBits; //Actually, it returns the number of bytes, but each byte represents a bit: 1 or 0 } //translate 11111100000 to 10 //rfLen = clock, fchigh = larger field clock, fclow = smaller field clock size_t aggregate_bits(uint8_t *dest, size_t size, uint8_t clk, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *startIdx) { - uint8_t lastval = dest[0]; - size_t i = 0; - size_t numBits = 0; - uint32_t n = 1; - uint8_t hclk = clk/2; + uint8_t lastval = dest[0]; + size_t i = 0; + size_t numBits = 0; + uint32_t n = 1; + uint8_t hclk = clk/2; - for( i = 1; i < size; i++) { - n++; - if (dest[i] == lastval) continue; //skip until we hit a transition + for( i = 1; i < size; i++) { + n++; + if (dest[i] == lastval) continue; //skip until we hit a transition - //find out how many bits (n) we collected (use 1/2 clk tolerance) + //find out how many bits (n) we collected (use 1/2 clk tolerance) - if (dest[i-1] == 1) - //if lastval was 1, we have a 1->0 crossing - n = (n * fclow + hclk) / clk; - else - // 0->1 crossing - n = (n * fchigh + hclk) / clk; + if (dest[i-1] == 1) + //if lastval was 1, we have a 1->0 crossing + n = (n * fclow + hclk) / clk; + else + // 0->1 crossing + n = (n * fchigh + hclk) / clk; - if (n == 0) - n = 1; + if (n == 0) + n = 1; - //first transition - save startidx - if (numBits == 0) { - if (lastval == 1) { //high to low - *startIdx += (fclow * i) - (n*clk); - if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fclow*idx %i, n*clk %u", *startIdx, fclow*i, n*clk); - } else { - *startIdx += (fchigh * i) - (n*clk); - if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fchigh*idx %i, n*clk %u", *startIdx, fchigh*i, n*clk); - } - } + //first transition - save startidx + if (numBits == 0) { + if (lastval == 1) { //high to low + *startIdx += (fclow * i) - (n*clk); + if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fclow*idx %i, n*clk %u", *startIdx, fclow*i, n*clk); + } else { + *startIdx += (fchigh * i) - (n*clk); + if (g_debugMode == 2) prnt("DEBUG (aggregate_bits) FSK startIdx %i, fchigh*idx %i, n*clk %u", *startIdx, fchigh*i, n*clk); + } + } - //add to our destination the bits we collected - memset(dest+numBits, dest[i-1] ^ invert , n); + //add to our destination the bits we collected + memset(dest+numBits, dest[i-1] ^ invert , n); - numBits += n; - n = 0; - lastval = dest[i]; + numBits += n; + n = 0; + lastval = dest[i]; - }//end for + }//end for - // if valid extra bits at the end were all the same frequency - add them in - if (n > clk/fchigh) { - if (dest[i-2] == 1) { - n = (n * fclow + clk/2) / clk; - } else { - n = (n * fchigh + clk/2) / clk; - } - memset(dest+numBits, dest[i-1] ^ invert , n); - numBits += n; - } - return numBits; + // if valid extra bits at the end were all the same frequency - add them in + if (n > clk/fchigh) { + if (dest[i-2] == 1) { + n = (n * fclow + clk/2) / clk; + } else { + n = (n * fchigh + clk/2) / clk; + } + memset(dest+numBits, dest[i-1] ^ invert , n); + numBits += n; + } + return numBits; } //by marshmellow (from holiman's base) // full fsk demod from GraphBuffer wave to decoded 1s and 0s (no mandemod) size_t fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8_t fchigh, uint8_t fclow, int *start_idx) { - if (signalprop.isnoise) return 0; - // FSK demodulator - size = fsk_wave_demod(dest, size, fchigh, fclow, start_idx); - size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow, start_idx); - return size; + if (signalprop.isnoise) return 0; + // FSK demodulator + size = fsk_wave_demod(dest, size, fchigh, fclow, start_idx); + size = aggregate_bits(dest, size, rfLen, invert, fchigh, fclow, start_idx); + return size; } // by marshmellow @@ -1784,31 +1784,31 @@ size_t fskdemod(uint8_t *dest, size_t size, uint8_t rfLen, uint8_t invert, uint8 // only transition waves are 1s //TODO: Iceman - hard coded value 7, should be #define void psk1TOpsk2(uint8_t *bits, size_t size) { - uint8_t lastbit = bits[0]; - for (size_t i = 1; i < size; i++){ - //ignore errors - if (bits[i] == 7) continue; + uint8_t lastbit = bits[0]; + for (size_t i = 1; i < size; i++){ + //ignore errors + if (bits[i] == 7) continue; - if (lastbit != bits[i]){ - lastbit = bits[i]; - bits[i] = 1; - } else { - bits[i] = 0; - } - } + if (lastbit != bits[i]){ + lastbit = bits[i]; + bits[i] = 1; + } else { + bits[i] = 0; + } + } } // by marshmellow // convert psk2 demod to psk1 demod // from only transition waves are 1s to phase shifts change bit void psk2TOpsk1(uint8_t *bits, size_t size) { - uint8_t phase = 0; - for (size_t i = 0; i < size; i++){ - if (bits[i] == 1){ - phase ^= 1; - } - bits[i] = phase; - } + uint8_t phase = 0; + for (size_t i = 0; i < size; i++){ + if (bits[i] == 1){ + phase ^= 1; + } + bits[i] = phase; + } } //by marshmellow - demodulate PSK1 wave @@ -1816,89 +1816,89 @@ void psk2TOpsk1(uint8_t *bits, size_t size) { //TODO: Iceman - hard coded value 7, should be #define int pskRawDemod_ext(uint8_t *dest, size_t *size, int *clock, int *invert, int *startIdx) { - // sanity check - if (*size < 170) return -1; + // sanity check + if (*size < 170) return -1; - uint8_t curPhase = *invert; - uint8_t fc=0; - size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0; - uint16_t fullWaveLen=0, waveLenCnt=0, avgWaveVal=0; - uint16_t errCnt=0, errCnt2=0; + uint8_t curPhase = *invert; + uint8_t fc=0; + size_t i=0, numBits=0, waveStart=1, waveEnd=0, firstFullWave=0, lastClkBit=0; + uint16_t fullWaveLen=0, waveLenCnt=0, avgWaveVal=0; + uint16_t errCnt=0, errCnt2=0; - *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc); - if (*clock <= 0) return -1; - //if clock detect found firstfullwave... - uint16_t tol = fc/2; - if (firstFullWave == 0) { - //find start of modulating data in trace - i = findModStart(dest, *size, fc); - //find first phase shift - firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen); - if (firstFullWave == 0) { - // no phase shift detected - could be all 1's or 0's - doesn't matter where we start - // so skip a little to ensure we are past any Start Signal - firstFullWave = 160; - memset(dest, curPhase, firstFullWave / *clock); - } else { - memset(dest, curPhase^1, firstFullWave / *clock); - } - } else { - memset(dest, curPhase^1, firstFullWave / *clock); - } - //advance bits - numBits += (firstFullWave / *clock); - *startIdx = firstFullWave - (*clock * numBits)+2; - //set start of wave as clock align - lastClkBit = firstFullWave; - if (g_debugMode == 2) prnt("DEBUG PSK: firstFullWave: %u, waveLen: %u, startIdx %i",firstFullWave,fullWaveLen, *startIdx); - if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit, fc); - waveStart = 0; - dest[numBits++] = curPhase; //set first read bit - for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){ - //top edge of wave = start of new wave - if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){ - if (waveStart == 0) { - waveStart = i+1; - waveLenCnt = 0; - avgWaveVal = dest[i+1]; - } else { //waveEnd - waveEnd = i+1; - waveLenCnt = waveEnd-waveStart; - if (waveLenCnt > fc){ - //this wave is a phase shift - //prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc); - if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit - curPhase ^= 1; - dest[numBits++] = curPhase; - lastClkBit += *clock; - } else if (i < lastClkBit+10+fc){ - //noise after a phase shift - ignore - } else { //phase shift before supposed to based on clock - errCnt++; - dest[numBits++] = 7; - } - } else if (i+1 > lastClkBit + *clock + tol + fc){ - lastClkBit += *clock; //no phase shift but clock bit - dest[numBits++] = curPhase; - } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often) - errCnt2++; - if (errCnt2 > 101) return errCnt2; - avgWaveVal += dest[i+1]; - continue; - } - avgWaveVal = 0; - waveStart = i+1; - } - } - avgWaveVal += dest[i+1]; - } - *size = numBits; - return errCnt; + *clock = DetectPSKClock(dest, *size, *clock, &firstFullWave, &curPhase, &fc); + if (*clock <= 0) return -1; + //if clock detect found firstfullwave... + uint16_t tol = fc/2; + if (firstFullWave == 0) { + //find start of modulating data in trace + i = findModStart(dest, *size, fc); + //find first phase shift + firstFullWave = pskFindFirstPhaseShift(dest, *size, &curPhase, i, fc, &fullWaveLen); + if (firstFullWave == 0) { + // no phase shift detected - could be all 1's or 0's - doesn't matter where we start + // so skip a little to ensure we are past any Start Signal + firstFullWave = 160; + memset(dest, curPhase, firstFullWave / *clock); + } else { + memset(dest, curPhase^1, firstFullWave / *clock); + } + } else { + memset(dest, curPhase^1, firstFullWave / *clock); + } + //advance bits + numBits += (firstFullWave / *clock); + *startIdx = firstFullWave - (*clock * numBits)+2; + //set start of wave as clock align + lastClkBit = firstFullWave; + if (g_debugMode == 2) prnt("DEBUG PSK: firstFullWave: %u, waveLen: %u, startIdx %i",firstFullWave,fullWaveLen, *startIdx); + if (g_debugMode == 2) prnt("DEBUG PSK: clk: %d, lastClkBit: %u, fc: %u", *clock, lastClkBit, fc); + waveStart = 0; + dest[numBits++] = curPhase; //set first read bit + for (i = firstFullWave + fullWaveLen - 1; i < *size-3; i++){ + //top edge of wave = start of new wave + if (dest[i]+fc < dest[i+1] && dest[i+1] >= dest[i+2]){ + if (waveStart == 0) { + waveStart = i+1; + waveLenCnt = 0; + avgWaveVal = dest[i+1]; + } else { //waveEnd + waveEnd = i+1; + waveLenCnt = waveEnd-waveStart; + if (waveLenCnt > fc){ + //this wave is a phase shift + //prnt("DEBUG: phase shift at: %d, len: %d, nextClk: %d, i: %d, fc: %d",waveStart,waveLenCnt,lastClkBit+*clock-tol,i+1,fc); + if (i+1 >= lastClkBit + *clock - tol){ //should be a clock bit + curPhase ^= 1; + dest[numBits++] = curPhase; + lastClkBit += *clock; + } else if (i < lastClkBit+10+fc){ + //noise after a phase shift - ignore + } else { //phase shift before supposed to based on clock + errCnt++; + dest[numBits++] = 7; + } + } else if (i+1 > lastClkBit + *clock + tol + fc){ + lastClkBit += *clock; //no phase shift but clock bit + dest[numBits++] = curPhase; + } else if (waveLenCnt < fc - 1) { //wave is smaller than field clock (shouldn't happen often) + errCnt2++; + if (errCnt2 > 101) return errCnt2; + avgWaveVal += dest[i+1]; + continue; + } + avgWaveVal = 0; + waveStart = i+1; + } + } + avgWaveVal += dest[i+1]; + } + *size = numBits; + return errCnt; } int pskRawDemod(uint8_t *dest, size_t *size, int *clock, int *invert) { - int start_idx = 0; - return pskRawDemod_ext(dest, size, clock, invert, &start_idx); + int start_idx = 0; + return pskRawDemod_ext(dest, size, clock, invert, &start_idx); } @@ -1910,167 +1910,167 @@ int pskRawDemod(uint8_t *dest, size_t *size, int *clock, int *invert) { // by marshmellow // FSK Demod then try to locate an AWID ID int detectAWID(uint8_t *dest, size_t *size, int *waveStartIdx) { - //make sure buffer has enough data (96bits * 50clock samples) - if (*size < 96*50) return -1; + //make sure buffer has enough data (96bits * 50clock samples) + if (*size < 96*50) return -1; - if (signalprop.isnoise) return -2; + if (signalprop.isnoise) return -2; - // FSK2a demodulator clock 50, invert 1, fcHigh 10, fcLow 8 - *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //awid fsk2a + // FSK2a demodulator clock 50, invert 1, fcHigh 10, fcLow 8 + *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //awid fsk2a - //did we get a good demod? - if (*size < 96) return -3; + //did we get a good demod? + if (*size < 96) return -3; - size_t start_idx = 0; - uint8_t preamble[] = {0,0,0,0,0,0,0,1}; - if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) - return -4; //preamble not found + size_t start_idx = 0; + uint8_t preamble[] = {0,0,0,0,0,0,0,1}; + if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) + return -4; //preamble not found - // wrong size? (between to preambles) - if (*size != 96) return -5; + // wrong size? (between to preambles) + if (*size != 96) return -5; - return (int)start_idx; + return (int)start_idx; } //by marshmellow //takes 1s and 0s and searches for EM410x format - output EM ID int Em410xDecode(uint8_t *bits, size_t *size, size_t *start_idx, uint32_t *hi, uint64_t *lo) { - // sanity check - if (bits[1] > 1) return -1; - if (*size < 64) return -2; + // sanity check + if (bits[1] > 1) return -1; + if (*size < 64) return -2; - uint8_t fmtlen; - *start_idx = 0; + uint8_t fmtlen; + *start_idx = 0; - // preamble 0111111111 - // include 0 in front to help get start pos - uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1}; - if (!preambleSearch(bits, preamble, sizeof(preamble), size, start_idx)) - return -4; + // preamble 0111111111 + // include 0 in front to help get start pos + uint8_t preamble[] = {0,1,1,1,1,1,1,1,1,1}; + if (!preambleSearch(bits, preamble, sizeof(preamble), size, start_idx)) + return -4; - // (iceman) if the preamble doesn't find two occuriences, this identification fails. - fmtlen = (*size == 128) ? 22 : 10; + // (iceman) if the preamble doesn't find two occuriences, this identification fails. + fmtlen = (*size == 128) ? 22 : 10; - //skip last 4bit parity row for simplicity - *size = removeParity(bits, *start_idx + sizeof(preamble), 5, 0, fmtlen * 5); + //skip last 4bit parity row for simplicity + *size = removeParity(bits, *start_idx + sizeof(preamble), 5, 0, fmtlen * 5); - switch (*size) { - case 40: { - // std em410x format - *hi = 0; - *lo = ((uint64_t)(bytebits_to_byte(bits, 8)) << 32) | (bytebits_to_byte(bits + 8, 32)); - break; - } - case 88: { - // long em format - *hi = (bytebits_to_byte(bits, 24)); - *lo = ((uint64_t)(bytebits_to_byte(bits + 24, 32)) << 32) | (bytebits_to_byte(bits + 24 + 32, 32)); - break; - } - default: return -6; - } - return 1; + switch (*size) { + case 40: { + // std em410x format + *hi = 0; + *lo = ((uint64_t)(bytebits_to_byte(bits, 8)) << 32) | (bytebits_to_byte(bits + 8, 32)); + break; + } + case 88: { + // long em format + *hi = (bytebits_to_byte(bits, 24)); + *lo = ((uint64_t)(bytebits_to_byte(bits + 24, 32)) << 32) | (bytebits_to_byte(bits + 24 + 32, 32)); + break; + } + default: return -6; + } + return 1; } // loop to get raw HID waveform then FSK demodulate the TAG ID from it int HIDdemodFSK(uint8_t *dest, size_t *size, uint32_t *hi2, uint32_t *hi, uint32_t *lo, int *waveStartIdx) { - //make sure buffer has data - if (*size < 96*50) return -1; + //make sure buffer has data + if (*size < 96*50) return -1; - if (signalprop.isnoise) return -2; + if (signalprop.isnoise) return -2; - // FSK demodulator fsk2a so invert and fc/10/8 - *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //hid fsk2a + // FSK demodulator fsk2a so invert and fc/10/8 + *size = fskdemod(dest, *size, 50, 1, 10, 8, waveStartIdx); //hid fsk2a - //did we get a good demod? - if (*size < 96*2) return -3; + //did we get a good demod? + if (*size < 96*2) return -3; - // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1 - size_t start_idx = 0; - uint8_t preamble[] = {0,0,0,1,1,1,0,1}; - if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) - return -4; //preamble not found + // 00011101 bit pattern represent start of frame, 01 pattern represents a 0 and 10 represents a 1 + size_t start_idx = 0; + uint8_t preamble[] = {0,0,0,1,1,1,0,1}; + if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) + return -4; //preamble not found - // wrong size? (between to preambles) - //if (*size != 96) return -5; + // wrong size? (between to preambles) + //if (*size != 96) return -5; - size_t num_start = start_idx + sizeof(preamble); - // final loop, go over previously decoded FSK data and manchester decode into usable tag ID - for (size_t idx = num_start; (idx - num_start) < *size - sizeof(preamble); idx += 2) { - if (dest[idx] == dest[idx+1]){ - return -5; //not manchester data - } - *hi2 = (*hi2 << 1) | (*hi >> 31); - *hi = (*hi << 1) | (*lo >> 31); - //Then, shift in a 0 or one into low - *lo <<= 1; - if (dest[idx] && !dest[idx+1]) // 1 0 - *lo |= 1; - else // 0 1 - *lo |= 0; - } - return (int)start_idx; + size_t num_start = start_idx + sizeof(preamble); + // final loop, go over previously decoded FSK data and manchester decode into usable tag ID + for (size_t idx = num_start; (idx - num_start) < *size - sizeof(preamble); idx += 2) { + if (dest[idx] == dest[idx+1]){ + return -5; //not manchester data + } + *hi2 = (*hi2 << 1) | (*hi >> 31); + *hi = (*hi << 1) | (*lo >> 31); + //Then, shift in a 0 or one into low + *lo <<= 1; + if (dest[idx] && !dest[idx+1]) // 1 0 + *lo |= 1; + else // 0 1 + *lo |= 0; + } + return (int)start_idx; } // Find IDTEC PSK1, RF Preamble == 0x4944544B, Demodsize 64bits // by iceman int detectIdteck(uint8_t *dest, size_t *size) { - //make sure buffer has data - if (*size < 64*2) return -1; + //make sure buffer has data + if (*size < 64*2) return -1; - if (signalprop.isnoise) return -2; + if (signalprop.isnoise) return -2; - size_t start_idx = 0; - uint8_t preamble[] = {0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,0,0,1,0,0,1,0,1,1}; + size_t start_idx = 0; + uint8_t preamble[] = {0,1,0,0,1,0,0,1,0,1,0,0,0,1,0,0,0,1,0,1,0,1,0,0,0,1,0,0,1,0,1,1}; - //preamble not found - if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) - return -3; + //preamble not found + if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) + return -3; - // wrong demoded size - if (*size != 64) return -4; - return (int)start_idx; + // wrong demoded size + if (*size != 64) return -4; + return (int)start_idx; } int detectIOProx(uint8_t *dest, size_t *size, int *waveStartIdx) { - //make sure buffer has data - if (*size < 66*64) return -1; + //make sure buffer has data + if (*size < 66*64) return -1; - if (signalprop.isnoise) return -2; + if (signalprop.isnoise) return -2; - // FSK demodulator RF/64, fsk2a so invert, and fc/10/8 - *size = fskdemod(dest, *size, 64, 1, 10, 8, waveStartIdx); //io fsk2a + // FSK demodulator RF/64, fsk2a so invert, and fc/10/8 + *size = fskdemod(dest, *size, 64, 1, 10, 8, waveStartIdx); //io fsk2a - //did we get enough demod data? - if (*size < 64) return -3; + //did we get enough demod data? + if (*size < 64) return -3; - //Index map - //0 10 20 30 40 50 60 - //| | | | | | | - //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 - //----------------------------------------------------------------------------- - //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11 - // - //XSF(version)facility:codeone+codetwo + //Index map + //0 10 20 30 40 50 60 + //| | | | | | | + //01234567 8 90123456 7 89012345 6 78901234 5 67890123 4 56789012 3 45678901 23 + //----------------------------------------------------------------------------- + //00000000 0 11110000 1 facility 1 version* 1 code*one 1 code*two 1 ???????? 11 + // + //XSF(version)facility:codeone+codetwo - size_t start_idx = 0; - uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1}; - if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) - return -4; //preamble not found + size_t start_idx = 0; + uint8_t preamble[] = {0,0,0,0,0,0,0,0,0,1}; + if (!preambleSearch(dest, preamble, sizeof(preamble), size, &start_idx)) + return -4; //preamble not found - // wrong size? (between to preambles) - if (*size != 64) return -5; + // wrong size? (between to preambles) + if (*size != 64) return -5; - if ( !dest[start_idx + 8] - && dest[start_idx + 17] == 1 - && dest[start_idx + 26] == 1 - && dest[start_idx + 35] == 1 - && dest[start_idx + 44] == 1 - && dest[start_idx + 53] == 1) { - //confirmed proper separator bits found - //return start position - return (int) start_idx; - } - return -6; + if ( !dest[start_idx + 8] + && dest[start_idx + 17] == 1 + && dest[start_idx + 26] == 1 + && dest[start_idx + 35] == 1 + && dest[start_idx + 44] == 1 + && dest[start_idx + 53] == 1) { + //confirmed proper separator bits found + //return start position + return (int) start_idx; + } + return -6; } diff --git a/common/lfdemod.h b/common/lfdemod.h index 222495a99..80f0e06a7 100644 --- a/common/lfdemod.h +++ b/common/lfdemod.h @@ -19,7 +19,7 @@ #include // for #include // for bool #include "parity.h" // for parity test -#include "util.h" // for ARRAYLEN +#include "util.h" // for ARRAYLEN //might not be high enough for noisy environments #define NOISE_AMPLITUDE_THRESHOLD 10 @@ -30,11 +30,11 @@ //generic typedef struct { - int low; - int high; - int mean; - int amplitude; - bool isnoise; + int low; + int high; + int mean; + int amplitude; + bool isnoise; } signal_t; signal_t* getSignalProperties(void); @@ -50,12 +50,12 @@ extern int askdemod(uint8_t *bits, size_t *size, int *clk, int *invert, int extern int askdemod_ext(uint8_t *bits, size_t *size, int *clk, int *invert, int maxErr, uint8_t amp, uint8_t askType, int *startIdx); extern void askAmp(uint8_t *bits, size_t size); extern int BiphaseRawDecode(uint8_t *bits, size_t *size, int *offset, int invert); -extern uint8_t bits_to_array(const uint8_t *bits, size_t size, uint8_t *dest); +extern uint8_t bits_to_array(const uint8_t *bits, size_t size, uint8_t *dest); extern uint32_t bytebits_to_byte(uint8_t *src, size_t numbits); extern uint32_t bytebits_to_byteLSBF(uint8_t *src, size_t numbits); extern uint16_t countFC(uint8_t *bits, size_t size, bool fskAdj); extern int DetectASKClock(uint8_t *dest, size_t size, int *clock, int maxErr); -extern bool DetectCleanAskWave(uint8_t *dest, size_t size, uint8_t high, uint8_t low); +extern bool DetectCleanAskWave(uint8_t *dest, size_t size, uint8_t high, uint8_t low); extern uint8_t detectFSKClk(uint8_t *bits, size_t size, uint8_t fcHigh, uint8_t fcLow, int *firstClockEdge); extern int DetectNRZClock(uint8_t *dest, size_t size, int clock, size_t *clockStartIdx); extern int DetectPSKClock(uint8_t *dest, size_t size, int clock, size_t *firstPhaseShift, uint8_t *curPhase, uint8_t *fc); @@ -69,8 +69,8 @@ extern int ManchesterEncode(uint8_t *bits, size_t size); extern int manrawdecode(uint8_t *bits, size_t *size, uint8_t invert, uint8_t *alignPos); extern int nrzRawDemod(uint8_t *dest, size_t *size, int *clk, int *invert, int *startIdx); extern bool parityTest(uint32_t bits, uint8_t bitLen, uint8_t pType); -extern bool preambleSearch(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx); -extern bool preambleSearchEx(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone); +extern bool preambleSearch(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx); +extern bool preambleSearchEx(uint8_t *bits, uint8_t *preamble, size_t pLen, size_t *size, size_t *startIdx, bool findone); extern int pskRawDemod(uint8_t *dest, size_t *size, int *clock, int *invert); extern int pskRawDemod_ext(uint8_t *dest, size_t *size, int *clock, int *invert, int *startIdx); extern void psk2TOpsk1(uint8_t *bits, size_t size); diff --git a/common/parity.h b/common/parity.h index 9aaf936ed..94d98d913 100644 --- a/common/parity.h +++ b/common/parity.h @@ -22,23 +22,23 @@ extern const uint8_t OddByteParity[256]; static inline bool oddparity8(const uint8_t x) { - return OddByteParity[x]; + return OddByteParity[x]; } static inline bool evenparity8(const uint8_t x) { - return !OddByteParity[x]; + return !OddByteParity[x]; } static inline bool evenparity32(uint32_t x) { #if !defined __GNUC__ - x ^= x >> 16; - x ^= x >> 8; - return evenparity8(x); + x ^= x >> 16; + x ^= x >> 8; + return evenparity8(x); #else - return __builtin_parity(x); + return __builtin_parity(x); #endif } @@ -46,11 +46,11 @@ static inline bool evenparity32(uint32_t x) static inline bool oddparity32(uint32_t x) { #if !defined __GNUC__ - x ^= x >> 16; - x ^= x >> 8; - return oddparity8(x); + x ^= x >> 16; + x ^= x >> 8; + return oddparity8(x); #else - return !__builtin_parity(x); + return !__builtin_parity(x); #endif } diff --git a/common/prng.c b/common/prng.c index b1a608d7e..fb39dd11c 100644 --- a/common/prng.c +++ b/common/prng.c @@ -17,7 +17,7 @@ uint32_t burtle_get_mod( prng_ctx *x ) { void burtle_init_mod(prng_ctx *x, uint32_t seed ) { x->a = 0xf1ea5eed; - x->b = x->c = x->d = seed; + x->b = x->c = x->d = seed; for (uint8_t i=0; i < 42; ++i) { (void)burtle_get_mod(x); } @@ -33,7 +33,7 @@ void burtle_init(prng_ctx *x, uint32_t seed ) { uint32_t GetSimplePrng( uint32_t seed ){ - seed *= 0x19660D; - seed += 0x3C6EF35F; - return seed; + seed *= 0x19660D; + seed += 0x3C6EF35F; + return seed; } diff --git a/common/prng.h b/common/prng.h index f7a875b6c..fc9abd346 100644 --- a/common/prng.h +++ b/common/prng.h @@ -9,10 +9,10 @@ #include #include typedef struct prng_ctx { - uint32_t a; - uint32_t b; - uint32_t c; - uint32_t d; + uint32_t a; + uint32_t b; + uint32_t c; + uint32_t d; } prng_ctx; //uint32_t burtle_get( prng_ctx *x ); diff --git a/common/protocols.c b/common/protocols.c index 9649c51d9..be1076846 100644 --- a/common/protocols.c +++ b/common/protocols.c @@ -2,17 +2,17 @@ // ATA55xx shared presets & routines uint32_t GetT55xxClockBit(uint32_t clock) { - switch (clock) { - case 128: return T55x7_BITRATE_RF_128; - case 100: return T55x7_BITRATE_RF_100; - case 64: return T55x7_BITRATE_RF_64; - case 50: return T55x7_BITRATE_RF_50; - case 40: return T55x7_BITRATE_RF_40; - case 32: return T55x7_BITRATE_RF_32; - case 16: return T55x7_BITRATE_RF_16; - case 8: return T55x7_BITRATE_RF_8; - default : return 0; - } + switch (clock) { + case 128: return T55x7_BITRATE_RF_128; + case 100: return T55x7_BITRATE_RF_100; + case 64: return T55x7_BITRATE_RF_64; + case 50: return T55x7_BITRATE_RF_50; + case 40: return T55x7_BITRATE_RF_40; + case 32: return T55x7_BITRATE_RF_32; + case 16: return T55x7_BITRATE_RF_16; + case 8: return T55x7_BITRATE_RF_8; + default : return 0; + } } #ifndef ON_DEVICE @@ -20,122 +20,122 @@ uint32_t GetT55xxClockBit(uint32_t clock) { #define PrintAndLogDevice(level, format, args...) PrintAndLogEx(level, format , ## args) uint8_t isset(uint8_t val, uint8_t mask) { - return (val & mask); + return (val & mask); } uint8_t notset(uint8_t val, uint8_t mask){ - return !(val & mask); + return !(val & mask); } void fuse_config(const picopass_hdr *hdr) { - uint8_t fuses = hdr->conf.fuses; + uint8_t fuses = hdr->conf.fuses; - if (isset(fuses,FUSE_FPERS)) - PrintAndLogDevice(SUCCESS, "\tMode: Personalization [Programmable]"); - else - PrintAndLogDevice(NORMAL, "\tMode: Application [Locked]"); + if (isset(fuses,FUSE_FPERS)) + PrintAndLogDevice(SUCCESS, "\tMode: Personalization [Programmable]"); + else + PrintAndLogDevice(NORMAL, "\tMode: Application [Locked]"); - if (isset(fuses, FUSE_CODING1)) { - PrintAndLogDevice(NORMAL, "\tCoding: RFU"); - } else { - if( isset( fuses , FUSE_CODING0)) - PrintAndLogDevice(NORMAL, "\tCoding: ISO 14443-2 B/ISO 15693"); - else - PrintAndLogDevice(NORMAL, "\tCoding: ISO 14443B only"); - } - // 1 1 - if( isset (fuses,FUSE_CRYPT1) && isset(fuses, FUSE_CRYPT0 )) PrintAndLogDevice(SUCCESS, "\tCrypt: Secured page, keys not locked"); - // 1 0 - if( isset (fuses,FUSE_CRYPT1) && notset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(NORMAL, "\tCrypt: Secured page, keys locked"); - // 0 1 - if( notset (fuses,FUSE_CRYPT1) && isset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(SUCCESS, "\tCrypt: Non secured page"); - // 0 0 - if( notset (fuses,FUSE_CRYPT1) && notset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(NORMAL, "\tCrypt: No auth possible. Read only if RA is enabled"); + if (isset(fuses, FUSE_CODING1)) { + PrintAndLogDevice(NORMAL, "\tCoding: RFU"); + } else { + if( isset( fuses , FUSE_CODING0)) + PrintAndLogDevice(NORMAL, "\tCoding: ISO 14443-2 B/ISO 15693"); + else + PrintAndLogDevice(NORMAL, "\tCoding: ISO 14443B only"); + } + // 1 1 + if( isset (fuses,FUSE_CRYPT1) && isset(fuses, FUSE_CRYPT0 )) PrintAndLogDevice(SUCCESS, "\tCrypt: Secured page, keys not locked"); + // 1 0 + if( isset (fuses,FUSE_CRYPT1) && notset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(NORMAL, "\tCrypt: Secured page, keys locked"); + // 0 1 + if( notset (fuses,FUSE_CRYPT1) && isset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(SUCCESS, "\tCrypt: Non secured page"); + // 0 0 + if( notset (fuses,FUSE_CRYPT1) && notset( fuses, FUSE_CRYPT0 )) PrintAndLogDevice(NORMAL, "\tCrypt: No auth possible. Read only if RA is enabled"); - if( isset( fuses, FUSE_RA)) - PrintAndLogDevice(NORMAL, "\tRA: Read access enabled"); - else - PrintAndLogDevice(WARNING, "\tRA: Read access not enabled"); + if( isset( fuses, FUSE_RA)) + PrintAndLogDevice(NORMAL, "\tRA: Read access enabled"); + else + PrintAndLogDevice(WARNING, "\tRA: Read access not enabled"); } void getMemConfig(uint8_t mem_cfg, uint8_t chip_cfg, uint8_t *max_blk, uint8_t *app_areas, uint8_t *kb) { - // mem-bit 5, mem-bit 7, chip-bit 4: defines chip type - uint8_t k16 = isset(mem_cfg, 0x80); - //uint8_t k2 = isset(mem_cfg, 0x08); - uint8_t book = isset(mem_cfg, 0x20); + // mem-bit 5, mem-bit 7, chip-bit 4: defines chip type + uint8_t k16 = isset(mem_cfg, 0x80); + //uint8_t k2 = isset(mem_cfg, 0x08); + uint8_t book = isset(mem_cfg, 0x20); - if(isset(chip_cfg, 0x10) && !k16 && !book) { - *kb = 2; - *app_areas = 2; - *max_blk = 31; - } else if(isset(chip_cfg, 0x10) && k16 && !book) { - *kb = 16; - *app_areas = 2; - *max_blk = 255; //16kb - } else if(notset(chip_cfg, 0x10) && !k16 && !book) { - *kb = 16; - *app_areas = 16; - *max_blk = 255; //16kb - } else if(isset(chip_cfg, 0x10) && k16 && book) { - *kb = 32; - *app_areas = 3; - *max_blk = 255; //16kb - } else if(notset(chip_cfg, 0x10) && !k16 && book) { - *kb = 32; - *app_areas = 17; - *max_blk = 255; //16kb - } else { - *kb = 32; - *app_areas = 2; - *max_blk = 255; - } + if(isset(chip_cfg, 0x10) && !k16 && !book) { + *kb = 2; + *app_areas = 2; + *max_blk = 31; + } else if(isset(chip_cfg, 0x10) && k16 && !book) { + *kb = 16; + *app_areas = 2; + *max_blk = 255; //16kb + } else if(notset(chip_cfg, 0x10) && !k16 && !book) { + *kb = 16; + *app_areas = 16; + *max_blk = 255; //16kb + } else if(isset(chip_cfg, 0x10) && k16 && book) { + *kb = 32; + *app_areas = 3; + *max_blk = 255; //16kb + } else if(notset(chip_cfg, 0x10) && !k16 && book) { + *kb = 32; + *app_areas = 17; + *max_blk = 255; //16kb + } else { + *kb = 32; + *app_areas = 2; + *max_blk = 255; + } } void mem_app_config(const picopass_hdr *hdr) { - uint8_t mem = hdr->conf.mem_config; - uint8_t chip = hdr->conf.chip_config; - uint8_t applimit = hdr->conf.app_limit; - uint8_t kb = 2; - uint8_t app_areas = 2; - uint8_t max_blk = 31; + uint8_t mem = hdr->conf.mem_config; + uint8_t chip = hdr->conf.chip_config; + uint8_t applimit = hdr->conf.app_limit; + uint8_t kb = 2; + uint8_t app_areas = 2; + uint8_t max_blk = 31; - getMemConfig(mem, chip, &max_blk, &app_areas, &kb); + getMemConfig(mem, chip, &max_blk, &app_areas, &kb); - if (applimit < 6) applimit = 26; - if (kb == 2 && (applimit > 0x1f) ) applimit = 26; + if (applimit < 6) applimit = 26; + if (kb == 2 && (applimit > 0x1f) ) applimit = 26; - PrintAndLogDevice(NORMAL, " Mem: %u KBits/%u App Areas (%u * 8 bytes) [%02X]", kb, app_areas, max_blk, mem); - PrintAndLogDevice(NORMAL, "\tAA1: blocks 06-%02X", applimit); - PrintAndLogDevice(NORMAL, "\tAA2: blocks %02X-%02X", applimit+1, max_blk); - PrintAndLogDevice(NORMAL, "\tOTP: 0x%02X%02X", hdr->conf.otp[1], hdr->conf.otp[0]); - PrintAndLogDevice(NORMAL, "\nKeyAccess:"); + PrintAndLogDevice(NORMAL, " Mem: %u KBits/%u App Areas (%u * 8 bytes) [%02X]", kb, app_areas, max_blk, mem); + PrintAndLogDevice(NORMAL, "\tAA1: blocks 06-%02X", applimit); + PrintAndLogDevice(NORMAL, "\tAA2: blocks %02X-%02X", applimit+1, max_blk); + PrintAndLogDevice(NORMAL, "\tOTP: 0x%02X%02X", hdr->conf.otp[1], hdr->conf.otp[0]); + PrintAndLogDevice(NORMAL, "\nKeyAccess:"); - uint8_t book = isset(mem, 0x20); - if (book) { - PrintAndLogDevice(NORMAL, "\tRead A - Kd"); - PrintAndLogDevice(NORMAL, "\tRead B - Kc"); - PrintAndLogDevice(NORMAL, "\tWrite A - Kd"); - PrintAndLogDevice(NORMAL, "\tWrite B - Kc"); - PrintAndLogDevice(NORMAL, "\tDebit - Kd or Kc"); - PrintAndLogDevice(NORMAL,"\tCredit - Kc"); - } else{ - PrintAndLogDevice(NORMAL, "\tRead A - Kd or Kc"); - PrintAndLogDevice(NORMAL, "\tRead B - Kd or Kc"); - PrintAndLogDevice(NORMAL, "\tWrite A - Kc"); - PrintAndLogDevice(NORMAL, "\tWrite B - Kc"); - PrintAndLogDevice(NORMAL, "\tDebit - Kd or Kc"); - PrintAndLogDevice(NORMAL, "\tCredit - Kc"); - } + uint8_t book = isset(mem, 0x20); + if (book) { + PrintAndLogDevice(NORMAL, "\tRead A - Kd"); + PrintAndLogDevice(NORMAL, "\tRead B - Kc"); + PrintAndLogDevice(NORMAL, "\tWrite A - Kd"); + PrintAndLogDevice(NORMAL, "\tWrite B - Kc"); + PrintAndLogDevice(NORMAL, "\tDebit - Kd or Kc"); + PrintAndLogDevice(NORMAL,"\tCredit - Kc"); + } else{ + PrintAndLogDevice(NORMAL, "\tRead A - Kd or Kc"); + PrintAndLogDevice(NORMAL, "\tRead B - Kd or Kc"); + PrintAndLogDevice(NORMAL, "\tWrite A - Kc"); + PrintAndLogDevice(NORMAL, "\tWrite B - Kc"); + PrintAndLogDevice(NORMAL, "\tDebit - Kd or Kc"); + PrintAndLogDevice(NORMAL, "\tCredit - Kc"); + } } void print_picopass_info(const picopass_hdr *hdr) { - fuse_config(hdr); - mem_app_config(hdr); + fuse_config(hdr); + mem_app_config(hdr); } void printIclassDumpInfo(uint8_t* iclass_dump) { - print_picopass_info((picopass_hdr *) iclass_dump); + print_picopass_info((picopass_hdr *) iclass_dump); } #else - #define PrintAndLogDevice(level, format, ...) { } + #define PrintAndLogDevice(level, format, ...) { } #endif //ON_DEVICE diff --git a/common/protocols.h b/common/protocols.h index ce56b3175..50e6e0b46 100644 --- a/common/protocols.h +++ b/common/protocols.h @@ -9,115 +9,115 @@ //The following data is taken from http://www.proxmark.org/forum/viewtopic.php?pid=13501#p13501 /* ISO14443A (usually NFC tags) - 26 (7bits) = REQA - 30 = Read (usage: 30+1byte block number+2bytes ISO14443A-CRC - answer: 16bytes) - A2 = Write (usage: A2+1byte block number+4bytes data+2bytes ISO14443A-CRC - answer: 0A [ACK] or 00 [NAK]) - 52 (7bits) = WUPA (usage: 52(7bits) - answer: 2bytes ATQA) - 93 20 = Anticollision (usage: 9320 - answer: 4bytes UID+1byte UID-bytes-xor) - 93 70 = Select (usage: 9370+5bytes 9320 answer - answer: 1byte SAK) - 95 20 = Anticollision of cascade level2 - 95 70 = Select of cascade level2 - 50 00 = Halt (usage: 5000+2bytes ISO14443A-CRC - no answer from card) + 26 (7bits) = REQA + 30 = Read (usage: 30+1byte block number+2bytes ISO14443A-CRC - answer: 16bytes) + A2 = Write (usage: A2+1byte block number+4bytes data+2bytes ISO14443A-CRC - answer: 0A [ACK] or 00 [NAK]) + 52 (7bits) = WUPA (usage: 52(7bits) - answer: 2bytes ATQA) + 93 20 = Anticollision (usage: 9320 - answer: 4bytes UID+1byte UID-bytes-xor) + 93 70 = Select (usage: 9370+5bytes 9320 answer - answer: 1byte SAK) + 95 20 = Anticollision of cascade level2 + 95 70 = Select of cascade level2 + 50 00 = Halt (usage: 5000+2bytes ISO14443A-CRC - no answer from card) Mifare - 60 = Authenticate with KeyA - 61 = Authenticate with KeyB - 40 (7bits) = Used to put Chinese Changeable UID cards in special mode (must be followed by 43 (8bits) - answer: 0A) - C0 = Decrement - C1 = Increment - C2 = Restore - B0 = Transfer + 60 = Authenticate with KeyA + 61 = Authenticate with KeyB + 40 (7bits) = Used to put Chinese Changeable UID cards in special mode (must be followed by 43 (8bits) - answer: 0A) + C0 = Decrement + C1 = Increment + C2 = Restore + B0 = Transfer Ultralight C - A0 = Compatibility Write (to accomodate MIFARE commands) - 1A = Step1 Authenticate - AF = Step2 Authenticate + A0 = Compatibility Write (to accomodate MIFARE commands) + 1A = Step1 Authenticate + AF = Step2 Authenticate ISO14443B - 05 = REQB - 1D = ATTRIB - 50 = HALT + 05 = REQB + 1D = ATTRIB + 50 = HALT - BA = PING (reader -> tag) - AB = PONG (tag -> reader) + BA = PING (reader -> tag) + AB = PONG (tag -> reader) SRIX4K (tag does not respond to 05) - 06 00 = INITIATE - 0E xx = SELECT ID (xx = Chip-ID) - 0B = Get UID - 08 yy = Read Block (yy = block number) - 09 yy dd dd dd dd = Write Block (yy = block number; dd dd dd dd = data to be written) - 0C = Reset to Inventory - 0F = Completion - 0A 11 22 33 44 55 66 = Authenticate (11 22 33 44 55 66 = data to authenticate) + 06 00 = INITIATE + 0E xx = SELECT ID (xx = Chip-ID) + 0B = Get UID + 08 yy = Read Block (yy = block number) + 09 yy dd dd dd dd = Write Block (yy = block number; dd dd dd dd = data to be written) + 0C = Reset to Inventory + 0F = Completion + 0A 11 22 33 44 55 66 = Authenticate (11 22 33 44 55 66 = data to authenticate) ISO15693 - MANDATORY COMMANDS (all ISO15693 tags must support those) - 01 = Inventory (usage: 260100+2bytes ISO15693-CRC - answer: 12bytes) - 02 = Stay Quiet - OPTIONAL COMMANDS (not all tags support them) - 20 = Read Block (usage: 0220+1byte block number+2bytes ISO15693-CRC - answer: 4bytes) - 21 = Write Block (usage: 0221+1byte block number+4bytes data+2bytes ISO15693-CRC - answer: 4bytes) - 22 = Lock Block - 23 = Read Multiple Blocks (usage: 0223+1byte 1st block to read+1byte last block to read+2bytes ISO15693-CRC) - 25 = Select - 26 = Reset to Ready - 27 = Write AFI - 28 = Lock AFI - 29 = Write DSFID - 2A = Lock DSFID - 2B = Get_System_Info (usage: 022B+2bytes ISO15693-CRC - answer: 14 or more bytes) - 2C = Read Multiple Block Security Status (usage: 022C+1byte 1st block security to read+1byte last block security to read+2bytes ISO15693-CRC) + MANDATORY COMMANDS (all ISO15693 tags must support those) + 01 = Inventory (usage: 260100+2bytes ISO15693-CRC - answer: 12bytes) + 02 = Stay Quiet + OPTIONAL COMMANDS (not all tags support them) + 20 = Read Block (usage: 0220+1byte block number+2bytes ISO15693-CRC - answer: 4bytes) + 21 = Write Block (usage: 0221+1byte block number+4bytes data+2bytes ISO15693-CRC - answer: 4bytes) + 22 = Lock Block + 23 = Read Multiple Blocks (usage: 0223+1byte 1st block to read+1byte last block to read+2bytes ISO15693-CRC) + 25 = Select + 26 = Reset to Ready + 27 = Write AFI + 28 = Lock AFI + 29 = Write DSFID + 2A = Lock DSFID + 2B = Get_System_Info (usage: 022B+2bytes ISO15693-CRC - answer: 14 or more bytes) + 2C = Read Multiple Block Security Status (usage: 022C+1byte 1st block security to read+1byte last block security to read+2bytes ISO15693-CRC) EM Microelectronic CUSTOM COMMANDS - A5 = Active EAS (followed by 1byte IC Manufacturer code+1byte EAS type) - A7 = Write EAS ID (followed by 1byte IC Manufacturer code+2bytes EAS value) - B8 = Get Protection Status for a specific block (followed by 1byte IC Manufacturer code+1byte block number+1byte of how many blocks after the previous is needed the info) - E4 = Login (followed by 1byte IC Manufacturer code+4bytes password) + A5 = Active EAS (followed by 1byte IC Manufacturer code+1byte EAS type) + A7 = Write EAS ID (followed by 1byte IC Manufacturer code+2bytes EAS value) + B8 = Get Protection Status for a specific block (followed by 1byte IC Manufacturer code+1byte block number+1byte of how many blocks after the previous is needed the info) + E4 = Login (followed by 1byte IC Manufacturer code+4bytes password) NXP/Philips CUSTOM COMMANDS - A0 = Inventory Read - A1 = Fast Inventory Read - A2 = Set EAS - A3 = Reset EAS - A4 = Lock EAS - A5 = EAS Alarm - A6 = Password Protect EAS - A7 = Write EAS ID - A8 = Read EPC - B0 = Inventory Page Read - B1 = Fast Inventory Page Read - B2 = Get Random Number - B3 = Set Password - B4 = Write Password - B5 = Lock Password - B6 = Bit Password Protection - B7 = Lock Page Protection Condition - B8 = Get Multiple Block Protection Status - B9 = Destroy SLI - BA = Enable Privacy - BB = 64bit Password Protection - 40 = Long Range CMD (Standard ISO/TR7003:1990) + A0 = Inventory Read + A1 = Fast Inventory Read + A2 = Set EAS + A3 = Reset EAS + A4 = Lock EAS + A5 = EAS Alarm + A6 = Password Protect EAS + A7 = Write EAS ID + A8 = Read EPC + B0 = Inventory Page Read + B1 = Fast Inventory Page Read + B2 = Get Random Number + B3 = Set Password + B4 = Write Password + B5 = Lock Password + B6 = Bit Password Protection + B7 = Lock Page Protection Condition + B8 = Get Multiple Block Protection Status + B9 = Destroy SLI + BA = Enable Privacy + BB = 64bit Password Protection + 40 = Long Range CMD (Standard ISO/TR7003:1990) ISO 7816-4 Basic interindustry commands. For command APDU's. - B0 = READ BINARY - D0 = WRITE BINARY - D6 = UPDATE BINARY - 0E = ERASE BINARY - B2 = READ RECORDS - D2 = WRITE RECORDS - E2 = APPEND RECORD - DC = UPDATE RECORD - CA = GET DATA - DA = PUT DATA - A4 = SELECT FILE - 20 = VERIFY - 88 = INTERNAL AUTHENTICATION - 82 = EXTERNAL AUTHENTICATION - B4 = GET CHALLENGE - 70 = MANAGE CHANNEL + B0 = READ BINARY + D0 = WRITE BINARY + D6 = UPDATE BINARY + 0E = ERASE BINARY + B2 = READ RECORDS + D2 = WRITE RECORDS + E2 = APPEND RECORD + DC = UPDATE RECORD + CA = GET DATA + DA = PUT DATA + A4 = SELECT FILE + 20 = VERIFY + 88 = INTERNAL AUTHENTICATION + 82 = EXTERNAL AUTHENTICATION + B4 = GET CHALLENGE + 70 = MANAGE CHANNEL - For response APDU's - 90 00 = OK - 6x xx = ERROR + For response APDU's + 90 00 = OK + 6x xx = ERROR */ // these cmds are adjusted to ISO15693 and Manchester encoding requests. // for instance ICLASS_CMD_SELECT 0x81 tells if ISO14443b/BPSK coding/106 kbits/s @@ -134,62 +134,62 @@ ISO 7816-4 Basic interindustry commands. For command APDU's. #define ICLASS_CMD_SELECT 0x81 #define ICLASS_CMD_PAGESEL 0x84 -#define ICLASS_CMD_UPDATE 0x87 +#define ICLASS_CMD_UPDATE 0x87 #define ICLASS_CMD_READCHECK_KC 0x18 #define ICLASS_CMD_READCHECK_KD 0x88 #define ICLASS_CMD_ACT 0x8E -#define ISO14443A_CMD_REQA 0x26 -#define ISO14443A_CMD_READBLOCK 0x30 -#define ISO14443A_CMD_WUPA 0x52 -#define ISO14443A_CMD_OPTS 0x35 +#define ISO14443A_CMD_REQA 0x26 +#define ISO14443A_CMD_READBLOCK 0x30 +#define ISO14443A_CMD_WUPA 0x52 +#define ISO14443A_CMD_OPTS 0x35 #define ISO14443A_CMD_ANTICOLL_OR_SELECT 0x93 #define ISO14443A_CMD_ANTICOLL_OR_SELECT_2 0x95 #define ISO14443A_CMD_ANTICOLL_OR_SELECT_3 0x97 -#define ISO14443A_CMD_WRITEBLOCK 0xA0 -#define ISO14443A_CMD_HALT 0x50 -#define ISO14443A_CMD_RATS 0xE0 +#define ISO14443A_CMD_WRITEBLOCK 0xA0 +#define ISO14443A_CMD_HALT 0x50 +#define ISO14443A_CMD_RATS 0xE0 -#define MIFARE_AUTH_KEYA 0x60 -#define MIFARE_AUTH_KEYB 0x61 -#define MIFARE_MAGICWUPC1 0x40 -#define MIFARE_MAGICWUPC2 0x43 -#define MIFARE_MAGICWIPEC 0x41 -#define MIFARE_CMD_INC 0xC0 -#define MIFARE_CMD_DEC 0xC1 -#define MIFARE_CMD_RESTORE 0xC2 -#define MIFARE_CMD_TRANSFER 0xB0 +#define MIFARE_AUTH_KEYA 0x60 +#define MIFARE_AUTH_KEYB 0x61 +#define MIFARE_MAGICWUPC1 0x40 +#define MIFARE_MAGICWUPC2 0x43 +#define MIFARE_MAGICWIPEC 0x41 +#define MIFARE_CMD_INC 0xC0 +#define MIFARE_CMD_DEC 0xC1 +#define MIFARE_CMD_RESTORE 0xC2 +#define MIFARE_CMD_TRANSFER 0xB0 -#define MIFARE_EV1_PERSONAL_UID 0x40 -#define MIFARE_EV1_SETMODE 0x43 +#define MIFARE_EV1_PERSONAL_UID 0x40 +#define MIFARE_EV1_SETMODE 0x43 -#define MIFARE_ULC_WRITE 0xA2 -//#define MIFARE_ULC__COMP_WRITE 0xA0 -#define MIFARE_ULC_AUTH_1 0x1A -#define MIFARE_ULC_AUTH_2 0xAF +#define MIFARE_ULC_WRITE 0xA2 +//#define MIFARE_ULC__COMP_WRITE 0xA0 +#define MIFARE_ULC_AUTH_1 0x1A +#define MIFARE_ULC_AUTH_2 0xAF -#define MIFARE_ULEV1_AUTH 0x1B -#define MIFARE_ULEV1_VERSION 0x60 -#define MIFARE_ULEV1_FASTREAD 0x3A -#define MIFARE_ULEV1_READ_CNT 0x39 -#define MIFARE_ULEV1_INCR_CNT 0xA5 -#define MIFARE_ULEV1_READSIG 0x3C -#define MIFARE_ULEV1_CHECKTEAR 0x3E -#define MIFARE_ULEV1_VCSL 0x4B +#define MIFARE_ULEV1_AUTH 0x1B +#define MIFARE_ULEV1_VERSION 0x60 +#define MIFARE_ULEV1_FASTREAD 0x3A +#define MIFARE_ULEV1_READ_CNT 0x39 +#define MIFARE_ULEV1_INCR_CNT 0xA5 +#define MIFARE_ULEV1_READSIG 0x3C +#define MIFARE_ULEV1_CHECKTEAR 0x3E +#define MIFARE_ULEV1_VCSL 0x4B // New Mifare UL Nano commands. Ref:: (https://www.nxp.com/docs/en/data-sheet/MF0UN_H_00.pdf) -#define MIFARE_ULNANO_WRITESIG 0xA9 -#define MIFARE_ULNANO_LOCKSIF 0xAC +#define MIFARE_ULNANO_WRITESIG 0xA9 +#define MIFARE_ULNANO_LOCKSIF 0xAC // mifare 4bit card answers #define CARD_ACK 0x0A // 1010 - ACK -#define CARD_NACK_IV 0x00 // 0000 - NACK, invalid argument (invalid page address) -#define CARD_NACK_PA 0x01 // 0001 - NACK, parity / crc error +#define CARD_NACK_IV 0x00 // 0000 - NACK, invalid argument (invalid page address) +#define CARD_NACK_PA 0x01 // 0001 - NACK, parity / crc error #define CARD_NACK_NA 0x04 // 0100 - NACK, not allowed (command not allowed) #define CARD_NACK_TR 0x05 // 0101 - NACK, transmission error -#define CARD_NACK_EE 0x07 // 0111 - NACK, EEPROM write error +#define CARD_NACK_EE 0x07 // 0111 - NACK, EEPROM write error // Magic Generation 1, parameter "work flags" // bit 0 - need get UID @@ -198,14 +198,14 @@ ISO 7816-4 Basic interindustry commands. For command APDU's. // bit 3 - turn on FPGA // bit 4 - turn off FPGA // bit 5 - set datain instead of issuing USB reply (called via ARM for StandAloneMode14a) -#define MAGIC_UID 0x01 -#define MAGIC_WUPC 0x02 -#define MAGIC_HALT 0x04 -#define MAGIC_INIT 0x08 -#define MAGIC_OFF 0x10 -#define MAGIC_DATAIN 0x20 -#define MAGIC_WIPE 0x40 -#define MAGIC_SINGLE (MAGIC_WUPC | MAGIC_HALT | MAGIC_INIT | MAGIC_OFF) //0x1E +#define MAGIC_UID 0x01 +#define MAGIC_WUPC 0x02 +#define MAGIC_HALT 0x04 +#define MAGIC_INIT 0x08 +#define MAGIC_OFF 0x10 +#define MAGIC_DATAIN 0x20 +#define MAGIC_WIPE 0x40 +#define MAGIC_SINGLE (MAGIC_WUPC | MAGIC_HALT | MAGIC_INIT | MAGIC_OFF) //0x1E /** 06 00 = INITIATE @@ -229,8 +229,8 @@ ISO 7816-4 Basic interindustry commands. For command APDU's. #define ISO14443B_RESET 0x0C #define ISO14443B_COMPLETION 0x0F #define ISO14443B_AUTHENTICATE 0x0A -#define ISO14443B_PING 0xBA -#define ISO14443B_PONG 0xAB +#define ISO14443B_PING 0xBA +#define ISO14443B_PONG 0xAB //First byte is 26 #define ISO15693_INVENTORY 0x01 @@ -251,31 +251,31 @@ ISO 7816-4 Basic interindustry commands. For command APDU's. // Topaz command set: -#define TOPAZ_REQA 0x26 // Request -#define TOPAZ_WUPA 0x52 // WakeUp -#define TOPAZ_RID 0x78 // Read ID -#define TOPAZ_RALL 0x00 // Read All (all bytes) -#define TOPAZ_READ 0x01 // Read (a single byte) -#define TOPAZ_WRITE_E 0x53 // Write-with-erase (a single byte) -#define TOPAZ_WRITE_NE 0x1a // Write-no-erase (a single byte) +#define TOPAZ_REQA 0x26 // Request +#define TOPAZ_WUPA 0x52 // WakeUp +#define TOPAZ_RID 0x78 // Read ID +#define TOPAZ_RALL 0x00 // Read All (all bytes) +#define TOPAZ_READ 0x01 // Read (a single byte) +#define TOPAZ_WRITE_E 0x53 // Write-with-erase (a single byte) +#define TOPAZ_WRITE_NE 0x1a // Write-no-erase (a single byte) // additional commands for Dynamic Memory Model -#define TOPAZ_RSEG 0x10 // Read segment -#define TOPAZ_READ8 0x02 // Read (eight bytes) -#define TOPAZ_WRITE_E8 0x54 // Write-with-erase (eight bytes) -#define TOPAZ_WRITE_NE8 0x1B // Write-no-erase (eight bytes) +#define TOPAZ_RSEG 0x10 // Read segment +#define TOPAZ_READ8 0x02 // Read (eight bytes) +#define TOPAZ_WRITE_E8 0x54 // Write-with-erase (eight bytes) +#define TOPAZ_WRITE_NE8 0x1B // Write-no-erase (eight bytes) // Definitions of which protocol annotations there are available -#define ISO_14443A 0 -#define ICLASS 1 -#define ISO_14443B 2 -#define TOPAZ 3 -#define ISO_7816_4 4 -#define MFDES 5 -#define LEGIC 6 -#define ISO_15693 7 -#define FELICA 8 -#define PROTO_MIFARE 9 +#define ISO_14443A 0 +#define ICLASS 1 +#define ISO_14443B 2 +#define TOPAZ 3 +#define ISO_7816_4 4 +#define MFDES 5 +#define LEGIC 6 +#define ISO_15693 7 +#define FELICA 8 +#define PROTO_MIFARE 9 //-- Picopass fuses #define FUSE_FPERS 0x80 @@ -288,131 +288,131 @@ ISO 7816-4 Basic interindustry commands. For command APDU's. #define FUSE_RA 0x01 // ISO 7816-4 Basic interindustry commands. For command APDU's. -#define ISO7816_READ_BINARY 0xB0 -#define ISO7816_WRITE_BINARY 0xD0 -#define ISO7816_UPDATE_BINARY 0xD6 -#define ISO7816_ERASE_BINARY 0x0E -#define ISO7816_READ_RECORDS 0xB2 -#define ISO7816_WRITE_RECORDS 0xD2 -#define ISO7816_APPEND_RECORD 0xE2 -#define ISO7816_UPDATE_RECORD 0xDC -#define ISO7816_GET_DATA 0xCA -#define ISO7816_PUT_DATA 0xDA -#define ISO7816_SELECT_FILE 0xA4 -#define ISO7816_VERIFY 0x20 +#define ISO7816_READ_BINARY 0xB0 +#define ISO7816_WRITE_BINARY 0xD0 +#define ISO7816_UPDATE_BINARY 0xD6 +#define ISO7816_ERASE_BINARY 0x0E +#define ISO7816_READ_RECORDS 0xB2 +#define ISO7816_WRITE_RECORDS 0xD2 +#define ISO7816_APPEND_RECORD 0xE2 +#define ISO7816_UPDATE_RECORD 0xDC +#define ISO7816_GET_DATA 0xCA +#define ISO7816_PUT_DATA 0xDA +#define ISO7816_SELECT_FILE 0xA4 +#define ISO7816_VERIFY 0x20 #define ISO7816_INTERNAL_AUTHENTICATION 0x88 #define ISO7816_EXTERNAL_AUTHENTICATION 0x82 -#define ISO7816_GET_CHALLENGE 0x84 -#define ISO7816_MANAGE_CHANNEL 0x70 +#define ISO7816_GET_CHALLENGE 0x84 +#define ISO7816_MANAGE_CHANNEL 0x70 -#define ISO7816_GET_RESPONSE 0xC0 -// ISO7816-4 For response APDU's -#define ISO7816_OK 0x9000 -// 6x xx = ERROR +#define ISO7816_GET_RESPONSE 0xC0 +// ISO7816-4 For response APDU's +#define ISO7816_OK 0x9000 +// 6x xx = ERROR // MIFARE DESFire command set: -#define MFDES_CREATE_APPLICATION 0xca -#define MFDES_DELETE_APPLICATION 0xda -#define MFDES_GET_APPLICATION_IDS 0x6a -#define MFDES_SELECT_APPLICATION 0x5a -#define MFDES_FORMAT_PICC 0xfc -#define MFDES_GET_VERSION 0x60 -#define MFDES_READ_DATA 0xbd -#define MFDES_WRITE_DATA 0x3d -#define MFDES_GET_VALUE 0x6c -#define MFDES_CREDIT 0x0c -#define MFDES_DEBIT 0xdc -#define MFDES_LIMITED_CREDIT 0x1c -#define MFDES_WRITE_RECORD 0x3b -#define MFDES_READ_RECORDS 0xbb -#define MFDES_CLEAR_RECORD_FILE 0xeb -#define MFDES_COMMIT_TRANSACTION 0xc7 -#define MFDES_ABORT_TRANSACTION 0xa7 -#define MFDES_GET_FREE_MEMORY 0x6e -#define MFDES_GET_FILE_IDS 0x6f -#define MFDES_GET_ISOFILE_IDS 0x61 -#define MFDES_GET_FILE_SETTINGS 0xf5 -#define MFDES_CHANGE_FILE_SETTINGS 0x5f -#define MFDES_CREATE_STD_DATA_FILE 0xcd -#define MFDES_CREATE_BACKUP_DATA_FILE 0xcb -#define MFDES_CREATE_VALUE_FILE 0xcc -#define MFDES_CREATE_LINEAR_RECORD_FILE 0xc1 -#define MFDES_CREATE_CYCLIC_RECORD_FILE 0xc0 -#define MFDES_DELETE_FILE 0xdf -#define MFDES_AUTHENTICATE 0x0a // AUTHENTICATE_NATIVE -#define MFDES_AUTHENTICATE_ISO 0x1a // AUTHENTICATE_STANDARD -#define MFDES_AUTHENTICATE_AES 0xaa -#define MFDES_CHANGE_KEY_SETTINGS 0x54 -#define MFDES_GET_KEY_SETTINGS 0x45 -#define MFDES_CHANGE_KEY 0xc4 -#define MFDES_GET_KEY_VERSION 0x64 -#define MFDES_AUTHENTICATION_FRAME 0xAF +#define MFDES_CREATE_APPLICATION 0xca +#define MFDES_DELETE_APPLICATION 0xda +#define MFDES_GET_APPLICATION_IDS 0x6a +#define MFDES_SELECT_APPLICATION 0x5a +#define MFDES_FORMAT_PICC 0xfc +#define MFDES_GET_VERSION 0x60 +#define MFDES_READ_DATA 0xbd +#define MFDES_WRITE_DATA 0x3d +#define MFDES_GET_VALUE 0x6c +#define MFDES_CREDIT 0x0c +#define MFDES_DEBIT 0xdc +#define MFDES_LIMITED_CREDIT 0x1c +#define MFDES_WRITE_RECORD 0x3b +#define MFDES_READ_RECORDS 0xbb +#define MFDES_CLEAR_RECORD_FILE 0xeb +#define MFDES_COMMIT_TRANSACTION 0xc7 +#define MFDES_ABORT_TRANSACTION 0xa7 +#define MFDES_GET_FREE_MEMORY 0x6e +#define MFDES_GET_FILE_IDS 0x6f +#define MFDES_GET_ISOFILE_IDS 0x61 +#define MFDES_GET_FILE_SETTINGS 0xf5 +#define MFDES_CHANGE_FILE_SETTINGS 0x5f +#define MFDES_CREATE_STD_DATA_FILE 0xcd +#define MFDES_CREATE_BACKUP_DATA_FILE 0xcb +#define MFDES_CREATE_VALUE_FILE 0xcc +#define MFDES_CREATE_LINEAR_RECORD_FILE 0xc1 +#define MFDES_CREATE_CYCLIC_RECORD_FILE 0xc0 +#define MFDES_DELETE_FILE 0xdf +#define MFDES_AUTHENTICATE 0x0a // AUTHENTICATE_NATIVE +#define MFDES_AUTHENTICATE_ISO 0x1a // AUTHENTICATE_STANDARD +#define MFDES_AUTHENTICATE_AES 0xaa +#define MFDES_CHANGE_KEY_SETTINGS 0x54 +#define MFDES_GET_KEY_SETTINGS 0x45 +#define MFDES_CHANGE_KEY 0xc4 +#define MFDES_GET_KEY_VERSION 0x64 +#define MFDES_AUTHENTICATION_FRAME 0xAF // LEGIC Commands -#define LEGIC_MIM_22 0x0D -#define LEGIC_MIM_256 0x1D -#define LEGIC_MIM_1024 0x3D -#define LEGIC_ACK_22 0x19 -#define LEGIC_ACK_256 0x39 -#define LEGIC_READ 0x01 -#define LEGIC_WRITE 0x00 +#define LEGIC_MIM_22 0x0D +#define LEGIC_MIM_256 0x1D +#define LEGIC_MIM_1024 0x3D +#define LEGIC_ACK_22 0x19 +#define LEGIC_ACK_256 0x39 +#define LEGIC_READ 0x01 +#define LEGIC_WRITE 0x00 void printIclassDumpInfo(uint8_t* iclass_dump); void getMemConfig(uint8_t mem_cfg, uint8_t chip_cfg, uint8_t *max_blk, uint8_t *app_areas, uint8_t *kb); /* T55x7 configuration register definitions */ -#define T55x7_POR_DELAY 0x00000001 -#define T55x7_ST_TERMINATOR 0x00000008 -#define T55x7_PWD 0x00000010 -#define T55x7_MAXBLOCK_SHIFT 5 -#define T55x7_AOR 0x00000200 -#define T55x7_PSKCF_RF_2 0 -#define T55x7_PSKCF_RF_4 0x00000400 -#define T55x7_PSKCF_RF_8 0x00000800 -#define T55x7_MODULATION_DIRECT 0 -#define T55x7_MODULATION_PSK1 0x00001000 -#define T55x7_MODULATION_PSK2 0x00002000 -#define T55x7_MODULATION_PSK3 0x00003000 -#define T55x7_MODULATION_FSK1 0x00004000 -#define T55x7_MODULATION_FSK2 0x00005000 -#define T55x7_MODULATION_FSK1a 0x00006000 -#define T55x7_MODULATION_FSK2a 0x00007000 -#define T55x7_MODULATION_MANCHESTER 0x00008000 -#define T55x7_MODULATION_BIPHASE 0x00010000 -#define T55x7_MODULATION_DIPHASE 0x00018000 -#define T55x7_X_MODE 0x00020000 -#define T55x7_BITRATE_RF_8 0 -#define T55x7_BITRATE_RF_16 0x00040000 -#define T55x7_BITRATE_RF_32 0x00080000 -#define T55x7_BITRATE_RF_40 0x000C0000 -#define T55x7_BITRATE_RF_50 0x00100000 -#define T55x7_BITRATE_RF_64 0x00140000 -#define T55x7_BITRATE_RF_100 0x00180000 -#define T55x7_BITRATE_RF_128 0x001C0000 -#define T55x7_TESTMODE_DISABLED 0x60000000 +#define T55x7_POR_DELAY 0x00000001 +#define T55x7_ST_TERMINATOR 0x00000008 +#define T55x7_PWD 0x00000010 +#define T55x7_MAXBLOCK_SHIFT 5 +#define T55x7_AOR 0x00000200 +#define T55x7_PSKCF_RF_2 0 +#define T55x7_PSKCF_RF_4 0x00000400 +#define T55x7_PSKCF_RF_8 0x00000800 +#define T55x7_MODULATION_DIRECT 0 +#define T55x7_MODULATION_PSK1 0x00001000 +#define T55x7_MODULATION_PSK2 0x00002000 +#define T55x7_MODULATION_PSK3 0x00003000 +#define T55x7_MODULATION_FSK1 0x00004000 +#define T55x7_MODULATION_FSK2 0x00005000 +#define T55x7_MODULATION_FSK1a 0x00006000 +#define T55x7_MODULATION_FSK2a 0x00007000 +#define T55x7_MODULATION_MANCHESTER 0x00008000 +#define T55x7_MODULATION_BIPHASE 0x00010000 +#define T55x7_MODULATION_DIPHASE 0x00018000 +#define T55x7_X_MODE 0x00020000 +#define T55x7_BITRATE_RF_8 0 +#define T55x7_BITRATE_RF_16 0x00040000 +#define T55x7_BITRATE_RF_32 0x00080000 +#define T55x7_BITRATE_RF_40 0x000C0000 +#define T55x7_BITRATE_RF_50 0x00100000 +#define T55x7_BITRATE_RF_64 0x00140000 +#define T55x7_BITRATE_RF_100 0x00180000 +#define T55x7_BITRATE_RF_128 0x001C0000 +#define T55x7_TESTMODE_DISABLED 0x60000000 /* T5555 (Q5) configuration register definitions */ -#define T5555_ST_TERMINATOR 0x00000001 -#define T5555_MAXBLOCK_SHIFT 0x00000001 -#define T5555_MODULATION_MANCHESTER 0 -#define T5555_MODULATION_PSK1 0x00000010 -#define T5555_MODULATION_PSK2 0x00000020 -#define T5555_MODULATION_PSK3 0x00000030 -#define T5555_MODULATION_FSK1 0x00000040 -#define T5555_MODULATION_FSK2 0x00000050 -#define T5555_MODULATION_BIPHASE 0x00000060 -#define T5555_MODULATION_DIRECT 0x00000070 -#define T5555_INVERT_OUTPUT 0x00000080 -#define T5555_PSK_RF_2 0 -#define T5555_PSK_RF_4 0x00000100 -#define T5555_PSK_RF_8 0x00000200 -#define T5555_USE_PWD 0x00000400 -#define T5555_USE_AOR 0x00000800 -#define T5555_SET_BITRATE(x) (((x-2)/2)<<12) -#define T5555_GET_BITRATE(x) ((((x >> 12) & 0x3F)*2)+2) -#define T5555_BITRATE_SHIFT 12 //(RF=2n+2) ie 64=2*0x1F+2 or n = (RF-2)/2 -#define T5555_FAST_WRITE 0x00004000 -#define T5555_PAGE_SELECT 0x00008000 +#define T5555_ST_TERMINATOR 0x00000001 +#define T5555_MAXBLOCK_SHIFT 0x00000001 +#define T5555_MODULATION_MANCHESTER 0 +#define T5555_MODULATION_PSK1 0x00000010 +#define T5555_MODULATION_PSK2 0x00000020 +#define T5555_MODULATION_PSK3 0x00000030 +#define T5555_MODULATION_FSK1 0x00000040 +#define T5555_MODULATION_FSK2 0x00000050 +#define T5555_MODULATION_BIPHASE 0x00000060 +#define T5555_MODULATION_DIRECT 0x00000070 +#define T5555_INVERT_OUTPUT 0x00000080 +#define T5555_PSK_RF_2 0 +#define T5555_PSK_RF_4 0x00000100 +#define T5555_PSK_RF_8 0x00000200 +#define T5555_USE_PWD 0x00000400 +#define T5555_USE_AOR 0x00000800 +#define T5555_SET_BITRATE(x) (((x-2)/2)<<12) +#define T5555_GET_BITRATE(x) ((((x >> 12) & 0x3F)*2)+2) +#define T5555_BITRATE_SHIFT 12 //(RF=2n+2) ie 64=2*0x1F+2 or n = (RF-2)/2 +#define T5555_FAST_WRITE 0x00004000 +#define T5555_PAGE_SELECT 0x00008000 #define T55XX_WRITE_TIMEOUT 1500 @@ -420,152 +420,152 @@ uint32_t GetT55xxClockBit(uint32_t clock); // em4x05 & em4x69 chip configuration register definitions -#define EM4x05_GET_BITRATE(x) (((x & 0x3F)*2)+2) -#define EM4x05_SET_BITRATE(x) ((x-2)/2) -#define EM4x05_MODULATION_NRZ 0x00000000 -#define EM4x05_MODULATION_MANCHESTER 0x00000040 -#define EM4x05_MODULATION_BIPHASE 0x00000080 -#define EM4x05_MODULATION_MILLER 0x000000C0 //not supported by all 4x05/4x69 chips -#define EM4x05_MODULATION_PSK1 0x00000100 //not supported by all 4x05/4x69 chips -#define EM4x05_MODULATION_PSK2 0x00000140 //not supported by all 4x05/4x69 chips -#define EM4x05_MODULATION_PSK3 0x00000180 //not supported by all 4x05/4x69 chips -#define EM4x05_MODULATION_FSK1 0x00000200 //not supported by all 4x05/4x69 chips -#define EM4x05_MODULATION_FSK2 0x00000240 //not supported by all 4x05/4x69 chips -#define EM4x05_PSK_RF_2 0 -#define EM4x05_PSK_RF_4 0x00000400 -#define EM4x05_PSK_RF_8 0x00000800 -#define EM4x05_MAXBLOCK_SHIFT 14 -#define EM4x05_FIRST_USER_BLOCK 5 -#define EM4x05_SET_NUM_BLOCKS(x) ((x+5-1)<<14) //# of blocks sent during default read mode -#define EM4x05_GET_NUM_BLOCKS(x) (((x>>14) & 0xF)-5+1) -#define EM4x05_READ_LOGIN_REQ 1<<18 -#define EM4x05_READ_HK_LOGIN_REQ 1<<19 -#define EM4x05_WRITE_LOGIN_REQ 1<<20 -#define EM4x05_WRITE_HK_LOGIN_REQ 1<<21 -#define EM4x05_READ_AFTER_WRITE 1<<22 -#define EM4x05_DISABLE_ALLOWED 1<<23 -#define EM4x05_READER_TALK_FIRST 1<<24 +#define EM4x05_GET_BITRATE(x) (((x & 0x3F)*2)+2) +#define EM4x05_SET_BITRATE(x) ((x-2)/2) +#define EM4x05_MODULATION_NRZ 0x00000000 +#define EM4x05_MODULATION_MANCHESTER 0x00000040 +#define EM4x05_MODULATION_BIPHASE 0x00000080 +#define EM4x05_MODULATION_MILLER 0x000000C0 //not supported by all 4x05/4x69 chips +#define EM4x05_MODULATION_PSK1 0x00000100 //not supported by all 4x05/4x69 chips +#define EM4x05_MODULATION_PSK2 0x00000140 //not supported by all 4x05/4x69 chips +#define EM4x05_MODULATION_PSK3 0x00000180 //not supported by all 4x05/4x69 chips +#define EM4x05_MODULATION_FSK1 0x00000200 //not supported by all 4x05/4x69 chips +#define EM4x05_MODULATION_FSK2 0x00000240 //not supported by all 4x05/4x69 chips +#define EM4x05_PSK_RF_2 0 +#define EM4x05_PSK_RF_4 0x00000400 +#define EM4x05_PSK_RF_8 0x00000800 +#define EM4x05_MAXBLOCK_SHIFT 14 +#define EM4x05_FIRST_USER_BLOCK 5 +#define EM4x05_SET_NUM_BLOCKS(x) ((x+5-1)<<14) //# of blocks sent during default read mode +#define EM4x05_GET_NUM_BLOCKS(x) (((x>>14) & 0xF)-5+1) +#define EM4x05_READ_LOGIN_REQ 1<<18 +#define EM4x05_READ_HK_LOGIN_REQ 1<<19 +#define EM4x05_WRITE_LOGIN_REQ 1<<20 +#define EM4x05_WRITE_HK_LOGIN_REQ 1<<21 +#define EM4x05_READ_AFTER_WRITE 1<<22 +#define EM4x05_DISABLE_ALLOWED 1<<23 +#define EM4x05_READER_TALK_FIRST 1<<24 // FeliCa protocol -#define FELICA_POLL_REQ 0x00 -#define FELICA_POLL_ACK 0x01 +#define FELICA_POLL_REQ 0x00 +#define FELICA_POLL_ACK 0x01 -#define FELICA_REQSRV_REQ 0x02 -#define FELICA_REQSRV_ACK 0x03 +#define FELICA_REQSRV_REQ 0x02 +#define FELICA_REQSRV_ACK 0x03 -#define FELICA_REQRESP_REQ 0x04 -#define FELICA_REQRESP_ACK 0x05 +#define FELICA_REQRESP_REQ 0x04 +#define FELICA_REQRESP_ACK 0x05 -#define FELICA_RDBLK_REQ 0x06 -#define FELICA_RDBLK_ACK 0x07 +#define FELICA_RDBLK_REQ 0x06 +#define FELICA_RDBLK_ACK 0x07 -#define FELICA_WRTBLK_REQ 0x08 -#define FELICA_WRTBLK_ACK 0x09 +#define FELICA_WRTBLK_REQ 0x08 +#define FELICA_WRTBLK_ACK 0x09 -#define FELICA_SRCHSYSCODE_REQ 0x0a -#define FELICA_SRCHSYSCODE_ACK 0x0b +#define FELICA_SRCHSYSCODE_REQ 0x0a +#define FELICA_SRCHSYSCODE_ACK 0x0b -#define FELICA_REQSYSCODE_REQ 0x0c -#define FELICA_REQSYSCODE_ACK 0x0d +#define FELICA_REQSYSCODE_REQ 0x0c +#define FELICA_REQSYSCODE_ACK 0x0d -#define FELICA_AUTH1_REQ 0x10 -#define FELICA_AUTH1_ACK 0x11 +#define FELICA_AUTH1_REQ 0x10 +#define FELICA_AUTH1_ACK 0x11 -#define FELICA_AUTH2_REQ 0x12 -#define FELICA_AUTH2_ACK 0x13 +#define FELICA_AUTH2_REQ 0x12 +#define FELICA_AUTH2_ACK 0x13 -#define FELICA_RDSEC_REQ 0x14 -#define FELICA_RDSEC_ACK 0x15 +#define FELICA_RDSEC_REQ 0x14 +#define FELICA_RDSEC_ACK 0x15 -#define FELICA_WRTSEC_REQ 0x16 -#define FELICA_WRTSEC_ACK 0x17 +#define FELICA_WRTSEC_REQ 0x16 +#define FELICA_WRTSEC_ACK 0x17 -#define FELICA_REQSRV2_REQ 0x32 -#define FELICA_REQSRV2_ACK 0x33 +#define FELICA_REQSRV2_REQ 0x32 +#define FELICA_REQSRV2_ACK 0x33 -#define FELICA_GETSTATUS_REQ 0x38 -#define FELICA_GETSTATUS_ACK 0x39 +#define FELICA_GETSTATUS_REQ 0x38 +#define FELICA_GETSTATUS_ACK 0x39 -#define FELICA_OSVER_REQ 0x3c -#define FELICA_OSVER_ACK 0x3d +#define FELICA_OSVER_REQ 0x3c +#define FELICA_OSVER_ACK 0x3d -#define FELICA_RESET_MODE_REQ 0x3e -#define FELICA_RESET_MODE_ACK 0x3f +#define FELICA_RESET_MODE_REQ 0x3e +#define FELICA_RESET_MODE_ACK 0x3f -#define FELICA_AUTH1V2_REQ 0x40 -#define FELICA_AUTH1V2_ACK 0x41 +#define FELICA_AUTH1V2_REQ 0x40 +#define FELICA_AUTH1V2_ACK 0x41 -#define FELICA_AUTH2V2_REQ 0x42 -#define FELICA_AUTH2V2_ACK 0x43 +#define FELICA_AUTH2V2_REQ 0x42 +#define FELICA_AUTH2V2_ACK 0x43 -#define FELICA_RDSECV2_REQ 0x44 -#define FELICA_RDSECV2_ACK 0x45 -#define FELICA_WRTSECV2_REQ 0x46 -#define FELICA_WRTSECV2_ACK 0x47 +#define FELICA_RDSECV2_REQ 0x44 +#define FELICA_RDSECV2_ACK 0x45 +#define FELICA_WRTSECV2_REQ 0x46 +#define FELICA_WRTSECV2_ACK 0x47 -#define FELICA_UPDATE_RNDID_REQ 0x4C -#define FELICA_UPDATE_RNDID_ACK 0x4D +#define FELICA_UPDATE_RNDID_REQ 0x4C +#define FELICA_UPDATE_RNDID_ACK 0x4D // FeliCa SYSTEM list -#define SYSTEMCODE_ANY 0xffff // ANY -#define SYSTEMCODE_FELICA_LITE 0x88b4 // FeliCa Lite -#define SYSTEMCODE_COMMON 0xfe00 // Common -#define SYSTEMCODE_EDY 0xfe00 // Edy -#define SYSTEMCODE_CYBERNE 0x0003 // Cyberne -#define SYSTEMCODE_SUICA 0x0003 // Suica -#define SYSTEMCODE_PASMO 0x0003 // Pasmo +#define SYSTEMCODE_ANY 0xffff // ANY +#define SYSTEMCODE_FELICA_LITE 0x88b4 // FeliCa Lite +#define SYSTEMCODE_COMMON 0xfe00 // Common +#define SYSTEMCODE_EDY 0xfe00 // Edy +#define SYSTEMCODE_CYBERNE 0x0003 // Cyberne +#define SYSTEMCODE_SUICA 0x0003 // Suica +#define SYSTEMCODE_PASMO 0x0003 // Pasmo //FeliCa Service list Suica/pasmo (little endian) -#define SERVICE_SUICA_INOUT 0x108f // SUICA/PASMO -#define SERVICE_SUICA_HISTORY 0x090f // SUICA/PASMO -#define SERVICE_FELICA_LITE_READONLY 0x0b00 // FeliCa Lite RO -#define SERVICE_FELICA_LITE_READWRITE 0x0900 // FeliCa Lite RW +#define SERVICE_SUICA_INOUT 0x108f // SUICA/PASMO +#define SERVICE_SUICA_HISTORY 0x090f // SUICA/PASMO +#define SERVICE_FELICA_LITE_READONLY 0x0b00 // FeliCa Lite RO +#define SERVICE_FELICA_LITE_READWRITE 0x0900 // FeliCa Lite RW // Calypso protocol -#define CALYPSO_GET_RESPONSE 0xC0 -#define CALYPSO_SELECT 0xA4 -#define CALYPSO_INVALIDATE 0x04 -#define CALYPSO_REHABILITATE 0x44 -#define CALYPSO_APPEND_RECORD 0xE2 -#define CALYPSO_DECREASE 0x30 -#define CALYPSO_INCREASE 0x32 -#define CALYPSO_READ_BINARY 0xB0 -#define CALYPSO_READ_RECORD 0xB2 -#define CALYPSO_UPDATE_BINARY 0xD6 -#define CALYPSO_UPDATE_RECORD 0xDC -#define CALYPSO_WRITE_RECORD 0xD2 -#define CALYPSO_OPEN_SESSION 0x8A -#define CALYPSO_CLOSE_SESSION 0x8E -#define CALYPSO_GET_CHALLENGE 0x84 -#define CALYPSO_CHANGE_PIN 0xD8 -#define CALYPSO_VERIFY_PIN 0x20 -#define CALYPSO_SV_GET 0x7C -#define CALYPSO_SV_DEBIT 0xBA -#define CALYPSO_SV_RELOAD 0xB8 -#define CALYPSO_SV_UN_DEBIT 0xBC -#define CALYPSO_SAM_SV_DEBIT 0x54 -#define CALYPSO_SAM_SV_RELOAD 0x56 +#define CALYPSO_GET_RESPONSE 0xC0 +#define CALYPSO_SELECT 0xA4 +#define CALYPSO_INVALIDATE 0x04 +#define CALYPSO_REHABILITATE 0x44 +#define CALYPSO_APPEND_RECORD 0xE2 +#define CALYPSO_DECREASE 0x30 +#define CALYPSO_INCREASE 0x32 +#define CALYPSO_READ_BINARY 0xB0 +#define CALYPSO_READ_RECORD 0xB2 +#define CALYPSO_UPDATE_BINARY 0xD6 +#define CALYPSO_UPDATE_RECORD 0xDC +#define CALYPSO_WRITE_RECORD 0xD2 +#define CALYPSO_OPEN_SESSION 0x8A +#define CALYPSO_CLOSE_SESSION 0x8E +#define CALYPSO_GET_CHALLENGE 0x84 +#define CALYPSO_CHANGE_PIN 0xD8 +#define CALYPSO_VERIFY_PIN 0x20 +#define CALYPSO_SV_GET 0x7C +#define CALYPSO_SV_DEBIT 0xBA +#define CALYPSO_SV_RELOAD 0xB8 +#define CALYPSO_SV_UN_DEBIT 0xBC +#define CALYPSO_SAM_SV_DEBIT 0x54 +#define CALYPSO_SAM_SV_RELOAD 0x56 // iclass / picopass chip config structures and shared routines typedef struct { - uint8_t app_limit; //[8] - uint8_t otp[2]; //[9-10] - uint8_t block_writelock;//[11] - uint8_t chip_config; //[12] - uint8_t mem_config; //[13] - uint8_t eas; //[14] - uint8_t fuses; //[15] + uint8_t app_limit; //[8] + uint8_t otp[2]; //[9-10] + uint8_t block_writelock;//[11] + uint8_t chip_config; //[12] + uint8_t mem_config; //[13] + uint8_t eas; //[14] + uint8_t fuses; //[15] } picopass_conf_block; typedef struct { - uint8_t csn[8]; - picopass_conf_block conf; - uint8_t epurse[8]; - uint8_t key_d[8]; - uint8_t key_c[8]; - uint8_t app_issuer_area[8]; + uint8_t csn[8]; + picopass_conf_block conf; + uint8_t epurse[8]; + uint8_t key_d[8]; + uint8_t key_c[8]; + uint8_t app_issuer_area[8]; } picopass_hdr; diff --git a/common/random.c b/common/random.c index 96cdbb628..060f0bd7a 100644 --- a/common/random.c +++ b/common/random.c @@ -14,17 +14,17 @@ static uint32_t g_nextrandom; */ inline void fast_prand(){ - fast_prandEx(GetTickCount()); + fast_prandEx(GetTickCount()); } inline void fast_prandEx(uint32_t seed) { - g_nextrandom = seed; + g_nextrandom = seed; } uint32_t prand() { -// g_nextrandom *= 6364136223846793005; -// g_nextrandom += 1; +// g_nextrandom *= 6364136223846793005; +// g_nextrandom += 1; //return (uint32_t)(g_nextrandom >> 32) % 0xffffffff; - g_nextrandom = (214013 * g_nextrandom + 2531011); - return (g_nextrandom>>16) & 0xFFFF; + g_nextrandom = (214013 * g_nextrandom + 2531011); + return (g_nextrandom>>16) & 0xFFFF; } diff --git a/common/tea.c b/common/tea.c index b3e1a39c1..287a12de9 100644 --- a/common/tea.c +++ b/common/tea.c @@ -13,52 +13,52 @@ void tea_encrypt(uint8_t *v, uint8_t *key) { - uint32_t a=0,b=0,c=0,d=0,y=0,z=0; - uint32_t sum = 0; - uint8_t n = ROUNDS; + uint32_t a=0,b=0,c=0,d=0,y=0,z=0; + uint32_t sum = 0; + uint8_t n = ROUNDS; - //key - a = bytes_to_num(key, 4); - b = bytes_to_num(key+4, 4); - c = bytes_to_num(key+8, 4); - d = bytes_to_num(key+12, 4); + //key + a = bytes_to_num(key, 4); + b = bytes_to_num(key+4, 4); + c = bytes_to_num(key+8, 4); + d = bytes_to_num(key+12, 4); - //input - y = bytes_to_num(v, 4); - z = bytes_to_num(v+4, 4); + //input + y = bytes_to_num(v, 4); + z = bytes_to_num(v+4, 4); - while ( n-- > 0 ) { - sum += DELTA; - y += ((z << 4) + a) ^ (z + sum) ^ ((z >> 5) + b); - z += ((y << 4) + c) ^ (y + sum) ^ ((y >> 5) + d); - } + while ( n-- > 0 ) { + sum += DELTA; + y += ((z << 4) + a) ^ (z + sum) ^ ((z >> 5) + b); + z += ((y << 4) + c) ^ (y + sum) ^ ((y >> 5) + d); + } - num_to_bytes(y, 4, v); - num_to_bytes(z, 4, v+4); + num_to_bytes(y, 4, v); + num_to_bytes(z, 4, v+4); } void tea_decrypt(uint8_t *v, uint8_t *key) { - uint32_t a=0,b=0,c=0,d=0,y=0,z=0; - uint32_t sum = SUM; - uint8_t n = ROUNDS; + uint32_t a=0,b=0,c=0,d=0,y=0,z=0; + uint32_t sum = SUM; + uint8_t n = ROUNDS; - //key - a = bytes_to_num(key, 4); - b = bytes_to_num(key+4, 4); - c = bytes_to_num(key+8, 4); - d = bytes_to_num(key+12, 4); + //key + a = bytes_to_num(key, 4); + b = bytes_to_num(key+4, 4); + c = bytes_to_num(key+8, 4); + d = bytes_to_num(key+12, 4); - //input - y = bytes_to_num(v, 4); - z = bytes_to_num(v+4, 4); + //input + y = bytes_to_num(v, 4); + z = bytes_to_num(v+4, 4); - /* sum = delta<<5, in general sum = delta * n */ - while ( n-- > 0 ) { - z -= ((y << 4) + c) ^ (y + sum) ^ ((y >> 5) + d); - y -= ((z << 4) + a) ^ (z + sum) ^ ((z >> 5) + b); - sum -= DELTA; - } - num_to_bytes(y, 4, v); - num_to_bytes(z, 4, v+4); + /* sum = delta<<5, in general sum = delta * n */ + while ( n-- > 0 ) { + z -= ((y << 4) + c) ^ (y + sum) ^ ((y >> 5) + d); + y -= ((z << 4) + a) ^ (z + sum) ^ ((z >> 5) + b); + sum -= DELTA; + } + num_to_bytes(y, 4, v); + num_to_bytes(z, 4, v+4); } diff --git a/common/usart.c b/common/usart.c index c62e3c8f6..9371e6243 100644 --- a/common/usart.c +++ b/common/usart.c @@ -11,19 +11,19 @@ #include "usart.h" #include "string.h" -#define AT91_BAUD_RATE 115200 +#define AT91_BAUD_RATE 115200 -volatile AT91PS_USART pUS1 = AT91C_BASE_US1; -volatile AT91PS_PIO pPIO = AT91C_BASE_PIOA; -volatile AT91PS_PDC pPDC = AT91C_BASE_PDC_US1; +volatile AT91PS_USART pUS1 = AT91C_BASE_US1; +volatile AT91PS_PIO pPIO = AT91C_BASE_PIOA; +volatile AT91PS_PDC pPDC = AT91C_BASE_PDC_US1; /* void usart_close(void) { // Reset the USART mode - pUS1->US_MR = 0; + pUS1->US_MR = 0; // Reset the baud rate divisor register - pUS1->US_BRGR = 0; + pUS1->US_BRGR = 0; // Reset the Timeguard Register pUS1->US_TTGR = 0; @@ -32,7 +32,7 @@ void usart_close(void) { pUS1->US_IDR = 0xFFFFFFFF; // Abort the Peripheral Data Transfers - pUS1->US_PTCR = AT91C_PDC_RXTDIS | AT91C_PDC_TXTDIS; + pUS1->US_PTCR = AT91C_PDC_RXTDIS | AT91C_PDC_TXTDIS; // Disable receiver and transmitter and stop any activity immediately pUS1->US_CR = AT91C_US_TXDIS | AT91C_US_RXDIS | AT91C_US_RSTTX | AT91C_US_RSTRX; @@ -46,12 +46,12 @@ static uint8_t us_outbuf[sizeof(UsbCommand)]; /// \param len Size of the data buffer (in bytes). inline int16_t usart_readbuffer(uint8_t *data, size_t len) { - // Check if the first PDC bank is free + // Check if the first PDC bank is free if (!(pUS1->US_RCR)) { pUS1->US_RPR = (uint32_t)data; pUS1->US_RCR = len; - pUS1->US_PTCR = AT91C_PDC_RXTEN | AT91C_PDC_TXTDIS; + pUS1->US_PTCR = AT91C_PDC_RXTEN | AT91C_PDC_TXTDIS; return 2; } // Check if the second PDC bank is free @@ -59,7 +59,7 @@ inline int16_t usart_readbuffer(uint8_t *data, size_t len) { pUS1->US_RNPR = (uint32_t)data; pUS1->US_RNCR = len; - pUS1->US_PTCR = AT91C_PDC_RXTEN | AT91C_PDC_TXTDIS; + pUS1->US_PTCR = AT91C_PDC_RXTEN | AT91C_PDC_TXTDIS; return 1; } else { return 0; @@ -72,21 +72,21 @@ inline int16_t usart_writebuffer(uint8_t *data, size_t len) { // Check if the first PDC bank is free if (!(pUS1->US_TCR)) { - memcpy(us_outbuf, data, len); - pUS1->US_TPR = (uint32_t)us_outbuf; + memcpy(us_outbuf, data, len); + pUS1->US_TPR = (uint32_t)us_outbuf; pUS1->US_TCR = sizeof(us_outbuf); - pUS1->US_PTCR = AT91C_PDC_TXTEN | AT91C_PDC_RXTDIS; - return 2; + pUS1->US_PTCR = AT91C_PDC_TXTEN | AT91C_PDC_RXTDIS; + return 2; } // Check if the second PDC bank is free else if (!(pUS1->US_TNCR)) { - memcpy(us_outbuf, data, len); + memcpy(us_outbuf, data, len); pUS1->US_TNPR = (uint32_t)us_outbuf; pUS1->US_TNCR = sizeof(us_outbuf); - pUS1->US_PTCR = AT91C_PDC_TXTEN | AT91C_PDC_RXTDIS; - return 1; + pUS1->US_PTCR = AT91C_PDC_TXTEN | AT91C_PDC_RXTDIS; + return 1; } else { return 0; } @@ -94,21 +94,21 @@ inline int16_t usart_writebuffer(uint8_t *data, size_t len) { void usart_init(void) { - // disable & reset receiver / transmitter for configuration - pUS1->US_CR = (AT91C_US_RSTRX | AT91C_US_RSTTX | AT91C_US_RXDIS | AT91C_US_TXDIS); + // disable & reset receiver / transmitter for configuration + pUS1->US_CR = (AT91C_US_RSTRX | AT91C_US_RSTTX | AT91C_US_RXDIS | AT91C_US_TXDIS); - //enable the USART1 Peripheral clock - AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_US1); + //enable the USART1 Peripheral clock + AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_US1); - // disable PIO control of receive / transmit pins - pPIO->PIO_PDR |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); + // disable PIO control of receive / transmit pins + pPIO->PIO_PDR |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); - // enable peripheral mode A on receive / transmit pins - pPIO->PIO_ASR |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); - pPIO->PIO_BSR = 0; + // enable peripheral mode A on receive / transmit pins + pPIO->PIO_ASR |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); + pPIO->PIO_BSR = 0; - // enable pull-up on receive / transmit pins (see 31.5.1 I/O Lines) - pPIO->PIO_PPUER |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); + // enable pull-up on receive / transmit pins (see 31.5.1 I/O Lines) + pPIO->PIO_PPUER |= (AT91C_PA21_RXD1 | AT91C_PA22_TXD1); // set mode pUS1->US_MR = AT91C_US_USMODE_NORMAL | // normal mode @@ -118,26 +118,26 @@ void usart_init(void) { AT91C_US_NBSTOP_1_BIT | // 1 stop bit AT91C_US_CHMODE_NORMAL; // channel mode: normal - // all interrupts disabled - pUS1->US_IDR = 0xFFFF; + // all interrupts disabled + pUS1->US_IDR = 0xFFFF; - // iceman, setting 115200 doesn't work. Only speed I got to work is 9600. - // something fishy with the AT91SAM7S512 USART.. Or I missed something - // For a nice detailed sample, interrupt driven but still relevant. - // See https://www.sparkfun.com/datasheets/DevTools/SAM7/at91sam7%20serial%20communications.pdf + // iceman, setting 115200 doesn't work. Only speed I got to work is 9600. + // something fishy with the AT91SAM7S512 USART.. Or I missed something + // For a nice detailed sample, interrupt driven but still relevant. + // See https://www.sparkfun.com/datasheets/DevTools/SAM7/at91sam7%20serial%20communications.pdf - // set baudrate to 115200 - // 115200 * 16 == 1843200 - // - //pUS1->US_BRGR = (48UL*1000*1000) / (9600*16); - pUS1->US_BRGR = 48054841 / (9600 << 4); + // set baudrate to 115200 + // 115200 * 16 == 1843200 + // + //pUS1->US_BRGR = (48UL*1000*1000) / (9600*16); + pUS1->US_BRGR = 48054841 / (9600 << 4); - // Write the Timeguard Register - pUS1->US_TTGR = 0; - pUS1->US_RTOR = 0; - pUS1->US_FIDI = 0; - pUS1->US_IF = 0; + // Write the Timeguard Register + pUS1->US_TTGR = 0; + pUS1->US_RTOR = 0; + pUS1->US_FIDI = 0; + pUS1->US_IF = 0; - // re-enable receiver / transmitter - pUS1->US_CR = (AT91C_US_RXEN | AT91C_US_TXEN); + // re-enable receiver / transmitter + pUS1->US_CR = (AT91C_US_RXEN | AT91C_US_TXEN); } \ No newline at end of file diff --git a/common/usb_cdc.c b/common/usb_cdc.c index b1acff692..b60ec8176 100644 --- a/common/usb_cdc.c +++ b/common/usb_cdc.c @@ -64,21 +64,21 @@ AT91SAM7S256 USB Device Port #define USB_DESCRIPTOR_OTHER_SPEED 0x07 // DescriptorType for a Other Speed Configuration. #define USB_DESCRIPTOR_INTERFACE_POWER 0x08 // DescriptorType for Interface Power. #define USB_DESCRIPTOR_OTG 0x09 // DescriptorType for an OTG Descriptor. -#define USB_DESCRIPTOR_IAD 0x0B // DescriptorType for a Interface Association Descriptor +#define USB_DESCRIPTOR_IAD 0x0B // DescriptorType for a Interface Association Descriptor #define USB_DESCRIPTOR_TYPE_BO 0x0F // DescriptorType for a BOS Descriptor. /* Configuration Attributes */ #define _DEFAULT (0x01<<7) //Default Value (Bit 7 is set) #define _SELF (0x01<<6) //Self-powered (Supports if set) #define _RWU (0x01<<5) //Remote Wakeup (Supports if set) -#define _HNP (0x01 << 1) //HNP (Supports if set) -#define _SRP (0x01) //SRP (Supports if set) +#define _HNP (0x01 << 1) //HNP (Supports if set) +#define _SRP (0x01) //SRP (Supports if set) /* Endpoint Transfer Type */ #define _CTRL 0x00 //Control Transfer #define _ISO 0x01 //Isochronous Transfer -#define _BULK 0x02 //Bulk Transfer -#define _INTERRUPT 0x03 //Interrupt Transfer +#define _BULK 0x02 //Bulk Transfer +#define _INTERRUPT 0x03 //Interrupt Transfer // (bit7 | 0 = OUT, 1 = IN) #define _EP_IN 0x80 @@ -92,12 +92,12 @@ AT91SAM7S256 USB Device Port /* WCID specific Request Code */ -#define MS_OS_DESCRIPTOR_INDEX 0xEE -#define MS_VENDOR_CODE 0x1C +#define MS_OS_DESCRIPTOR_INDEX 0xEE +#define MS_VENDOR_CODE 0x1C #define MS_EXTENDED_COMPAT_ID 0x04 #define MS_EXTENDED_PROPERTIES 0x05 -#define MS_WCID_GET_DESCRIPTOR 0xC0 -#define MS_WCID_GET_FEATURE_DESCRIPTOR 0xC1 +#define MS_WCID_GET_DESCRIPTOR 0xC0 +#define MS_WCID_GET_FEATURE_DESCRIPTOR 0xC1 /* USB standard request code */ #define STD_GET_STATUS_ZERO 0x0080 @@ -132,275 +132,275 @@ uint8_t btConnection = 0; uint8_t btReceiveBank = AT91C_UDP_RX_DATA_BK0; static const char devDescriptor[] = { - /* Device descriptor */ - 0x12, // Length - USB_DESCRIPTOR_DEVICE, // Descriptor Type (DEVICE) - 0x00,0x02, // Complies with USB Spec. Release (0200h = release 2.00) 0210 == release 2.10 - 2, // Device Class: Communication Device Class - 0, // Device Subclass: CDC class sub code ACM [ice 0x02 = win10 virtual comport ] - 0, // Device Protocol: CDC Device protocol (unused) - AT91C_EP_CONTROL_SIZE, // MaxPacketSize0 + /* Device descriptor */ + 0x12, // Length + USB_DESCRIPTOR_DEVICE, // Descriptor Type (DEVICE) + 0x00,0x02, // Complies with USB Spec. Release (0200h = release 2.00) 0210 == release 2.10 + 2, // Device Class: Communication Device Class + 0, // Device Subclass: CDC class sub code ACM [ice 0x02 = win10 virtual comport ] + 0, // Device Protocol: CDC Device protocol (unused) + AT91C_EP_CONTROL_SIZE, // MaxPacketSize0 0xc4,0x9a, // Vendor ID [0x9ac4 = J. Westhues] 0x8f,0x4b, // Product ID [0x4b8f = Proxmark-3 RFID Instrument] - 0x00,0x01, // BCD Device release number (1.00) - 1, // index Manufacturer - 2, // index Product - 3, // index SerialNumber - 1 // Number of Configs + 0x00,0x01, // BCD Device release number (1.00) + 1, // index Manufacturer + 2, // index Product + 3, // index SerialNumber + 1 // Number of Configs }; static const char cfgDescriptor[] = { - /* Configuration 1 descriptor */ - // ----------------------------- - 9, // Length - USB_DESCRIPTOR_CONFIGURATION, // Descriptor Type - (9+9+5+5+4+5+7+9+7+7), 0, // Total Length 2 EP + Control - 2, // Number of Interfaces - 1, // Index value of this Configuration (used in SetConfiguration from Host) - 0, // Configuration string index - _DEFAULT, // Attributes 0xA0 - 0xFA, // Max Power consumption + /* Configuration 1 descriptor */ + // ----------------------------- + 9, // Length + USB_DESCRIPTOR_CONFIGURATION, // Descriptor Type + (9+9+5+5+4+5+7+9+7+7), 0, // Total Length 2 EP + Control + 2, // Number of Interfaces + 1, // Index value of this Configuration (used in SetConfiguration from Host) + 0, // Configuration string index + _DEFAULT, // Attributes 0xA0 + 0xFA, // Max Power consumption - // IAD to associate the one CDC interface + // IAD to associate the one CDC interface // -------------------------------------- /* - 8, // Length - USB_DESCRIPTOR_IAD, // IAD_DESCRIPTOR (0x0B) - 0, // CDC_INT_INTERFACE NUMBER ( - 2, // IAD INTERFACE COUNT (two interfaces) - 2, // Function Class: CDC_CLASS - 2, // Function SubClass: ACM - 1, // Function Protocol: v.25term - 0, // iInterface + 8, // Length + USB_DESCRIPTOR_IAD, // IAD_DESCRIPTOR (0x0B) + 0, // CDC_INT_INTERFACE NUMBER ( + 2, // IAD INTERFACE COUNT (two interfaces) + 2, // Function Class: CDC_CLASS + 2, // Function SubClass: ACM + 1, // Function Protocol: v.25term + 0, // iInterface */ - /* Interface 0 Descriptor */ - /* CDC Communication Class Interface Descriptor Requirement for Notification*/ - // ----------------------------------------------------------- - 9, // Length - USB_DESCRIPTOR_INTERFACE, // Descriptor Type - 0, // Interface Number - 0, // Alternate Setting - 1, // Number of Endpoints in this interface - 2, // Interface Class code (Communication Interface Class) - 2, // Interface Subclass code (Abstract Control Model) - 1, // InterfaceProtocol (Common AT Commands, V.25term) - 0, // iInterface + /* Interface 0 Descriptor */ + /* CDC Communication Class Interface Descriptor Requirement for Notification*/ + // ----------------------------------------------------------- + 9, // Length + USB_DESCRIPTOR_INTERFACE, // Descriptor Type + 0, // Interface Number + 0, // Alternate Setting + 1, // Number of Endpoints in this interface + 2, // Interface Class code (Communication Interface Class) + 2, // Interface Subclass code (Abstract Control Model) + 1, // InterfaceProtocol (Common AT Commands, V.25term) + 0, // iInterface - /* Header Functional Descriptor */ - 5, // Function Length - 0x24, // Descriptor type: CS_INTERFACE - 0, // Descriptor subtype: Header Functional Descriptor - 0x10,0x01, // bcd CDC:1.1 + /* Header Functional Descriptor */ + 5, // Function Length + 0x24, // Descriptor type: CS_INTERFACE + 0, // Descriptor subtype: Header Functional Descriptor + 0x10,0x01, // bcd CDC:1.1 - /* ACM Functional Descriptor */ - 4, // Function Length - 0x24, // Descriptor Type: CS_INTERFACE - 2, // Descriptor Subtype: Abstract Control Management Functional Descriptor - 2, // Capabilities D1, Device supports the request combination of Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, and the notification Serial_State + /* ACM Functional Descriptor */ + 4, // Function Length + 0x24, // Descriptor Type: CS_INTERFACE + 2, // Descriptor Subtype: Abstract Control Management Functional Descriptor + 2, // Capabilities D1, Device supports the request combination of Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, and the notification Serial_State - /* Union Functional Descriptor */ - 5, // Function Length - 0x24, // Descriptor Type: CS_INTERFACE - 6, // Descriptor Subtype: Union Functional Descriptor - 0, // MasterInterface: Communication Class Interface - 1, // SlaveInterface0: Data Class Interface + /* Union Functional Descriptor */ + 5, // Function Length + 0x24, // Descriptor Type: CS_INTERFACE + 6, // Descriptor Subtype: Union Functional Descriptor + 0, // MasterInterface: Communication Class Interface + 1, // SlaveInterface0: Data Class Interface - /* Call Management Functional Descriptor */ - 5, // Function Length - 0x24, // Descriptor Type: CS_INTERFACE - 1, // Descriptor Subtype: Call Management Functional Descriptor - 0, // Capabilities: Device sends/receives call management information only over the Communication Class interface. Device does not handle call management itself - 1, // Data Interface: Data Class Interface + /* Call Management Functional Descriptor */ + 5, // Function Length + 0x24, // Descriptor Type: CS_INTERFACE + 1, // Descriptor Subtype: Call Management Functional Descriptor + 0, // Capabilities: Device sends/receives call management information only over the Communication Class interface. Device does not handle call management itself + 1, // Data Interface: Data Class Interface - /* Protocol Functional Descriptor */ - /* - 6, - 0x24, // Descriptor Type: CS_INTERFACE - 0x0B, // Descriptor Subtype: Protocol Unit functional Descriptor - 0xDD, // constant uniq ID of unit - 0xFE, // protocol - */ + /* Protocol Functional Descriptor */ + /* + 6, + 0x24, // Descriptor Type: CS_INTERFACE + 0x0B, // Descriptor Subtype: Protocol Unit functional Descriptor + 0xDD, // constant uniq ID of unit + 0xFE, // protocol + */ - /* CDC Notification Endpoint descriptor */ - // --------------------------------------- - 7, // Length - USB_DESCRIPTOR_ENDPOINT, // Descriptor Type - _EP03_IN, // EndpointAddress: Endpoint 03 - IN - _INTERRUPT, // Attributes - AT91C_EP_CONTROL_SIZE, 0x00, // MaxPacket Size: EP0 - 8 - 0xFF, // Interval polling + /* CDC Notification Endpoint descriptor */ + // --------------------------------------- + 7, // Length + USB_DESCRIPTOR_ENDPOINT, // Descriptor Type + _EP03_IN, // EndpointAddress: Endpoint 03 - IN + _INTERRUPT, // Attributes + AT91C_EP_CONTROL_SIZE, 0x00, // MaxPacket Size: EP0 - 8 + 0xFF, // Interval polling - /* Interface 1 Descriptor */ - /* CDC Data Class Interface 1 Descriptor Requirement */ - 9, // Length - USB_DESCRIPTOR_INTERFACE, // Descriptor Type - 1, // Interface Number - 0, // Alternate Setting - 2, // Number of Endpoints - 0x0A, // Interface Class: CDC Data interface class - 0, // Interface Subclass: not used - 0, // Interface Protocol: No class specific protocol required (usb spec) - 0, // Interface + /* Interface 1 Descriptor */ + /* CDC Data Class Interface 1 Descriptor Requirement */ + 9, // Length + USB_DESCRIPTOR_INTERFACE, // Descriptor Type + 1, // Interface Number + 0, // Alternate Setting + 2, // Number of Endpoints + 0x0A, // Interface Class: CDC Data interface class + 0, // Interface Subclass: not used + 0, // Interface Protocol: No class specific protocol required (usb spec) + 0, // Interface - /* Endpoint descriptor */ - 7, // Length - USB_DESCRIPTOR_ENDPOINT, // Descriptor Type - _EP01_OUT, // Endpoint Address: Endpoint 01 - OUT - _BULK, // Attributes: BULK - AT91C_EP_OUT_SIZE, 0x00, // MaxPacket Size: 64 bytes - 0, // Interval: ignored for bulk + /* Endpoint descriptor */ + 7, // Length + USB_DESCRIPTOR_ENDPOINT, // Descriptor Type + _EP01_OUT, // Endpoint Address: Endpoint 01 - OUT + _BULK, // Attributes: BULK + AT91C_EP_OUT_SIZE, 0x00, // MaxPacket Size: 64 bytes + 0, // Interval: ignored for bulk - /* Endpoint descriptor */ - 7, // Length - USB_DESCRIPTOR_ENDPOINT, // Descriptor Type - _EP02_IN, // Endpoint Address: Endpoint 02 - IN - _BULK, // Attribute: BULK - AT91C_EP_IN_SIZE, 0x00, // MaxPacket Size: 64 bytes - 0 // Interval: ignored for bulk + /* Endpoint descriptor */ + 7, // Length + USB_DESCRIPTOR_ENDPOINT, // Descriptor Type + _EP02_IN, // Endpoint Address: Endpoint 02 - IN + _BULK, // Attribute: BULK + AT91C_EP_IN_SIZE, 0x00, // MaxPacket Size: 64 bytes + 0 // Interval: ignored for bulk }; // BOS descriptor static const char bosDescriptor[] = { - 0x5, - USB_DESCRIPTOR_TYPE_BO, - 0xC, - 0x0, - 0x1, // 1 device capability - 0x7, - 0x10, //USB_DEVICE_CAPABITY_TYPE, - 0x2, - 0x2, // LPM capability bit set - 0x0, - 0x0, - 0x0 + 0x5, + USB_DESCRIPTOR_TYPE_BO, + 0xC, + 0x0, + 0x1, // 1 device capability + 0x7, + 0x10, // USB_DEVICE_CAPABITY_TYPE, + 0x2, + 0x2, // LPM capability bit set + 0x0, + 0x0, + 0x0 }; // Microsoft OS Extended Configuration Compatible ID Descriptor /* static const char CompatIDFeatureDescriptor[] = { - 0x28, 0x00, 0x00, 0x00, // Descriptor Length 40bytes (0x28) - 0x00, 0x01, // Version ('1.0') - MS_EXTENDED_COMPAT_ID, 0x00, // Compatibility ID Descriptor Index 0x0004 - 0x01, // Number of sections. 0x1 - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Reserved (7bytes) - //-----function section 1------ - 0x00, // Interface Number #0 - 0x01, // reserved (0x1) - 0x57, 0x49, 0x4E, 0x55, 0x53, 0x42, 0x00, 0x00, // Compatible ID ('WINUSB\0\0') (8bytes) - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-Compatible ID (8byte) - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved (6bytes) + 0x28, 0x00, 0x00, 0x00, // Descriptor Length 40bytes (0x28) + 0x00, 0x01, // Version ('1.0') + MS_EXTENDED_COMPAT_ID, 0x00, // Compatibility ID Descriptor Index 0x0004 + 0x01, // Number of sections. 0x1 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Reserved (7bytes) + //-----function section 1------ + 0x00, // Interface Number #0 + 0x01, // reserved (0x1) + 0x57, 0x49, 0x4E, 0x55, 0x53, 0x42, 0x00, 0x00, // Compatible ID ('WINUSB\0\0') (8bytes) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Sub-Compatible ID (8byte) + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved (6bytes) }; */ // Microsoft Extended Properties Feature Descriptor /* static const char OSprop[] = { - // u32 Descriptor Length (10+132+64+102 == 308 - 0x34, 0x01, 0, 0, - // u16 Version ('1.0') - 0, 1, - // u16 wIndex - MS_EXTENDED_PROPERTIES, 0, - // u16 wCount -- three section - 3, 0, + // u32 Descriptor Length (10+132+64+102 == 308 + 0x34, 0x01, 0, 0, + // u16 Version ('1.0') + 0, 1, + // u16 wIndex + MS_EXTENDED_PROPERTIES, 0, + // u16 wCount -- three section + 3, 0, - //-----property section 1------ - // u32 size ( 14+40+78 == 132) - 132, 0, 0, 0, - // u32 type - 1, 0, 0, 0, // unicode string - // u16 namelen (20*2 = 40) - 40, 0, - // name DeviceInterfaceGUID - 'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,'I',0,'n',0,'t',0,'e',0,'r',0,'f',0,'a',0,'c',0,'e',0,'G',0,'U',0,'I',0,'D',0,0,0, - // u32 datalen (39*2 = 78) - 78, 0, 0, 0, - // data {4D36E978-E325-11CE-BFC1-08002BE10318} - '{',0,'4',0,'d',0,'3',0,'6',0,'e',0,'9',0,'7',0,'8',0,'-',0,'e',0,'3',0,'2',0,'5',0, - '-',0,'1',0,'1',0,'c',0,'e',0,'-',0,'b',0,'f',0,'c',0,'1',0,'-',0,'0',0,'8',0,'0',0, - '0',0,'2',0,'b',0,'e',0,'1',0,'0',0,'3',0,'1',0,'8',0,'}',0,0,0, + //-----property section 1------ + // u32 size ( 14+40+78 == 132) + 132, 0, 0, 0, + // u32 type + 1, 0, 0, 0, // unicode string + // u16 namelen (20*2 = 40) + 40, 0, + // name DeviceInterfaceGUID + 'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,'I',0,'n',0,'t',0,'e',0,'r',0,'f',0,'a',0,'c',0,'e',0,'G',0,'U',0,'I',0,'D',0,0,0, + // u32 datalen (39*2 = 78) + 78, 0, 0, 0, + // data {4D36E978-E325-11CE-BFC1-08002BE10318} + '{',0,'4',0,'d',0,'3',0,'6',0,'e',0,'9',0,'7',0,'8',0,'-',0,'e',0,'3',0,'2',0,'5',0, + '-',0,'1',0,'1',0,'c',0,'e',0,'-',0,'b',0,'f',0,'c',0,'1',0,'-',0,'0',0,'8',0,'0',0, + '0',0,'2',0,'b',0,'e',0,'1',0,'0',0,'3',0,'1',0,'8',0,'}',0,0,0, - //-----property section 2------ - // u32 size ( 14+12+38 == 64) - 64, 0, 0, 0, - // u32 type - 1, 0, 0, 0, // unicode string - // u16 namelen (12) - 12, 0, - // name Label - 'L',0,'a',0,'b',0,'e',0,'l',0,0,0, - // u32 datalen ( 19*2 = 38 ) - 38, 0, 0, 0, - // data 'Awesome PM3 Device' - 'A',0,'w',0,'e',0,'s',0,'o',0,'m',0,'e',0,' ',0,'P',0,'M',0,'3',0,' ',0,'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,0,0, + //-----property section 2------ + // u32 size ( 14+12+38 == 64) + 64, 0, 0, 0, + // u32 type + 1, 0, 0, 0, // unicode string + // u16 namelen (12) + 12, 0, + // name Label + 'L',0,'a',0,'b',0,'e',0,'l',0,0,0, + // u32 datalen ( 19*2 = 38 ) + 38, 0, 0, 0, + // data 'Awesome PM3 Device' + 'A',0,'w',0,'e',0,'s',0,'o',0,'m',0,'e',0,' ',0,'P',0,'M',0,'3',0,' ',0,'D',0,'e',0,'v',0,'i',0,'c',0,'e',0,0,0, - //-----property section 3------ - // u32 size ( 14+12+76 == 102) - 102, 0, 0, 0, - // u32 type - 2, 0, 0, 0, //Unicode string with environment variables - // u16 namelen (12) - 12, 0, - // name Icons - 'I',0,'c',0,'o',0,'n',0,'s',0,0,0, - // u32 datalen ( 38*2 == 76) - 76, 0, 0, 0, - // data '%SystemRoot%\\system32\\Shell32.dll,-13' - '%',0,'S',0,'y',0,'s',0,'t',0,'e',0,'m',0,'R',0,'o',0,'o',0,'t',0,'%',0, - '\\',0,'s',0,'y',0,'s',0,'t',0,'e',0,'m',0,'3',0,'2',0,'\\',0, - 'S',0,'h',0,'e',0,'l',0,'l',0,'3',0,'2',0,'.',0,'d',0,'l',0,'l',0,',',0, - '-',0,'1',0,'3',0,0,0 + //-----property section 3------ + // u32 size ( 14+12+76 == 102) + 102, 0, 0, 0, + // u32 type + 2, 0, 0, 0, //Unicode string with environment variables + // u16 namelen (12) + 12, 0, + // name Icons + 'I',0,'c',0,'o',0,'n',0,'s',0,0,0, + // u32 datalen ( 38*2 == 76) + 76, 0, 0, 0, + // data '%SystemRoot%\\system32\\Shell32.dll,-13' + '%',0,'S',0,'y',0,'s',0,'t',0,'e',0,'m',0,'R',0,'o',0,'o',0,'t',0,'%',0, + '\\',0,'s',0,'y',0,'s',0,'t',0,'e',0,'m',0,'3',0,'2',0,'\\',0, + 'S',0,'h',0,'e',0,'l',0,'l',0,'3',0,'2',0,'.',0,'d',0,'l',0,'l',0,',',0, + '-',0,'1',0,'3',0,0,0 }; */ static const char StrLanguageCodes[] = { - 4, // Length - 0x03, // Type is string - 0x09, 0x04 // supported language Code 0 = 0x0409 (English) + 4, // Length + 0x03, // Type is string + 0x09, 0x04 // supported language Code 0 = 0x0409 (English) }; // Note: ModemManager (Linux) ignores Proxmark3 devices by matching the // manufacturer string "proxmark.org". Don't change this. // or use the blacklisting file. static const char StrManufacturer[] = { - 26, // Length - 0x03, // Type is string - 'p',0,'r',0,'o',0,'x',0,'m',0,'a',0,'r',0,'k',0,'.',0,'o',0,'r',0,'g',0, + 26, // Length + 0x03, // Type is string + 'p',0,'r',0,'o',0,'x',0,'m',0,'a',0,'r',0,'k',0,'.',0,'o',0,'r',0,'g',0, }; static const char StrProduct[] = { - 20, // Length - 0x03, // Type is string - 'p',0,'r',0,'o',0,'x',0,'m',0,'a',0,'r',0,'k',0,'3',0 + 20, // Length + 0x03, // Type is string + 'p',0,'r',0,'o',0,'x',0,'m',0,'a',0,'r',0,'k',0,'3',0 }; static const char StrSerialNumber[] = { - 14, // Length - 0x03, // Type is string - 'i',0,'c',0,'e',0,'m',0,'a',0,'n',0 + 14, // Length + 0x03, // Type is string + 'i',0,'c',0,'e',0,'m',0,'a',0,'n',0 }; // size includes their own field. static const char StrMS_OSDescriptor[] = { - 18, // length 0x12 - 0x03, // Type is string + 18, // length 0x12 + 0x03, // Type is string 'M',0,'S',0,'F',0,'T',0,'1',0,'0',0,'0',0,MS_VENDOR_CODE,0 }; const char* getStringDescriptor(uint8_t idx) { - switch(idx) { - case 0: return StrLanguageCodes; - case 1: return StrManufacturer; - case 2: return StrProduct; - case 3: return StrSerialNumber; - case MS_OS_DESCRIPTOR_INDEX: return StrMS_OSDescriptor; - default: - return(NULL); - } + switch(idx) { + case 0: return StrLanguageCodes; + case 1: return StrManufacturer; + case 2: return StrProduct; + case 3: return StrSerialNumber; + case MS_OS_DESCRIPTOR_INDEX: return StrMS_OSDescriptor; + default: + return(NULL); + } } // Bitmap for all status bits in CSR which must be written as 1 to cause no effect @@ -410,57 +410,57 @@ const char* getStringDescriptor(uint8_t idx) { // Clear flags in the UDP_CSR register and waits for synchronization #define UDP_CLEAR_EP_FLAGS(endpoint, flags) { \ - volatile unsigned int reg; \ - reg = pUdp->UDP_CSR[(endpoint)]; \ - reg |= REG_NO_EFFECT_1_ALL; \ - reg &= ~(flags); \ - pUdp->UDP_CSR[(endpoint)] = reg; \ + volatile unsigned int reg; \ + reg = pUdp->UDP_CSR[(endpoint)]; \ + reg |= REG_NO_EFFECT_1_ALL; \ + reg &= ~(flags); \ + pUdp->UDP_CSR[(endpoint)] = reg; \ } \ // reset flags in the UDP_CSR register and waits for synchronization #define UDP_SET_EP_FLAGS(endpoint, flags) { \ - volatile unsigned int reg; \ - reg = pUdp->UDP_CSR[(endpoint)]; \ - reg |= REG_NO_EFFECT_1_ALL; \ - reg |= (flags); \ - pUdp->UDP_CSR[(endpoint)] = reg; \ + volatile unsigned int reg; \ + reg = pUdp->UDP_CSR[(endpoint)]; \ + reg |= REG_NO_EFFECT_1_ALL; \ + reg |= (flags); \ + pUdp->UDP_CSR[(endpoint)] = reg; \ } \ typedef struct { - uint32_t BitRate; - uint8_t Format; - uint8_t ParityType; - uint8_t DataBits; + uint32_t BitRate; + uint8_t Format; + uint8_t ParityType; + uint8_t DataBits; } AT91S_CDC_LINE_CODING, *AT91PS_CDC_LINE_CODING; AT91S_CDC_LINE_CODING line = { - 115200, // baudrate - 0, // 1 Stop Bit - 0, // None Parity - 8}; // 8 Data bits + 115200, // baudrate + 0, // 1 Stop Bit + 0, // None Parity + 8}; // 8 Data bits static void SpinDelay(int ms) { - int us = ms * 1000; - int ticks = (48 * us) >> 10; + int us = ms * 1000; + int ticks = (48 * us) >> 10; - // Borrow a PWM unit for my real-time clock - AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0); + // Borrow a PWM unit for my real-time clock + AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0); - // 48 MHz / 1024 gives 46.875 kHz - AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10); - AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0; - AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff; + // 48 MHz / 1024 gives 46.875 kHz + AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10); + AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0; + AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff; - uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; + uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; - for(;;) { - uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; - if (now == (uint16_t)(start + ticks)) - return; + for(;;) { + uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; + if (now == (uint16_t)(start + ticks)) + return; - WDT_HIT(); - } + WDT_HIT(); + } } //*---------------------------------------------------------------------------- @@ -468,13 +468,13 @@ static void SpinDelay(int ms) { //* \brief This function deactivates the USB device //*---------------------------------------------------------------------------- void usb_disable() { - // Disconnect the USB device - AT91C_BASE_PIOA->PIO_ODR = GPIO_USB_PU; + // Disconnect the USB device + AT91C_BASE_PIOA->PIO_ODR = GPIO_USB_PU; - // Clear all lingering interrupts - if (pUdp->UDP_ISR & AT91C_UDP_ENDBUSRES) { - pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES; - } + // Clear all lingering interrupts + if (pUdp->UDP_ISR & AT91C_UDP_ENDBUSRES) { + pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES; + } } //*---------------------------------------------------------------------------- @@ -482,35 +482,35 @@ void usb_disable() { //* \brief This function Activates the USB device //*---------------------------------------------------------------------------- void usb_enable() { - // Set the PLL USB Divider - AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1 ; + // Set the PLL USB Divider + AT91C_BASE_CKGR->CKGR_PLLR |= AT91C_CKGR_USBDIV_1 ; - // Specific Chip USB Initialisation - // Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock - AT91C_BASE_PMC->PMC_SCER |= AT91C_PMC_UDP; - AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP); + // Specific Chip USB Initialisation + // Enables the 48MHz USB clock UDPCK and System Peripheral USB Clock + AT91C_BASE_PMC->PMC_SCER |= AT91C_PMC_UDP; + AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_UDP); - AT91C_BASE_UDP->UDP_FADDR = 0; - AT91C_BASE_UDP->UDP_GLBSTATE = 0; + AT91C_BASE_UDP->UDP_FADDR = 0; + AT91C_BASE_UDP->UDP_GLBSTATE = 0; - // Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO - // Set in PIO mode and Configure in Output - AT91C_BASE_PIOA->PIO_PER = GPIO_USB_PU; // Set in PIO mode - AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; // Configure as Output + // Enable UDP PullUp (USB_DP_PUP) : enable & Clear of the corresponding PIO + // Set in PIO mode and Configure in Output + AT91C_BASE_PIOA->PIO_PER = GPIO_USB_PU; // Set in PIO mode + AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; // Configure as Output - // Clear for set the Pullup resistor - AT91C_BASE_PIOA->PIO_CODR = GPIO_USB_PU; + // Clear for set the Pullup resistor + AT91C_BASE_PIOA->PIO_CODR = GPIO_USB_PU; - // Disconnect and reconnect USB controller for 100ms - usb_disable(); + // Disconnect and reconnect USB controller for 100ms + usb_disable(); - SpinDelay(100); - // Wait for a short while - //for (volatile size_t i=0; i<0x100000; i++) {}; + SpinDelay(100); + // Wait for a short while + //for (volatile size_t i=0; i<0x100000; i++) {}; - // Reconnect USB reconnect - AT91C_BASE_PIOA->PIO_SODR = GPIO_USB_PU; - AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; + // Reconnect USB reconnect + AT91C_BASE_PIOA->PIO_SODR = GPIO_USB_PU; + AT91C_BASE_PIOA->PIO_OER = GPIO_USB_PU; } //*---------------------------------------------------------------------------- @@ -520,87 +520,87 @@ void usb_enable() { static int usb_reconnect = 0; static int usb_configured = 0; void SetUSBreconnect(int value) { - usb_reconnect = value; + usb_reconnect = value; } int GetUSBreconnect(void) { - return usb_reconnect; + return usb_reconnect; } void SetUSBconfigured(int value) { - usb_configured = value; + usb_configured = value; } int GetUSBconfigured(void){ - return usb_configured; + return usb_configured; } bool usb_check() { - /* - // reconnected ONCE and - if ( !USB_ATTACHED() ){ - usb_reconnect = 1; - return false; - } + /* + // reconnected ONCE and + if ( !USB_ATTACHED() ){ + usb_reconnect = 1; + return false; + } - // only one time after USB been disengaged and re-engaged - if ( USB_ATTACHED() && usb_reconnect == 1 ) { + // only one time after USB been disengaged and re-engaged + if ( USB_ATTACHED() && usb_reconnect == 1 ) { - if ( usb_configured == 0) { - usb_disable(); - usb_enable(); + if ( usb_configured == 0) { + usb_disable(); + usb_enable(); - AT91F_CDC_Enumerate(); + AT91F_CDC_Enumerate(); - usb_configured = 1; - return false; - } - } - */ + usb_configured = 1; + return false; + } + } + */ - // interrupt status register - AT91_REG isr = pUdp->UDP_ISR; + // interrupt status register + AT91_REG isr = pUdp->UDP_ISR; - // end of bus reset - if (isr & AT91C_UDP_ENDBUSRES) { - pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES; - // reset all endpoints - pUdp->UDP_RSTEP = (unsigned int)-1; - pUdp->UDP_RSTEP = 0; - // Enable the function - pUdp->UDP_FADDR = AT91C_UDP_FEN; - // Configure endpoint 0 (enable control endpoint) - pUdp->UDP_CSR[AT91C_EP_CONTROL] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL); - } - else if (isr & AT91C_UDP_EPINT0) { - pUdp->UDP_ICR = AT91C_UDP_EPINT0; - AT91F_CDC_Enumerate(); - } - /* - else if (isr & AT91C_UDP_EPINT3 ) { - pUdp->UDP_ICR = AT91C_UDP_EPINT3; - AT91F_CDC_Enumerate(); - //pUdp->UDP_ICR |= AT91C_UDP_EPINT3; - } - */ - return (btConfiguration) ? true : false; + // end of bus reset + if (isr & AT91C_UDP_ENDBUSRES) { + pUdp->UDP_ICR = AT91C_UDP_ENDBUSRES; + // reset all endpoints + pUdp->UDP_RSTEP = (unsigned int)-1; + pUdp->UDP_RSTEP = 0; + // Enable the function + pUdp->UDP_FADDR = AT91C_UDP_FEN; + // Configure endpoint 0 (enable control endpoint) + pUdp->UDP_CSR[AT91C_EP_CONTROL] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL); + } + else if (isr & AT91C_UDP_EPINT0) { + pUdp->UDP_ICR = AT91C_UDP_EPINT0; + AT91F_CDC_Enumerate(); + } + /* + else if (isr & AT91C_UDP_EPINT3 ) { + pUdp->UDP_ICR = AT91C_UDP_EPINT3; + AT91F_CDC_Enumerate(); + //pUdp->UDP_ICR |= AT91C_UDP_EPINT3; + } + */ + return (btConfiguration) ? true : false; } bool usb_poll() { - if (!usb_check()) return false; - return (pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank); + if (!usb_check()) return false; + return (pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank); } /** - In github PR #129, some users appears to get a false positive from - usb_poll, which returns true, but the usb_read operation - still returns 0. - This check is basically the same as above, but also checks - that the length available to read is non-zero, thus hopefully fixes the - bug. + In github PR #129, some users appears to get a false positive from + usb_poll, which returns true, but the usb_read operation + still returns 0. + This check is basically the same as above, but also checks + that the length available to read is non-zero, thus hopefully fixes the + bug. **/ bool usb_poll_validate_length() { - if (!usb_check()) return false; - if (!(pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false; - return ((pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16) > 0; + if (!usb_check()) return false; + if (!(pUdp->UDP_CSR[AT91C_EP_OUT] & btReceiveBank)) return false; + return ((pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16) > 0; } //*---------------------------------------------------------------------------- @@ -609,36 +609,36 @@ bool usb_poll_validate_length() { //*---------------------------------------------------------------------------- uint32_t usb_read(byte_t* data, size_t len) { - if ( len == 0 ) return 0; + if ( len == 0 ) return 0; - uint8_t bank = btReceiveBank; - uint32_t packetSize, nbBytesRcv = 0; - uint32_t time_out = 0; + uint8_t bank = btReceiveBank; + uint32_t packetSize, nbBytesRcv = 0; + uint32_t time_out = 0; - while (len) { - if (!usb_check()) break; + while (len) { + if (!usb_check()) break; - if ( pUdp->UDP_CSR[AT91C_EP_OUT] & bank ) { + if ( pUdp->UDP_CSR[AT91C_EP_OUT] & bank ) { - packetSize = (pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16; - packetSize = MIN( packetSize, len); - len -= packetSize; - while (packetSize--) - data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT]; + packetSize = (pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16; + packetSize = MIN( packetSize, len); + len -= packetSize; + while (packetSize--) + data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT]; - // flip bank - UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank) + // flip bank + UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank) - if (bank == AT91C_UDP_RX_DATA_BK0) - bank = AT91C_UDP_RX_DATA_BK1; - else - bank = AT91C_UDP_RX_DATA_BK0; - } - if (time_out++ == 0x1fff) break; - } + if (bank == AT91C_UDP_RX_DATA_BK0) + bank = AT91C_UDP_RX_DATA_BK1; + else + bank = AT91C_UDP_RX_DATA_BK0; + } + if (time_out++ == 0x1fff) break; + } - btReceiveBank = bank; - return nbBytesRcv; + btReceiveBank = bank; + return nbBytesRcv; } //*---------------------------------------------------------------------------- @@ -647,54 +647,54 @@ uint32_t usb_read(byte_t* data, size_t len) { //*---------------------------------------------------------------------------- uint32_t usb_write(const byte_t* data, const size_t len) { - if (!len) return 0; - if (!usb_check()) return 0; + if (!len) return 0; + if (!usb_check()) return 0; - // can we write? - if ( (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY) != 0 ) return 0; + // can we write? + if ( (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXPKTRDY) != 0 ) return 0; - size_t length = len; - uint32_t cpt = 0; + size_t length = len; + uint32_t cpt = 0; - // send first chunk - cpt = MIN(length, AT91C_EP_IN_SIZE); - length -= cpt; - while (cpt--) { - pUdp->UDP_FDR[AT91C_EP_IN] = *data++; - } + // send first chunk + cpt = MIN(length, AT91C_EP_IN_SIZE); + length -= cpt; + while (cpt--) { + pUdp->UDP_FDR[AT91C_EP_IN] = *data++; + } - UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY); + UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY); - while (length) { - // Send next chunk - cpt = MIN(length, AT91C_EP_IN_SIZE); - length -= cpt; - while (cpt--) { - pUdp->UDP_FDR[AT91C_EP_IN] = *data++; - } + while (length) { + // Send next chunk + cpt = MIN(length, AT91C_EP_IN_SIZE); + length -= cpt; + while (cpt--) { + pUdp->UDP_FDR[AT91C_EP_IN] = *data++; + } - // Wait for previous chunk to be sent - // (iceman) when is the bankswapping done? - while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) { - if (!usb_check()) return length; - } + // Wait for previous chunk to be sent + // (iceman) when is the bankswapping done? + while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) { + if (!usb_check()) return length; + } - UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP); - while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP); - UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY); + UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP); + while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP); + UDP_SET_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXPKTRDY); - } + } - // Wait for the end of transfer - while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) { - if (!usb_check()) return length; - } + // Wait for the end of transfer + while (!(pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP)) { + if (!usb_check()) return length; + } - UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP); - while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP); + UDP_CLEAR_EP_FLAGS(AT91C_EP_IN, AT91C_UDP_TXCOMP); + while (pUdp->UDP_CSR[AT91C_EP_IN] & AT91C_UDP_TXCOMP); - return length; + return length; } //*---------------------------------------------------------------------------- @@ -702,39 +702,39 @@ uint32_t usb_write(const byte_t* data, const size_t len) { //* \brief Send Data through the control endpoint //*---------------------------------------------------------------------------- void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, uint32_t length) { - uint32_t cpt = 0; - AT91_REG csr; + uint32_t cpt = 0; + AT91_REG csr; - do { - cpt = MIN(length, AT91C_EP_CONTROL_SIZE); - length -= cpt; + do { + cpt = MIN(length, AT91C_EP_CONTROL_SIZE); + length -= cpt; - while (cpt--) - pUdp->UDP_FDR[AT91C_EP_CONTROL] = *pData++; + while (cpt--) + pUdp->UDP_FDR[AT91C_EP_CONTROL] = *pData++; - if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) { - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); - while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); - } + if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) { + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); + while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); + } - UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY); + UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY); - do { - csr = pUdp->UDP_CSR[AT91C_EP_CONTROL]; - // Data IN stage has been stopped by a status OUT - if ( csr & AT91C_UDP_RX_DATA_BK0) { + do { + csr = pUdp->UDP_CSR[AT91C_EP_CONTROL]; + // Data IN stage has been stopped by a status OUT + if ( csr & AT91C_UDP_RX_DATA_BK0) { - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0) - return; - } - } while ( !(csr & AT91C_UDP_TXCOMP) ); + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0) + return; + } + } while ( !(csr & AT91C_UDP_TXCOMP) ); - } while (length); + } while (length); - if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) { - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); - while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); - } + if (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) { + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); + while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); + } } @@ -743,10 +743,10 @@ void AT91F_USB_SendData(AT91PS_UDP pUdp, const char *pData, uint32_t length) { //* \brief Send zero length packet through the control endpoint //*---------------------------------------------------------------------------- void AT91F_USB_SendZlp(AT91PS_UDP pUdp) { - UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY); - while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) ); - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); - while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); + UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXPKTRDY); + while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP) ); + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_TXCOMP); + while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_TXCOMP); } //*---------------------------------------------------------------------------- @@ -754,10 +754,10 @@ void AT91F_USB_SendZlp(AT91PS_UDP pUdp) { //* \brief Stall the control endpoint //*---------------------------------------------------------------------------- void AT91F_USB_SendStall(AT91PS_UDP pUdp) { - UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL); - while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_ISOERROR) ); - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR) ); - while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)); + UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_FORCESTALL); + while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_ISOERROR) ); + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR) ); + while (pUdp->UDP_CSR[AT91C_EP_CONTROL] & (AT91C_UDP_FORCESTALL | AT91C_UDP_ISOERROR)); } //*---------------------------------------------------------------------------- @@ -768,181 +768,181 @@ void AT91F_USB_SendStall(AT91PS_UDP pUdp) { //* 2. mixed with CDC ACM endpoint3 , interrupt, control endpoint //*---------------------------------------------------------------------------- void AT91F_CDC_Enumerate() { - byte_t bmRequestType, bRequest; - uint16_t wValue, wIndex, wLength, wStatus; + byte_t bmRequestType, bRequest; + uint16_t wValue, wIndex, wLength, wStatus; - if ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP) ) - return; + if ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP) ) + return; - bmRequestType = pUdp->UDP_FDR[AT91C_EP_CONTROL]; - bRequest = pUdp->UDP_FDR[AT91C_EP_CONTROL]; - wValue = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); - wValue |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); - wIndex = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); - wIndex |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); - wLength = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); - wLength |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); + bmRequestType = pUdp->UDP_FDR[AT91C_EP_CONTROL]; + bRequest = pUdp->UDP_FDR[AT91C_EP_CONTROL]; + wValue = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); + wValue |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); + wIndex = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); + wIndex |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); + wLength = (pUdp->UDP_FDR[AT91C_EP_CONTROL] & 0xFF); + wLength |= (pUdp->UDP_FDR[AT91C_EP_CONTROL] << 8); - if (bmRequestType & 0x80) { // Data Phase Transfer Direction Device to Host - UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_DIR); - while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_DIR) ); - } - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RXSETUP); - while ( (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP) ); + if (bmRequestType & 0x80) { // Data Phase Transfer Direction Device to Host + UDP_SET_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_DIR); + while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_DIR) ); + } + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RXSETUP); + while ( (pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RXSETUP) ); - /* - if ( bRequest == MS_VENDOR_CODE) { - if ( bmRequestType == MS_WCID_GET_DESCRIPTOR ) { // C0 - if ( wIndex == MS_EXTENDED_COMPAT_ID ) { // 4 - //AT91F_USB_SendData(pUdp, CompatIDFeatureDescriptor, MIN(sizeof(CompatIDFeatureDescriptor), wLength)); - //return; - } - } + /* + if ( bRequest == MS_VENDOR_CODE) { + if ( bmRequestType == MS_WCID_GET_DESCRIPTOR ) { // C0 + if ( wIndex == MS_EXTENDED_COMPAT_ID ) { // 4 + //AT91F_USB_SendData(pUdp, CompatIDFeatureDescriptor, MIN(sizeof(CompatIDFeatureDescriptor), wLength)); + //return; + } + } - if ( bmRequestType == MS_WCID_GET_FEATURE_DESCRIPTOR ) { //C1 - // if ( wIndex == MS_EXTENDED_PROPERTIES ) { // 5 - winusb bug with wIndex == interface index, so I just send it always) - //AT91F_USB_SendData(pUdp, OSprop, MIN(sizeof(OSprop), wLength)); - //return; - // } - } - } - */ + if ( bmRequestType == MS_WCID_GET_FEATURE_DESCRIPTOR ) { //C1 + // if ( wIndex == MS_EXTENDED_PROPERTIES ) { // 5 - winusb bug with wIndex == interface index, so I just send it always) + //AT91F_USB_SendData(pUdp, OSprop, MIN(sizeof(OSprop), wLength)); + //return; + // } + } + } + */ - // Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1 - switch ((bRequest << 8) | bmRequestType) { - case STD_GET_DESCRIPTOR: { + // Handle supported standard device request Cf Table 9-3 in USB specification Rev 1.1 + switch ((bRequest << 8) | bmRequestType) { + case STD_GET_DESCRIPTOR: { - if ( wValue == 0x100 ) // Return Device Descriptor - AT91F_USB_SendData(pUdp, devDescriptor, MIN(sizeof(devDescriptor), wLength)); - else if ( wValue == 0x200 ) // Return Configuration Descriptor - AT91F_USB_SendData(pUdp, cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength)); - else if ( (wValue & 0xF00) == 0xF00) // Return BOS Descriptor - AT91F_USB_SendData(pUdp, bosDescriptor, MIN(sizeof(bosDescriptor), wLength)); - else if ( (wValue & 0x300) == 0x300) { // Return String Descriptor + if ( wValue == 0x100 ) // Return Device Descriptor + AT91F_USB_SendData(pUdp, devDescriptor, MIN(sizeof(devDescriptor), wLength)); + else if ( wValue == 0x200 ) // Return Configuration Descriptor + AT91F_USB_SendData(pUdp, cfgDescriptor, MIN(sizeof(cfgDescriptor), wLength)); + else if ( (wValue & 0xF00) == 0xF00) // Return BOS Descriptor + AT91F_USB_SendData(pUdp, bosDescriptor, MIN(sizeof(bosDescriptor), wLength)); + else if ( (wValue & 0x300) == 0x300) { // Return String Descriptor - const char *strDescriptor = getStringDescriptor(wValue & 0xff); - if (strDescriptor != NULL) { - AT91F_USB_SendData(pUdp, strDescriptor, MIN(strDescriptor[0], wLength)); - } else { - AT91F_USB_SendStall(pUdp); - } - } else { - AT91F_USB_SendStall(pUdp); - } - } - break; - case STD_SET_ADDRESS: - AT91F_USB_SendZlp(pUdp); - pUdp->UDP_FADDR = (AT91C_UDP_FEN | (wValue & 0x7F) ); - pUdp->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0; - break; - case STD_SET_CONFIGURATION: + const char *strDescriptor = getStringDescriptor(wValue & 0xff); + if (strDescriptor != NULL) { + AT91F_USB_SendData(pUdp, strDescriptor, MIN(strDescriptor[0], wLength)); + } else { + AT91F_USB_SendStall(pUdp); + } + } else { + AT91F_USB_SendStall(pUdp); + } + } + break; + case STD_SET_ADDRESS: + AT91F_USB_SendZlp(pUdp); + pUdp->UDP_FADDR = (AT91C_UDP_FEN | (wValue & 0x7F) ); + pUdp->UDP_GLBSTATE = (wValue) ? AT91C_UDP_FADDEN : 0; + break; + case STD_SET_CONFIGURATION: - /* - * Set or clear the device "configured" state. - * The LSB of wValue is the "Configuration Number". If this value is non-zero, - * it should be the same number as defined in the Configuration Descriptor; - * otherwise an error must have occurred. - * This device has only one configuration and its Config Number is CONF_NB (= 1). - */ - AT91F_USB_SendZlp(pUdp); - btConfiguration = wValue; - pUdp->UDP_GLBSTATE = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN; + /* + * Set or clear the device "configured" state. + * The LSB of wValue is the "Configuration Number". If this value is non-zero, + * it should be the same number as defined in the Configuration Descriptor; + * otherwise an error must have occurred. + * This device has only one configuration and its Config Number is CONF_NB (= 1). + */ + AT91F_USB_SendZlp(pUdp); + btConfiguration = wValue; + pUdp->UDP_GLBSTATE = (wValue) ? AT91C_UDP_CONFG : AT91C_UDP_FADDEN; - // make sure we are not stalled - /* - UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT , AT91C_UDP_FORCESTALL); + // make sure we are not stalled + /* + UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT , AT91C_UDP_FORCESTALL); UDP_CLEAR_EP_FLAGS(AT91C_EP_IN , AT91C_UDP_FORCESTALL); - UDP_CLEAR_EP_FLAGS(AT91C_EP_NOTIFY, AT91C_UDP_FORCESTALL); - */ + UDP_CLEAR_EP_FLAGS(AT91C_EP_NOTIFY, AT91C_UDP_FORCESTALL); + */ - // enable endpoints - pUdp->UDP_CSR[AT91C_EP_OUT] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0; - pUdp->UDP_CSR[AT91C_EP_IN] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN) : 0; - pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN) : 0; - break; - case STD_GET_CONFIGURATION: - AT91F_USB_SendData(pUdp, (char *) &(btConfiguration), sizeof(btConfiguration)); - break; - case STD_GET_STATUS_ZERO: - wStatus = 0; // Device is Bus powered, remote wakeup disabled - AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); - break; - case STD_GET_STATUS_INTERFACE: - wStatus = 0; // reserved for future use - AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); - break; - case STD_GET_STATUS_ENDPOINT: - wStatus = 0; - wIndex &= 0x0F; - if ((pUdp->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= AT91C_EP_NOTIFY)) { - wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1; - AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); - } - else if ((pUdp->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == AT91C_EP_CONTROL)) { - wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1; - AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); - } else { - AT91F_USB_SendStall(pUdp); - } - break; - case STD_SET_FEATURE_ZERO: - AT91F_USB_SendStall(pUdp); - break; - case STD_SET_FEATURE_INTERFACE: - AT91F_USB_SendZlp(pUdp); - break; - case STD_SET_FEATURE_ENDPOINT: - wIndex &= 0x0F; - if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) { - pUdp->UDP_CSR[wIndex] = 0; - AT91F_USB_SendZlp(pUdp); - } else { - AT91F_USB_SendStall(pUdp); - } - break; - case STD_CLEAR_FEATURE_ZERO: - AT91F_USB_SendStall(pUdp); - break; - case STD_CLEAR_FEATURE_INTERFACE: - AT91F_USB_SendZlp(pUdp); - break; - case STD_CLEAR_FEATURE_ENDPOINT: - wIndex &= 0x0F; - if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) { + // enable endpoints + pUdp->UDP_CSR[AT91C_EP_OUT] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT) : 0; + pUdp->UDP_CSR[AT91C_EP_IN] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN) : 0; + pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (wValue) ? (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN) : 0; + break; + case STD_GET_CONFIGURATION: + AT91F_USB_SendData(pUdp, (char *) &(btConfiguration), sizeof(btConfiguration)); + break; + case STD_GET_STATUS_ZERO: + wStatus = 0; // Device is Bus powered, remote wakeup disabled + AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); + break; + case STD_GET_STATUS_INTERFACE: + wStatus = 0; // reserved for future use + AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); + break; + case STD_GET_STATUS_ENDPOINT: + wStatus = 0; + wIndex &= 0x0F; + if ((pUdp->UDP_GLBSTATE & AT91C_UDP_CONFG) && (wIndex <= AT91C_EP_NOTIFY)) { + wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1; + AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); + } + else if ((pUdp->UDP_GLBSTATE & AT91C_UDP_FADDEN) && (wIndex == AT91C_EP_CONTROL)) { + wStatus = (pUdp->UDP_CSR[wIndex] & AT91C_UDP_EPEDS) ? 0 : 1; + AT91F_USB_SendData(pUdp, (char *) &wStatus, sizeof(wStatus)); + } else { + AT91F_USB_SendStall(pUdp); + } + break; + case STD_SET_FEATURE_ZERO: + AT91F_USB_SendStall(pUdp); + break; + case STD_SET_FEATURE_INTERFACE: + AT91F_USB_SendZlp(pUdp); + break; + case STD_SET_FEATURE_ENDPOINT: + wIndex &= 0x0F; + if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) { + pUdp->UDP_CSR[wIndex] = 0; + AT91F_USB_SendZlp(pUdp); + } else { + AT91F_USB_SendStall(pUdp); + } + break; + case STD_CLEAR_FEATURE_ZERO: + AT91F_USB_SendStall(pUdp); + break; + case STD_CLEAR_FEATURE_INTERFACE: + AT91F_USB_SendZlp(pUdp); + break; + case STD_CLEAR_FEATURE_ENDPOINT: + wIndex &= 0x0F; + if ((wValue == 0) && (wIndex >= AT91C_EP_OUT) && (wIndex <= AT91C_EP_NOTIFY)) { - if (wIndex == AT91C_EP_OUT) pUdp->UDP_CSR[AT91C_EP_OUT] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT); - else if (wIndex == AT91C_EP_IN) pUdp->UDP_CSR[AT91C_EP_IN] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN); - else if (wIndex == AT91C_EP_NOTIFY) pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN); + if (wIndex == AT91C_EP_OUT) pUdp->UDP_CSR[AT91C_EP_OUT] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_OUT); + else if (wIndex == AT91C_EP_IN) pUdp->UDP_CSR[AT91C_EP_IN] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_BULK_IN); + else if (wIndex == AT91C_EP_NOTIFY) pUdp->UDP_CSR[AT91C_EP_NOTIFY] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_INT_IN); - AT91F_USB_SendZlp(pUdp); - } else { - AT91F_USB_SendStall(pUdp); - } - break; + AT91F_USB_SendZlp(pUdp); + } else { + AT91F_USB_SendStall(pUdp); + } + break; - // handle CDC class requests - case SET_LINE_CODING: { - /* - uint8_t i; - for ( i = 0 ; i < 7 ; i++ ) { - ((uint8_t*)&line)[i] = pUdp->UDP_FDR[AT91C_EP_CONTROL]; - } */ - // ignore SET_LINE_CODING... - while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RX_DATA_BK0) ); - UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0); - AT91F_USB_SendZlp(pUdp); - break; - } - case GET_LINE_CODING: - AT91F_USB_SendData(pUdp, (char *) &line, MIN(sizeof(line), wLength)); - break; - case SET_CONTROL_LINE_STATE: - btConnection = wValue; - AT91F_USB_SendZlp(pUdp); - break; - default: - AT91F_USB_SendStall(pUdp); - break; - } + // handle CDC class requests + case SET_LINE_CODING: { + /* + uint8_t i; + for ( i = 0 ; i < 7 ; i++ ) { + ((uint8_t*)&line)[i] = pUdp->UDP_FDR[AT91C_EP_CONTROL]; + } */ + // ignore SET_LINE_CODING... + while ( !(pUdp->UDP_CSR[AT91C_EP_CONTROL] & AT91C_UDP_RX_DATA_BK0) ); + UDP_CLEAR_EP_FLAGS(AT91C_EP_CONTROL, AT91C_UDP_RX_DATA_BK0); + AT91F_USB_SendZlp(pUdp); + break; + } + case GET_LINE_CODING: + AT91F_USB_SendData(pUdp, (char *) &line, MIN(sizeof(line), wLength)); + break; + case SET_CONTROL_LINE_STATE: + btConnection = wValue; + AT91F_USB_SendZlp(pUdp); + break; + default: + AT91F_USB_SendStall(pUdp); + break; + } } \ No newline at end of file diff --git a/common/wiegand.c b/common/wiegand.c index 730cb0206..18f1bca7d 100644 --- a/common/wiegand.c +++ b/common/wiegand.c @@ -37,27 +37,27 @@ uint8_t checkParity(uint32_t bits, uint8_t len, uint8_t type); // takes a array of binary values, start position, length of bits per parity (includes parity bit), // Parity Type (1 for odd; 0 for even; 2 for Always 1's; 3 for Always 0's), and binary Length (length to run) size_t removeParity(uint8_t *bitstream, size_t startIdx, uint8_t pLen, uint8_t pType, size_t bLen) { - uint32_t parityWd = 0; - size_t j = 0, bitcount = 0; - for (int word = 0; word < (bLen); word += pLen){ - for (int bit = 0; bit < pLen; ++bit){ - parityWd = (parityWd << 1) | bitstream[startIdx+word+bit]; - bitstream[j++] = (bitstream[startIdx + word + bit]); - } - j--; // overwrite parity with next data - // if parity fails then return 0 - switch (pType) { - case 3: if (bitstream[j] == 1) return 0; break; //should be 0 spacer bit - case 2: if (bitstream[j] == 0) return 0; break; //should be 1 spacer bit - default: //test parity - if (parityTest(parityWd, pLen, pType) == 0) return 0; break; - } - bitcount += ( pLen - 1 ); - parityWd = 0; - } - // if we got here then all the parities passed - //return ID start index and size - return bitcount; + uint32_t parityWd = 0; + size_t j = 0, bitcount = 0; + for (int word = 0; word < (bLen); word += pLen){ + for (int bit = 0; bit < pLen; ++bit){ + parityWd = (parityWd << 1) | bitstream[startIdx+word+bit]; + bitstream[j++] = (bitstream[startIdx + word + bit]); + } + j--; // overwrite parity with next data + // if parity fails then return 0 + switch (pType) { + case 3: if (bitstream[j] == 1) return 0; break; //should be 0 spacer bit + case 2: if (bitstream[j] == 0) return 0; break; //should be 1 spacer bit + default: //test parity + if (parityTest(parityWd, pLen, pType) == 0) return 0; break; + } + bitcount += ( pLen - 1 ); + parityWd = 0; + } + // if we got here then all the parities passed + //return ID start index and size + return bitcount; } @@ -76,28 +76,28 @@ size_t removeParity(uint8_t *bitstream, size_t startIdx, uint8_t pLen, uint8_t p */ size_t addParity(uint8_t *bits, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, uint8_t pType) { - uint32_t parityWd = 0; - size_t j = 0, bitCnt = 0; - for (int word = 0; word < sourceLen; word += pLen-1) { - for (int bit = 0; bit < pLen-1; ++bit){ - parityWd = (parityWd << 1) | bits[word+bit]; - dest[j++] = (bits[word+bit]); - } + uint32_t parityWd = 0; + size_t j = 0, bitCnt = 0; + for (int word = 0; word < sourceLen; word += pLen-1) { + for (int bit = 0; bit < pLen-1; ++bit){ + parityWd = (parityWd << 1) | bits[word+bit]; + dest[j++] = (bits[word+bit]); + } - // if parity fails then return 0 - switch (pType) { - case 3: dest[j++] = 0; break; // marker bit which should be a 0 - case 2: dest[j++] = 1; break; // marker bit which should be a 1 - default: - dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1; - break; - } - bitCnt += pLen; - parityWd = 0; - } - // if we got here then all the parities passed - //return ID start index and size - return bitCnt; + // if parity fails then return 0 + switch (pType) { + case 3: dest[j++] = 0; break; // marker bit which should be a 0 + case 2: dest[j++] = 1; break; // marker bit which should be a 1 + default: + dest[j++] = parityTest(parityWd, pLen-1, pType) ^ 1; + break; + } + bitCnt += pLen; + parityWd = 0; + } + // if we got here then all the parities passed + //return ID start index and size + return bitCnt; } // by marshmellow @@ -110,18 +110,18 @@ size_t addParity(uint8_t *bits, uint8_t *dest, uint8_t sourceLen, uint8_t pLen, */ void wiegand_add_parity(uint8_t *source, uint8_t *dest, uint8_t len) { - // Copy to destination, shifted one step to make room for EVEN parity + // Copy to destination, shifted one step to make room for EVEN parity memcpy(dest+1, source, length); - // half length, Even and Odd is calculated to the middle. - uint8_t len_h2 = length >> 1; + // half length, Even and Odd is calculated to the middle. + uint8_t len_h2 = length >> 1; - // add EVEN parity at the beginning + // add EVEN parity at the beginning *(dest) = GetParity(source, EVEN, len_h2); dest += length + 1; - // add ODD parity at the very end + // add ODD parity at the very end *(dest) = GetParity(source + len_h2, ODD, len_h2); } @@ -138,18 +138,18 @@ void wiegand_add_parity(uint8_t *source, uint8_t *dest, uint8_t len) { */ void num_to_wiegand_bytes(uint64_t oem, uint64_t fc, uint64_t cn, uint8_t *dest, uint8_t formatlen){ - uint8_t data[MAX_BITS_TXX55] = {0}; - memset(data, 0, sizeof(data)); + uint8_t data[MAX_BITS_TXX55] = {0}; + memset(data, 0, sizeof(data)); - num_to_wiegand_bits(oem, fc, cn, data, formatlen); + num_to_wiegand_bits(oem, fc, cn, data, formatlen); - // loop - // (formatlen / 32 ) + 1 - // (formatlen >> 5) + 1 - for (int i = 0; i < formatlen ; ++i){ - uint32_t value = bytebits_to_byte( data + (i * 32), 32); - num_to_bytes(value, 32, dest + (i*4) ); - } + // loop + // (formatlen / 32 ) + 1 + // (formatlen >> 5) + 1 + for (int i = 0; i < formatlen ; ++i){ + uint32_t value = bytebits_to_byte( data + (i * 32), 32); + num_to_bytes(value, 32, dest + (i*4) ); + } } /* @@ -162,70 +162,70 @@ void num_to_wiegand_bytes(uint64_t oem, uint64_t fc, uint64_t cn, uint8_t *dest, */ void num_to_wiegand_bits(uint64_t oem, uint64_t fc, uint64_t cn, uint8_t *dest, uint8_t formatlen){ - uint8_t bits[MAX_BITS_TXX55] = {0}; - memset(bits, 0, sizeof(bits)); - uint8_t *temp = bits; - uint64_t value = 0; + uint8_t bits[MAX_BITS_TXX55] = {0}; + memset(bits, 0, sizeof(bits)); + uint8_t *temp = bits; + uint64_t value = 0; - switch ( formatlen ){ - case 26 : // 26bit HID H10301 - fc &= 0xFF; // 8bits - cn &= 0xFFFF; // 16bits - value = fc << 16 | cn; - num_to_bytebits(value, 24, temp); - wiegand_add_parity(temp, dest, 24); - break; - case 261: // 26bit Indala - fc &= 0xFFF; // 12bits - cn &= 0xFFF; // 12bits - value = fc << 12 | cn; - num_to_bytebits(value, 24, temp); - wiegand_add_parity(temp, dest, 24); - break; - case 34 : // 34bits HID - fc &= 0xFFFF; // 16bits - cn &= 0xFFFF; // 16bits - value = fc << 16 | cn; - num_to_bytebits(value, 32, temp); - wiegand_add_parity(temp, dest, 32); - break; - case 35 : // 35bits HID - fc &= 0xFFF; // 12bits - cn &= 0xFFFFFF; // 20bits - value = fc << 20 | cn; - num_to_bytebits(value, 32, temp); - wiegand_add_parity(temp, dest, 32); - break; - case 37 : // H10304 - fc &= 0xFFFF; // 16bits - cn &= 0x7FFFF; // 19bits - value = fc << 19 | cn; - num_to_bytebits(value, 35, temp); - wiegand_add_parity(temp, dest, 35); - break; - case 39 : // 39bit KERI System Pyramid - fc &= 0x1FFFF; // 17bits - cn &= 0xFFFFFFFF; // 20bits - value = fc << 20 | cn; - num_to_bytebits(value, 37, temp); - wiegand_add_parity(temp, dest, 37); - break; - case 44 : // 44bit KERI system Pyramid - oem &= 0xFF; // 8bits - fc &= 0xFFF; // 12bits - cn &= 0xFFFFFFFF; // 21bits - value = oem << 20 | fc << 12 | cn; - num_to_bytebits(value, 42, temp); - wiegand_add_parity(temp, dest, 42); - break; - case 50 : // AWID 50 RBH - fc &= 0xFFFF; // 16bits - cn &= 0xFFFFFFFF // 32bits - value = fc << 32 | cn; - num_to_bytebits(value, 48, temp); - wiegand_add_parity(temp, dest, 48); // verify! - break; - default: - break; - } + switch ( formatlen ){ + case 26 : // 26bit HID H10301 + fc &= 0xFF; // 8bits + cn &= 0xFFFF; // 16bits + value = fc << 16 | cn; + num_to_bytebits(value, 24, temp); + wiegand_add_parity(temp, dest, 24); + break; + case 261: // 26bit Indala + fc &= 0xFFF; // 12bits + cn &= 0xFFF; // 12bits + value = fc << 12 | cn; + num_to_bytebits(value, 24, temp); + wiegand_add_parity(temp, dest, 24); + break; + case 34 : // 34bits HID + fc &= 0xFFFF; // 16bits + cn &= 0xFFFF; // 16bits + value = fc << 16 | cn; + num_to_bytebits(value, 32, temp); + wiegand_add_parity(temp, dest, 32); + break; + case 35 : // 35bits HID + fc &= 0xFFF; // 12bits + cn &= 0xFFFFFF; // 20bits + value = fc << 20 | cn; + num_to_bytebits(value, 32, temp); + wiegand_add_parity(temp, dest, 32); + break; + case 37 : // H10304 + fc &= 0xFFFF; // 16bits + cn &= 0x7FFFF; // 19bits + value = fc << 19 | cn; + num_to_bytebits(value, 35, temp); + wiegand_add_parity(temp, dest, 35); + break; + case 39 : // 39bit KERI System Pyramid + fc &= 0x1FFFF; // 17bits + cn &= 0xFFFFFFFF; // 20bits + value = fc << 20 | cn; + num_to_bytebits(value, 37, temp); + wiegand_add_parity(temp, dest, 37); + break; + case 44 : // 44bit KERI system Pyramid + oem &= 0xFF; // 8bits + fc &= 0xFFF; // 12bits + cn &= 0xFFFFFFFF; // 21bits + value = oem << 20 | fc << 12 | cn; + num_to_bytebits(value, 42, temp); + wiegand_add_parity(temp, dest, 42); + break; + case 50 : // AWID 50 RBH + fc &= 0xFFFF; // 16bits + cn &= 0xFFFFFFFF // 32bits + value = fc << 32 | cn; + num_to_bytebits(value, 48, temp); + wiegand_add_parity(temp, dest, 48); // verify! + break; + default: + break; + } } \ No newline at end of file