DAP.c

00001 /*
00002  * Copyright (c) 2013-2017 ARM Limited. All rights reserved.
00003  * Copyright 2019, Cypress Semiconductor Corporation 
00004  * or a subsidiary of Cypress Semiconductor Corporation.
00005  *
00006  * SPDX-License-Identifier: Apache-2.0
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the License); you may
00009  * not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  * www.apache.org/licenses/LICENSE-2.0
00013  *
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00016  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
00019  *
00020  * ----------------------------------------------------------------------
00021  *
00022  * $Date:        1. December 2017
00023  * $Revision:    V2.0.0
00024  *
00025  * Project:      CMSIS-DAP Source
00026  * Title:        DAP.c CMSIS-DAP Commands
00027  *
00028  *---------------------------------------------------------------------------*/
00029 
00030 #include <string.h>
00031 #ifdef RTE_CMSIS_RTOS
00032 #include "cmsis_os.h"
00033 #endif
00034 #include "DAP_config.h"
00035 #include "DAP.h"
00036 #include "info.h"
00037 #include "dap_strings.h"
00038 
00039 
00040 #if (DAP_PACKET_SIZE < 64U)
00041 #error "Minimum Packet Size is 64!"
00042 #endif
00043 #if (DAP_PACKET_SIZE > 32768U)
00044 #error "Maximum Packet Size is 32768!"
00045 #endif
00046 #if (DAP_PACKET_COUNT < 1U)
00047 #error "Minimum Packet Count is 1!"
00048 #endif
00049 #if (DAP_PACKET_COUNT > 255U)
00050 #error "Maximum Packet Count is 255!"
00051 #endif
00052 
00053 
00054 // Clock Macros
00055 
00056 #define MAX_SWJ_CLOCK(delay_cycles) \
00057   ((CPU_CLOCK/2U) / (IO_PORT_WRITE_CYCLES + delay_cycles))
00058 
00059 #define CLOCK_DELAY(swj_clock) \
00060  (((CPU_CLOCK/2U) / swj_clock) - IO_PORT_WRITE_CYCLES)
00061 
00062 
00063          DAP_Data_t DAP_Data;           // DAP Data
00064 volatile uint8_t    DAP_TransferAbort;  // Transfer Abort Flag
00065 
00066 
00067 // static const char DAP_FW_Ver [] = DAP_FW_VER;
00068 
00069 #if TARGET_DEVICE_FIXED
00070 static const char TargetDeviceVendor [] = TARGET_DEVICE_VENDOR;
00071 static const char TargetDeviceName   [] = TARGET_DEVICE_NAME;
00072 #endif
00073 
00074 
00075 // Get DAP Information
00076 //   id:      info identifier
00077 //   info:    pointer to info data
00078 //   return:  number of bytes in info data
00079 static uint8_t DAP_Info(uint8_t id, uint8_t *info) {
00080   uint8_t length = 0U;
00081 
00082   switch (id) {
00083     case DAP_ID_VENDOR:
00084       length = DAP_GetVendorString((char *)info);
00085       break;
00086     case DAP_ID_PRODUCT:
00087       length = DAP_GetProductString((char *)info);
00088       break;
00089     case DAP_ID_SER_NUM:
00090       length = DAP_GetSerNumString((char *)info);
00091       break;
00092     case DAP_ID_FW_VER: {
00093 // --- begin DAPLink change ---
00094       length = DAP_GetFirmwareVersionString((char *)info);
00095 // Original:
00096 //       memcpy(info, DAP_FW_Ver, sizeof(DAP_FW_Ver));
00097 //       length = (uint8_t)sizeof(DAP_FW_Ver);
00098 // --- end DAPLink change ---
00099       break;
00100     }
00101     case DAP_ID_DEVICE_VENDOR:
00102 #if TARGET_DEVICE_FIXED
00103       length = (uint8_t)sizeof(TargetDeviceVendor);
00104       memcpy(info, TargetDeviceVendor, length);
00105 #endif
00106       break;
00107     case DAP_ID_DEVICE_NAME:
00108 #if TARGET_DEVICE_FIXED
00109       length = (uint8_t)sizeof(TargetDeviceName);
00110       memcpy(info, TargetDeviceName, length);
00111 #endif
00112       break;
00113     case DAP_ID_CAPABILITIES:
00114       info[0] = ((DAP_SWD  != 0)         ? (1U << 0) : 0U) |
00115                 ((DAP_JTAG != 0)         ? (1U << 1) : 0U) |
00116                 ((SWO_UART != 0)         ? (1U << 2) : 0U) |
00117                 ((SWO_MANCHESTER != 0)   ? (1U << 3) : 0U) |
00118                 /* Atomic Commands  */     (1U << 4)       |
00119                 ((TIMESTAMP_CLOCK != 0U) ? (1U << 5) : 0U) |
00120                 ((SWO_STREAM != 0U)      ? (1U << 6) : 0U);
00121       length = 1U;
00122       break;
00123     case DAP_ID_TIMESTAMP_CLOCK:
00124 #if (TIMESTAMP_CLOCK != 0U)
00125       info[0] = (uint8_t)(TIMESTAMP_CLOCK >>  0);
00126       info[1] = (uint8_t)(TIMESTAMP_CLOCK >>  8);
00127       info[2] = (uint8_t)(TIMESTAMP_CLOCK >> 16);
00128       info[3] = (uint8_t)(TIMESTAMP_CLOCK >> 24);
00129       length = 4U;
00130 #endif
00131       break;
00132     case DAP_ID_SWO_BUFFER_SIZE:
00133 #if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
00134       info[0] = (uint8_t)(SWO_BUFFER_SIZE >>  0);
00135       info[1] = (uint8_t)(SWO_BUFFER_SIZE >>  8);
00136       info[2] = (uint8_t)(SWO_BUFFER_SIZE >> 16);
00137       info[3] = (uint8_t)(SWO_BUFFER_SIZE >> 24);
00138       length = 4U;
00139 #endif
00140       break;
00141     case DAP_ID_PACKET_SIZE:
00142       info[0] = (uint8_t)(DAP_PACKET_SIZE >> 0);
00143       info[1] = (uint8_t)(DAP_PACKET_SIZE >> 8);
00144       length = 2U;
00145       break;
00146     case DAP_ID_PACKET_COUNT:
00147       info[0] = DAP_PACKET_COUNT;
00148       length = 1U;
00149       break;
00150     default:
00151       break;
00152   }
00153 
00154   return (length);
00155 }
00156 
00157 
00158 // Delay for specified time
00159 //    delay:  delay time in ms
00160 void Delayms(uint32_t delay) {
00161   delay *= ((CPU_CLOCK/1000U) + (DELAY_SLOW_CYCLES-1U)) / DELAY_SLOW_CYCLES;
00162   PIN_DELAY_SLOW(delay);
00163 }
00164 
00165 
00166 // Process Delay command and prepare response
00167 //   request:  pointer to request data
00168 //   response: pointer to response data
00169 //   return:   number of bytes in response (lower 16 bits)
00170 //             number of bytes in request (upper 16 bits)
00171 static uint32_t DAP_Delay(const uint8_t *request, uint8_t *response) {
00172   uint32_t delay;
00173 
00174   delay  = (uint32_t)(*(request+0)) |
00175            (uint32_t)(*(request+1) << 8);
00176   delay *= ((CPU_CLOCK/1000000U) + (DELAY_SLOW_CYCLES-1U)) / DELAY_SLOW_CYCLES;
00177 
00178   PIN_DELAY_SLOW(delay);
00179 
00180   *response = DAP_OK;
00181   return ((2U << 16) | 1U);
00182 }
00183 
00184 
00185 // Process Host Status command and prepare response
00186 //   request:  pointer to request data
00187 //   response: pointer to response data
00188 //   return:   number of bytes in response (lower 16 bits)
00189 //             number of bytes in request (upper 16 bits)
00190 static uint32_t DAP_HostStatus(const uint8_t *request, uint8_t *response) {
00191 
00192   switch (*request) {
00193     case DAP_DEBUGGER_CONNECTED:
00194       LED_CONNECTED_OUT((*(request+1) & 1U));
00195       break;
00196     case DAP_TARGET_RUNNING:
00197       LED_RUNNING_OUT((*(request+1) & 1U));
00198       break;
00199     default:
00200       *response = DAP_ERROR;
00201       return ((2U << 16) | 1U);
00202   }
00203 
00204   *response = DAP_OK;
00205   return ((2U << 16) | 1U);
00206 }
00207 
00208 
00209 // Process Connect command and prepare response
00210 //   request:  pointer to request data
00211 //   response: pointer to response data
00212 //   return:   number of bytes in response (lower 16 bits)
00213 //             number of bytes in request (upper 16 bits)
00214 static uint32_t DAP_Connect(const uint8_t *request, uint8_t *response) {
00215   uint32_t port;
00216 
00217   if (*request == DAP_PORT_AUTODETECT) {
00218     port = DAP_DEFAULT_PORT;
00219   } else {
00220     port = *request;
00221   }
00222 
00223   switch (port) {
00224 #if (DAP_SWD != 0)
00225     case DAP_PORT_SWD:
00226       DAP_Data.debug_port = DAP_PORT_SWD;
00227       PORT_SWD_SETUP();
00228       break;
00229 #endif
00230 #if (DAP_JTAG != 0)
00231     case DAP_PORT_JTAG:
00232       DAP_Data.debug_port = DAP_PORT_JTAG;
00233       PORT_JTAG_SETUP();
00234       break;
00235 #endif
00236     default:
00237       port = DAP_PORT_DISABLED;
00238       break;
00239   }
00240 
00241   *response = (uint8_t)port;
00242   return ((1U << 16) | 1U);
00243 }
00244 
00245 
00246 // Process Disconnect command and prepare response
00247 //   response: pointer to response data
00248 //   return:   number of bytes in response
00249 static uint32_t DAP_Disconnect(uint8_t *response) {
00250 
00251   DAP_Data.debug_port = DAP_PORT_DISABLED;
00252   PORT_OFF();
00253 
00254   *response = DAP_OK;
00255   return (1U);
00256 }
00257 
00258 
00259 // Process Reset Target command and prepare response
00260 //   response: pointer to response data
00261 //   return:   number of bytes in response
00262 static uint32_t DAP_ResetTarget(uint8_t *response) {
00263 
00264   *(response+1) = RESET_TARGET();
00265   *(response+0) = DAP_OK;
00266   return (2U);
00267 }
00268 
00269 
00270 // Process SWJ Pins command and prepare response
00271 //   request:  pointer to request data
00272 //   response: pointer to response data
00273 //   return:   number of bytes in response (lower 16 bits)
00274 //             number of bytes in request (upper 16 bits)
00275 static uint32_t DAP_SWJ_Pins(const uint8_t *request, uint8_t *response) {
00276 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))
00277   uint32_t value;
00278   uint32_t select;
00279   uint32_t wait;
00280   uint32_t timestamp;
00281 
00282   value  = (uint32_t) *(request+0);
00283   select = (uint32_t) *(request+1);
00284   wait   = (uint32_t)(*(request+2) <<  0) |
00285            (uint32_t)(*(request+3) <<  8) |
00286            (uint32_t)(*(request+4) << 16) |
00287            (uint32_t)(*(request+5) << 24);
00288 
00289   if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
00290     if ((value & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
00291       PIN_SWCLK_TCK_SET();
00292     } else {
00293       PIN_SWCLK_TCK_CLR();
00294     }
00295   }
00296   if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
00297     if ((value & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
00298       PIN_SWDIO_TMS_SET();
00299     } else {
00300       PIN_SWDIO_TMS_CLR();
00301     }
00302   }
00303   if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
00304     PIN_TDI_OUT(value >> DAP_SWJ_TDI);
00305   }
00306   if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
00307     PIN_nTRST_OUT(value >> DAP_SWJ_nTRST);
00308   }
00309   if ((select & (1U << DAP_SWJ_nRESET)) != 0U){
00310     PIN_nRESET_OUT(value >> DAP_SWJ_nRESET);
00311   }
00312 
00313   if (wait != 0U) {
00314 #if (TIMESTAMP_CLOCK != 0U)
00315     if (wait > 3000000U) {
00316       wait = 3000000U;
00317     }
00318 #if (TIMESTAMP_CLOCK >= 1000000U)
00319     wait *= TIMESTAMP_CLOCK / 1000000U;
00320 #else
00321     wait /= 1000000U / TIMESTAMP_CLOCK;
00322 #endif
00323 #else
00324     wait  = 1U;
00325 #endif
00326     timestamp = TIMESTAMP_GET();
00327     do {
00328       if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
00329         if ((value >> DAP_SWJ_SWCLK_TCK) ^ PIN_SWCLK_TCK_IN()) {
00330           continue;
00331         }
00332       }
00333       if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
00334         if ((value >> DAP_SWJ_SWDIO_TMS) ^ PIN_SWDIO_TMS_IN()) {
00335           continue;
00336         }
00337       }
00338       if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
00339         if ((value >> DAP_SWJ_TDI) ^ PIN_TDI_IN()) {
00340           continue;
00341         }
00342       }
00343       if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
00344         if ((value >> DAP_SWJ_nTRST) ^ PIN_nTRST_IN()) {
00345           continue;
00346         }
00347       }
00348       if ((select & (1U << DAP_SWJ_nRESET)) != 0U) {
00349         if ((value >> DAP_SWJ_nRESET) ^ PIN_nRESET_IN()) {
00350           continue;
00351         }
00352       }
00353       break;
00354     } while ((TIMESTAMP_GET() - timestamp) < wait);
00355   }
00356 
00357   value = (PIN_SWCLK_TCK_IN() << DAP_SWJ_SWCLK_TCK) |
00358           (PIN_SWDIO_TMS_IN() << DAP_SWJ_SWDIO_TMS) |
00359           (PIN_TDI_IN()       << DAP_SWJ_TDI)       |
00360           (PIN_TDO_IN()       << DAP_SWJ_TDO)       |
00361           (PIN_nTRST_IN()     << DAP_SWJ_nTRST)     |
00362           (PIN_nRESET_IN()    << DAP_SWJ_nRESET);
00363 
00364   *response = (uint8_t)value;
00365 #else
00366   *response = 0U;
00367 #endif
00368 
00369   return ((6U << 16) | 1U);
00370 }
00371 
00372 
00373 // Process SWJ Clock command and prepare response
00374 //   request:  pointer to request data
00375 //   response: pointer to response data
00376 //   return:   number of bytes in response (lower 16 bits)
00377 //             number of bytes in request (upper 16 bits)
00378 static uint32_t DAP_SWJ_Clock(const uint8_t *request, uint8_t *response) {
00379 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))
00380   uint32_t clock;
00381   uint32_t delay;
00382 
00383   clock = (uint32_t)(*(request+0) <<  0) |
00384           (uint32_t)(*(request+1) <<  8) |
00385           (uint32_t)(*(request+2) << 16) |
00386           (uint32_t)(*(request+3) << 24);
00387 
00388   if (clock == 0U) {
00389     *response = DAP_ERROR;
00390     return ((4U << 16) | 1U);
00391   }
00392 
00393   if (clock >= MAX_SWJ_CLOCK(DELAY_FAST_CYCLES)) {
00394     DAP_Data.fast_clock  = 1U;
00395     DAP_Data.clock_delay = 1U;
00396   } else {
00397     DAP_Data.fast_clock  = 0U;
00398 
00399     delay = ((CPU_CLOCK/2U) + (clock - 1U)) / clock;
00400     if (delay > IO_PORT_WRITE_CYCLES) {
00401       delay -= IO_PORT_WRITE_CYCLES;
00402       delay  = (delay + (DELAY_SLOW_CYCLES - 1U)) / DELAY_SLOW_CYCLES;
00403     } else {
00404       delay  = 1U;
00405     }
00406 
00407     DAP_Data.clock_delay = delay;
00408   }
00409 
00410   *response = DAP_OK;
00411 #else
00412   *response = DAP_ERROR;
00413 #endif
00414 
00415   return ((4U << 16) | 1U);
00416 }
00417 
00418 
00419 // Process SWJ Sequence command and prepare response
00420 //   request:  pointer to request data
00421 //   response: pointer to response data
00422 //   return:   number of bytes in response (lower 16 bits)
00423 //             number of bytes in request (upper 16 bits)
00424 static uint32_t DAP_SWJ_Sequence(const uint8_t *request, uint8_t *response) {
00425   uint32_t count;
00426 
00427   count = *request++;
00428   if (count == 0U) {
00429     count = 256U;
00430   }
00431 
00432 #if ((DAP_SWD != 0) || (DAP_JTAG != 0))
00433   SWJ_Sequence(count, request);
00434   *response = DAP_OK;
00435 #else
00436   *response = DAP_ERROR;
00437 #endif
00438 
00439   count = (count + 7U) >> 3;
00440 
00441   return (((count + 1U) << 16) | 1U);
00442 }
00443 
00444 
00445 // Process SWD Configure command and prepare response
00446 //   request:  pointer to request data
00447 //   response: pointer to response data
00448 //   return:   number of bytes in response (lower 16 bits)
00449 //             number of bytes in request (upper 16 bits)
00450 static uint32_t DAP_SWD_Configure(const uint8_t *request, uint8_t *response) {
00451 #if (DAP_SWD != 0)
00452   uint8_t value;
00453 
00454   value = *request;
00455   DAP_Data.swd_conf.turnaround = (value & 0x03U) + 1U;
00456   DAP_Data.swd_conf.data_phase = (value & 0x04U) ? 1U : 0U;
00457 
00458   *response = DAP_OK;
00459 #else
00460   *response = DAP_ERROR;
00461 #endif
00462 
00463   return ((1U << 16) | 1U);
00464 }
00465 
00466 
00467 // Process SWD Sequence command and prepare response
00468 //   request:  pointer to request data
00469 //   response: pointer to response data
00470 //   return:   number of bytes in response (lower 16 bits)
00471 //             number of bytes in request (upper 16 bits)
00472 static uint32_t DAP_SWD_Sequence(const uint8_t *request, uint8_t *response) {
00473   uint32_t sequence_info;
00474   uint32_t sequence_count;
00475   uint32_t request_count;
00476   uint32_t response_count;
00477   uint32_t count;
00478 
00479 #if (DAP_SWD != 0)
00480   *response++ = DAP_OK;
00481 #else
00482   *response++ = DAP_ERROR;
00483 #endif
00484   request_count  = 1U;
00485   response_count = 1U;
00486 
00487   sequence_count = *request++;
00488   while (sequence_count--) {
00489     sequence_info = *request++;
00490     count = sequence_info & SWD_SEQUENCE_CLK;
00491     if (count == 0U) {
00492       count = 64U;
00493     }
00494     count = (count + 7U) / 8U;
00495 #if (DAP_SWD != 0)
00496     if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
00497       PIN_SWDIO_OUT_DISABLE();
00498     } else {
00499       PIN_SWDIO_OUT_ENABLE();
00500     }
00501     SWD_Sequence(sequence_info, request, response);
00502     if (sequence_count == 0U) {
00503       PIN_SWDIO_OUT_ENABLE();
00504     }
00505 #endif
00506     if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
00507       request_count++;
00508 #if (DAP_SWD != 0)
00509       response += count;
00510       response_count += count;
00511 #endif
00512     } else {
00513       request += count;
00514       request_count += count + 1U;
00515     }
00516   }
00517 
00518   return ((request_count << 16) | response_count);
00519 }
00520 
00521 
00522 // Process JTAG Sequence command and prepare response
00523 //   request:  pointer to request data
00524 //   response: pointer to response data
00525 //   return:   number of bytes in response (lower 16 bits)
00526 //             number of bytes in request (upper 16 bits)
00527 static uint32_t DAP_JTAG_Sequence(const uint8_t *request, uint8_t *response) {
00528   uint32_t sequence_info;
00529   uint32_t sequence_count;
00530   uint32_t request_count;
00531   uint32_t response_count;
00532   uint32_t count;
00533 
00534 #if (DAP_JTAG != 0)
00535   *response++ = DAP_OK;
00536 #else
00537   *response++ = DAP_ERROR;
00538 #endif
00539   request_count  = 1U;
00540   response_count = 1U;
00541 
00542   sequence_count = *request++;
00543   while (sequence_count--) {
00544     sequence_info = *request++;
00545     count = sequence_info & JTAG_SEQUENCE_TCK;
00546     if (count == 0U) {
00547       count = 64U;
00548     }
00549     count = (count + 7U) / 8U;
00550 #if (DAP_JTAG != 0)
00551     JTAG_Sequence(sequence_info, request, response);
00552 #endif
00553     request += count;
00554     request_count += count + 1U;
00555 #if (DAP_JTAG != 0)
00556     if ((sequence_info & JTAG_SEQUENCE_TDO) != 0U) {
00557       response += count;
00558       response_count += count;
00559     }
00560 #endif
00561   }
00562 
00563   return ((request_count << 16) | response_count);
00564 }
00565 
00566 
00567 // Process JTAG Configure command and prepare response
00568 //   request:  pointer to request data
00569 //   response: pointer to response data
00570 //   return:   number of bytes in response (lower 16 bits)
00571 //             number of bytes in request (upper 16 bits)
00572 static uint32_t DAP_JTAG_Configure(const uint8_t *request, uint8_t *response) {
00573   uint32_t count;
00574 #if (DAP_JTAG != 0)
00575   uint32_t length;
00576   uint32_t bits;
00577   uint32_t n;
00578 
00579   count = *request++;
00580   DAP_Data.jtag_dev.count = (uint8_t)count;
00581 
00582   bits = 0U;
00583   for (n = 0U; n < count; n++) {
00584     length = *request++;
00585     DAP_Data.jtag_dev.ir_length[n] =  (uint8_t)length;
00586     DAP_Data.jtag_dev.ir_before[n] = (uint16_t)bits;
00587     bits += length;
00588   }
00589   for (n = 0U; n < count; n++) {
00590     bits -= DAP_Data.jtag_dev.ir_length[n];
00591     DAP_Data.jtag_dev.ir_after[n] = (uint16_t)bits;
00592   }
00593 
00594   *response = DAP_OK;
00595 #else
00596   count = *request;
00597   *response = DAP_ERROR;
00598 #endif
00599 
00600   return (((count + 1U) << 16) | 1U);
00601 }
00602 
00603 
00604 // Process JTAG IDCODE command and prepare response
00605 //   request:  pointer to request data
00606 //   response: pointer to response data
00607 //   return:   number of bytes in response (lower 16 bits)
00608 //             number of bytes in request (upper 16 bits)
00609 static uint32_t DAP_JTAG_IDCode(const uint8_t *request, uint8_t *response) {
00610 #if (DAP_JTAG != 0)
00611   uint32_t data;
00612 
00613   if (DAP_Data.debug_port != DAP_PORT_JTAG) {
00614     goto id_error;
00615   }
00616 
00617   // Device index (JTAP TAP)
00618   DAP_Data.jtag_dev.index = *request;
00619   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
00620     goto id_error;
00621   }
00622 
00623   // Select JTAG chain
00624   JTAG_IR(JTAG_IDCODE);
00625 
00626   // Read IDCODE register
00627   data = JTAG_ReadIDCode();
00628 
00629   // Store Data
00630   *(response+0) =  DAP_OK;
00631   *(response+1) = (uint8_t)(data >>  0);
00632   *(response+2) = (uint8_t)(data >>  8);
00633   *(response+3) = (uint8_t)(data >> 16);
00634   *(response+4) = (uint8_t)(data >> 24);
00635 
00636   return ((1U << 16) | 5U);
00637 
00638 id_error:
00639 #endif
00640   *response = DAP_ERROR;
00641   return ((1U << 16) | 1U);
00642 }
00643 
00644 
00645 // Process Transfer Configure command and prepare response
00646 //   request:  pointer to request data
00647 //   response: pointer to response data
00648 //   return:   number of bytes in response (lower 16 bits)
00649 //             number of bytes in request (upper 16 bits)
00650 static uint32_t DAP_TransferConfigure(const uint8_t *request, uint8_t *response) {
00651 
00652   DAP_Data.transfer.idle_cycles =            *(request+0);
00653   DAP_Data.transfer.retry_count = (uint16_t) *(request+1) |
00654                                   (uint16_t)(*(request+2) << 8);
00655   DAP_Data.transfer.match_retry = (uint16_t) *(request+3) |
00656                                   (uint16_t)(*(request+4) << 8);
00657 
00658   *response = DAP_OK;
00659   return ((5U << 16) | 1U);
00660 }
00661 
00662 
00663 // Process SWD Transfer command and prepare response
00664 //   request:  pointer to request data
00665 //   response: pointer to response data
00666 //   return:   number of bytes in response (lower 16 bits)
00667 //             number of bytes in request (upper 16 bits)
00668 #if (DAP_SWD != 0)
00669 static uint32_t DAP_SWD_Transfer(const uint8_t *request, uint8_t *response) {
00670   const
00671   uint8_t  *request_head;
00672   uint32_t  request_count;
00673   uint32_t  request_value;
00674   uint8_t  *response_head;
00675   uint32_t  response_count;
00676   uint32_t  response_value;
00677   uint32_t  post_read;
00678   uint32_t  check_write;
00679   uint32_t  match_value;
00680   uint32_t  match_retry;
00681   uint32_t  retry;
00682   uint32_t  data;
00683 #if (TIMESTAMP_CLOCK != 0U)
00684   uint32_t  timestamp;
00685 #endif
00686 
00687   request_head   = request;
00688 
00689   response_count = 0U;
00690   response_value = 0U;
00691   response_head  = response;
00692   response      += 2;
00693 
00694   DAP_TransferAbort = 0U;
00695 
00696   post_read   = 0U;
00697   check_write = 0U;
00698 
00699   request++;            // Ignore DAP index
00700 
00701   request_count = *request++;
00702 
00703   for (; request_count != 0U; request_count--) {
00704     request_value = *request++;
00705     if ((request_value & DAP_TRANSFER_RnW) != 0U) {
00706       // Read register
00707       if (post_read) {
00708         // Read was posted before
00709         retry = DAP_Data.transfer.retry_count;
00710         if ((request_value & (DAP_TRANSFER_APnDP | DAP_TRANSFER_MATCH_VALUE)) == DAP_TRANSFER_APnDP) {
00711           // Read previous AP data and post next AP read
00712           do {
00713             response_value = SWD_Transfer(request_value, &data);
00714           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00715         } else {
00716           // Read previous AP data
00717           do {
00718             response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
00719           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00720           post_read = 0U;
00721         }
00722         if (response_value != DAP_TRANSFER_OK) {
00723           break;
00724         }
00725         // Store previous AP data
00726         *response++ = (uint8_t) data;
00727         *response++ = (uint8_t)(data >>  8);
00728         *response++ = (uint8_t)(data >> 16);
00729         *response++ = (uint8_t)(data >> 24);
00730 #if (TIMESTAMP_CLOCK != 0U)
00731         if (post_read) {
00732           // Store Timestamp of next AP read
00733           if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
00734             timestamp = DAP_Data.timestamp;
00735             *response++ = (uint8_t) timestamp;
00736             *response++ = (uint8_t)(timestamp >>  8);
00737             *response++ = (uint8_t)(timestamp >> 16);
00738             *response++ = (uint8_t)(timestamp >> 24);
00739           }
00740         }
00741 #endif
00742       }
00743       if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
00744         // Read with value match
00745         match_value = (uint32_t)(*(request+0) <<  0) |
00746                       (uint32_t)(*(request+1) <<  8) |
00747                       (uint32_t)(*(request+2) << 16) |
00748                       (uint32_t)(*(request+3) << 24);
00749         request += 4;
00750         match_retry = DAP_Data.transfer.match_retry;
00751         if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
00752           // Post AP read
00753           retry = DAP_Data.transfer.retry_count;
00754           do {
00755             response_value = SWD_Transfer(request_value, NULL);
00756           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00757           if (response_value != DAP_TRANSFER_OK) {
00758             break;
00759           }
00760         }
00761         do {
00762           // Read register until its value matches or retry counter expires
00763           retry = DAP_Data.transfer.retry_count;
00764           do {
00765             response_value = SWD_Transfer(request_value, &data);
00766           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00767           if (response_value != DAP_TRANSFER_OK) {
00768             break;
00769           }
00770         } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
00771         if ((data & DAP_Data.transfer.match_mask) != match_value) {
00772           response_value |= DAP_TRANSFER_MISMATCH;
00773         }
00774         if (response_value != DAP_TRANSFER_OK) {
00775           break;
00776         }
00777       } else {
00778         // Normal read
00779         retry = DAP_Data.transfer.retry_count;
00780         if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
00781           // Read AP register
00782           if (post_read == 0U) {
00783             // Post AP read
00784             do {
00785               response_value = SWD_Transfer(request_value, NULL);
00786             } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00787             if (response_value != DAP_TRANSFER_OK) {
00788               break;
00789             }
00790 #if (TIMESTAMP_CLOCK != 0U)
00791             // Store Timestamp
00792             if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
00793               timestamp = DAP_Data.timestamp;
00794               *response++ = (uint8_t) timestamp;
00795               *response++ = (uint8_t)(timestamp >>  8);
00796               *response++ = (uint8_t)(timestamp >> 16);
00797               *response++ = (uint8_t)(timestamp >> 24);
00798             }
00799 #endif
00800             post_read = 1U;
00801           }
00802         } else {
00803           // Read DP register
00804           do {
00805             response_value = SWD_Transfer(request_value, &data);
00806           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00807           if (response_value != DAP_TRANSFER_OK) {
00808             break;
00809           }
00810 #if (TIMESTAMP_CLOCK != 0U)
00811           // Store Timestamp
00812           if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
00813             timestamp = DAP_Data.timestamp;
00814             *response++ = (uint8_t) timestamp;
00815             *response++ = (uint8_t)(timestamp >>  8);
00816             *response++ = (uint8_t)(timestamp >> 16);
00817             *response++ = (uint8_t)(timestamp >> 24);
00818           }
00819 #endif
00820           // Store data
00821           *response++ = (uint8_t) data;
00822           *response++ = (uint8_t)(data >>  8);
00823           *response++ = (uint8_t)(data >> 16);
00824           *response++ = (uint8_t)(data >> 24);
00825         }
00826       }
00827       check_write = 0U;
00828     } else {
00829       // Write register
00830       if (post_read) {
00831         // Read previous data
00832         retry = DAP_Data.transfer.retry_count;
00833         do {
00834           response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
00835         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00836         if (response_value != DAP_TRANSFER_OK) {
00837           break;
00838         }
00839         // Store previous data
00840         *response++ = (uint8_t) data;
00841         *response++ = (uint8_t)(data >>  8);
00842         *response++ = (uint8_t)(data >> 16);
00843         *response++ = (uint8_t)(data >> 24);
00844         post_read = 0U;
00845       }
00846       // Load data
00847       data = (uint32_t)(*(request+0) <<  0) |
00848              (uint32_t)(*(request+1) <<  8) |
00849              (uint32_t)(*(request+2) << 16) |
00850              (uint32_t)(*(request+3) << 24);
00851       request += 4;
00852       if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
00853         // Write match mask
00854         DAP_Data.transfer.match_mask = data;
00855         response_value = DAP_TRANSFER_OK;
00856       } else {
00857         // Write DP/AP register
00858         retry = DAP_Data.transfer.retry_count;
00859         do {
00860           response_value = SWD_Transfer(request_value, &data);
00861         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00862         if (response_value != DAP_TRANSFER_OK) {
00863           break;
00864         }
00865 #if (TIMESTAMP_CLOCK != 0U)
00866         // Store Timestamp
00867         if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
00868           timestamp = DAP_Data.timestamp;
00869           *response++ = (uint8_t) timestamp;
00870           *response++ = (uint8_t)(timestamp >>  8);
00871           *response++ = (uint8_t)(timestamp >> 16);
00872           *response++ = (uint8_t)(timestamp >> 24);
00873         }
00874 #endif
00875         check_write = 1U;
00876       }
00877     }
00878     response_count++;
00879     if (DAP_TransferAbort) {
00880       break;
00881     }
00882   }
00883 
00884   for (; request_count != 0U; request_count--) {
00885     // Process canceled requests
00886     request_value = *request++;
00887     if ((request_value & DAP_TRANSFER_RnW) != 0U) {
00888       // Read register
00889       if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
00890         // Read with value match
00891         request += 4;
00892       }
00893     } else {
00894       // Write register
00895       request += 4;
00896     }
00897   }
00898 
00899   if (response_value == DAP_TRANSFER_OK) {
00900     if (post_read) {
00901       // Read previous data
00902       retry = DAP_Data.transfer.retry_count;
00903       do {
00904         response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
00905       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00906       if (response_value != DAP_TRANSFER_OK) {
00907         goto end;
00908       }
00909       // Store previous data
00910       *response++ = (uint8_t) data;
00911       *response++ = (uint8_t)(data >>  8);
00912       *response++ = (uint8_t)(data >> 16);
00913       *response++ = (uint8_t)(data >> 24);
00914     } else if (check_write) {
00915       // Check last write
00916       retry = DAP_Data.transfer.retry_count;
00917       do {
00918         response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
00919       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00920     }
00921   }
00922 
00923 end:
00924   *(response_head+0) = (uint8_t)response_count;
00925   *(response_head+1) = (uint8_t)response_value;
00926 
00927   return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
00928 }
00929 #endif
00930 
00931 
00932 // Process JTAG Transfer command and prepare response
00933 //   request:  pointer to request data
00934 //   response: pointer to response data
00935 //   return:   number of bytes in response (lower 16 bits)
00936 //             number of bytes in request (upper 16 bits)
00937 #if (DAP_JTAG != 0)
00938 static uint32_t DAP_JTAG_Transfer(const uint8_t *request, uint8_t *response) {
00939   const
00940   uint8_t  *request_head;
00941   uint32_t  request_count;
00942   uint32_t  request_value;
00943   uint32_t  request_ir;
00944   uint8_t  *response_head;
00945   uint32_t  response_count;
00946   uint32_t  response_value;
00947   uint32_t  post_read;
00948   uint32_t  match_value;
00949   uint32_t  match_retry;
00950   uint32_t  retry;
00951   uint32_t  data;
00952   uint32_t  ir;
00953 #if (TIMESTAMP_CLOCK != 0U)
00954   uint32_t  timestamp;
00955 #endif
00956 
00957   request_head   = request;
00958 
00959   response_count = 0U;
00960   response_value = 0U;
00961   response_head  = response;
00962   response      += 2;
00963 
00964   DAP_TransferAbort = 0U;
00965 
00966   ir        = 0U;
00967   post_read = 0U;
00968 
00969   // Device index (JTAP TAP)
00970   DAP_Data.jtag_dev.index = *request++;
00971   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
00972     goto end;
00973   }
00974 
00975   request_count = *request++;
00976 
00977   for (; request_count != 0U; request_count--) {
00978     request_value = *request++;
00979     request_ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
00980     if ((request_value & DAP_TRANSFER_RnW) != 0U) {
00981       // Read register
00982       if (post_read) {
00983         // Read was posted before
00984         retry = DAP_Data.transfer.retry_count;
00985         if ((ir == request_ir) && ((request_value & DAP_TRANSFER_MATCH_VALUE) == 0U)) {
00986           // Read previous data and post next read
00987           do {
00988             response_value = JTAG_Transfer(request_value, &data);
00989           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
00990         } else {
00991           // Select JTAG chain
00992           if (ir != JTAG_DPACC) {
00993             ir = JTAG_DPACC;
00994             JTAG_IR(ir);
00995           }
00996           // Read previous data
00997           do {
00998             response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
00999           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01000           post_read = 0U;
01001         }
01002         if (response_value != DAP_TRANSFER_OK) {
01003           break;
01004         }
01005         // Store previous data
01006         *response++ = (uint8_t) data;
01007         *response++ = (uint8_t)(data >>  8);
01008         *response++ = (uint8_t)(data >> 16);
01009         *response++ = (uint8_t)(data >> 24);
01010 #if (TIMESTAMP_CLOCK != 0U)
01011         if (post_read) {
01012           // Store Timestamp of next AP read
01013           if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
01014             timestamp = DAP_Data.timestamp;
01015             *response++ = (uint8_t) timestamp;
01016             *response++ = (uint8_t)(timestamp >>  8);
01017             *response++ = (uint8_t)(timestamp >> 16);
01018             *response++ = (uint8_t)(timestamp >> 24);
01019           }
01020         }
01021 #endif
01022       }
01023       if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
01024         // Read with value match
01025         match_value = (uint32_t)(*(request+0) <<  0) |
01026                       (uint32_t)(*(request+1) <<  8) |
01027                       (uint32_t)(*(request+2) << 16) |
01028                       (uint32_t)(*(request+3) << 24);
01029         request += 4;
01030         match_retry  = DAP_Data.transfer.match_retry;
01031         // Select JTAG chain
01032         if (ir != request_ir) {
01033           ir = request_ir;
01034           JTAG_IR(ir);
01035         }
01036         // Post DP/AP read
01037         retry = DAP_Data.transfer.retry_count;
01038         do {
01039           response_value = JTAG_Transfer(request_value, NULL);
01040         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01041         if (response_value != DAP_TRANSFER_OK) {
01042           break;
01043         }
01044         do {
01045           // Read register until its value matches or retry counter expires
01046           retry = DAP_Data.transfer.retry_count;
01047           do {
01048             response_value = JTAG_Transfer(request_value, &data);
01049           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01050           if (response_value != DAP_TRANSFER_OK) {
01051             break;
01052           }
01053         } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
01054         if ((data & DAP_Data.transfer.match_mask) != match_value) {
01055           response_value |= DAP_TRANSFER_MISMATCH;
01056         }
01057         if (response_value != DAP_TRANSFER_OK) {
01058           break;
01059         }
01060       } else {
01061         // Normal read
01062         if (post_read == 0U) {
01063           // Select JTAG chain
01064           if (ir != request_ir) {
01065             ir = request_ir;
01066             JTAG_IR(ir);
01067           }
01068           // Post DP/AP read
01069           retry = DAP_Data.transfer.retry_count;
01070           do {
01071             response_value = JTAG_Transfer(request_value, NULL);
01072           } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01073           if (response_value != DAP_TRANSFER_OK) {
01074             break;
01075           }
01076 #if (TIMESTAMP_CLOCK != 0U)
01077           // Store Timestamp
01078           if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
01079             timestamp = DAP_Data.timestamp;
01080             *response++ = (uint8_t) timestamp;
01081             *response++ = (uint8_t)(timestamp >>  8);
01082             *response++ = (uint8_t)(timestamp >> 16);
01083             *response++ = (uint8_t)(timestamp >> 24);
01084           }
01085 #endif
01086           post_read = 1U;
01087         }
01088       }
01089     } else {
01090       // Write register
01091       if (post_read) {
01092         // Select JTAG chain
01093         if (ir != JTAG_DPACC) {
01094           ir = JTAG_DPACC;
01095           JTAG_IR(ir);
01096         }
01097         // Read previous data
01098         retry = DAP_Data.transfer.retry_count;
01099         do {
01100           response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
01101         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01102         if (response_value != DAP_TRANSFER_OK) {
01103           break;
01104         }
01105         // Store previous data
01106         *response++ = (uint8_t) data;
01107         *response++ = (uint8_t)(data >>  8);
01108         *response++ = (uint8_t)(data >> 16);
01109         *response++ = (uint8_t)(data >> 24);
01110         post_read = 0U;
01111       }
01112       // Load data
01113       data = (uint32_t)(*(request+0) <<  0) |
01114              (uint32_t)(*(request+1) <<  8) |
01115              (uint32_t)(*(request+2) << 16) |
01116              (uint32_t)(*(request+3) << 24);
01117       request += 4;
01118       if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
01119         // Write match mask
01120         DAP_Data.transfer.match_mask = data;
01121         response_value = DAP_TRANSFER_OK;
01122       } else {
01123         // Select JTAG chain
01124         if (ir != request_ir) {
01125           ir = request_ir;
01126           JTAG_IR(ir);
01127         }
01128         // Write DP/AP register
01129         retry = DAP_Data.transfer.retry_count;
01130         do {
01131           response_value = JTAG_Transfer(request_value, &data);
01132         } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01133         if (response_value != DAP_TRANSFER_OK) {
01134           break;
01135         }
01136 #if (TIMESTAMP_CLOCK != 0U)
01137         // Store Timestamp
01138         if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
01139           timestamp = DAP_Data.timestamp;
01140           *response++ = (uint8_t) timestamp;
01141           *response++ = (uint8_t)(timestamp >>  8);
01142           *response++ = (uint8_t)(timestamp >> 16);
01143           *response++ = (uint8_t)(timestamp >> 24);
01144         }
01145 #endif
01146       }
01147     }
01148     response_count++;
01149     if (DAP_TransferAbort) {
01150       break;
01151     }
01152   }
01153 
01154   for (; request_count != 0U; request_count--) {
01155     // Process canceled requests
01156     request_value = *request++;
01157     if ((request_value & DAP_TRANSFER_RnW) != 0U) {
01158       // Read register
01159       if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
01160         // Read with value match
01161         request += 4;
01162       }
01163     } else {
01164       // Write register
01165       request += 4;
01166     }
01167   }
01168 
01169   if (response_value == DAP_TRANSFER_OK) {
01170     // Select JTAG chain
01171     if (ir != JTAG_DPACC) {
01172       ir = JTAG_DPACC;
01173       JTAG_IR(ir);
01174     }
01175     if (post_read) {
01176       // Read previous data
01177       retry = DAP_Data.transfer.retry_count;
01178       do {
01179         response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
01180       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01181       if (response_value != DAP_TRANSFER_OK) {
01182         goto end;
01183       }
01184       // Store previous data
01185       *response++ = (uint8_t) data;
01186       *response++ = (uint8_t)(data >>  8);
01187       *response++ = (uint8_t)(data >> 16);
01188       *response++ = (uint8_t)(data >> 24);
01189     } else {
01190       // Check last write
01191       retry = DAP_Data.transfer.retry_count;
01192       do {
01193         response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
01194       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01195     }
01196   }
01197 
01198 end:
01199   *(response_head+0) = (uint8_t)response_count;
01200   *(response_head+1) = (uint8_t)response_value;
01201 
01202   return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
01203 }
01204 #endif
01205 
01206 
01207 // Process Dummy Transfer command and prepare response
01208 //   request:  pointer to request data
01209 //   response: pointer to response data
01210 //   return:   number of bytes in response (lower 16 bits)
01211 //             number of bytes in request (upper 16 bits)
01212 static uint32_t DAP_Dummy_Transfer(const uint8_t *request, uint8_t *response) {
01213   const
01214   uint8_t  *request_head;
01215   uint32_t  request_count;
01216   uint32_t  request_value;
01217 
01218   request_head  =  request;
01219 
01220   request++;            // Ignore DAP index
01221 
01222   request_count = *request++;
01223 
01224   for (; request_count != 0U; request_count--) {
01225     // Process dummy requests
01226     request_value = *request++;
01227     if ((request_value & DAP_TRANSFER_RnW) != 0U) {
01228       // Read register
01229       if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
01230         // Read with value match
01231         request += 4;
01232       }
01233     } else {
01234       // Write register
01235       request += 4;
01236     }
01237   }
01238 
01239   *(response+0) = 0U;   // Response count
01240   *(response+1) = 0U;   // Response value
01241 
01242   return (((uint32_t)(request - request_head) << 16) | 2U);
01243 }
01244 
01245 
01246 // Process Transfer command and prepare response
01247 //   request:  pointer to request data
01248 //   response: pointer to response data
01249 //   return:   number of bytes in response (lower 16 bits)
01250 //             number of bytes in request (upper 16 bits)
01251 static uint32_t DAP_Transfer(const uint8_t *request, uint8_t *response) {
01252   uint32_t num;
01253 
01254   switch (DAP_Data.debug_port) {
01255 #if (DAP_SWD != 0)
01256     case DAP_PORT_SWD:
01257       num = DAP_SWD_Transfer(request, response);
01258       break;
01259 #endif
01260 #if (DAP_JTAG != 0)
01261     case DAP_PORT_JTAG:
01262       num = DAP_JTAG_Transfer(request, response);
01263       break;
01264 #endif
01265     default:
01266       num = DAP_Dummy_Transfer(request, response);
01267       break;
01268   }
01269 
01270   return (num);
01271 }
01272 
01273 
01274 // Process SWD Transfer Block command and prepare response
01275 //   request:  pointer to request data
01276 //   response: pointer to response data
01277 //   return:   number of bytes in response
01278 #if (DAP_SWD != 0)
01279 static uint32_t DAP_SWD_TransferBlock(const uint8_t *request, uint8_t *response) {
01280   uint32_t  request_count;
01281   uint32_t  request_value;
01282   uint32_t  response_count;
01283   uint32_t  response_value;
01284   uint8_t  *response_head;
01285   uint32_t  retry;
01286   uint32_t  data;
01287 
01288   response_count = 0U;
01289   response_value = 0U;
01290   response_head  = response;
01291   response      += 3;
01292 
01293   DAP_TransferAbort = 0U;
01294 
01295   request++;            // Ignore DAP index
01296 
01297   request_count = (uint32_t)(*(request+0) << 0) |
01298                   (uint32_t)(*(request+1) << 8);
01299   request += 2;
01300   if (request_count == 0U) {
01301     goto end;
01302   }
01303 
01304   request_value = *request++;
01305   if ((request_value & DAP_TRANSFER_RnW) != 0U) {
01306     // Read register block
01307     if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
01308       // Post AP read
01309       retry = DAP_Data.transfer.retry_count;
01310       do {
01311         response_value = SWD_Transfer(request_value, NULL);
01312       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01313       if (response_value != DAP_TRANSFER_OK) {
01314         goto end;
01315       }
01316     }
01317     while (request_count--) {
01318       // Read DP/AP register
01319       if ((request_count == 0U) && ((request_value & DAP_TRANSFER_APnDP) != 0U)) {
01320         // Last AP read
01321         request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
01322       }
01323       retry = DAP_Data.transfer.retry_count;
01324       do {
01325         response_value = SWD_Transfer(request_value, &data);
01326       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01327       if (response_value != DAP_TRANSFER_OK) {
01328         goto end;
01329       }
01330       // Store data
01331       *response++ = (uint8_t) data;
01332       *response++ = (uint8_t)(data >>  8);
01333       *response++ = (uint8_t)(data >> 16);
01334       *response++ = (uint8_t)(data >> 24);
01335       response_count++;
01336     }
01337   } else {
01338     // Write register block
01339     while (request_count--) {
01340       // Load data
01341       data = (uint32_t)(*(request+0) <<  0) |
01342              (uint32_t)(*(request+1) <<  8) |
01343              (uint32_t)(*(request+2) << 16) |
01344              (uint32_t)(*(request+3) << 24);
01345       request += 4;
01346       // Write DP/AP register
01347       retry = DAP_Data.transfer.retry_count;
01348       do {
01349         response_value = SWD_Transfer(request_value, &data);
01350       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01351       if (response_value != DAP_TRANSFER_OK) {
01352         goto end;
01353       }
01354       response_count++;
01355     }
01356     // Check last write
01357     retry = DAP_Data.transfer.retry_count;
01358     do {
01359       response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
01360     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01361   }
01362 
01363 end:
01364   *(response_head+0) = (uint8_t)(response_count >> 0);
01365   *(response_head+1) = (uint8_t)(response_count >> 8);
01366   *(response_head+2) = (uint8_t) response_value;
01367 
01368   return ((uint32_t)(response - response_head));
01369 }
01370 #endif
01371 
01372 
01373 // Process JTAG Transfer Block command and prepare response
01374 //   request:  pointer to request data
01375 //   response: pointer to response data
01376 //   return:   number of bytes in response
01377 #if (DAP_JTAG != 0)
01378 static uint32_t DAP_JTAG_TransferBlock(const uint8_t *request, uint8_t *response) {
01379   uint32_t  request_count;
01380   uint32_t  request_value;
01381   uint32_t  response_count;
01382   uint32_t  response_value;
01383   uint8_t  *response_head;
01384   uint32_t  retry;
01385   uint32_t  data;
01386   uint32_t  ir;
01387 
01388   response_count = 0U;
01389   response_value = 0U;
01390   response_head  = response;
01391   response      += 3;
01392 
01393   DAP_TransferAbort = 0U;
01394 
01395   // Device index (JTAP TAP)
01396   DAP_Data.jtag_dev.index = *request++;
01397   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
01398     goto end;
01399   }
01400 
01401   request_count = (uint32_t)(*(request+0) << 0) |
01402                   (uint32_t)(*(request+1) << 8);
01403   request += 2;
01404   if (request_count == 0U) {
01405     goto end;
01406   }
01407 
01408   request_value = *request++;
01409 
01410   // Select JTAG chain
01411   ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
01412   JTAG_IR(ir);
01413 
01414   if ((request_value & DAP_TRANSFER_RnW) != 0U) {
01415     // Post read
01416     retry = DAP_Data.transfer.retry_count;
01417     do {
01418       response_value = JTAG_Transfer(request_value, NULL);
01419     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01420     if (response_value != DAP_TRANSFER_OK) {
01421       goto end;
01422     }
01423     // Read register block
01424     while (request_count--) {
01425       // Read DP/AP register
01426       if (request_count == 0U) {
01427         // Last read
01428         if (ir != JTAG_DPACC) {
01429           JTAG_IR(JTAG_DPACC);
01430         }
01431         request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
01432       }
01433       retry = DAP_Data.transfer.retry_count;
01434       do {
01435         response_value = JTAG_Transfer(request_value, &data);
01436       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01437       if (response_value != DAP_TRANSFER_OK) {
01438         goto end;
01439       }
01440       // Store data
01441       *response++ = (uint8_t) data;
01442       *response++ = (uint8_t)(data >>  8);
01443       *response++ = (uint8_t)(data >> 16);
01444       *response++ = (uint8_t)(data >> 24);
01445       response_count++;
01446     }
01447   } else {
01448     // Write register block
01449     while (request_count--) {
01450       // Load data
01451       data = (uint32_t)(*(request+0) <<  0) |
01452              (uint32_t)(*(request+1) <<  8) |
01453              (uint32_t)(*(request+2) << 16) |
01454              (uint32_t)(*(request+3) << 24);
01455       request += 4;
01456       // Write DP/AP register
01457       retry = DAP_Data.transfer.retry_count;
01458       do {
01459         response_value = JTAG_Transfer(request_value, &data);
01460       } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01461       if (response_value != DAP_TRANSFER_OK) {
01462         goto end;
01463       }
01464       response_count++;
01465     }
01466     // Check last write
01467     if (ir != JTAG_DPACC) {
01468       JTAG_IR(JTAG_DPACC);
01469     }
01470     retry = DAP_Data.transfer.retry_count;
01471     do {
01472       response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
01473     } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
01474   }
01475 
01476 end:
01477   *(response_head+0) = (uint8_t)(response_count >> 0);
01478   *(response_head+1) = (uint8_t)(response_count >> 8);
01479   *(response_head+2) = (uint8_t) response_value;
01480 
01481   return ((uint32_t)(response - response_head));
01482 }
01483 #endif
01484 
01485 
01486 // Process Transfer Block command and prepare response
01487 //   request:  pointer to request data
01488 //   response: pointer to response data
01489 //   return:   number of bytes in response (lower 16 bits)
01490 //             number of bytes in request (upper 16 bits)
01491 static uint32_t DAP_TransferBlock(const uint8_t *request, uint8_t *response) {
01492   uint32_t num;
01493 
01494   switch (DAP_Data.debug_port) {
01495 #if (DAP_SWD != 0)
01496     case DAP_PORT_SWD:
01497       num = DAP_SWD_TransferBlock (request, response);
01498       break;
01499 #endif
01500 #if (DAP_JTAG != 0)
01501     case DAP_PORT_JTAG:
01502       num = DAP_JTAG_TransferBlock(request, response);
01503       break;
01504 #endif
01505     default:
01506       *(response+0) = 0U;       // Response count [7:0]
01507       *(response+1) = 0U;       // Response count[15:8]
01508       *(response+2) = 0U;       // Response value
01509       num = 3U;
01510       break;
01511   }
01512 
01513   if ((*(request+3) & DAP_TRANSFER_RnW) != 0U) {
01514     // Read register block
01515     num |=  4U << 16;
01516   } else {
01517     // Write register block
01518     num |= (4U + (((uint32_t)(*(request+1)) | (uint32_t)(*(request+2) << 8)) * 4)) << 16;
01519   }
01520 
01521   return (num);
01522 }
01523 
01524 
01525 // Process SWD Write ABORT command and prepare response
01526 //   request:  pointer to request data
01527 //   response: pointer to response data
01528 //   return:   number of bytes in response
01529 #if (DAP_SWD != 0)
01530 static uint32_t DAP_SWD_WriteAbort(const uint8_t *request, uint8_t *response) {
01531   uint32_t data;
01532 
01533   // Load data (Ignore DAP index)
01534   data = (uint32_t)(*(request+1) <<  0) |
01535          (uint32_t)(*(request+2) <<  8) |
01536          (uint32_t)(*(request+3) << 16) |
01537          (uint32_t)(*(request+4) << 24);
01538 
01539   // Write Abort register
01540   SWD_Transfer(DP_ABORT, &data);
01541 
01542   *response = DAP_OK;
01543   return (1U);
01544 }
01545 #endif
01546 
01547 
01548 // Process JTAG Write ABORT command and prepare response
01549 //   request:  pointer to request data
01550 //   response: pointer to response data
01551 //   return:   number of bytes in response
01552 #if (DAP_JTAG != 0)
01553 static uint32_t DAP_JTAG_WriteAbort(const uint8_t *request, uint8_t *response) {
01554   uint32_t data;
01555 
01556   // Device index (JTAP TAP)
01557   DAP_Data.jtag_dev.index = *request;
01558   if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
01559     *response = DAP_ERROR;
01560     return (1U);
01561   }
01562 
01563   // Select JTAG chain
01564   JTAG_IR(JTAG_ABORT);
01565 
01566   // Load data
01567   data = (uint32_t)(*(request+1) <<  0) |
01568          (uint32_t)(*(request+2) <<  8) |
01569          (uint32_t)(*(request+3) << 16) |
01570          (uint32_t)(*(request+4) << 24);
01571 
01572   // Write Abort register
01573   JTAG_WriteAbort(data);
01574 
01575   *response = DAP_OK;
01576   return (1U);
01577 }
01578 #endif
01579 
01580 
01581 // Process Write ABORT command and prepare response
01582 //   request:  pointer to request data
01583 //   response: pointer to response data
01584 //   return:   number of bytes in response (lower 16 bits)
01585 //             number of bytes in request (upper 16 bits)
01586 static uint32_t DAP_WriteAbort(const uint8_t *request, uint8_t *response) {
01587   uint32_t num;
01588 
01589   switch (DAP_Data.debug_port) {
01590 #if (DAP_SWD != 0)
01591     case DAP_PORT_SWD:
01592       num = DAP_SWD_WriteAbort (request, response);
01593       break;
01594 #endif
01595 #if (DAP_JTAG != 0)
01596     case DAP_PORT_JTAG:
01597       num = DAP_JTAG_WriteAbort(request, response);
01598       break;
01599 #endif
01600     default:
01601       *response = DAP_ERROR;
01602       num = 1U;
01603       break;
01604   }
01605   return ((5U << 16) | num);
01606 }
01607 
01608 
01609 // Process DAP Vendor command request and prepare response
01610 // Default function (can be overridden)
01611 //   request:  pointer to request data
01612 //   response: pointer to response data
01613 //   return:   number of bytes in response (lower 16 bits)
01614 //             number of bytes in request (upper 16 bits)
01615 __WEAK uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response) {
01616   (void)request;
01617   *response = ID_DAP_Invalid;
01618   return ((1U << 16) | 1U);
01619 }
01620 
01621 // Process DAP Vendor extended command request and prepare response
01622 // Default function (can be overridden)
01623 //   request:  pointer to request data
01624 //   response: pointer to response data
01625 //   return:   number of bytes in response (lower 16 bits)
01626 //             number of bytes in request (upper 16 bits)
01627 __weak uint32_t DAP_ProcessVendorCommandEx(const uint8_t *request, uint8_t *response) {
01628   *response = ID_DAP_Invalid;
01629   return ((1U << 16) | 1U);
01630 }
01631 
01632 // Process DAP command request and prepare response
01633 //   request:  pointer to request data
01634 //   response: pointer to response data
01635 //   return:   number of bytes in response (lower 16 bits)
01636 //             number of bytes in request (upper 16 bits)
01637 uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {
01638   uint32_t num;
01639 
01640   if ((*request >= ID_DAP_Vendor0) && (*request <= ID_DAP_Vendor31)) {
01641     return DAP_ProcessVendorCommand(request, response);
01642   }
01643 
01644   if ((*request >= ID_DAP_VendorExFirst) && (*request <= ID_DAP_VendorExLast)) {
01645     return DAP_ProcessVendorCommandEx(request, response);
01646   }  
01647 
01648   *response++ = *request;
01649 
01650   switch (*request++) {
01651     case ID_DAP_Info:
01652       num = DAP_Info(*request, response+1);
01653       *response = (uint8_t)num;
01654       return ((2U << 16) + 2U + num);
01655 
01656     case ID_DAP_HostStatus:
01657       num = DAP_HostStatus(request, response);
01658       break;
01659 
01660     case ID_DAP_Connect:
01661       num = DAP_Connect(request, response);
01662       break;
01663     case ID_DAP_Disconnect:
01664       num = DAP_Disconnect(response);
01665       break;
01666 
01667     case ID_DAP_Delay:
01668       num = DAP_Delay(request, response);
01669       break;
01670 
01671     case ID_DAP_ResetTarget:
01672       num = DAP_ResetTarget(response);
01673       break;
01674 
01675     case ID_DAP_SWJ_Pins:
01676       num = DAP_SWJ_Pins(request, response);
01677       break;
01678     case ID_DAP_SWJ_Clock:
01679       num = DAP_SWJ_Clock(request, response);
01680       break;
01681     case ID_DAP_SWJ_Sequence:
01682       num = DAP_SWJ_Sequence(request, response);
01683       break;
01684 
01685     case ID_DAP_SWD_Configure:
01686       num = DAP_SWD_Configure(request, response);
01687       break;
01688     case ID_DAP_SWD_Sequence:
01689       num = DAP_SWD_Sequence(request, response);
01690       break;
01691 
01692     case ID_DAP_JTAG_Sequence:
01693       num = DAP_JTAG_Sequence(request, response);
01694       break;
01695     case ID_DAP_JTAG_Configure:
01696       num = DAP_JTAG_Configure(request, response);
01697       break;
01698     case ID_DAP_JTAG_IDCODE:
01699       num = DAP_JTAG_IDCode(request, response);
01700       break;
01701 
01702     case ID_DAP_TransferConfigure:
01703       num = DAP_TransferConfigure(request, response);
01704       break;
01705     case ID_DAP_Transfer:
01706       num = DAP_Transfer(request, response);
01707       break;
01708     case ID_DAP_TransferBlock:
01709       num = DAP_TransferBlock(request, response);
01710       break;
01711 
01712     case ID_DAP_WriteABORT:
01713       num = DAP_WriteAbort(request, response);
01714       break;
01715 
01716 #if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
01717     case ID_DAP_SWO_Transport:
01718       num = SWO_Transport(request, response);
01719       break;
01720     case ID_DAP_SWO_Mode:
01721       num = SWO_Mode(request, response);
01722       break;
01723     case ID_DAP_SWO_Baudrate:
01724       num = SWO_Baudrate(request, response);
01725       break;
01726     case ID_DAP_SWO_Control:
01727       num = SWO_Control(request, response);
01728       break;
01729     case ID_DAP_SWO_Status:
01730       num = SWO_Status(response);
01731       break;
01732     case ID_DAP_SWO_ExtendedStatus:
01733       num = SWO_ExtendedStatus(request, response);
01734       break;
01735     case ID_DAP_SWO_Data:
01736       num = SWO_Data(request, response);
01737       break;
01738 #endif
01739 
01740     default:
01741       *(response-1) = ID_DAP_Invalid;
01742       return ((1U << 16) | 1U);
01743   }
01744 
01745   return ((1U << 16) + 1U + num);
01746 }
01747 
01748 
01749 // Execute DAP command (process request and prepare response)
01750 //   request:  pointer to request data
01751 //   response: pointer to response data
01752 //   return:   number of bytes in response (lower 16 bits)
01753 //             number of bytes in request (upper 16 bits)
01754 uint32_t DAP_ExecuteCommand(const uint8_t *request, uint8_t *response) {
01755   uint32_t cnt, num, n;
01756 
01757   if (*request == ID_DAP_ExecuteCommands) {
01758     *response++ = *request++;
01759     cnt = *request++;
01760     *response++ = (uint8_t)cnt;
01761     num = (2U << 16) | 2U;
01762     while (cnt--) {
01763       n = DAP_ProcessCommand(request, response);
01764       num += n;
01765       request  += (uint16_t)(n >> 16);
01766       response += (uint16_t) n;
01767     }
01768     return (num);
01769   }
01770 
01771   return DAP_ProcessCommand(request, response);
01772 }
01773 
01774 
01775 // Setup DAP
01776 void DAP_Setup(void) {
01777 
01778   // Default settings
01779   DAP_Data.debug_port  = 0U;
01780   DAP_Data.fast_clock  = 0U;
01781   DAP_Data.clock_delay = CLOCK_DELAY(DAP_DEFAULT_SWJ_CLOCK);
01782   DAP_Data.transfer.idle_cycles = 0U;
01783   DAP_Data.transfer.retry_count = 100U;
01784   DAP_Data.transfer.match_retry = 0U;
01785   DAP_Data.transfer.match_mask  = 0x00000000U;
01786 #if (DAP_SWD != 0)
01787   DAP_Data.swd_conf.turnaround  = 1U;
01788   DAP_Data.swd_conf.data_phase  = 0U;
01789 #endif
01790 #if (DAP_JTAG != 0)
01791   DAP_Data.jtag_dev.count = 0U;
01792 #endif
01793 
01794   DAP_SETUP();  // Device specific setup
01795 }