git.gag.com Git - fw/openocd/blob - src/jtag/drivers/kitprog.c

   1 /***************************************************************************
   2  *   Copyright (C) 2007 by Juergen Stuber <juergen@jstuber.net>            *
   3  *   based on Dominic Rath's and Benedikt Sauter's usbprog.c               *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   Copyright (C) 2011 by Jean-Christophe PLAGNIOL-VIILARD                *
   9  *   plagnioj@jcrosoft.com                                                 *
  10  *                                                                         *
  11  *   Copyright (C) 2015 by Marc Schink                                     *
  12  *   openocd-dev@marcschink.de                                             *
  13  *                                                                         *
  14  *   Copyright (C) 2015 by Paul Fertser                                    *
  15  *   fercerpav@gmail.com                                                   *
  16  *                                                                         *
  17  *   Copyright (C) 2015-2017 by Forest Crossman                            *
  18  *   cyrozap@gmail.com                                                     *
  19  *                                                                         *
  20  *   This program is free software; you can redistribute it and/or modify  *
  21  *   it under the terms of the GNU General Public License as published by  *
  22  *   the Free Software Foundation; either version 2 of the License, or     *
  23  *   (at your option) any later version.                                   *
  24  *                                                                         *
  25  *   This program is distributed in the hope that it will be useful,       *
  26  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  27  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  28  *   GNU General Public License for more details.                          *
  29  *                                                                         *
  30  *   You should have received a copy of the GNU General Public License     *
  31  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  32  ***************************************************************************/
  33
  34 #ifdef HAVE_CONFIG_H
  35 #include "config.h"
  36 #endif
  37
  38 #include <stdint.h>
  39
  40 #include <hidapi.h>
  41
  42 #include <jtag/adapter.h>
  43 #include <jtag/interface.h>
  44 #include <jtag/swd.h>
  45 #include <jtag/commands.h>
  46
  47 #include "libusb_helper.h"
  48
  49 #define VID 0x04b4
  50 #define PID 0xf139
  51
  52 #define BULK_EP_IN  1
  53 #define BULK_EP_OUT 2
  54
  55 #define CONTROL_TYPE_READ  0x01
  56 #define CONTROL_TYPE_WRITE 0x02
  57
  58 #define CONTROL_COMMAND_PROGRAM 0x07
  59
  60 #define CONTROL_MODE_POLL_PROGRAMMER_STATUS  0x01
  61 #define CONTROL_MODE_RESET_TARGET            0x04
  62 #define CONTROL_MODE_SET_PROGRAMMER_PROTOCOL 0x40
  63 #define CONTROL_MODE_SYNCHRONIZE_TRANSFER    0x41
  64 #define CONTROL_MODE_ACQUIRE_SWD_TARGET      0x42
  65 #define CONTROL_MODE_SEND_SWD_SEQUENCE       0x43
  66
  67 #define PROTOCOL_JTAG 0x00
  68 #define PROTOCOL_SWD  0x01
  69
  70 #define DEVICE_PSOC4   0x00
  71 #define DEVICE_PSOC3   0x01
  72 #define DEVICE_UNKNOWN 0x02
  73 #define DEVICE_PSOC5   0x03
  74
  75 #define ACQUIRE_MODE_RESET       0x00
  76 #define ACQUIRE_MODE_POWER_CYCLE 0x01
  77
  78 #define SEQUENCE_LINE_RESET  0x00
  79 #define SEQUENCE_JTAG_TO_SWD 0x01
  80
  81 #define PROGRAMMER_NOK_NACK 0x00
  82 #define PROGRAMMER_OK_ACK   0x01
  83
  84 #define HID_TYPE_WRITE 0x00
  85 #define HID_TYPE_READ  0x01
  86 #define HID_TYPE_START 0x02
  87
  88 #define HID_COMMAND_POWER      0x80
  89 #define HID_COMMAND_VERSION    0x81
  90 #define HID_COMMAND_RESET      0x82
  91 #define HID_COMMAND_CONFIGURE  0x8f
  92 #define HID_COMMAND_BOOTLOADER 0xa0
  93
  94 /* 512 bytes seems to work reliably */
  95 #define SWD_MAX_BUFFER_LENGTH 512
  96
  97 struct kitprog {
  98         hid_device *hid_handle;
  99         struct libusb_device_handle *usb_handle;
 100         uint16_t packet_size;
 101         uint16_t packet_index;
 102         uint8_t *packet_buffer;
 103         char *serial;
 104         uint8_t hardware_version;
 105         uint8_t minor_version;
 106         uint8_t major_version;
 107         uint16_t millivolts;
 108
 109         bool supports_jtag_to_swd;
 110 };
 111
 112 struct pending_transfer_result {
 113         uint8_t cmd;
 114         uint32_t data;
 115         void *buffer;
 116 };
 117
 118 static bool kitprog_init_acquire_psoc;
 119
 120 static int pending_transfer_count, pending_queue_len;
 121 static struct pending_transfer_result *pending_transfers;
 122
 123 static int queued_retval;
 124
 125 static struct kitprog *kitprog_handle;
 126
 127 static int kitprog_usb_open(void);
 128 static void kitprog_usb_close(void);
 129
 130 static int kitprog_hid_command(uint8_t *command, size_t command_length,
 131                 uint8_t *data, size_t data_length);
 132 static int kitprog_get_version(void);
 133 static int kitprog_get_millivolts(void);
 134 static int kitprog_get_info(void);
 135 static int kitprog_set_protocol(uint8_t protocol);
 136 static int kitprog_get_status(void);
 137 static int kitprog_set_unknown(void);
 138 static int kitprog_acquire_psoc(uint8_t psoc_type, uint8_t acquire_mode,
 139                 uint8_t max_attempts);
 140 static int kitprog_reset_target(void);
 141 static int kitprog_swd_sync(void);
 142 static int kitprog_swd_seq(uint8_t seq_type);
 143
 144 static int kitprog_generic_acquire(void);
 145
 146 static int kitprog_swd_run_queue(void);
 147 static void kitprog_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data);
 148 static int kitprog_swd_switch_seq(enum swd_special_seq seq);
 149
 150
 151 static inline int mm_to_version(uint8_t major, uint8_t minor)
 152 {
 153         return (major << 8) | minor;
 154 }
 155
 156 static int kitprog_init(void)
 157 {
 158         int retval;
 159
 160         kitprog_handle = malloc(sizeof(struct kitprog));
 161         if (!kitprog_handle) {
 162                 LOG_ERROR("Failed to allocate memory");
 163                 return ERROR_FAIL;
 164         }
 165
 166         if (kitprog_usb_open() != ERROR_OK) {
 167                 LOG_ERROR("Can't find a KitProg device! Please check device connections and permissions.");
 168                 return ERROR_JTAG_INIT_FAILED;
 169         }
 170
 171         /* Get the current KitProg version and target voltage */
 172         if (kitprog_get_info() != ERROR_OK)
 173                 return ERROR_FAIL;
 174
 175         /* Compatibility check */
 176         kitprog_handle->supports_jtag_to_swd = true;
 177         int kitprog_version = mm_to_version(kitprog_handle->major_version, kitprog_handle->minor_version);
 178         if (kitprog_version < mm_to_version(2, 14)) {
 179                 LOG_WARNING("KitProg firmware versions below v2.14 do not support sending JTAG to SWD sequences. These sequences will be substituted with SWD line resets.");
 180                 kitprog_handle->supports_jtag_to_swd = false;
 181         }
 182
 183         /* I have no idea what this does */
 184         if (kitprog_set_unknown() != ERROR_OK)
 185                 return ERROR_FAIL;
 186
 187         /* SWD won't work unless we do this */
 188         if (kitprog_swd_sync() != ERROR_OK)
 189                 return ERROR_FAIL;
 190
 191         /* Set the protocol to SWD */
 192         if (kitprog_set_protocol(PROTOCOL_SWD) != ERROR_OK)
 193                 return ERROR_FAIL;
 194
 195         /* Reset the SWD bus */
 196         if (kitprog_swd_seq(SEQUENCE_LINE_RESET) != ERROR_OK)
 197                 return ERROR_FAIL;
 198
 199         if (kitprog_init_acquire_psoc) {
 200                 /* Try to acquire any device that will respond */
 201                 retval = kitprog_generic_acquire();
 202                 if (retval != ERROR_OK) {
 203                         LOG_ERROR("No PSoC devices found");
 204                         return retval;
 205                 }
 206         }
 207
 208         /* Allocate packet buffers and queues */
 209         kitprog_handle->packet_size = SWD_MAX_BUFFER_LENGTH;
 210         kitprog_handle->packet_buffer = malloc(SWD_MAX_BUFFER_LENGTH);
 211         if (!kitprog_handle->packet_buffer) {
 212                 LOG_ERROR("Failed to allocate memory for the packet buffer");
 213                 return ERROR_FAIL;
 214         }
 215
 216         pending_queue_len = SWD_MAX_BUFFER_LENGTH / 5;
 217         pending_transfers = malloc(pending_queue_len * sizeof(*pending_transfers));
 218         if (!pending_transfers) {
 219                 LOG_ERROR("Failed to allocate memory for the SWD transfer queue");
 220                 return ERROR_FAIL;
 221         }
 222
 223         return ERROR_OK;
 224 }
 225
 226 static int kitprog_quit(void)
 227 {
 228         kitprog_usb_close();
 229
 230         free(kitprog_handle->packet_buffer);
 231         free(kitprog_handle->serial);
 232         free(kitprog_handle);
 233         free(pending_transfers);
 234
 235         return ERROR_OK;
 236 }
 237
 238 /*************** kitprog usb functions *********************/
 239
 240 static int kitprog_get_usb_serial(void)
 241 {
 242         int retval;
 243         const uint8_t str_index = 128; /* This seems to be a constant */
 244         char desc_string[256+1]; /* Max size of string descriptor */
 245
 246         retval = libusb_get_string_descriptor_ascii(kitprog_handle->usb_handle,
 247                         str_index, (unsigned char *)desc_string, sizeof(desc_string)-1);
 248         if (retval < 0) {
 249                 LOG_ERROR("libusb_get_string_descriptor_ascii() failed with %d", retval);
 250                 return ERROR_FAIL;
 251         }
 252
 253         /* Null terminate descriptor string */
 254         desc_string[retval] = '\0';
 255
 256         /* Allocate memory for the serial number */
 257         kitprog_handle->serial = calloc(retval + 1, sizeof(char));
 258         if (!kitprog_handle->serial) {
 259                 LOG_ERROR("Failed to allocate memory for the serial number");
 260                 return ERROR_FAIL;
 261         }
 262
 263         /* Store the serial number */
 264         strncpy(kitprog_handle->serial, desc_string, retval + 1);
 265
 266         return ERROR_OK;
 267 }
 268
 269 static int kitprog_usb_open(void)
 270 {
 271         const uint16_t vids[] = { VID, 0 };
 272         const uint16_t pids[] = { PID, 0 };
 273
 274         if (jtag_libusb_open(vids, pids, adapter_get_required_serial(),
 275                         &kitprog_handle->usb_handle, NULL) != ERROR_OK) {
 276                 LOG_ERROR("Failed to open or find the device");
 277                 return ERROR_FAIL;
 278         }
 279
 280         /* Get the serial number for the device */
 281         if (kitprog_get_usb_serial() != ERROR_OK)
 282                 LOG_WARNING("Failed to get KitProg serial number");
 283
 284         /* Convert the ASCII serial number into a (wchar_t *) */
 285         size_t len = strlen(kitprog_handle->serial);
 286         wchar_t *hid_serial = calloc(len + 1, sizeof(wchar_t));
 287         if (!hid_serial) {
 288                 LOG_ERROR("Failed to allocate memory for the serial number");
 289                 return ERROR_FAIL;
 290         }
 291         if (mbstowcs(hid_serial, kitprog_handle->serial, len + 1) == (size_t)-1) {
 292                 free(hid_serial);
 293                 LOG_ERROR("Failed to convert serial number");
 294                 return ERROR_FAIL;
 295         }
 296
 297         /* Use HID for the KitBridge interface */
 298         kitprog_handle->hid_handle = hid_open(VID, PID, hid_serial);
 299         free(hid_serial);
 300         if (!kitprog_handle->hid_handle) {
 301                 LOG_ERROR("Failed to open KitBridge (HID) interface");
 302                 return ERROR_FAIL;
 303         }
 304
 305         /* Claim the KitProg Programmer (bulk transfer) interface */
 306         if (libusb_claim_interface(kitprog_handle->usb_handle, 1) != ERROR_OK) {
 307                 LOG_ERROR("Failed to claim KitProg Programmer (bulk transfer) interface");
 308                 return ERROR_FAIL;
 309         }
 310
 311         return ERROR_OK;
 312 }
 313
 314 static void kitprog_usb_close(void)
 315 {
 316         if (kitprog_handle->hid_handle) {
 317                 hid_close(kitprog_handle->hid_handle);
 318                 hid_exit();
 319         }
 320
 321         jtag_libusb_close(kitprog_handle->usb_handle);
 322 }
 323
 324 /*************** kitprog lowlevel functions *********************/
 325
 326 static int kitprog_hid_command(uint8_t *command, size_t command_length,
 327                 uint8_t *data, size_t data_length)
 328 {
 329         int ret;
 330
 331         ret = hid_write(kitprog_handle->hid_handle, command, command_length);
 332         if (ret < 0) {
 333                 LOG_DEBUG("HID write returned %i", ret);
 334                 return ERROR_FAIL;
 335         }
 336
 337         ret = hid_read(kitprog_handle->hid_handle, data, data_length);
 338         if (ret < 0) {
 339                 LOG_DEBUG("HID read returned %i", ret);
 340                 return ERROR_FAIL;
 341         }
 342
 343         return ERROR_OK;
 344 }
 345
 346 static int kitprog_get_version(void)
 347 {
 348         int ret;
 349
 350         unsigned char command[3] = {HID_TYPE_START | HID_TYPE_WRITE, 0x00, HID_COMMAND_VERSION};
 351         unsigned char data[64];
 352
 353         ret = kitprog_hid_command(command, sizeof(command), data, sizeof(data));
 354         if (ret != ERROR_OK)
 355                 return ret;
 356
 357         kitprog_handle->hardware_version = data[1];
 358         kitprog_handle->minor_version = data[2];
 359         kitprog_handle->major_version = data[3];
 360
 361         return ERROR_OK;
 362 }
 363
 364 static int kitprog_get_millivolts(void)
 365 {
 366         int ret;
 367
 368         unsigned char command[3] = {HID_TYPE_START | HID_TYPE_READ, 0x00, HID_COMMAND_POWER};
 369         unsigned char data[64];
 370
 371         ret = kitprog_hid_command(command, sizeof(command), data, sizeof(data));
 372         if (ret != ERROR_OK)
 373                 return ret;
 374
 375         kitprog_handle->millivolts = (data[4] << 8) | data[3];
 376
 377         return ERROR_OK;
 378 }
 379
 380 static int kitprog_get_info(void)
 381 {
 382         /* Get the device version information */
 383         if (kitprog_get_version() == ERROR_OK) {
 384                 LOG_INFO("KitProg v%u.%02u",
 385                         kitprog_handle->major_version, kitprog_handle->minor_version);
 386                 LOG_INFO("Hardware version: %u",
 387                         kitprog_handle->hardware_version);
 388         } else {
 389                 LOG_ERROR("Failed to get KitProg version");
 390                 return ERROR_FAIL;
 391         }
 392
 393         /* Get the current reported target voltage */
 394         if (kitprog_get_millivolts() == ERROR_OK) {
 395                 LOG_INFO("VTARG = %u.%03u V",
 396                         kitprog_handle->millivolts / 1000, kitprog_handle->millivolts % 1000);
 397         } else {
 398                 LOG_ERROR("Failed to get target voltage");
 399                 return ERROR_FAIL;
 400         }
 401
 402         return ERROR_OK;
 403 }
 404
 405 static int kitprog_set_protocol(uint8_t protocol)
 406 {
 407         int transferred;
 408         char status = PROGRAMMER_NOK_NACK;
 409
 410         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 411                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 412                 CONTROL_TYPE_WRITE,
 413                 (CONTROL_MODE_SET_PROGRAMMER_PROTOCOL << 8) | CONTROL_COMMAND_PROGRAM,
 414                 protocol, &status, 1, 0);
 415
 416         if (transferred == 0) {
 417                 LOG_DEBUG("Zero bytes transferred");
 418                 return ERROR_FAIL;
 419         }
 420
 421         if (status != PROGRAMMER_OK_ACK) {
 422                 LOG_DEBUG("Programmer did not respond OK");
 423                 return ERROR_FAIL;
 424         }
 425
 426         return ERROR_OK;
 427 }
 428
 429 static int kitprog_get_status(void)
 430 {
 431         int transferred = 0;
 432         char status = PROGRAMMER_NOK_NACK;
 433
 434         /* Try a maximum of three times */
 435         for (int i = 0; (i < 3) && (transferred == 0); i++) {
 436                 transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 437                         LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 438                         CONTROL_TYPE_READ,
 439                         (CONTROL_MODE_POLL_PROGRAMMER_STATUS << 8) | CONTROL_COMMAND_PROGRAM,
 440                         0, &status, 1, 0);
 441                 jtag_sleep(1000);
 442         }
 443
 444         if (transferred == 0) {
 445                 LOG_DEBUG("Zero bytes transferred");
 446                 return ERROR_FAIL;
 447         }
 448
 449         if (status != PROGRAMMER_OK_ACK) {
 450                 LOG_DEBUG("Programmer did not respond OK");
 451                 return ERROR_FAIL;
 452         }
 453
 454         return ERROR_OK;
 455 }
 456
 457 static int kitprog_set_unknown(void)
 458 {
 459         int transferred;
 460         char status = PROGRAMMER_NOK_NACK;
 461
 462         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 463                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 464                 CONTROL_TYPE_WRITE,
 465                 (0x03 << 8) | 0x04,
 466                 0, &status, 1, 0);
 467
 468         if (transferred == 0) {
 469                 LOG_DEBUG("Zero bytes transferred");
 470                 return ERROR_FAIL;
 471         }
 472
 473         if (status != PROGRAMMER_OK_ACK) {
 474                 LOG_DEBUG("Programmer did not respond OK");
 475                 return ERROR_FAIL;
 476         }
 477
 478         return ERROR_OK;
 479 }
 480
 481 static int kitprog_acquire_psoc(uint8_t psoc_type, uint8_t acquire_mode,
 482                 uint8_t max_attempts)
 483 {
 484         int transferred;
 485         char status = PROGRAMMER_NOK_NACK;
 486
 487         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 488                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 489                 CONTROL_TYPE_WRITE,
 490                 (CONTROL_MODE_ACQUIRE_SWD_TARGET << 8) | CONTROL_COMMAND_PROGRAM,
 491                 (max_attempts << 8) | (acquire_mode << 4) | psoc_type, &status, 1, 0);
 492
 493         if (transferred == 0) {
 494                 LOG_DEBUG("Zero bytes transferred");
 495                 return ERROR_FAIL;
 496         }
 497
 498         if (status != PROGRAMMER_OK_ACK) {
 499                 LOG_DEBUG("Programmer did not respond OK");
 500                 return ERROR_FAIL;
 501         }
 502
 503         return ERROR_OK;
 504 }
 505
 506 static int kitprog_reset_target(void)
 507 {
 508         int transferred;
 509         char status = PROGRAMMER_NOK_NACK;
 510
 511         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 512                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 513                 CONTROL_TYPE_WRITE,
 514                 (CONTROL_MODE_RESET_TARGET << 8) | CONTROL_COMMAND_PROGRAM,
 515                 0, &status, 1, 0);
 516
 517         if (transferred == 0) {
 518                 LOG_DEBUG("Zero bytes transferred");
 519                 return ERROR_FAIL;
 520         }
 521
 522         if (status != PROGRAMMER_OK_ACK) {
 523                 LOG_DEBUG("Programmer did not respond OK");
 524                 return ERROR_FAIL;
 525         }
 526
 527         return ERROR_OK;
 528 }
 529
 530 static int kitprog_swd_sync(void)
 531 {
 532         int transferred;
 533         char status = PROGRAMMER_NOK_NACK;
 534
 535         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 536                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 537                 CONTROL_TYPE_WRITE,
 538                 (CONTROL_MODE_SYNCHRONIZE_TRANSFER << 8) | CONTROL_COMMAND_PROGRAM,
 539                 0, &status, 1, 0);
 540
 541         if (transferred == 0) {
 542                 LOG_DEBUG("Zero bytes transferred");
 543                 return ERROR_FAIL;
 544         }
 545
 546         if (status != PROGRAMMER_OK_ACK) {
 547                 LOG_DEBUG("Programmer did not respond OK");
 548                 return ERROR_FAIL;
 549         }
 550
 551         return ERROR_OK;
 552 }
 553
 554 static int kitprog_swd_seq(uint8_t seq_type)
 555 {
 556         int transferred;
 557         char status = PROGRAMMER_NOK_NACK;
 558
 559         transferred = jtag_libusb_control_transfer(kitprog_handle->usb_handle,
 560                 LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE,
 561                 CONTROL_TYPE_WRITE,
 562                 (CONTROL_MODE_SEND_SWD_SEQUENCE << 8) | CONTROL_COMMAND_PROGRAM,
 563                 seq_type, &status, 1, 0);
 564
 565         if (transferred == 0) {
 566                 LOG_DEBUG("Zero bytes transferred");
 567                 return ERROR_FAIL;
 568         }
 569
 570         if (status != PROGRAMMER_OK_ACK) {
 571                 LOG_DEBUG("Programmer did not respond OK");
 572                 return ERROR_FAIL;
 573         }
 574
 575         return ERROR_OK;
 576 }
 577
 578 static int kitprog_generic_acquire(void)
 579 {
 580         const uint8_t devices[] = {DEVICE_PSOC4, DEVICE_PSOC3, DEVICE_PSOC5};
 581
 582         int retval;
 583         int acquire_count = 0;
 584
 585         /* Due to the way the SWD port is shared between the Test Controller (TC)
 586          * and the Cortex-M3 DAP on the PSoC 5LP, the TC is the default SWD target
 587          * after power is applied. To access the DAP, the PSoC 5LP requires at least
 588          * one acquisition sequence to be run (which switches the SWD mux from the
 589          * TC to the DAP). However, after the mux is switched, the Cortex-M3 will be
 590          * held in reset until a series of registers are written to (see section 5.2
 591          * of the PSoC 5LP Device Programming Specifications for details).
 592          *
 593          * Instead of writing the registers in this function, we just do what the
 594          * Cypress tools do and run the acquisition sequence a second time. This
 595          * will take the Cortex-M3 out of reset and enable debugging.
 596          */
 597         for (int i = 0; i < 2; i++) {
 598                 for (uint8_t j = 0; j < sizeof(devices) && acquire_count == i; j++) {
 599                         retval = kitprog_acquire_psoc(devices[j], ACQUIRE_MODE_RESET, 3);
 600                         if (retval != ERROR_OK) {
 601                                 LOG_DEBUG("Acquisition function failed for device 0x%02x.", devices[j]);
 602                                 return retval;
 603                         }
 604
 605                         if (kitprog_get_status() == ERROR_OK)
 606                                 acquire_count++;
 607                 }
 608
 609                 jtag_sleep(10);
 610         }
 611
 612         if (acquire_count < 2)
 613                 return ERROR_FAIL;
 614
 615         return ERROR_OK;
 616 }
 617
 618 /*************** swd wrapper functions *********************/
 619
 620 static int kitprog_swd_init(void)
 621 {
 622         return ERROR_OK;
 623 }
 624
 625 static void kitprog_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk)
 626 {
 627         assert(!(cmd & SWD_CMD_RNW));
 628         kitprog_swd_queue_cmd(cmd, NULL, value);
 629 }
 630
 631 static void kitprog_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk)
 632 {
 633         assert(cmd & SWD_CMD_RNW);
 634         kitprog_swd_queue_cmd(cmd, value, 0);
 635 }
 636
 637 /*************** swd lowlevel functions ********************/
 638
 639 static int kitprog_swd_switch_seq(enum swd_special_seq seq)
 640 {
 641         switch (seq) {
 642                 case JTAG_TO_SWD:
 643                         if (kitprog_handle->supports_jtag_to_swd) {
 644                                 LOG_DEBUG("JTAG to SWD");
 645                                 if (kitprog_swd_seq(SEQUENCE_JTAG_TO_SWD) != ERROR_OK)
 646                                         return ERROR_FAIL;
 647                                 break;
 648                         } else {
 649                                 LOG_DEBUG("JTAG to SWD not supported");
 650                                 /* Fall through to fix target reset issue */
 651                         }
 652                         /* fallthrough */
 653                 case LINE_RESET:
 654                         LOG_DEBUG("SWD line reset");
 655                         if (kitprog_swd_seq(SEQUENCE_LINE_RESET) != ERROR_OK)
 656                                 return ERROR_FAIL;
 657                         break;
 658                 default:
 659                         LOG_ERROR("Sequence %d not supported.", seq);
 660                         return ERROR_FAIL;
 661         }
 662
 663         return ERROR_OK;
 664 }
 665
 666 static int kitprog_swd_run_queue(void)
 667 {
 668         int ret;
 669
 670         size_t read_count = 0;
 671         size_t read_index = 0;
 672         size_t write_count = 0;
 673         uint8_t *buffer = kitprog_handle->packet_buffer;
 674
 675         do {
 676                 LOG_DEBUG_IO("Executing %d queued transactions", pending_transfer_count);
 677
 678                 if (queued_retval != ERROR_OK) {
 679                         LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
 680                         break;
 681                 }
 682
 683                 if (!pending_transfer_count)
 684                         break;
 685
 686                 for (int i = 0; i < pending_transfer_count; i++) {
 687                         uint8_t cmd = pending_transfers[i].cmd;
 688                         uint32_t data = pending_transfers[i].data;
 689
 690                         /* When proper WAIT handling is implemented in the
 691                          * common SWD framework, this kludge can be
 692                          * removed. However, this might lead to minor
 693                          * performance degradation as the adapter wouldn't be
 694                          * able to automatically retry anything (because ARM
 695                          * has forgotten to implement sticky error flags
 696                          * clearing). See also comments regarding
 697                          * cmsis_dap_cmd_DAP_TFER_Configure() and
 698                          * cmsis_dap_cmd_DAP_SWD_Configure() in
 699                          * cmsis_dap_init().
 700                          */
 701                         if (!(cmd & SWD_CMD_RNW) &&
 702                                 !(cmd & SWD_CMD_APNDP) &&
 703                                 (cmd & SWD_CMD_A32) >> 1 == DP_CTRL_STAT &&
 704                                 (data & CORUNDETECT)) {
 705                                 LOG_DEBUG("refusing to enable sticky overrun detection");
 706                                 data &= ~CORUNDETECT;
 707                         }
 708
 709                         LOG_DEBUG_IO("%s %s reg %x %"PRIx32,
 710                                         cmd & SWD_CMD_APNDP ? "AP" : "DP",
 711                                         cmd & SWD_CMD_RNW ? "read" : "write",
 712                                   (cmd & SWD_CMD_A32) >> 1, data);
 713
 714                         buffer[write_count++] = (cmd | SWD_CMD_START | SWD_CMD_PARK) & ~SWD_CMD_STOP;
 715                         read_count++;
 716                         if (!(cmd & SWD_CMD_RNW)) {
 717                                 buffer[write_count++] = (data) & 0xff;
 718                                 buffer[write_count++] = (data >> 8) & 0xff;
 719                                 buffer[write_count++] = (data >> 16) & 0xff;
 720                                 buffer[write_count++] = (data >> 24) & 0xff;
 721                         } else {
 722                                 read_count += 4;
 723                         }
 724                 }
 725
 726                 if (jtag_libusb_bulk_write(kitprog_handle->usb_handle,
 727                                            BULK_EP_OUT, (char *)buffer,
 728                                            write_count, 0, &ret)) {
 729                         LOG_ERROR("Bulk write failed");
 730                         queued_retval = ERROR_FAIL;
 731                         break;
 732                 } else {
 733                         queued_retval = ERROR_OK;
 734                 }
 735
 736                 /* KitProg firmware does not send a zero length packet
 737                  * after the bulk-in transmission of a length divisible by bulk packet
 738                  * size (64 bytes) as required by the USB specification.
 739                  * Therefore libusb would wait for continuation of transmission.
 740                  * Workaround: Limit bulk read size to expected number of bytes
 741                  * for problematic transfer sizes. Otherwise use the maximum buffer
 742                  * size here because the KitProg sometimes doesn't like bulk reads
 743                  * of fewer than 62 bytes. (?!?!)
 744                  */
 745                 size_t read_count_workaround = SWD_MAX_BUFFER_LENGTH;
 746                 if (read_count % 64 == 0)
 747                         read_count_workaround = read_count;
 748
 749                 if (jtag_libusb_bulk_read(kitprog_handle->usb_handle,
 750                                 BULK_EP_IN | LIBUSB_ENDPOINT_IN, (char *)buffer,
 751                                 read_count_workaround, 1000, &ret)) {
 752                         LOG_ERROR("Bulk read failed");
 753                         queued_retval = ERROR_FAIL;
 754                         break;
 755                 } else {
 756                         /* Handle garbage data by offsetting the initial read index */
 757                         if ((unsigned int)ret > read_count)
 758                                 read_index = ret - read_count;
 759                         queued_retval = ERROR_OK;
 760                 }
 761
 762                 for (int i = 0; i < pending_transfer_count; i++) {
 763                         if (pending_transfers[i].cmd & SWD_CMD_RNW) {
 764                                 uint32_t data = le_to_h_u32(&buffer[read_index]);
 765
 766                                 LOG_DEBUG_IO("Read result: %"PRIx32, data);
 767
 768                                 if (pending_transfers[i].buffer)
 769                                         *(uint32_t *)pending_transfers[i].buffer = data;
 770
 771                                 read_index += 4;
 772                         }
 773
 774                         uint8_t ack = buffer[read_index] & 0x07;
 775                         if (ack != SWD_ACK_OK || (buffer[read_index] & 0x08)) {
 776                                 LOG_DEBUG("SWD ack not OK: %d %s", i,
 777                                           ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
 778                                 queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
 779                                 break;
 780                         }
 781                         read_index++;
 782                 }
 783         } while (0);
 784
 785         pending_transfer_count = 0;
 786         int retval = queued_retval;
 787         queued_retval = ERROR_OK;
 788
 789         return retval;
 790 }
 791
 792 static void kitprog_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data)
 793 {
 794         if (pending_transfer_count == pending_queue_len) {
 795                 /* Not enough room in the queue. Run the queue. */
 796                 queued_retval = kitprog_swd_run_queue();
 797         }
 798
 799         if (queued_retval != ERROR_OK)
 800                 return;
 801
 802         pending_transfers[pending_transfer_count].data = data;
 803         pending_transfers[pending_transfer_count].cmd = cmd;
 804         if (cmd & SWD_CMD_RNW) {
 805                 /* Queue a read transaction */
 806                 pending_transfers[pending_transfer_count].buffer = dst;
 807         }
 808         pending_transfer_count++;
 809 }
 810
 811 /*************** jtag lowlevel functions ********************/
 812
 813 static int kitprog_reset(int trst, int srst)
 814 {
 815         int retval = ERROR_OK;
 816
 817         if (trst == 1) {
 818                 LOG_ERROR("KitProg: Interface has no TRST");
 819                 return ERROR_FAIL;
 820         }
 821
 822         if (srst == 1) {
 823                 retval = kitprog_reset_target();
 824                 /* Since the previous command also disables SWCLK output, we need to send an
 825                  * SWD bus reset command to re-enable it. For some reason, running
 826                  * kitprog_swd_seq() immediately after kitprog_reset_target() won't
 827                  * actually fix this. Instead, kitprog_swd_seq() will be run once OpenOCD
 828                  * tries to send a JTAG-to-SWD sequence, which should happen during
 829                  * swd_check_reconnect (see the JTAG_TO_SWD case in kitprog_swd_switch_seq).
 830                  */
 831         }
 832
 833         if (retval != ERROR_OK)
 834                 LOG_ERROR("KitProg: Interface reset failed");
 835         return retval;
 836 }
 837
 838 COMMAND_HANDLER(kitprog_handle_info_command)
 839 {
 840         int retval = kitprog_get_info();
 841
 842         return retval;
 843 }
 844
 845
 846 COMMAND_HANDLER(kitprog_handle_acquire_psoc_command)
 847 {
 848         int retval = kitprog_generic_acquire();
 849
 850         return retval;
 851 }
 852
 853 COMMAND_HANDLER(kitprog_handle_init_acquire_psoc_command)
 854 {
 855         kitprog_init_acquire_psoc = true;
 856
 857         return ERROR_OK;
 858 }
 859
 860 static const struct command_registration kitprog_subcommand_handlers[] = {
 861         {
 862                 .name = "info",
 863                 .handler = &kitprog_handle_info_command,
 864                 .mode = COMMAND_EXEC,
 865                 .usage = "",
 866                 .help = "show KitProg info",
 867         },
 868         {
 869                 .name = "acquire_psoc",
 870                 .handler = &kitprog_handle_acquire_psoc_command,
 871                 .mode = COMMAND_EXEC,
 872                 .usage = "",
 873                 .help = "try to acquire a PSoC",
 874         },
 875         COMMAND_REGISTRATION_DONE
 876 };
 877
 878 static const struct command_registration kitprog_command_handlers[] = {
 879         {
 880                 .name = "kitprog",
 881                 .mode = COMMAND_ANY,
 882                 .help = "perform KitProg management",
 883                 .usage = "<cmd>",
 884                 .chain = kitprog_subcommand_handlers,
 885         },
 886         {
 887                 .name = "kitprog_init_acquire_psoc",
 888                 .handler = &kitprog_handle_init_acquire_psoc_command,
 889                 .mode = COMMAND_CONFIG,
 890                 .help = "try to acquire a PSoC during init",
 891                 .usage = "",
 892         },
 893         COMMAND_REGISTRATION_DONE
 894 };
 895
 896 static const struct swd_driver kitprog_swd = {
 897         .init = kitprog_swd_init,
 898         .switch_seq = kitprog_swd_switch_seq,
 899         .read_reg = kitprog_swd_read_reg,
 900         .write_reg = kitprog_swd_write_reg,
 901         .run = kitprog_swd_run_queue,
 902 };
 903
 904 static const char * const kitprog_transports[] = { "swd", NULL };
 905
 906 struct adapter_driver kitprog_adapter_driver = {
 907         .name = "kitprog",
 908         .transports = kitprog_transports,
 909         .commands = kitprog_command_handlers,
 910
 911         .init = kitprog_init,
 912         .quit = kitprog_quit,
 913         .reset = kitprog_reset,
 914
 915         .swd_ops = &kitprog_swd,
 916 };