/
libmsr.c
219 lines (189 loc) · 4.19 KB
/
libmsr.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include "libmsr.h"
int
msr_dumpbits (uint8_t * buf, int len)
{
int bytes, i;
for (bytes = 0; bytes < len; bytes++) {
/*
* Note: we want to display the bits in the order in
* which they're read off the card, which means we
* have to decode each byte from most significant bit
* to least significant bit.
*/
for (i = 7; i > -1; i--) {
if (buf[bytes] & (1 << i))
printf("1");
else
printf("0");
}
}
printf ("\n");
return (0);
}
int
msr_getbit (uint8_t * buf, uint8_t len, int bit)
{
int byte;
int b;
if (bit > (len * 8)) {
printf ("%d > %d\n", bit, len * 8);
return (-1);
}
byte = bit / 8;
b = 7 - (bit % 8);
if (buf[byte] & (1 << b))
return (1);
return (0);
}
int
msr_setbit (uint8_t * buf, uint8_t len, int bit, int val)
{
int byte;
int b;
if (bit > (len * 8)) {
printf ("%d > %d\n", bit, len * 8);
return (-1);
}
byte = bit / 8;
b = 7 - (bit % 8);
if (val)
buf[byte] |= 1 << b;
else
buf[byte] &= ~(1 << b);
return (0);
}
int
msr_decode(uint8_t * inbuf, uint8_t inlen,
uint8_t * outbuf, uint8_t * outlen, int bpc)
{
uint8_t * b;
uint8_t len;
int ch = 0;
char byte = 0;
int i, x;
len = inlen;
b = inbuf;
x = 0;
for (i = 0; i < len * 8; i++) {
byte |= msr_getbit (b, len, i) << ch;
if (ch == (bpc - 1)) {
/* Strip the parity bit */
byte &= ~(1 << ch);
if (bpc < 7)
byte |= 0x30;
else {
if (byte < 0x20)
byte |= 0x20;
else {
byte |= 0x40;
byte -= 0x20;
}
}
outbuf[x] = byte;
x++;
/* Don't overflow output buffer */
if (x == *outlen)
break;
#ifdef MSR_DEBUG
printf ("%c", byte);
#endif
ch = 0;
byte = 0;
} else
ch++;
}
#ifdef MSR_DEBUG
printf ("\n");
#endif
/* Output buffer was too small. */
if (x == *outlen)
return (-1);
*outlen = x;
return (0);
}
/* Some cards require a swipe in the opposite direction of the reader. */
/* We can get the expected bit stream by reversing the data in place. */
int
msr_reverse_tracks (msr_tracks_t * tracks)
{
int i, status;
for (i = 0; i < MSR_MAX_TRACKS; i++) {
status = msr_reverse_track(i, tracks);
if (status != 0) {
printf("Unable to reverse track: %i\n", i);
status = -1;
break;
}
}
return status;
}
/* We want to take a track and reverse the order of each byte. */
/* Additionally, we want to flip each byte. */
int
msr_reverse_track (int track_number, msr_tracks_t * tracks)
{
int i;
int status = 0;
int bytes_to_shuffle;
char first_byte;
char last_byte;
unsigned char * head;
unsigned char * tail;
/* First we need to know the size of the track */
bytes_to_shuffle = tracks->msr_tracks[track_number].msr_tk_len;
/* Then we need to read each byte from a track */
for (i=0; i <= bytes_to_shuffle / 2; i++) {
head = &tracks->msr_tracks[track_number].msr_tk_data[i];
tail = &tracks->msr_tracks[track_number].msr_tk_data[(bytes_to_shuffle -1) -i];
/* Reverse the full track byte order */
first_byte = msr_reverse_byte(*head);
last_byte = msr_reverse_byte(*tail);
/* Reverse the byte order */
*head = last_byte;
*tail = first_byte;
}
return status;
}
/* Take a track structure and print it as hex bytes. */
void
msr_pretty_printer_hex (msr_tracks_t tracks)
{
int track_number;
for (track_number = 0; track_number < MSR_MAX_TRACKS; track_number++) {
int x;
printf("Track %d: \n", track_number);
for (x = 0; x < tracks.msr_tracks[track_number].msr_tk_len; x++)
printf("%02x ", tracks.msr_tracks[track_number].msr_tk_data[x]);
printf("\n");
}
}
/* Take a track structure and print it as a string. */
void
msr_pretty_printer_string (msr_tracks_t tracks)
{
int track_number;
for (track_number = 0; track_number < MSR_MAX_TRACKS; track_number++) {
if (tracks.msr_tracks[track_number].msr_tk_len)
printf ("Track %d: \n[%s]\n", track_number,
tracks.msr_tracks[track_number].msr_tk_data);
}
}
/* Reverse a byte. */
const unsigned char
msr_reverse_byte(const unsigned char byte)
{
return
((byte & 1<<7) >> 7) |
((byte & 1<<6) >> 5) |
((byte & 1<<5) >> 3) |
((byte & 1<<4) >> 1) |
((byte & 1<<3) << 1) |
((byte & 1<<2) << 3) |
((byte & 1<<1) << 5) |
((byte & 1<<0) << 7);
}