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