initial commit
[armstart-ibdap] / src / DAP.c
1 /******************************************************************************\r
2  * @file     DAP.c\r
3  * @brief    CMSIS-DAP Commands\r
4  * @version  V1.00\r
5  * @date     31. May 2012\r
6  *\r
7  * @note\r
8  * Copyright (C) 2012 ARM Limited. All rights reserved.\r
9  *\r
10  * @par\r
11  * ARM Limited (ARM) is supplying this software for use with Cortex-M\r
12  * processor based microcontrollers.\r
13  *\r
14  * @par\r
15  * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED\r
16  * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF\r
17  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.\r
18  * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR\r
19  * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.\r
20  *\r
21  ******************************************************************************/\r
22 \r
23 #include <string.h>\r
24 #include "DAP_config.h"\r
25 #include "DAP.h"\r
26 \r
27 \r
28 #define DAP_FW_VER      "1.0"   // Firmware Version\r
29 \r
30 \r
31 #if (DAP_PACKET_SIZE < 64)\r
32 #error "Minimum Packet Size is 64"\r
33 #endif\r
34 #if (DAP_PACKET_SIZE > 32768)\r
35 #error "Maximum Packet Size is 32768"\r
36 #endif\r
37 #if (DAP_PACKET_COUNT < 1)\r
38 #error "Minimum Packet Count is 1"\r
39 #endif\r
40 #if (DAP_PACKET_COUNT > 255)\r
41 #error "Maximum Packet Count is 255"\r
42 #endif\r
43 \r
44 \r
45 // Clock Macros\r
46 \r
47 #define MAX_SWJ_CLOCK(delay_cycles) \\r
48   (CPU_CLOCK/2 / (IO_PORT_WRITE_CYCLES + delay_cycles))\r
49 \r
50 #define CLOCK_DELAY(swj_clock) \\r
51  ((CPU_CLOCK/2 / swj_clock) - IO_PORT_WRITE_CYCLES)\r
52 \r
53 \r
54          DAP_Data_t DAP_Data;           // DAP Data\r
55 volatile uint8_t    DAP_TransferAbort;  // Trasfer Abort Flag\r
56 \r
57 \r
58 #ifdef DAP_VENDOR\r
59 const char DAP_Vendor [] = DAP_VENDOR;\r
60 #endif\r
61 #ifdef DAP_PRODUCT\r
62 const char DAP_Product[] = DAP_PRODUCT;\r
63 #endif\r
64 #ifdef DAP_SER_NUM\r
65 const char DAP_SerNum [] = DAP_SER_NUM;\r
66 #endif\r
67 const char DAP_FW_Ver [] = DAP_FW_VER;\r
68 \r
69 #if TARGET_DEVICE_FIXED\r
70 const char TargetDeviceVendor [] = TARGET_DEVICE_VENDOR;\r
71 const char TargetDeviceName   [] = TARGET_DEVICE_NAME;\r
72 #endif\r
73 \r
74 \r
75 // Get DAP Information\r
76 //   id:      info identifier\r
77 //   info:    pointer to info data\r
78 //   return:  number of bytes in info data\r
79 static uint8_t DAP_Info(uint8_t id, uint8_t *info) {\r
80   uint8_t length = 0;\r
81 \r
82   switch (id) {\r
83     case DAP_ID_VENDOR:\r
84 #ifdef DAP_VENDOR\r
85       memcpy(info, DAP_Vendor, sizeof(DAP_Vendor));\r
86       length = sizeof(DAP_Vendor);\r
87 #endif\r
88       break;\r
89     case DAP_ID_PRODUCT:\r
90 #ifdef DAP_PRODUCT\r
91       memcpy(info, DAP_Product, sizeof(DAP_Product));\r
92       length = sizeof(DAP_Product);\r
93 #endif\r
94       break;\r
95     case DAP_ID_SER_NUM:\r
96 #ifdef DAP_SER_NUM\r
97       memcpy(info, DAP_SerNum, sizeof(DAP_SerNum));\r
98       length = sizeof(DAP_SerNum);\r
99 #endif\r
100       break;\r
101     case DAP_ID_FW_VER:\r
102       memcpy(info, DAP_FW_Ver, sizeof(DAP_FW_Ver));\r
103       length = sizeof(DAP_FW_Ver);\r
104       break;\r
105     case DAP_ID_DEVICE_VENDOR:\r
106 #if TARGET_DEVICE_FIXED\r
107       memcpy(info, TargetDeviceVendor, sizeof(TargetDeviceVendor));\r
108       length = sizeof(TargetDeviceVendor);\r
109 #endif\r
110       break;\r
111     case DAP_ID_DEVICE_NAME:\r
112 #if TARGET_DEVICE_FIXED\r
113       memcpy(info, TargetDeviceName, sizeof(TargetDeviceName));\r
114       length = sizeof(TargetDeviceName);\r
115 #endif\r
116       break;\r
117     case DAP_ID_CAPABILITIES:\r
118       info[0] = ((DAP_SWD  != 0) ? (1 << 0) : 0) |\r
119                 ((DAP_JTAG != 0) ? (1 << 1) : 0);\r
120       length = 1;\r
121       break;\r
122     case DAP_ID_PACKET_SIZE:\r
123       info[0] = (uint8_t)(DAP_PACKET_SIZE >> 0);\r
124       info[1] = (uint8_t)(DAP_PACKET_SIZE >> 8);\r
125       length = 2;\r
126       break;\r
127     case DAP_ID_PACKET_COUNT:\r
128       info[0] = DAP_PACKET_COUNT;\r
129       length = 1;\r
130       break;\r
131   }\r
132 \r
133   return (length);\r
134 }\r
135 \r
136 \r
137 // Timer Functions\r
138 \r
139 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))\r
140 \r
141 // Start Timer\r
142 static __inline void TIMER_START (uint32_t usec) {\r
143   SysTick->VAL  = 0;\r
144   SysTick->LOAD = usec * CPU_CLOCK/1000000;\r
145   SysTick->CTRL = (1 << SysTick_CTRL_ENABLE_Pos) |\r
146                   (1 << SysTick_CTRL_CLKSOURCE_Pos);\r
147 }\r
148 \r
149 // Stop Timer\r
150 static __inline void TIMER_STOP (void) {\r
151   SysTick->CTRL = 0;\r
152 }\r
153 \r
154 // Check if Timer expired\r
155 static __inline uint32_t TIMER_EXPIRED (void) {\r
156   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) ? 1 : 0);\r
157 }\r
158 \r
159 #endif\r
160 \r
161 \r
162 // Delay for specified time\r
163 //    delay:  delay time in ms\r
164 void Delayms(uint32_t delay) {\r
165   delay *= (CPU_CLOCK/1000 + (DELAY_SLOW_CYCLES-1)) / DELAY_SLOW_CYCLES;\r
166   PIN_DELAY_SLOW(delay);\r
167 }\r
168 \r
169 \r
170 // Process Delay command and prepare response\r
171 //   request:  pointer to request data\r
172 //   response: pointer to response data\r
173 //   return:   number of bytes in response\r
174 static uint32_t DAP_Delay(uint8_t *request, uint8_t *response) {\r
175   uint32_t delay;\r
176 \r
177   delay  = *(request+0) | (*(request+1) << 8);\r
178   delay *= (CPU_CLOCK/1000000 + (DELAY_SLOW_CYCLES-1)) / DELAY_SLOW_CYCLES;\r
179 \r
180   PIN_DELAY_SLOW(delay);\r
181 \r
182   *response = DAP_OK;\r
183   return (1);\r
184 }\r
185 \r
186 \r
187 // Process LED command and prepare response\r
188 //   request:  pointer to request data\r
189 //   response: pointer to response data\r
190 //   return:   number of bytes in response\r
191 static uint32_t DAP_LED(uint8_t *request, uint8_t *response) {\r
192 \r
193   switch (*request) {\r
194     case DAP_LED_DEBUGGER_CONNECTED:\r
195       LED_CONNECTED_OUT((*(request+1) & 1));\r
196       break;\r
197     case DAP_LED_TARGET_RUNNING:\r
198       LED_RUNNING_OUT((*(request+1) & 1));\r
199       break;\r
200     default:\r
201       *response = DAP_ERROR;\r
202       return (1);\r
203   }\r
204 \r
205   *response = DAP_OK;\r
206   return (1);\r
207 }\r
208 \r
209 \r
210 // Process Connect command and prepare response\r
211 //   request:  pointer to request data\r
212 //   response: pointer to response data\r
213 //   return:   number of bytes in response\r
214 static uint32_t DAP_Connect(uint8_t *request, uint8_t *response) {\r
215   uint32_t port;\r
216 \r
217   if (*request == DAP_PORT_AUTODETECT) {\r
218     port = DAP_DEFAULT_PORT;\r
219   } else {\r
220     port = *request;\r
221   }\r
222   \r
223   switch (port) {\r
224 #if (DAP_SWD != 0)\r
225     case DAP_PORT_SWD:\r
226       DAP_Data.debug_port = DAP_PORT_SWD;\r
227       PORT_SWD_SETUP();\r
228       break;\r
229 #endif\r
230 #if (DAP_JTAG != 0)\r
231     case DAP_PORT_JTAG:\r
232       DAP_Data.debug_port = DAP_PORT_JTAG;\r
233       PORT_JTAG_SETUP();\r
234       break;\r
235 #endif\r
236     default:\r
237       *response = DAP_PORT_DISABLED;\r
238       return (1);\r
239   }\r
240 \r
241   *response = port;\r
242   return (1);\r
243 }\r
244 \r
245 \r
246 // Process Disconnect command and prepare response\r
247 //   request:  pointer to request data\r
248 //   response: pointer to response data\r
249 //   return:   number of bytes in response\r
250 static uint32_t DAP_Disconnect(uint8_t *response) {\r
251 \r
252   DAP_Data.debug_port = DAP_PORT_DISABLED;\r
253   PORT_OFF();\r
254 \r
255   *response = DAP_OK;\r
256   return (1);\r
257 }\r
258 \r
259 \r
260 // Process Reset Target command and prepare response\r
261 //   request:  pointer to request data\r
262 //   response: pointer to response data\r
263 //   return:   number of bytes in response\r
264 static uint32_t DAP_ResetTarget(uint8_t *response) {\r
265 \r
266   *(response+1) = RESET_TARGET();\r
267   *(response+0) = DAP_OK;\r
268   return (2);\r
269 }\r
270 \r
271 \r
272 // Process SWJ Pins command and prepare response\r
273 //   request:  pointer to request data\r
274 //   response: pointer to response data\r
275 //   return:   number of bytes in response\r
276 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))\r
277 static uint32_t DAP_SWJ_Pins(uint8_t *request, uint8_t *response) {\r
278   uint32_t value;\r
279   uint32_t select;\r
280   uint32_t wait;\r
281   \r
282   value  =  *(request+0);\r
283   select =  *(request+1); \r
284   wait   = (*(request+2) <<  0) |\r
285            (*(request+3) <<  8) |\r
286            (*(request+4) << 16) |\r
287            (*(request+5) << 24);\r
288 \r
289   if (select & (1 << DAP_SWJ_SWCLK_TCK)) {\r
290     if (value & (1 << DAP_SWJ_SWCLK_TCK)) {\r
291       PIN_SWCLK_TCK_SET();\r
292     } else {\r
293       PIN_SWCLK_TCK_CLR();\r
294     }\r
295   }\r
296   if (select & (1 << DAP_SWJ_SWDIO_TMS)) {\r
297     if (value & (1 << DAP_SWJ_SWDIO_TMS)) {\r
298       PIN_SWDIO_TMS_SET();\r
299     } else {\r
300       PIN_SWDIO_TMS_CLR();\r
301     }\r
302   }\r
303   if (select & (1 << DAP_SWJ_TDI)) {\r
304     PIN_TDI_OUT(value >> DAP_SWJ_TDI);\r
305   }\r
306   if (select & (1 << DAP_SWJ_nTRST)) {\r
307     PIN_nTRST_OUT(value >> DAP_SWJ_nTRST);\r
308   }\r
309   if (select & (1 << DAP_SWJ_nRESET)) {\r
310     PIN_nRESET_OUT(value >> DAP_SWJ_nRESET);\r
311   }\r
312 \r
313   if (wait) {\r
314     if (wait > 3000000) wait = 3000000;\r
315     TIMER_START(wait);\r
316     do {\r
317       if (select & (1 << DAP_SWJ_SWCLK_TCK)) {\r
318         if ((value >> DAP_SWJ_SWCLK_TCK) ^ PIN_SWCLK_TCK_IN()) continue;\r
319       }\r
320       if (select & (1 << DAP_SWJ_SWDIO_TMS)) {\r
321         if ((value >> DAP_SWJ_SWDIO_TMS) ^ PIN_SWDIO_TMS_IN()) continue;\r
322       }\r
323       if (select & (1 << DAP_SWJ_TDI)) {\r
324         if ((value >> DAP_SWJ_TDI) ^ PIN_TDI_IN()) continue;\r
325       }\r
326       if (select & (1 << DAP_SWJ_nTRST)) {\r
327         if ((value >> DAP_SWJ_nTRST) ^ PIN_nTRST_IN()) continue;\r
328       }\r
329       if (select & (1 << DAP_SWJ_nRESET)) {\r
330         if ((value >> DAP_SWJ_nRESET) ^ PIN_nRESET_IN()) continue;\r
331       }\r
332       break;\r
333     } while (!TIMER_EXPIRED());\r
334     TIMER_STOP();\r
335   }\r
336 \r
337   value = (PIN_SWCLK_TCK_IN() << DAP_SWJ_SWCLK_TCK) |\r
338           (PIN_SWDIO_TMS_IN() << DAP_SWJ_SWDIO_TMS) |\r
339           (PIN_TDI_IN()       << DAP_SWJ_TDI)       |\r
340           (PIN_TDO_IN()       << DAP_SWJ_TDO)       |\r
341           (PIN_nTRST_IN()     << DAP_SWJ_nTRST)     |\r
342           (PIN_nRESET_IN()    << DAP_SWJ_nRESET);\r
343 \r
344   *response = (uint8_t)value;\r
345   return (1);\r
346 }\r
347 #endif\r
348 \r
349 \r
350 // Process SWJ Clock command and prepare response\r
351 //   request:  pointer to request data\r
352 //   response: pointer to response data\r
353 //   return:   number of bytes in response\r
354 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))\r
355 static uint32_t DAP_SWJ_Clock(uint8_t *request, uint8_t *response) {\r
356   uint32_t clock;\r
357   uint32_t delay;\r
358 \r
359   clock = (*(request+0) <<  0) |\r
360           (*(request+1) <<  8) |\r
361           (*(request+2) << 16) |\r
362           (*(request+3) << 24);\r
363 \r
364   if (clock == 0) {\r
365     *response = DAP_ERROR;\r
366     return (1);\r
367   }\r
368 \r
369   if (clock >= MAX_SWJ_CLOCK(DELAY_FAST_CYCLES)) {\r
370     DAP_Data.fast_clock  = 1;\r
371     DAP_Data.clock_delay = 1;\r
372   } else {\r
373     DAP_Data.fast_clock  = 0;\r
374 \r
375     delay = (CPU_CLOCK/2 + (clock - 1)) / clock;\r
376     if (delay > IO_PORT_WRITE_CYCLES) {\r
377       delay -= IO_PORT_WRITE_CYCLES;\r
378       delay  = (delay + (DELAY_SLOW_CYCLES - 1)) / DELAY_SLOW_CYCLES;\r
379     } else {\r
380       delay  = 1;\r
381     }\r
382 \r
383     DAP_Data.clock_delay = delay;\r
384   }\r
385 \r
386   *response = DAP_OK;\r
387   return (1);\r
388 }\r
389 #endif\r
390 \r
391 \r
392 // Process SWJ Sequence command and prepare response\r
393 //   request:  pointer to request data\r
394 //   response: pointer to response data\r
395 //   return:   number of bytes in response\r
396 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))\r
397 static uint32_t DAP_SWJ_Sequence(uint8_t *request, uint8_t *response) {\r
398   uint32_t count;\r
399 \r
400   count = *request++;\r
401   if (count == 0) count = 256;\r
402 \r
403   SWJ_Sequence(count, request);\r
404 \r
405   *response = DAP_OK;\r
406   return (1);\r
407 }\r
408 #endif\r
409 \r
410 \r
411 // Process SWD Configure command and prepare response\r
412 //   request:  pointer to request data\r
413 //   response: pointer to response data\r
414 //   return:   number of bytes in response\r
415 #if (DAP_SWD != 0)\r
416 static uint32_t DAP_SWD_Configure(uint8_t *request, uint8_t *response) {\r
417   uint8_t value;\r
418 \r
419   value = *request;\r
420   DAP_Data.swd_conf.turnaround  = (value & 0x03) + 1;\r
421   DAP_Data.swd_conf.data_phase  = (value & 0x04) ? 1 : 0;\r
422   \r
423   *response = DAP_OK;\r
424 \r
425   return (1);\r
426 }\r
427 #endif\r
428 \r
429 \r
430 // Process SWD Abort command and prepare response\r
431 //   request:  pointer to request data\r
432 //   response: pointer to response data\r
433 //   return:   number of bytes in response\r
434 #if (DAP_SWD != 0)\r
435 static uint32_t DAP_SWD_Abort(uint8_t *request, uint8_t *response) {\r
436   uint32_t data;\r
437 \r
438   if (DAP_Data.debug_port != DAP_PORT_SWD) {\r
439     *response = DAP_ERROR;\r
440     return (1);\r
441   }\r
442 \r
443   // Load data (Ignore DAP index)\r
444   data = (*(request+1) <<  0) |\r
445          (*(request+2) <<  8) |\r
446          (*(request+3) << 16) |\r
447          (*(request+4) << 24);\r
448 \r
449   // Write Abort register\r
450   SWD_Transfer(DP_ABORT, &data);\r
451   *response = DAP_OK;\r
452 \r
453   return (1);\r
454 }\r
455 #endif\r
456 \r
457 \r
458 // Process JTAG Sequence command and prepare response\r
459 //   request:  pointer to request data\r
460 //   response: pointer to response data\r
461 //   return:   number of bytes in response\r
462 #if (DAP_JTAG != 0)\r
463 static uint32_t DAP_JTAG_Sequence(uint8_t *request, uint8_t *response) {\r
464   uint32_t sequence_info;\r
465   uint32_t sequence_count;\r
466   uint32_t response_count;\r
467   uint32_t count;\r
468 \r
469   *response++ = DAP_OK;\r
470   response_count = 1;\r
471 \r
472   sequence_count = *request++;\r
473   while (sequence_count--) {\r
474     sequence_info = *request++;\r
475     JTAG_Sequence(sequence_info, request, response);\r
476     count = sequence_info & JTAG_SEQUENCE_TCK;\r
477     if (count == 0) count = 64;\r
478     count = (count + 7) / 8;\r
479     request += count;\r
480     if (sequence_info & JTAG_SEQUENCE_TDO) {\r
481       response += count;\r
482       response_count += count;\r
483     }\r
484   }\r
485 \r
486   return (response_count);\r
487 }\r
488 #endif\r
489 \r
490 \r
491 // Process JTAG Configure command and prepare response\r
492 //   request:  pointer to request data\r
493 //   response: pointer to response data\r
494 //   return:   number of bytes in response\r
495 #if (DAP_JTAG != 0)\r
496 static uint32_t DAP_JTAG_Configure(uint8_t *request, uint8_t *response) {\r
497   uint32_t count;\r
498   uint32_t length;\r
499   uint32_t bits;\r
500   uint32_t n;\r
501 \r
502   count = *request++;\r
503   DAP_Data.jtag_dev.count = count;\r
504 \r
505   bits = 0;\r
506   for (n = 0; n < count; n++) {\r
507     length = *request++;\r
508     DAP_Data.jtag_dev.ir_length[n] = length;\r
509     DAP_Data.jtag_dev.ir_before[n] = bits;\r
510     bits += length;\r
511   }\r
512   for (n = 0; n < count; n++) {\r
513     bits -= DAP_Data.jtag_dev.ir_length[n];\r
514     DAP_Data.jtag_dev.ir_after[n] = bits;\r
515   }\r
516 \r
517   *response = DAP_OK;\r
518   return (1);\r
519 }\r
520 #endif\r
521 \r
522 \r
523 // Process JTAG IDCODE command and prepare response\r
524 //   request:  pointer to request data\r
525 //   response: pointer to response data\r
526 //   return:   number of bytes in response\r
527 #if (DAP_JTAG != 0)\r
528 static uint32_t DAP_JTAG_IDCode(uint8_t *request, uint8_t *response) {\r
529   uint32_t data;\r
530 \r
531   if (DAP_Data.debug_port != DAP_PORT_JTAG) {\r
532 err:*response = DAP_ERROR;\r
533     return (1);\r
534   }\r
535 \r
536   // Device index (JTAP TAP)\r
537   DAP_Data.jtag_dev.index = *request;\r
538   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto err;\r
539 \r
540   // Select JTAG chain\r
541   JTAG_IR(JTAG_IDCODE);\r
542 \r
543   // Read IDCODE register\r
544   data = JTAG_ReadIDCode();\r
545 \r
546   // Store Data\r
547   *(response+0) =  DAP_OK;\r
548   *(response+1) = (uint8_t)(data >>  0);\r
549   *(response+2) = (uint8_t)(data >>  8);\r
550   *(response+3) = (uint8_t)(data >> 16);\r
551   *(response+4) = (uint8_t)(data >> 24);\r
552 \r
553   return (1+4);\r
554 }\r
555 #endif\r
556 \r
557 \r
558 // Process JTAG Abort command and prepare response\r
559 //   request:  pointer to request data\r
560 //   response: pointer to response data\r
561 //   return:   number of bytes in response\r
562 #if (DAP_JTAG != 0)\r
563 static uint32_t DAP_JTAG_Abort(uint8_t *request, uint8_t *response) {\r
564   uint32_t data;\r
565 \r
566   if (DAP_Data.debug_port != DAP_PORT_JTAG) {\r
567 err:*response = DAP_ERROR;\r
568     return (1);\r
569   }\r
570 \r
571   // Device index (JTAP TAP)\r
572   DAP_Data.jtag_dev.index = *request;\r
573   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto err;\r
574 \r
575   // Select JTAG chain\r
576   JTAG_IR(JTAG_ABORT);\r
577 \r
578   // Load data\r
579   data = (*(request+1) <<  0) |\r
580          (*(request+2) <<  8) |\r
581          (*(request+3) << 16) |\r
582          (*(request+4) << 24);\r
583 \r
584   // Write Abort register\r
585   JTAG_WriteAbort(data);\r
586   *response = DAP_OK;\r
587 \r
588   return (1);\r
589 }\r
590 #endif\r
591 \r
592 \r
593 // Process Transfer Configure command and prepare response\r
594 //   request:  pointer to request data\r
595 //   response: pointer to response data\r
596 //   return:   number of bytes in response\r
597 static uint32_t DAP_TransferConfigure(uint8_t *request, uint8_t *response) {\r
598 \r
599   DAP_Data.transfer.idle_cycles = *(request+0);\r
600   DAP_Data.transfer.retry_count = *(request+1) | (*(request+2) << 8);\r
601   DAP_Data.transfer.match_retry = *(request+3) | (*(request+4) << 8);\r
602   \r
603   *response = DAP_OK;\r
604 \r
605   return (1);\r
606 }\r
607 \r
608 \r
609 // Process SWD Transfer command and prepare response\r
610 //   request:  pointer to request data\r
611 //   response: pointer to response data\r
612 //   return:   number of bytes in response\r
613 #if (DAP_SWD != 0)\r
614 static uint32_t DAP_SWD_Transfer(uint8_t *request, uint8_t *response) {\r
615   uint32_t  request_count;\r
616   uint32_t  request_value;\r
617   uint32_t  response_count;\r
618   uint32_t  response_value;\r
619   uint8_t  *response_head;\r
620   uint32_t  post_read;\r
621   uint32_t  check_write;\r
622   uint32_t  match_value;\r
623   uint32_t  match_retry;\r
624   uint32_t  retry;\r
625   uint32_t  data;\r
626 \r
627   response_count = 0;\r
628   response_value = 0;\r
629   response_head  = response;\r
630   response      += 2;\r
631 \r
632   DAP_TransferAbort = 0;\r
633 \r
634   post_read   = 0;\r
635   check_write = 0;\r
636 \r
637   request++;            // Ignore DAP index\r
638 \r
639   request_count = *request++;\r
640   while (request_count--) {\r
641     request_value = *request++;\r
642     if (request_value & DAP_TRANSFER_RnW) {\r
643       // Read register\r
644       if (post_read) {\r
645         // Read was posted before\r
646         retry = DAP_Data.transfer.retry_count;\r
647         if ((request_value & (DAP_TRANSFER_APnDP | DAP_TRANSFER_MATCH_VALUE)) == DAP_TRANSFER_APnDP) {\r
648           // Read previous AP data and post next AP read\r
649           do {\r
650             response_value = SWD_Transfer(request_value, &data);\r
651           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
652         } else {\r
653           // Read previous AP data\r
654           do {\r
655             response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
656           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
657           post_read = 0;\r
658         }\r
659         if (response_value != DAP_TRANSFER_OK) break;\r
660         // Store previous AP data\r
661         *response++ = (uint8_t) data;\r
662         *response++ = (uint8_t)(data >>  8);\r
663         *response++ = (uint8_t)(data >> 16);\r
664         *response++ = (uint8_t)(data >> 24);\r
665       }\r
666       if (request_value & DAP_TRANSFER_MATCH_VALUE) {\r
667         // Read with value match\r
668         match_value = (*(request+0) <<  0) |\r
669                       (*(request+1) <<  8) |\r
670                       (*(request+2) << 16) |\r
671                       (*(request+3) << 24);\r
672         request += 4;\r
673         match_retry = DAP_Data.transfer.match_retry;\r
674         if (request_value & DAP_TRANSFER_APnDP) {\r
675           // Post AP read\r
676           retry = DAP_Data.transfer.retry_count;\r
677           do {\r
678             response_value = SWD_Transfer(request_value, NULL);\r
679           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
680           if (response_value != DAP_TRANSFER_OK) break;\r
681         }\r
682         do {\r
683           // Read register until its value matches or retry counter expires\r
684           retry = DAP_Data.transfer.retry_count;\r
685           do {\r
686             response_value = SWD_Transfer(request_value, &data);\r
687           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
688           if (response_value != DAP_TRANSFER_OK) break;\r
689         } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);\r
690         if ((data & DAP_Data.transfer.match_mask) != match_value) {\r
691           response_value |= DAP_TRANSFER_MISMATCH;\r
692         }\r
693         if (response_value != DAP_TRANSFER_OK) break;\r
694       } else {\r
695         // Normal read\r
696         retry = DAP_Data.transfer.retry_count;\r
697         if (request_value & DAP_TRANSFER_APnDP) {\r
698           // Read AP register\r
699           if (post_read == 0) {\r
700             // Post AP read\r
701             do {\r
702               response_value = SWD_Transfer(request_value, NULL);\r
703             } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
704             if (response_value != DAP_TRANSFER_OK) break;\r
705             post_read = 1;\r
706           }\r
707         } else {\r
708           // Read DP register\r
709           do {\r
710             response_value = SWD_Transfer(request_value, &data);\r
711           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
712           if (response_value != DAP_TRANSFER_OK) break;\r
713           // Store data\r
714           *response++ = (uint8_t) data;\r
715           *response++ = (uint8_t)(data >>  8);\r
716           *response++ = (uint8_t)(data >> 16);\r
717           *response++ = (uint8_t)(data >> 24);\r
718         }\r
719       }\r
720       check_write = 0;\r
721     } else {\r
722       // Write register\r
723       if (post_read) {\r
724         // Read previous data\r
725         retry = DAP_Data.transfer.retry_count;\r
726         do {\r
727           response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
728         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
729         if (response_value != DAP_TRANSFER_OK) break;\r
730         // Store previous data\r
731         *response++ = (uint8_t) data;\r
732         *response++ = (uint8_t)(data >>  8);\r
733         *response++ = (uint8_t)(data >> 16);\r
734         *response++ = (uint8_t)(data >> 24);\r
735         post_read = 0;\r
736       }\r
737       // Load data\r
738       data = (*(request+0) <<  0) |\r
739              (*(request+1) <<  8) |\r
740              (*(request+2) << 16) |\r
741              (*(request+3) << 24);\r
742       request += 4;\r
743       if (request_value & DAP_TRANSFER_MATCH_MASK) {\r
744         // Write match mask\r
745         DAP_Data.transfer.match_mask = data;\r
746         response_value = DAP_TRANSFER_OK;\r
747       } else {\r
748         // Write DP/AP register\r
749         retry = DAP_Data.transfer.retry_count;\r
750         do {\r
751           response_value = SWD_Transfer(request_value, &data);\r
752         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
753         if (response_value != DAP_TRANSFER_OK) break;\r
754         check_write = 1;\r
755       }\r
756     }\r
757     response_count++;\r
758     if (DAP_TransferAbort) break;\r
759   }\r
760 \r
761   if (response_value == DAP_TRANSFER_OK) {\r
762     if (post_read) {\r
763       // Read previous data\r
764       retry = DAP_Data.transfer.retry_count;\r
765       do {\r
766         response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
767       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
768       if (response_value != DAP_TRANSFER_OK) goto end;\r
769       // Store previous data\r
770       *response++ = (uint8_t) data;\r
771       *response++ = (uint8_t)(data >>  8);\r
772       *response++ = (uint8_t)(data >> 16);\r
773       *response++ = (uint8_t)(data >> 24);\r
774     } else if (check_write) {\r
775       // Check last write\r
776       retry = DAP_Data.transfer.retry_count;\r
777       do {\r
778         response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);\r
779       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
780     }\r
781   }\r
782 \r
783 end:\r
784   *(response_head+0) = (uint8_t)response_count;\r
785   *(response_head+1) = (uint8_t)response_value;\r
786 \r
787   return (response - response_head);\r
788 }\r
789 #endif\r
790 \r
791 \r
792 // Process JTAG Transfer command and prepare response\r
793 //   request:  pointer to request data\r
794 //   response: pointer to response data\r
795 //   return:   number of bytes in response\r
796 #if (DAP_JTAG != 0)\r
797 static uint32_t DAP_JTAG_Transfer(uint8_t *request, uint8_t *response) {\r
798   uint32_t  request_count;\r
799   uint32_t  request_value;\r
800   uint32_t  request_ir;\r
801   uint32_t  response_count;\r
802   uint32_t  response_value;\r
803   uint8_t  *response_head;\r
804   uint32_t  post_read;\r
805   uint32_t  match_value;\r
806   uint32_t  match_retry;\r
807   uint32_t  retry;\r
808   uint32_t  data;\r
809   uint32_t  ir;\r
810 \r
811   response_count = 0;\r
812   response_value = 0;\r
813   response_head  = response;\r
814   response      += 2;\r
815 \r
816   DAP_TransferAbort = 0;\r
817 \r
818   ir        = 0;\r
819   post_read = 0;\r
820 \r
821   // Device index (JTAP TAP)\r
822   DAP_Data.jtag_dev.index = *request++;\r
823   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto end;\r
824 \r
825   request_count = *request++;\r
826   while (request_count--) {\r
827     request_value = *request++;\r
828     request_ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;\r
829     if (request_value & DAP_TRANSFER_RnW) {\r
830       // Read register\r
831       if (post_read) {\r
832         // Read was posted before\r
833         retry = DAP_Data.transfer.retry_count;\r
834         if ((ir == request_ir) && ((request_value & DAP_TRANSFER_MATCH_VALUE) == 0)) {\r
835           // Read previous data and post next read\r
836           do {\r
837             response_value = JTAG_Transfer(request_value, &data);\r
838           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
839         } else {\r
840           // Select JTAG chain\r
841           if (ir != JTAG_DPACC) {\r
842             ir = JTAG_DPACC;\r
843             JTAG_IR(ir);\r
844           }\r
845           // Read previous data\r
846           do {\r
847             response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
848           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
849           post_read = 0;\r
850         }\r
851         if (response_value != DAP_TRANSFER_OK) break;\r
852         // Store previous data\r
853         *response++ = (uint8_t) data;\r
854         *response++ = (uint8_t)(data >>  8);\r
855         *response++ = (uint8_t)(data >> 16);\r
856         *response++ = (uint8_t)(data >> 24);\r
857       }\r
858       if (request_value & DAP_TRANSFER_MATCH_VALUE) {\r
859         // Read with value match\r
860         match_value = (*(request+0) <<  0) |\r
861                       (*(request+1) <<  8) |\r
862                       (*(request+2) << 16) |\r
863                       (*(request+3) << 24);\r
864         request += 4;\r
865         match_retry  = DAP_Data.transfer.match_retry;\r
866         // Select JTAG chain\r
867         if (ir != request_ir) {\r
868           ir = request_ir;\r
869           JTAG_IR(ir);\r
870         }\r
871         // Post DP/AP read\r
872         retry = DAP_Data.transfer.retry_count;\r
873         do {\r
874           response_value = JTAG_Transfer(request_value, NULL);\r
875         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
876         if (response_value != DAP_TRANSFER_OK) break;\r
877         do {\r
878           // Read register until its value matches or retry counter expires\r
879           retry = DAP_Data.transfer.retry_count;\r
880           do {\r
881             response_value = JTAG_Transfer(request_value, &data);\r
882           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
883           if (response_value != DAP_TRANSFER_OK) break;\r
884         } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);\r
885         if ((data & DAP_Data.transfer.match_mask) != match_value) {\r
886           response_value |= DAP_TRANSFER_MISMATCH;\r
887         }\r
888         if (response_value != DAP_TRANSFER_OK) break;\r
889       } else {\r
890         // Normal read\r
891         if (post_read == 0) {\r
892           // Select JTAG chain\r
893           if (ir != request_ir) {\r
894             ir = request_ir;\r
895             JTAG_IR(ir);\r
896           }\r
897           // Post DP/AP read\r
898           retry = DAP_Data.transfer.retry_count;\r
899           do {\r
900             response_value = JTAG_Transfer(request_value, NULL);\r
901           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
902           if (response_value != DAP_TRANSFER_OK) break;\r
903           post_read = 1;\r
904         }\r
905       }\r
906     } else {\r
907       // Write register\r
908       if (post_read) {\r
909         // Select JTAG chain\r
910         if (ir != JTAG_DPACC) {\r
911           ir = JTAG_DPACC;\r
912           JTAG_IR(ir);\r
913         }\r
914         // Read previous data\r
915         retry = DAP_Data.transfer.retry_count;\r
916         do {\r
917           response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
918         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
919         if (response_value != DAP_TRANSFER_OK) break;\r
920         // Store previous data\r
921         *response++ = (uint8_t) data;\r
922         *response++ = (uint8_t)(data >>  8);\r
923         *response++ = (uint8_t)(data >> 16);\r
924         *response++ = (uint8_t)(data >> 24);\r
925         post_read = 0;\r
926       }\r
927       // Load data\r
928       data = (*(request+0) <<  0) |\r
929              (*(request+1) <<  8) |\r
930              (*(request+2) << 16) |\r
931              (*(request+3) << 24);\r
932       request += 4;\r
933       if (request_value & DAP_TRANSFER_MATCH_MASK) {\r
934         // Write match mask\r
935         DAP_Data.transfer.match_mask = data;\r
936         response_value = DAP_TRANSFER_OK;\r
937       } else {\r
938         // Select JTAG chain\r
939         if (ir != request_ir) {\r
940           ir = request_ir;\r
941           JTAG_IR(ir);\r
942         }\r
943         // Write DP/AP register\r
944         retry = DAP_Data.transfer.retry_count;\r
945         do {\r
946           response_value = JTAG_Transfer(request_value, &data);\r
947         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
948         if (response_value != DAP_TRANSFER_OK) break;\r
949       }\r
950     }\r
951     response_count++;\r
952     if (DAP_TransferAbort) break;\r
953   }\r
954 \r
955   if (response_value == DAP_TRANSFER_OK) {\r
956     // Select JTAG chain\r
957     if (ir != JTAG_DPACC) {\r
958       ir = JTAG_DPACC;\r
959       JTAG_IR(ir);\r
960     }\r
961     if (post_read) {\r
962       // Read previous data\r
963       retry = DAP_Data.transfer.retry_count;\r
964       do {\r
965         response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);\r
966       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
967       if (response_value != DAP_TRANSFER_OK) goto end;\r
968       // Store previous data\r
969       *response++ = (uint8_t) data;\r
970       *response++ = (uint8_t)(data >>  8);\r
971       *response++ = (uint8_t)(data >> 16);\r
972       *response++ = (uint8_t)(data >> 24);\r
973     } else {\r
974       // Check last write\r
975       retry = DAP_Data.transfer.retry_count;\r
976       do {\r
977         response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);\r
978       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
979     }\r
980   }\r
981 \r
982 end:\r
983   *(response_head+0) = (uint8_t)response_count;\r
984   *(response_head+1) = (uint8_t)response_value;\r
985 \r
986   return (response - response_head);\r
987 }\r
988 #endif\r
989 \r
990 \r
991 // Process SWD Transfer Block command and prepare response\r
992 //   request:  pointer to request data\r
993 //   response: pointer to response data\r
994 //   return:   number of bytes in response\r
995 #if (DAP_SWD != 0)\r
996 static uint32_t DAP_SWD_TransferBlock(uint8_t *request, uint8_t *response) {\r
997   uint32_t  request_count;\r
998   uint32_t  request_value;\r
999   uint32_t  response_count;\r
1000   uint32_t  response_value;\r
1001   uint8_t  *response_head;\r
1002   uint32_t  retry;\r
1003   uint32_t  data;\r
1004 \r
1005   response_count = 0;\r
1006   response_value = 0;\r
1007   response_head  = response;\r
1008   response      += 3;\r
1009 \r
1010   DAP_TransferAbort = 0;\r
1011 \r
1012   request++;            // Ignore DAP index\r
1013 \r
1014   request_count = *request | (*(request+1) << 8);\r
1015   request += 2;\r
1016   if (request_count == 0) goto end;\r
1017 \r
1018   request_value = *request++;\r
1019   if (request_value & DAP_TRANSFER_RnW) {\r
1020     // Read register block\r
1021     if (request_value & DAP_TRANSFER_APnDP) {\r
1022       // Post AP read\r
1023       retry = DAP_Data.transfer.retry_count;\r
1024       do {\r
1025         response_value = SWD_Transfer(request_value, NULL);\r
1026       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1027       if (response_value != DAP_TRANSFER_OK) goto end;\r
1028     }\r
1029     while (request_count--) {\r
1030       // Read DP/AP register\r
1031       if ((request_count == 0) && (request_value & DAP_TRANSFER_APnDP)) {\r
1032         // Last AP read\r
1033         request_value = DP_RDBUFF | DAP_TRANSFER_RnW;\r
1034       }\r
1035       retry = DAP_Data.transfer.retry_count;\r
1036       do {\r
1037         response_value = SWD_Transfer(request_value, &data);\r
1038       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1039       if (response_value != DAP_TRANSFER_OK) goto end;\r
1040       // Store data\r
1041       *response++ = (uint8_t) data;\r
1042       *response++ = (uint8_t)(data >>  8);\r
1043       *response++ = (uint8_t)(data >> 16);\r
1044       *response++ = (uint8_t)(data >> 24);\r
1045       response_count++;\r
1046     }\r
1047   } else {\r
1048     // Write register block\r
1049     while (request_count--) {\r
1050       // Load data\r
1051       data = (*(request+0) <<  0) |\r
1052              (*(request+1) <<  8) |\r
1053              (*(request+2) << 16) |\r
1054              (*(request+3) << 24);\r
1055       request += 4;\r
1056       // Write DP/AP register\r
1057       retry = DAP_Data.transfer.retry_count;\r
1058       do {\r
1059         response_value = SWD_Transfer(request_value, &data);\r
1060       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1061       if (response_value != DAP_TRANSFER_OK) goto end;\r
1062       response_count++;\r
1063     }\r
1064     // Check last write\r
1065     retry = DAP_Data.transfer.retry_count;\r
1066     do {\r
1067       response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);\r
1068     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1069   }\r
1070 \r
1071 end:\r
1072   *(response_head+0) = (uint8_t)(response_count >> 0);\r
1073   *(response_head+1) = (uint8_t)(response_count >> 8);\r
1074   *(response_head+2) = (uint8_t) response_value;\r
1075 \r
1076   return (response - response_head);\r
1077 }\r
1078 #endif\r
1079 \r
1080 \r
1081 // Process JTAG Transfer Block command and prepare response\r
1082 //   request:  pointer to request data\r
1083 //   response: pointer to response data\r
1084 //   return:   number of bytes in response\r
1085 #if (DAP_JTAG != 0)\r
1086 static uint32_t DAP_JTAG_TransferBlock(uint8_t *request, uint8_t *response) {\r
1087   uint32_t  request_count;\r
1088   uint32_t  request_value;\r
1089   uint32_t  response_count;\r
1090   uint32_t  response_value;\r
1091   uint8_t  *response_head;\r
1092   uint32_t  retry;\r
1093   uint32_t  data;\r
1094   uint32_t  ir;\r
1095 \r
1096   response_count = 0;\r
1097   response_value = 0;\r
1098   response_head  = response;\r
1099   response      += 3;\r
1100 \r
1101   DAP_TransferAbort = 0;\r
1102 \r
1103   // Device index (JTAP TAP)\r
1104   DAP_Data.jtag_dev.index = *request++;\r
1105   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto end;\r
1106 \r
1107   request_count = *request | (*(request+1) << 8);\r
1108   request += 2;\r
1109   if (request_count == 0) goto end;\r
1110 \r
1111   request_value = *request++;\r
1112 \r
1113   // Select JTAG chain\r
1114   ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;\r
1115   JTAG_IR(ir);\r
1116 \r
1117   if (request_value & DAP_TRANSFER_RnW) {\r
1118     // Post read\r
1119     retry = DAP_Data.transfer.retry_count;\r
1120     do {\r
1121       response_value = JTAG_Transfer(request_value, NULL);\r
1122     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1123     if (response_value != DAP_TRANSFER_OK) goto end;\r
1124     // Read register block\r
1125     while (request_count--) {\r
1126       // Read DP/AP register\r
1127       if (request_count == 0) {\r
1128         // Last read\r
1129         if (ir != JTAG_DPACC) {\r
1130           JTAG_IR(JTAG_DPACC);\r
1131         }\r
1132         request_value = DP_RDBUFF | DAP_TRANSFER_RnW;\r
1133       }\r
1134       retry = DAP_Data.transfer.retry_count;\r
1135       do {\r
1136         response_value = JTAG_Transfer(request_value, &data);\r
1137       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1138       if (response_value != DAP_TRANSFER_OK) goto end;\r
1139       // Store data\r
1140       *response++ = (uint8_t) data;\r
1141       *response++ = (uint8_t)(data >>  8);\r
1142       *response++ = (uint8_t)(data >> 16);\r
1143       *response++ = (uint8_t)(data >> 24);\r
1144       response_count++;\r
1145     }\r
1146   } else {\r
1147     // Write register block\r
1148     while (request_count--) {\r
1149       // Load data\r
1150       data = (*(request+0) <<  0) |\r
1151              (*(request+1) <<  8) |\r
1152              (*(request+2) << 16) |\r
1153              (*(request+3) << 24);\r
1154       request += 4;\r
1155       // Write DP/AP register\r
1156       retry = DAP_Data.transfer.retry_count;\r
1157       do {\r
1158         response_value = JTAG_Transfer(request_value, &data);\r
1159       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1160       if (response_value != DAP_TRANSFER_OK) goto end;\r
1161       response_count++;\r
1162     }\r
1163     // Check last write\r
1164     if (ir != JTAG_DPACC) {\r
1165       JTAG_IR(JTAG_DPACC);\r
1166     }\r
1167     retry = DAP_Data.transfer.retry_count;\r
1168     do {\r
1169       response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);\r
1170     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);\r
1171   }\r
1172 \r
1173 end:\r
1174   *(response_head+0) = (uint8_t)(response_count >> 0);\r
1175   *(response_head+1) = (uint8_t)(response_count >> 8);\r
1176   *(response_head+2) = (uint8_t) response_value;\r
1177 \r
1178   return (response - response_head);\r
1179 }\r
1180 #endif\r
1181 \r
1182 \r
1183 // Process DAP Vendor command and prepare response\r
1184 // Default function (can be overridden)\r
1185 //   request:  pointer to request data\r
1186 //   response: pointer to response data\r
1187 //   return:   number of bytes in response\r
1188 __weak uint32_t DAP_ProcessVendorCommand(uint8_t *request, uint8_t *response) {\r
1189   *response = ID_DAP_Invalid;\r
1190   return (1);\r
1191 }\r
1192 \r
1193 \r
1194 // Process DAP command and prepare response\r
1195 //   request:  pointer to request data\r
1196 //   response: pointer to response data\r
1197 //   return:   number of bytes in response\r
1198 uint32_t DAP_ProcessCommand(uint8_t *request, uint8_t *response) {\r
1199   uint32_t num;\r
1200 \r
1201   if ((*request >= ID_DAP_Vendor0) && (*request <= ID_DAP_Vendor31)) {\r
1202     return DAP_ProcessVendorCommand(request, response);\r
1203   }\r
1204 \r
1205   *response++ = *request;\r
1206 \r
1207   switch (*request++) {\r
1208     case ID_DAP_Info:\r
1209       num = DAP_Info(*request, response+1);\r
1210       *response = num;\r
1211       return (2 + num);\r
1212     case ID_DAP_LED:\r
1213       num = DAP_LED(request, response);\r
1214       break;\r
1215     case ID_DAP_Connect:\r
1216       num = DAP_Connect(request, response);\r
1217       break;\r
1218     case ID_DAP_Disconnect:\r
1219       num = DAP_Disconnect(response);\r
1220       break;\r
1221     case ID_DAP_Delay:\r
1222       num = DAP_Delay(request, response);\r
1223       break;\r
1224     case ID_DAP_ResetTarget:\r
1225       num = DAP_ResetTarget(response);\r
1226       break;\r
1227 \r
1228 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))\r
1229     case ID_DAP_SWJ_Pins:\r
1230       num = DAP_SWJ_Pins(request, response);\r
1231       break;\r
1232     case ID_DAP_SWJ_Clock:\r
1233       num = DAP_SWJ_Clock(request, response);\r
1234       break;\r
1235     case ID_DAP_SWJ_Sequence:\r
1236       num = DAP_SWJ_Sequence(request, response);\r
1237       break;\r
1238 #else\r
1239     case ID_DAP_SWJ_Pins:\r
1240     case ID_DAP_SWJ_Clock:\r
1241     case ID_DAP_SWJ_Sequence:\r
1242       *response = DAP_ERROR;\r
1243       return (2);\r
1244 #endif\r
1245 \r
1246 #if (DAP_SWD != 0)\r
1247     case ID_DAP_SWD_Configure:\r
1248       num = DAP_SWD_Configure(request, response);\r
1249       break;\r
1250 #else\r
1251     case ID_DAP_SWD_Configure:\r
1252       *response = DAP_ERROR;\r
1253       return (2);\r
1254 #endif\r
1255 \r
1256 #if (DAP_JTAG != 0)\r
1257     case ID_DAP_JTAG_Sequence:\r
1258       num = DAP_JTAG_Sequence(request, response);\r
1259       break;\r
1260     case ID_DAP_JTAG_Configure:\r
1261       num = DAP_JTAG_Configure(request, response);\r
1262       break;\r
1263     case ID_DAP_JTAG_IDCODE:\r
1264       num = DAP_JTAG_IDCode(request, response);\r
1265       break;\r
1266 #else\r
1267     case ID_DAP_JTAG_Sequence:\r
1268     case ID_DAP_JTAG_Configure:\r
1269     case ID_DAP_JTAG_IDCODE:\r
1270       *response = DAP_ERROR;\r
1271       return (2);\r
1272 #endif\r
1273 \r
1274     case ID_DAP_TransferConfigure:\r
1275       num = DAP_TransferConfigure(request, response);\r
1276       break;\r
1277 \r
1278     case ID_DAP_Transfer:\r
1279       switch (DAP_Data.debug_port) {\r
1280 #if (DAP_SWD != 0)\r
1281         case DAP_PORT_SWD:\r
1282           num = DAP_SWD_Transfer (request, response);\r
1283           break;\r
1284 #endif\r
1285 #if (DAP_JTAG != 0)\r
1286         case DAP_PORT_JTAG:\r
1287           num = DAP_JTAG_Transfer(request, response);\r
1288           break;\r
1289 #endif\r
1290         default:\r
1291           *(response+0) = 0;    // Response count\r
1292           *(response+1) = 0;    // Response value\r
1293           num = 2;\r
1294       }\r
1295       break;\r
1296 \r
1297     case ID_DAP_TransferBlock:\r
1298       switch (DAP_Data.debug_port) {\r
1299 #if (DAP_SWD != 0)\r
1300         case DAP_PORT_SWD:\r
1301           num = DAP_SWD_TransferBlock (request, response);\r
1302           break;\r
1303 #endif\r
1304 #if (DAP_JTAG != 0)\r
1305         case DAP_PORT_JTAG:\r
1306           num = DAP_JTAG_TransferBlock(request, response);\r
1307           break;\r
1308 #endif\r
1309         default:\r
1310           *(response+0) = 0;    // Response count [7:0]\r
1311           *(response+1) = 0;    // Response count[15:8]\r
1312           *(response+2) = 0;    // Response value\r
1313           num = 3;\r
1314       }\r
1315       break;\r
1316 \r
1317     case ID_DAP_WriteABORT:\r
1318       switch (DAP_Data.debug_port) {\r
1319 #if (DAP_SWD != 0)\r
1320         case DAP_PORT_SWD:\r
1321           num = DAP_SWD_Abort (request, response);\r
1322           break;\r
1323 #endif\r
1324 #if (DAP_JTAG != 0)\r
1325         case DAP_PORT_JTAG:\r
1326           num = DAP_JTAG_Abort(request, response);\r
1327           break;\r
1328 #endif\r
1329         default:\r
1330           *response = DAP_ERROR;\r
1331           return (2);\r
1332       }\r
1333       break;\r
1334 \r
1335     default:\r
1336       *(response-1) = ID_DAP_Invalid;\r
1337       return (1);\r
1338   }\r
1339 \r
1340   return (1 + num);\r
1341 }\r
1342 \r
1343 \r
1344 // Setup DAP\r
1345 void DAP_Setup(void) {\r
1346 \r
1347   // Default settings (only non-zero values)\r
1348 //DAP_Data.debug_port  = 0;\r
1349 //DAP_Data.fast_clock  = 0;\r
1350   DAP_Data.clock_delay = CLOCK_DELAY(DAP_DEFAULT_SWJ_CLOCK);\r
1351 //DAP_Data.transfer.idle_cycles = 0;\r
1352   DAP_Data.transfer.retry_count = 100;\r
1353 //DAP_Data.transfer.match_retry = 0;\r
1354 //DAP_Data.transfer.match_mask  = 0x000000;\r
1355 #if (DAP_SWD != 0)\r
1356   DAP_Data.swd_conf.turnaround  = 1;\r
1357 //DAP_Data.swd_conf.data_phase  = 0;\r
1358 #endif\r
1359 #if (DAP_JTAG != 0)\r
1360 //DAP_Data.jtag_dev.count = 0;\r
1361 #endif\r
1362 \r
1363   DAP_SETUP();  // Device specific setup\r
1364 }\r