]>
Commit | Line | Data |
---|---|---|
1 | //----------------------------------------------------------------------------- | |
2 | // Jonathan Westhues, Sept 2005 | |
3 | // | |
4 | // This code is licensed to you under the terms of the GNU GPL, version 2 or, | |
5 | // at your option, any later version. See the LICENSE.txt file for the text of | |
6 | // the license. | |
7 | //----------------------------------------------------------------------------- | |
8 | // Utility functions used in many places, not specific to any piece of code. | |
9 | //----------------------------------------------------------------------------- | |
10 | ||
11 | #include "proxmark3.h" | |
12 | #include "util.h" | |
13 | #include "string.h" | |
14 | #include "apps.h" | |
15 | ||
16 | uint8_t *trace = (uint8_t *) BigBuf+TRACE_OFFSET; | |
17 | int traceLen = 0; | |
18 | int tracing = TRUE; | |
19 | ||
20 | ||
21 | void print_result(char *name, uint8_t *buf, size_t len) { | |
22 | uint8_t *p = buf; | |
23 | ||
24 | if ( len % 16 == 0 ) { | |
25 | for(; p-buf < len; p += 16) | |
26 | Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x", | |
27 | name, | |
28 | p-buf, | |
29 | len, | |
30 | p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7],p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15] | |
31 | ); | |
32 | } | |
33 | else { | |
34 | for(; p-buf < len; p += 8) | |
35 | Dbprintf("[%s:%d/%d] %02x %02x %02x %02x %02x %02x %02x %02x", name, p-buf, len, p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7]); | |
36 | } | |
37 | } | |
38 | ||
39 | size_t nbytes(size_t nbits) { | |
40 | return (nbits/8)+((nbits%8)>0); | |
41 | } | |
42 | ||
43 | uint32_t SwapBits(uint32_t value, int nrbits) { | |
44 | int i; | |
45 | uint32_t newvalue = 0; | |
46 | for(i = 0; i < nrbits; i++) { | |
47 | newvalue ^= ((value >> i) & 1) << (nrbits - 1 - i); | |
48 | } | |
49 | return newvalue; | |
50 | } | |
51 | ||
52 | void num_to_bytes(uint64_t n, size_t len, uint8_t* dest) | |
53 | { | |
54 | while (len--) { | |
55 | dest[len] = (uint8_t) n; | |
56 | n >>= 8; | |
57 | } | |
58 | } | |
59 | ||
60 | uint64_t bytes_to_num(uint8_t* src, size_t len) | |
61 | { | |
62 | uint64_t num = 0; | |
63 | while (len--) | |
64 | { | |
65 | num = (num << 8) | (*src); | |
66 | src++; | |
67 | } | |
68 | return num; | |
69 | } | |
70 | ||
71 | // RotateLeft - Ultralight, Desfire | |
72 | void rol(uint8_t *data, const size_t len){ | |
73 | uint8_t first = data[0]; | |
74 | for (size_t i = 0; i < len-1; i++) { | |
75 | data[i] = data[i+1]; | |
76 | } | |
77 | data[len-1] = first; | |
78 | } | |
79 | void lsl (uint8_t *data, size_t len) { | |
80 | for (size_t n = 0; n < len - 1; n++) { | |
81 | data[n] = (data[n] << 1) | (data[n+1] >> 7); | |
82 | } | |
83 | data[len - 1] <<= 1; | |
84 | } | |
85 | ||
86 | int32_t le24toh (uint8_t data[3]) | |
87 | { | |
88 | return (data[2] << 16) | (data[1] << 8) | data[0]; | |
89 | } | |
90 | ||
91 | void LEDsoff() | |
92 | { | |
93 | LED_A_OFF(); | |
94 | LED_B_OFF(); | |
95 | LED_C_OFF(); | |
96 | LED_D_OFF(); | |
97 | } | |
98 | ||
99 | // LEDs: R(C) O(A) G(B) -- R(D) [1, 2, 4 and 8] | |
100 | void LED(int led, int ms) | |
101 | { | |
102 | if (led & LED_RED) | |
103 | LED_C_ON(); | |
104 | if (led & LED_ORANGE) | |
105 | LED_A_ON(); | |
106 | if (led & LED_GREEN) | |
107 | LED_B_ON(); | |
108 | if (led & LED_RED2) | |
109 | LED_D_ON(); | |
110 | ||
111 | if (!ms) | |
112 | return; | |
113 | ||
114 | SpinDelay(ms); | |
115 | ||
116 | if (led & LED_RED) | |
117 | LED_C_OFF(); | |
118 | if (led & LED_ORANGE) | |
119 | LED_A_OFF(); | |
120 | if (led & LED_GREEN) | |
121 | LED_B_OFF(); | |
122 | if (led & LED_RED2) | |
123 | LED_D_OFF(); | |
124 | } | |
125 | ||
126 | ||
127 | // Determine if a button is double clicked, single clicked, | |
128 | // not clicked, or held down (for ms || 1sec) | |
129 | // In general, don't use this function unless you expect a | |
130 | // double click, otherwise it will waste 500ms -- use BUTTON_HELD instead | |
131 | int BUTTON_CLICKED(int ms) | |
132 | { | |
133 | // Up to 500ms in between clicks to mean a double click | |
134 | int ticks = (48000 * (ms ? ms : 1000)) >> 10; | |
135 | ||
136 | // If we're not even pressed, forget about it! | |
137 | if (!BUTTON_PRESS()) | |
138 | return BUTTON_NO_CLICK; | |
139 | ||
140 | // Borrow a PWM unit for my real-time clock | |
141 | AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0); | |
142 | // 48 MHz / 1024 gives 46.875 kHz | |
143 | AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10); | |
144 | AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0; | |
145 | AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff; | |
146 | ||
147 | uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
148 | ||
149 | int letoff = 0; | |
150 | for(;;) | |
151 | { | |
152 | uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
153 | ||
154 | // We haven't let off the button yet | |
155 | if (!letoff) | |
156 | { | |
157 | // We just let it off! | |
158 | if (!BUTTON_PRESS()) | |
159 | { | |
160 | letoff = 1; | |
161 | ||
162 | // reset our timer for 500ms | |
163 | start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
164 | ticks = (48000 * (500)) >> 10; | |
165 | } | |
166 | ||
167 | // Still haven't let it off | |
168 | else | |
169 | // Have we held down a full second? | |
170 | if (now == (uint16_t)(start + ticks)) | |
171 | return BUTTON_HOLD; | |
172 | } | |
173 | ||
174 | // We already let off, did we click again? | |
175 | else | |
176 | // Sweet, double click! | |
177 | if (BUTTON_PRESS()) | |
178 | return BUTTON_DOUBLE_CLICK; | |
179 | ||
180 | // Have we ran out of time to double click? | |
181 | else | |
182 | if (now == (uint16_t)(start + ticks)) | |
183 | // At least we did a single click | |
184 | return BUTTON_SINGLE_CLICK; | |
185 | ||
186 | WDT_HIT(); | |
187 | } | |
188 | ||
189 | // We should never get here | |
190 | return BUTTON_ERROR; | |
191 | } | |
192 | ||
193 | // Determine if a button is held down | |
194 | int BUTTON_HELD(int ms) | |
195 | { | |
196 | // If button is held for one second | |
197 | int ticks = (48000 * (ms ? ms : 1000)) >> 10; | |
198 | ||
199 | // If we're not even pressed, forget about it! | |
200 | if (!BUTTON_PRESS()) | |
201 | return BUTTON_NO_CLICK; | |
202 | ||
203 | // Borrow a PWM unit for my real-time clock | |
204 | AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0); | |
205 | // 48 MHz / 1024 gives 46.875 kHz | |
206 | AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10); | |
207 | AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0; | |
208 | AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff; | |
209 | ||
210 | uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
211 | ||
212 | for(;;) | |
213 | { | |
214 | uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
215 | ||
216 | // As soon as our button let go, we didn't hold long enough | |
217 | if (!BUTTON_PRESS()) | |
218 | return BUTTON_SINGLE_CLICK; | |
219 | ||
220 | // Have we waited the full second? | |
221 | else | |
222 | if (now == (uint16_t)(start + ticks)) | |
223 | return BUTTON_HOLD; | |
224 | ||
225 | WDT_HIT(); | |
226 | } | |
227 | ||
228 | // We should never get here | |
229 | return BUTTON_ERROR; | |
230 | } | |
231 | ||
232 | // attempt at high resolution microsecond timer | |
233 | // beware: timer counts in 21.3uS increments (1024/48Mhz) | |
234 | void SpinDelayUs(int us) | |
235 | { | |
236 | int ticks = (48*us) >> 10; | |
237 | ||
238 | // Borrow a PWM unit for my real-time clock | |
239 | AT91C_BASE_PWMC->PWMC_ENA = PWM_CHANNEL(0); | |
240 | // 48 MHz / 1024 gives 46.875 kHz | |
241 | AT91C_BASE_PWMC_CH0->PWMC_CMR = PWM_CH_MODE_PRESCALER(10); | |
242 | AT91C_BASE_PWMC_CH0->PWMC_CDTYR = 0; | |
243 | AT91C_BASE_PWMC_CH0->PWMC_CPRDR = 0xffff; | |
244 | ||
245 | uint16_t start = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
246 | ||
247 | for(;;) { | |
248 | uint16_t now = AT91C_BASE_PWMC_CH0->PWMC_CCNTR; | |
249 | if (now == (uint16_t)(start + ticks)) | |
250 | return; | |
251 | ||
252 | WDT_HIT(); | |
253 | } | |
254 | } | |
255 | ||
256 | void SpinDelay(int ms) | |
257 | { | |
258 | // convert to uS and call microsecond delay function | |
259 | SpinDelayUs(ms*1000); | |
260 | } | |
261 | ||
262 | /* Similar to FpgaGatherVersion this formats stored version information | |
263 | * into a string representation. It takes a pointer to the struct version_information, | |
264 | * verifies the magic properties, then stores a formatted string, prefixed by | |
265 | * prefix in dst. | |
266 | */ | |
267 | void FormatVersionInformation(char *dst, int len, const char *prefix, void *version_information) | |
268 | { | |
269 | struct version_information *v = (struct version_information*)version_information; | |
270 | dst[0] = 0; | |
271 | strncat(dst, prefix, len-1); | |
272 | if(v->magic != VERSION_INFORMATION_MAGIC) { | |
273 | strncat(dst, "Missing/Invalid version information", len - strlen(dst) - 1); | |
274 | return; | |
275 | } | |
276 | if(v->versionversion != 1) { | |
277 | strncat(dst, "Version information not understood", len - strlen(dst) - 1); | |
278 | return; | |
279 | } | |
280 | if(!v->present) { | |
281 | strncat(dst, "Version information not available", len - strlen(dst) - 1); | |
282 | return; | |
283 | } | |
284 | ||
285 | strncat(dst, v->gitversion, len - strlen(dst) - 1); | |
286 | if(v->clean == 0) { | |
287 | strncat(dst, "-unclean", len - strlen(dst) - 1); | |
288 | } else if(v->clean == 2) { | |
289 | strncat(dst, "-suspect", len - strlen(dst) - 1); | |
290 | } | |
291 | ||
292 | strncat(dst, " ", len - strlen(dst) - 1); | |
293 | strncat(dst, v->buildtime, len - strlen(dst) - 1); | |
294 | } | |
295 | ||
296 | // ------------------------------------------------------------------------- | |
297 | // timer lib | |
298 | // ------------------------------------------------------------------------- | |
299 | // test procedure: | |
300 | // | |
301 | // ti = GetTickCount(); | |
302 | // SpinDelay(1000); | |
303 | // ti = GetTickCount() - ti; | |
304 | // Dbprintf("timer(1s): %d t=%d", ti, GetTickCount()); | |
305 | ||
306 | void StartTickCount() | |
307 | { | |
308 | // must be 0x40, but on my cpu - included divider is optimal | |
309 | // 0x20 - 1 ms / bit | |
310 | // 0x40 - 2 ms / bit | |
311 | ||
312 | AT91C_BASE_RTTC->RTTC_RTMR = AT91C_RTTC_RTTRST + 0x001D; // was 0x003B | |
313 | } | |
314 | ||
315 | /* | |
316 | * Get the current count. | |
317 | */ | |
318 | uint32_t RAMFUNC GetTickCount(){ | |
319 | return AT91C_BASE_RTTC->RTTC_RTVR;// was * 2; | |
320 | } | |
321 | ||
322 | // ------------------------------------------------------------------------- | |
323 | // microseconds timer | |
324 | // ------------------------------------------------------------------------- | |
325 | void StartCountUS() | |
326 | { | |
327 | AT91C_BASE_PMC->PMC_PCER |= (0x1 << 12) | (0x1 << 13) | (0x1 << 14); | |
328 | // AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC1XC1S_TIOA0; | |
329 | AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_NONE | AT91C_TCB_TC1XC1S_TIOA0 | AT91C_TCB_TC2XC2S_NONE; | |
330 | ||
331 | // fast clock | |
332 | AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // timer disable | |
333 | AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | // MCK(48MHz)/32 -- tick=1.5mks | |
334 | AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_ACPA_CLEAR | | |
335 | AT91C_TC_ACPC_SET | AT91C_TC_ASWTRG_SET; | |
336 | AT91C_BASE_TC0->TC_RA = 1; | |
337 | AT91C_BASE_TC0->TC_RC = 0xBFFF + 1; // 0xC000 | |
338 | ||
339 | AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // timer disable | |
340 | AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_XC1; // from timer 0 | |
341 | ||
342 | AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN; | |
343 | AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN; | |
344 | AT91C_BASE_TCB->TCB_BCR = 1; | |
345 | } | |
346 | ||
347 | uint32_t RAMFUNC GetCountUS(){ | |
348 | return (AT91C_BASE_TC1->TC_CV * 0x8000) + ((AT91C_BASE_TC0->TC_CV / 15) * 10); | |
349 | } | |
350 | ||
351 | static uint32_t GlobalUsCounter = 0; | |
352 | ||
353 | uint32_t RAMFUNC GetDeltaCountUS(){ | |
354 | uint32_t g_cnt = GetCountUS(); | |
355 | uint32_t g_res = g_cnt - GlobalUsCounter; | |
356 | GlobalUsCounter = g_cnt; | |
357 | return g_res; | |
358 | } | |
359 | ||
360 | ||
361 | // ------------------------------------------------------------------------- | |
362 | // Timer for iso14443 commands. Uses ssp_clk from FPGA | |
363 | // ------------------------------------------------------------------------- | |
364 | void StartCountSspClk() | |
365 | { | |
366 | AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0) | (1 << AT91C_ID_TC1) | (1 << AT91C_ID_TC2); // Enable Clock to all timers | |
367 | AT91C_BASE_TCB->TCB_BMR = AT91C_TCB_TC0XC0S_TIOA1 // XC0 Clock = TIOA1 | |
368 | | AT91C_TCB_TC1XC1S_NONE // XC1 Clock = none | |
369 | | AT91C_TCB_TC2XC2S_TIOA0; // XC2 Clock = TIOA0 | |
370 | ||
371 | // configure TC1 to create a short pulse on TIOA1 when a rising edge on TIOB1 (= ssp_clk from FPGA) occurs: | |
372 | AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; // disable TC1 | |
373 | AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV1_CLOCK // TC1 Clock = MCK(48MHz)/2 = 24MHz | |
374 | | AT91C_TC_CPCSTOP // Stop clock on RC compare | |
375 | | AT91C_TC_EEVTEDG_RISING // Trigger on rising edge of Event | |
376 | | AT91C_TC_EEVT_TIOB // Event-Source: TIOB1 (= ssp_clk from FPGA = 13,56MHz/16) | |
377 | | AT91C_TC_ENETRG // Enable external trigger event | |
378 | | AT91C_TC_WAVESEL_UP // Upmode without automatic trigger on RC compare | |
379 | | AT91C_TC_WAVE // Waveform Mode | |
380 | | AT91C_TC_AEEVT_SET // Set TIOA1 on external event | |
381 | | AT91C_TC_ACPC_CLEAR; // Clear TIOA1 on RC Compare | |
382 | AT91C_BASE_TC1->TC_RC = 0x04; // RC Compare value = 0x04 | |
383 | ||
384 | // use TC0 to count TIOA1 pulses | |
385 | AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS; // disable TC0 | |
386 | AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_XC0 // TC0 clock = XC0 clock = TIOA1 | |
387 | | AT91C_TC_WAVE // Waveform Mode | |
388 | | AT91C_TC_WAVESEL_UP // just count | |
389 | | AT91C_TC_ACPA_CLEAR // Clear TIOA0 on RA Compare | |
390 | | AT91C_TC_ACPC_SET; // Set TIOA0 on RC Compare | |
391 | AT91C_BASE_TC0->TC_RA = 1; // RA Compare value = 1; pulse width to TC2 | |
392 | AT91C_BASE_TC0->TC_RC = 0; // RC Compare value = 0; increment TC2 on overflow | |
393 | ||
394 | // use TC2 to count TIOA0 pulses (giving us a 32bit counter (TC0/TC2) clocked by ssp_clk) | |
395 | AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKDIS; // disable TC2 | |
396 | AT91C_BASE_TC2->TC_CMR = AT91C_TC_CLKS_XC2 // TC2 clock = XC2 clock = TIOA0 | |
397 | | AT91C_TC_WAVE // Waveform Mode | |
398 | | AT91C_TC_WAVESEL_UP; // just count | |
399 | ||
400 | AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN; // enable TC0 | |
401 | AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN; // enable TC1 | |
402 | AT91C_BASE_TC2->TC_CCR = AT91C_TC_CLKEN; // enable TC2 | |
403 | ||
404 | // | |
405 | // synchronize the counter with the ssp_frame signal. Note: FPGA must be in any iso14446 mode, otherwise the frame signal would not be present | |
406 | // | |
407 | while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME)); // wait for ssp_frame to go high (start of frame) | |
408 | while(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_FRAME); // wait for ssp_frame to be low | |
409 | while(!(AT91C_BASE_PIOA->PIO_PDSR & GPIO_SSC_CLK)); // wait for ssp_clk to go high | |
410 | // note: up to now two ssp_clk rising edges have passed since the rising edge of ssp_frame | |
411 | // it is now safe to assert a sync signal. This sets all timers to 0 on next active clock edge | |
412 | AT91C_BASE_TCB->TCB_BCR = 1; // assert Sync (set all timers to 0 on next active clock edge) | |
413 | // at the next (3rd) ssp_clk rising edge, TC1 will be reset (and not generate a clock signal to TC0) | |
414 | // at the next (4th) ssp_clk rising edge, TC0 (the low word of our counter) will be reset. From now on, | |
415 | // whenever the last three bits of our counter go 0, we can be sure to be in the middle of a frame transfer. | |
416 | // (just started with the transfer of the 4th Bit). | |
417 | // The high word of the counter (TC2) will not reset until the low word (TC0) overflows. Therefore need to wait quite some time before | |
418 | // we can use the counter. | |
419 | while (AT91C_BASE_TC0->TC_CV < 0xFFF0); | |
420 | } | |
421 | ||
422 | ||
423 | uint32_t RAMFUNC GetCountSspClk(){ | |
424 | uint32_t tmp_count; | |
425 | tmp_count = (AT91C_BASE_TC2->TC_CV << 16) | AT91C_BASE_TC0->TC_CV; | |
426 | if ((tmp_count & 0x0000ffff) == 0) { //small chance that we may have missed an increment in TC2 | |
427 | return (AT91C_BASE_TC2->TC_CV << 16); | |
428 | } | |
429 | else { | |
430 | return tmp_count; | |
431 | } | |
432 | } | |
433 | void iso14a_clear_trace() { | |
434 | clear_trace(); | |
435 | } | |
436 | ||
437 | void iso14a_set_tracing(bool enable) { | |
438 | set_tracing(enable); | |
439 | } | |
440 | ||
441 | void clear_trace() { | |
442 | memset(trace, 0x44, TRACE_SIZE); | |
443 | traceLen = 0; | |
444 | } | |
445 | ||
446 | void set_tracing(bool enable) { | |
447 | tracing = enable; | |
448 | } | |
449 | ||
450 | /** | |
451 | This is a function to store traces. All protocols can use this generic tracer-function. | |
452 | The traces produced by calling this function can be fetched on the client-side | |
453 | by 'hf list raw', alternatively 'hf list <proto>' for protocol-specific | |
454 | annotation of commands/responses. | |
455 | ||
456 | **/ | |
457 | bool RAMFUNC LogTrace(const uint8_t *btBytes, uint16_t iLen, uint32_t timestamp_start, uint32_t timestamp_end, uint8_t *parity, bool readerToTag) | |
458 | { | |
459 | if (!tracing) return FALSE; | |
460 | ||
461 | uint16_t num_paritybytes = (iLen-1)/8 + 1; // number of valid paritybytes in *parity | |
462 | uint16_t duration = timestamp_end - timestamp_start; | |
463 | ||
464 | // Return when trace is full | |
465 | if (traceLen + sizeof(iLen) + sizeof(timestamp_start) + sizeof(duration) + num_paritybytes + iLen >= TRACE_SIZE) { | |
466 | tracing = FALSE; // don't trace any more | |
467 | return FALSE; | |
468 | } | |
469 | // Traceformat: | |
470 | // 32 bits timestamp (little endian) | |
471 | // 16 bits duration (little endian) | |
472 | // 16 bits data length (little endian, Highest Bit used as readerToTag flag) | |
473 | // y Bytes data | |
474 | // x Bytes parity (one byte per 8 bytes data) | |
475 | ||
476 | // timestamp (start) | |
477 | trace[traceLen++] = ((timestamp_start >> 0) & 0xff); | |
478 | trace[traceLen++] = ((timestamp_start >> 8) & 0xff); | |
479 | trace[traceLen++] = ((timestamp_start >> 16) & 0xff); | |
480 | trace[traceLen++] = ((timestamp_start >> 24) & 0xff); | |
481 | ||
482 | // duration | |
483 | trace[traceLen++] = ((duration >> 0) & 0xff); | |
484 | trace[traceLen++] = ((duration >> 8) & 0xff); | |
485 | ||
486 | // data length | |
487 | trace[traceLen++] = ((iLen >> 0) & 0xff); | |
488 | trace[traceLen++] = ((iLen >> 8) & 0xff); | |
489 | ||
490 | // readerToTag flag | |
491 | if (!readerToTag) { | |
492 | trace[traceLen - 1] |= 0x80; | |
493 | } | |
494 | ||
495 | // data bytes | |
496 | if (btBytes != NULL && iLen != 0) { | |
497 | memcpy(trace + traceLen, btBytes, iLen); | |
498 | } | |
499 | traceLen += iLen; | |
500 | ||
501 | // parity bytes | |
502 | if (parity != NULL && iLen != 0) { | |
503 | memcpy(trace + traceLen, parity, num_paritybytes); | |
504 | } | |
505 | traceLen += num_paritybytes; | |
506 | ||
507 | if(traceLen +4 < TRACE_SIZE) | |
508 | { //If it hadn't been cleared, for whatever reason.. | |
509 | memset(trace+traceLen,0x44, 4); | |
510 | } | |
511 | ||
512 | return TRUE; | |
513 | } | |
514 |