git.sur5r.net Git - armstart-ibdap/blob - 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