angie.c

Go to the documentation of this file.

// SPDX-License-Identifier: GPL-2.0-or-later
/***************************************************************************
    File : angie.c                                                          *
    Contents : OpenOCD driver code for NanoXplore USB-JTAG ANGIE            *
    adapter hardware.                                                       *
    Based on openULINK driver code by: Martin Schmoelzer.                   *
    Copyright 2023, Ahmed Errached BOUDJELIDA, NanoXplore SAS.              *
    <aboudjelida@nanoxplore.com>                                            *
    <ahmederrachedbjld@gmail.com>                                           *
 ***************************************************************************/
 
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
 
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "helper/system.h"
#include <helper/types.h>
#include <jtag/interface.h>
#include <jtag/commands.h>
#include <target/image.h>
#include <libusb.h>
#include "libusb_helper.h"
#include "angie/include/msgtypes.h"
 
#define ANGIE_VID               0x584e
 
#define ANGIE_PID 0x414F
#define ANGIE_PID_2 0x424e
#define ANGIE_PID_3 0x4255
#define ANGIE_PID_4 0x4355
#define ANGIE_PID_5 0x4a55
 
#define CPUCS_REG               0xE600
 
#define REQUEST_FIRMWARE_LOAD   0xA0
 
#define CPU_RESET               0x01
 
#define CPU_START               0x00
 
#define FIRMWARE_ADDR           0x0000
 
#define USB_INTERFACE           0
 
#define ANGIE_RENUMERATION_DELAY_US 1500000
 
#define ANGIE_FIRMWARE_FILE     PKGDATADIR "/angie/angie_firmware.bin"
 
#define ANGIE_BITSTREAM_FILE        PKGDATADIR "/angie/angie_bitstream.bit"
 
#define SECTION_BUFFERSIZE      16384
 
#define SPLIT_SCAN_THRESHOLD    10
 
enum angie_type {
    ANGIE,
};
 
enum angie_payload_direction {
    PAYLOAD_DIRECTION_OUT,
    PAYLOAD_DIRECTION_IN
};
 
enum angie_delay_type {
    DELAY_CLOCK_TCK,
    DELAY_CLOCK_TMS,
    DELAY_SCAN_IN,
    DELAY_SCAN_OUT,
    DELAY_SCAN_IO
};
 
struct angie_cmd {
    uint8_t id;         
    uint8_t *payload_out;       
    uint8_t payload_out_size;   
    uint8_t *payload_in_start;  
    uint8_t *payload_in;        
    uint8_t payload_in_size;    
    bool needs_postprocessing;
 
    bool free_payload_in_start;
 
    struct jtag_command *cmd_origin;
 
    struct angie_cmd *next;     
};
 
struct angie {
    struct libusb_context *libusb_ctx;
    struct libusb_device_handle *usb_device_handle;
    enum angie_type type;
 
    unsigned int ep_in;     
    unsigned int ep_out;        
    /* delay value for "SLOW_CLOCK commands" in [0:255] range in units of 4 us;
        -1 means no need for delay */
    int delay_scan_in;  
    int delay_scan_out; 
    int delay_scan_io;  
    int delay_clock_tck;    
    int delay_clock_tms;    
    int commands_in_queue;      
    struct angie_cmd *queue_start;  
    struct angie_cmd *queue_end;    
};
 
/**************************** Function Prototypes *****************************/
 
/* USB helper functions */
static int angie_usb_open(struct angie *device);
static int angie_usb_close(struct angie *device);
 
/* ANGIE MCU (Cypress EZ-USB) specific functions */
static int angie_cpu_reset(struct angie *device, char reset_bit);
static int angie_load_firmware_and_renumerate(struct angie *device, const char *filename,
        uint32_t delay_us);
static int angie_load_firmware(struct angie *device, const char *filename);
static int angie_load_bitstream(struct angie *device, const char *filename);
static int angie_i2c_write(struct angie *device, uint8_t *i2c_data, uint8_t i2c_data_size);
static int angie_io_extender_config(struct angie *device, uint8_t i2c_adr, uint8_t cfg_value);
static int angie_write_firmware_section(struct angie *device,
        struct image *firmware_image, int section_index);
 
/* Generic helper functions */
static void angie_dump_signal_states(uint8_t input_signals, uint8_t output_signals);
 
/* ANGIE command generation helper functions */
static int angie_allocate_payload(struct angie_cmd *angie_cmd, int size,
        enum angie_payload_direction direction);
 
/* ANGIE command queue helper functions */
static int angie_get_queue_size(struct angie *device,
        enum angie_payload_direction direction);
static void angie_clear_queue(struct angie *device);
static int angie_append_queue(struct angie *device, struct angie_cmd *angie_cmd);
static int angie_execute_queued_commands(struct angie *device, int timeout_ms);
 
static void angie_dump_queue(struct angie *device);
 
static int angie_append_scan_cmd(struct angie *device,
        enum scan_type scan_type,
        int scan_size_bits,
        uint8_t *tdi,
        uint8_t *tdo_start,
        uint8_t *tdo,
        uint8_t tms_count_start,
        uint8_t tms_sequence_start,
        uint8_t tms_count_end,
        uint8_t tms_sequence_end,
        struct jtag_command *origin,
        bool postprocess);
static int angie_append_clock_tms_cmd(struct angie *device, uint8_t count,
        uint8_t sequence);
static int angie_append_clock_tck_cmd(struct angie *device, uint16_t count);
static int angie_append_get_signals_cmd(struct angie *device);
static int angie_append_set_signals_cmd(struct angie *device, uint8_t low,
        uint8_t high);
static int angie_append_sleep_cmd(struct angie *device, uint32_t us);
static int angie_append_configure_tck_cmd(struct angie *device,
        int delay_scan_in,
        int delay_scan_out,
        int delay_scan_io,
        int delay_tck,
        int delay_tms);
static int angie_append_test_cmd(struct angie *device);
 
/* ANGIE TCK frequency helper functions */
static int angie_calculate_delay(enum angie_delay_type type, long f, int *delay);
 
/* Interface between ANGIE and OpenOCD */
static void angie_set_end_state(tap_state_t endstate);
static int angie_queue_statemove(struct angie *device);
 
static int angie_queue_scan(struct angie *device, struct jtag_command *cmd);
static int angie_queue_tlr_reset(struct angie *device, struct jtag_command *cmd);
static int angie_queue_runtest(struct angie *device, struct jtag_command *cmd);
static int angie_queue_pathmove(struct angie *device, struct jtag_command *cmd);
static int angie_queue_sleep(struct angie *device, struct jtag_command *cmd);
static int angie_queue_stableclocks(struct angie *device, struct jtag_command *cmd);
 
static int angie_post_process_scan(struct angie_cmd *angie_cmd);
static int angie_post_process_queue(struct angie *device);
 
/* adapter driver functions */
static int angie_execute_queue(struct jtag_command *cmd_queue);
static int angie_khz(int khz, int *jtag_speed);
static int angie_speed(int speed);
static int angie_speed_div(int speed, int *khz);
static int angie_init(void);
static int angie_quit(void);
static int angie_reset(int trst, int srst);
 
/****************************** Global Variables ******************************/
 
static struct angie *angie_handle;
 
/**************************** USB helper functions ****************************/
 
static int angie_usb_open(struct angie *device)
{
    struct libusb_device_handle *usb_device_handle;
    const uint16_t vids[] = {ANGIE_VID, ANGIE_VID, ANGIE_VID, ANGIE_VID, ANGIE_VID, 0};
    const uint16_t pids[] = {ANGIE_PID, ANGIE_PID_2, ANGIE_PID_3, ANGIE_PID_4, ANGIE_PID_5, 0};
 
    int ret = jtag_libusb_open(vids, pids, NULL, &usb_device_handle, NULL);
 
    if (ret != ERROR_OK) {
        LOG_ERROR("Could not find and open ANGIE");
        return ret;
    }
 
    device->usb_device_handle = usb_device_handle;
    device->type = ANGIE;
 
    return ERROR_OK;
}
 
static int angie_usb_close(struct angie *device)
{
    if (device->usb_device_handle) {
        if (libusb_release_interface(device->usb_device_handle, 0) != 0) {
            LOG_ERROR("Could not release interface 0");
            return ERROR_FAIL;
        }
 
        jtag_libusb_close(device->usb_device_handle);
        device->usb_device_handle = NULL;
    }
    return ERROR_OK;
}
 
/******************* ANGIE CPU (EZ-USB) specific functions ********************/
 
static int angie_cpu_reset(struct angie *device, char reset_bit)
{
    return jtag_libusb_control_transfer(device->usb_device_handle,
            (LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE),
            REQUEST_FIRMWARE_LOAD, CPUCS_REG, 0, &reset_bit, 1, LIBUSB_TIMEOUT_MS, NULL);
}
 
static int angie_load_firmware_and_renumerate(struct angie *device,
    const char *filename, uint32_t delay_us)
{
    int ret;
 
    /* Basic process: After downloading the firmware, the ANGIE will disconnect
     * itself and re-connect after a short amount of time so we have to close
     * the handle and re-enumerate USB devices */
 
    ret = angie_load_firmware(device, filename);
    if (ret != ERROR_OK)
        return ret;
 
    ret = angie_usb_close(device);
    if (ret != ERROR_OK)
        return ret;
 
    usleep(delay_us);
 
    ret = angie_usb_open(device);
    if (ret != ERROR_OK)
        return ret;
 
    ret = libusb_claim_interface(angie_handle->usb_device_handle, 0);
    if (ret != LIBUSB_SUCCESS)
        return ERROR_FAIL;
 
    return ERROR_OK;
}
 
static int angie_load_firmware(struct angie *device, const char *filename)
{
    struct image angie_firmware_image;
    int ret;
 
    ret = angie_cpu_reset(device, CPU_RESET);
    if (ret != ERROR_OK) {
        LOG_ERROR("Could not halt ANGIE CPU");
        return ret;
    }
 
    angie_firmware_image.base_address = 0;
    angie_firmware_image.base_address_set = false;
 
    ret = image_open(&angie_firmware_image, filename, "bin");
    if (ret != ERROR_OK) {
        LOG_ERROR("Could not load firmware image");
        return ret;
    }
 
    /* Download all sections in the image to ANGIE */
    for (unsigned int i = 0; i < angie_firmware_image.num_sections; i++) {
        ret = angie_write_firmware_section(device, &angie_firmware_image, i);
        if (ret != ERROR_OK) {
            LOG_ERROR("Could not write firmware section");
            return ret;
        }
    }
 
    image_close(&angie_firmware_image);
 
    ret = angie_cpu_reset(device, CPU_START);
    if (ret != ERROR_OK) {
        LOG_ERROR("Could not restart ANGIE CPU");
        return ret;
    }
 
    return ERROR_OK;
}
 
static int angie_load_bitstream(struct angie *device, const char *filename)
{
    int ret, transferred;
    const char *bitstream_file_path = filename;
    FILE *bitstream_file = NULL;
    char *bitstream_data = NULL;
    size_t bitstream_size = 0;
    uint8_t gpifcnt[4];
 
    /* Open the bitstream file */
    bitstream_file = fopen(bitstream_file_path, "rb");
    if (!bitstream_file) {
        LOG_ERROR("Failed to open bitstream file: %s\n", bitstream_file_path);
        return ERROR_FAIL;
    }
 
    /* Get the size of the bitstream file */
    fseek(bitstream_file, 0, SEEK_END);
    bitstream_size = ftell(bitstream_file);
    fseek(bitstream_file, 0, SEEK_SET);
 
    /* Allocate memory for the bitstream data */
    bitstream_data = malloc(bitstream_size);
    if (!bitstream_data) {
        LOG_ERROR("Failed to allocate memory for bitstream data.");
        fclose(bitstream_file);
        return ERROR_FAIL;
    }
 
    /* Read the bitstream data from the file */
    if (fread(bitstream_data, 1, bitstream_size, bitstream_file) != bitstream_size) {
        LOG_ERROR("Failed to read bitstream data.");
        free(bitstream_data);
        fclose(bitstream_file);
        return ERROR_FAIL;
    }
 
    h_u32_to_be(gpifcnt, bitstream_size);
 
    /* CFGopen */
    ret = jtag_libusb_control_transfer(device->usb_device_handle,
        0x00, 0xB0, 0, 0, (char *)gpifcnt, 4, LIBUSB_TIMEOUT_MS, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Failed opencfg");
        /* Abort if libusb sent less data than requested */
        return ERROR_FAIL;
    }
 
    /* Send the bitstream data to the microcontroller */
    int actual_length = 0;
    ret = jtag_libusb_bulk_write(device->usb_device_handle, 0x02, bitstream_data, bitstream_size, 1000, &actual_length);
    if (ret != ERROR_OK) {
        LOG_ERROR("Failed to send bitstream data: %s", libusb_strerror(ret));
        free(bitstream_data);
        fclose(bitstream_file);
        return ERROR_FAIL;
    }
 
    LOG_INFO("Bitstream sent successfully.");
 
    /* Clean up */
    free(bitstream_data);
    fclose(bitstream_file);
 
    /* CFGclose */
    transferred = 0;
    ret = jtag_libusb_control_transfer(device->usb_device_handle,
        0x00, 0xB1, 0, 0, NULL, 0, LIBUSB_TIMEOUT_MS, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Failed cfgclose");
        /* Abort if libusb sent less data than requested */
        return ERROR_FAIL;
    }
    return ERROR_OK;
}
 
static int angie_i2c_write(struct angie *device, uint8_t *i2c_data, uint8_t i2c_data_size)
{
    char i2c_data_buffer[i2c_data_size + 2];
    char buffer_received[1];
    int ret, transferred;
    i2c_data_buffer[0] = 0; // write = 0
    i2c_data_buffer[1] = i2c_data_size - 1; // i2c_data count (without address)
 
    for (uint8_t i = 0; i < i2c_data_size; i++)
        i2c_data_buffer[i + 2] = i2c_data[i];
 
    // Send i2c packet to Dev-board and configure its clock source /
    ret = jtag_libusb_bulk_write(device->usb_device_handle, 0x06, i2c_data_buffer,
                                 i2c_data_size + 2, 1000, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Error in i2c clock gen configuration : ret ERROR");
        return ret;
    }
    if (transferred != i2c_data_size + 2) {
        LOG_ERROR("Error in i2c clock gen configuration : bytes transferred");
        return ERROR_FAIL;
    }
 
    usleep(500);
 
    // Receive packet from ANGIE /
    ret = jtag_libusb_bulk_write(device->usb_device_handle, 0x88, buffer_received, 1, 1000, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Error in i2c clock gen configuration : ret ERROR");
        return ret;
    }
    return ERROR_OK;
}
 
static int angie_io_extender_config(struct angie *device, uint8_t i2c_adr, uint8_t cfg_value)
{
    uint8_t ioconfig[3] = {i2c_adr, 3, cfg_value};
    int ret = angie_i2c_write(device, ioconfig, 3);
    if (ret != ERROR_OK)
        return ret;
 
    usleep(500);
    return ret;
}
 
static int angie_write_firmware_section(struct angie *device,
    struct image *firmware_image, int section_index)
{
    int addr, bytes_remaining, chunk_size;
    uint8_t data[SECTION_BUFFERSIZE];
    uint8_t *data_ptr = data;
    uint16_t size;
    size_t size_read;
    int ret, transferred;
 
    size = (uint16_t)firmware_image->sections[section_index].size;
    addr = (uint16_t)firmware_image->sections[section_index].base_address;
 
    LOG_DEBUG("section %02i at addr 0x%04x (size 0x%04" PRIx16 ")", section_index, addr,
        size);
 
    /* Copy section contents to local buffer */
    ret = image_read_section(firmware_image, section_index, 0, size, data,
            &size_read);
 
    if (ret != ERROR_OK)
        return ret;
    if (size_read != size)
        return ERROR_FAIL;
 
    bytes_remaining = size;
 
    /* Send section data in chunks of up to 64 bytes to ANGIE */
    while (bytes_remaining > 0) {
        if (bytes_remaining > 64)
            chunk_size = 64;
        else
            chunk_size = bytes_remaining;
 
        ret = jtag_libusb_control_transfer(device->usb_device_handle,
                (LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE),
                REQUEST_FIRMWARE_LOAD, addr, FIRMWARE_ADDR, (char *)data_ptr,
                chunk_size, LIBUSB_TIMEOUT_MS, &transferred);
 
        if (ret != ERROR_OK)
            return ret;
 
        if (transferred != chunk_size) {
            /* Abort if libusb sent less data than requested */
            return ERROR_FAIL;
        }
 
        bytes_remaining -= chunk_size;
        addr += chunk_size;
        data_ptr += chunk_size;
    }
 
    return ERROR_OK;
}
 
/************************** Generic helper functions **************************/
 
static void angie_dump_signal_states(uint8_t input_signals, uint8_t output_signals)
{
    LOG_INFO("ANGIE signal states: TDI: %i, TDO: %i, TMS: %i, TCK: %i, TRST: %i "
        "SRST: %i",
        (output_signals & SIGNAL_TDI   ? 1 : 0),
        (input_signals  & SIGNAL_TDO   ? 1 : 0),
        (output_signals & SIGNAL_TMS   ? 1 : 0),
        (output_signals & SIGNAL_TCK   ? 1 : 0),
        (output_signals & SIGNAL_TRST  ? 1 : 0),
        (output_signals & SIGNAL_SRST  ? 1 : 0));
}
 
/**************** ANGIE command generation helper functions ***************/
 
static int angie_allocate_payload(struct angie_cmd *angie_cmd, int size,
    enum angie_payload_direction direction)
{
    uint8_t *payload;
 
    payload = calloc(size, sizeof(uint8_t));
 
    if (!payload) {
        LOG_ERROR("Could not allocate ANGIE command payload: out of memory");
        return ERROR_FAIL;
    }
 
    switch (direction) {
        case PAYLOAD_DIRECTION_OUT:
            if (angie_cmd->payload_out) {
                LOG_ERROR("BUG: Duplicate payload allocation for ANGIE command");
                free(payload);
                return ERROR_FAIL;
            }
            angie_cmd->payload_out = payload;
            angie_cmd->payload_out_size = size;
            break;
        case PAYLOAD_DIRECTION_IN:
            if (angie_cmd->payload_in_start) {
                LOG_ERROR("BUG: Duplicate payload allocation for ANGIE command");
                free(payload);
                return ERROR_FAIL;
            }
 
            angie_cmd->payload_in_start = payload;
            angie_cmd->payload_in = payload;
            angie_cmd->payload_in_size = size;
 
            /* By default, free payload_in_start in angie_clear_queue(). Commands
             * that do not want this behavior (e. g. split scans) must turn it off
             * separately! */
            angie_cmd->free_payload_in_start = true;
 
            break;
    }
 
    return ERROR_OK;
}
 
/****************** ANGIE command queue helper functions ******************/
 
static int angie_get_queue_size(struct angie *device,
    enum angie_payload_direction direction)
{
    struct angie_cmd *current = device->queue_start;
    int sum = 0;
 
    while (current) {
        switch (direction) {
            case PAYLOAD_DIRECTION_OUT:
                sum += current->payload_out_size + 1;   /* + 1 byte for Command ID */
                break;
            case PAYLOAD_DIRECTION_IN:
                sum += current->payload_in_size;
                break;
        }
 
        current = current->next;
    }
 
    return sum;
}
 
static void angie_clear_queue(struct angie *device)
{
    struct angie_cmd *current = device->queue_start;
    struct angie_cmd *next = NULL;
 
    while (current) {
        /* Save pointer to next element */
        next = current->next;
 
        /* Free payloads: OUT payload can be freed immediately */
        free(current->payload_out);
        current->payload_out = NULL;
 
        /* IN payload MUST be freed ONLY if no other commands use the
         * payload_in_start buffer */
        if (current->free_payload_in_start) {
            free(current->payload_in_start);
            current->payload_in_start = NULL;
            current->payload_in = NULL;
        }
 
        /* Free queue element */
        free(current);
 
        /* Proceed with next element */
        current = next;
    }
 
    device->commands_in_queue = 0;
    device->queue_start = NULL;
    device->queue_end = NULL;
}
 
static int angie_append_queue(struct angie *device, struct angie_cmd *angie_cmd)
{
    int newsize_out, newsize_in;
    int ret = ERROR_OK;
 
    newsize_out = angie_get_queue_size(device, PAYLOAD_DIRECTION_OUT) + 1
        + angie_cmd->payload_out_size;
 
    newsize_in = angie_get_queue_size(device, PAYLOAD_DIRECTION_IN)
        + angie_cmd->payload_in_size;
 
    /* Check if the current command can be appended to the queue */
    if (newsize_out > 64 || newsize_in > 64) {
        /* New command does not fit. Execute all commands in queue before starting
         * new queue with the current command as first entry. */
        ret = angie_execute_queued_commands(device, LIBUSB_TIMEOUT_MS);
 
        if (ret == ERROR_OK)
            ret = angie_post_process_queue(device);
 
        if (ret == ERROR_OK)
            angie_clear_queue(device);
    }
 
    if (!device->queue_start) {
        /* Queue was empty */
        device->commands_in_queue = 1;
 
        device->queue_start = angie_cmd;
        device->queue_end = angie_cmd;
    } else {
        /* There are already commands in the queue */
        device->commands_in_queue++;
 
        device->queue_end->next = angie_cmd;
        device->queue_end = angie_cmd;
    }
 
    if (ret != ERROR_OK)
        angie_clear_queue(device);
 
    return ret;
}
 
static int angie_execute_queued_commands(struct angie *device, int timeout_ms)
{
    struct angie_cmd *current;
    int ret, i, index_out, index_in, count_out, count_in, transferred;
    uint8_t buffer[64];
 
    if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO))
        angie_dump_queue(device);
 
    index_out = 0;
    count_out = 0;
    count_in = 0;
 
    for (current = device->queue_start; current; current = current->next) {
        /* Add command to packet */
        buffer[index_out] = current->id;
        index_out++;
        count_out++;
 
        for (i = 0; i < current->payload_out_size; i++)
            buffer[index_out + i] = current->payload_out[i];
        index_out += current->payload_out_size;
        count_in += current->payload_in_size;
        count_out += current->payload_out_size;
    }
 
    /* Send packet to ANGIE */
    ret = jtag_libusb_bulk_write(device->usb_device_handle, device->ep_out,
            (char *)buffer, count_out, timeout_ms, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Libusb bulk write queued commands failed.");
        return ret;
    }
    if (transferred != count_out) {
        LOG_ERROR("Libusb bulk write queued commands failed: transferred byte count");
        return ERROR_FAIL;
    }
 
    /* Wait for response if commands contain IN payload data */
    if (count_in > 0) {
        ret = jtag_libusb_bulk_write(device->usb_device_handle, device->ep_in,
                (char *)buffer, count_in, timeout_ms, &transferred);
    if (ret != ERROR_OK) {
        LOG_ERROR("Libusb bulk write input payload data failed");
        return ret;
    }
    if (transferred != count_in) {
        LOG_ERROR("Libusb bulk write input payload data failed: transferred byte count");
        return ERROR_FAIL;
    }
 
        /* Write back IN payload data */
        index_in = 0;
        for (current = device->queue_start; current; current = current->next) {
            for (i = 0; i < current->payload_in_size; i++) {
                current->payload_in[i] = buffer[index_in];
                index_in++;
            }
        }
    }
    return ERROR_OK;
}
 
static const char *angie_cmd_id_string(uint8_t id)
{
    switch (id) {
    case CMD_SCAN_IN:
        return "CMD_SCAN_IN";
    case CMD_SLOW_SCAN_IN:
        return "CMD_SLOW_SCAN_IN";
    case CMD_SCAN_OUT:
        return "CMD_SCAN_OUT";
    case CMD_SLOW_SCAN_OUT:
        return "CMD_SLOW_SCAN_OUT";
    case CMD_SCAN_IO:
        return "CMD_SCAN_IO";
    case CMD_SLOW_SCAN_IO:
        return "CMD_SLOW_SCAN_IO";
    case CMD_CLOCK_TMS:
        return "CMD_CLOCK_TMS";
    case CMD_SLOW_CLOCK_TMS:
        return "CMD_SLOW_CLOCK_TMS";
    case CMD_CLOCK_TCK:
        return "CMD_CLOCK_TCK";
    case CMD_SLOW_CLOCK_TCK:
        return "CMD_SLOW_CLOCK_TCK";
    case CMD_SLEEP_US:
        return "CMD_SLEEP_US";
    case CMD_SLEEP_MS:
        return "CMD_SLEEP_MS";
    case CMD_GET_SIGNALS:
        return "CMD_GET_SIGNALS";
    case CMD_SET_SIGNALS:
        return "CMD_SET_SIGNALS";
    case CMD_CONFIGURE_TCK_FREQ:
        return "CMD_CONFIGURE_TCK_FREQ";
    case CMD_SET_LEDS:
        return "CMD_SET_LEDS";
    case CMD_TEST:
        return "CMD_TEST";
    default:
        return "CMD_UNKNOWN";
    }
}
 
static void angie_dump_command(struct angie_cmd *angie_cmd)
{
    char hex[64 * 3];
    for (int i = 0; i < angie_cmd->payload_out_size; i++)
        sprintf(hex + 3 * i, "%02" PRIX8 " ", angie_cmd->payload_out[i]);
 
    hex[3 * angie_cmd->payload_out_size - 1] = 0;
    LOG_DEBUG_IO(" %-22s | OUT size = %" PRIi8 ", bytes = %s",
                    angie_cmd_id_string(angie_cmd->id), angie_cmd->payload_out_size, hex);
 
    LOG_DEBUG_IO("\n                         | IN size  =  %" PRIi8 "\n", angie_cmd->payload_in_size);
}
 
static void angie_dump_queue(struct angie *device)
{
    struct angie_cmd *current;
 
    LOG_DEBUG_IO("ANGIE command queue:\n");
 
    for (current = device->queue_start; current; current = current->next)
        angie_dump_command(current);
}
 
static int angie_append_scan_cmd(struct angie *device, enum scan_type scan_type,
    int scan_size_bits, uint8_t *tdi, uint8_t *tdo_start, uint8_t *tdo,
    uint8_t tms_count_start, uint8_t tms_sequence_start, uint8_t tms_count_end,
    uint8_t tms_sequence_end, struct jtag_command *origin, bool postprocess)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret, i, scan_size_bytes;
    uint8_t bits_last_byte;
 
    if (!cmd)
        return ERROR_FAIL;
 
    /* Check size of command. USB buffer can hold 64 bytes, 1 byte is command ID,
     * 5 bytes are setup data -> 58 remaining payload bytes for TDI data */
    if (scan_size_bits > (58 * 8)) {
        LOG_ERROR("BUG: Tried to create CMD_SCAN_IO ANGIE command with too"
            " large payload");
        free(cmd);
        return ERROR_FAIL;
    }
 
    scan_size_bytes = DIV_ROUND_UP(scan_size_bits, 8);
 
    bits_last_byte = scan_size_bits % 8;
    if (bits_last_byte == 0)
        bits_last_byte = 8;
 
    /* Allocate out_payload depending on scan type */
    switch (scan_type) {
        case SCAN_IN:
            if (device->delay_scan_in < 0)
                cmd->id = CMD_SCAN_IN;
            else
                cmd->id = CMD_SLOW_SCAN_IN;
            ret = angie_allocate_payload(cmd, 5, PAYLOAD_DIRECTION_IN);
            break;
        case SCAN_OUT:
            if (device->delay_scan_out < 0)
                cmd->id = CMD_SCAN_OUT;
            else
                cmd->id = CMD_SLOW_SCAN_OUT;
            ret = angie_allocate_payload(cmd, scan_size_bytes + 5, PAYLOAD_DIRECTION_OUT);
            break;
        case SCAN_IO:
            if (device->delay_scan_io < 0)
                cmd->id = CMD_SCAN_IO;
            else
                cmd->id = CMD_SLOW_SCAN_IO;
            ret = angie_allocate_payload(cmd, scan_size_bytes + 5, PAYLOAD_DIRECTION_OUT);
            break;
        default:
            LOG_ERROR("BUG: 'append scan cmd' encountered an unknown scan type");
            ret = ERROR_FAIL;
            break;
    }
 
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    /* Build payload_out that is common to all scan types */
    cmd->payload_out[0] = scan_size_bytes & 0xFF;
    cmd->payload_out[1] = bits_last_byte & 0xFF;
    cmd->payload_out[2] = ((tms_count_start & 0x0F) << 4) | (tms_count_end & 0x0F);
    cmd->payload_out[3] = tms_sequence_start;
    cmd->payload_out[4] = tms_sequence_end;
 
    /* Setup payload_out for types with OUT transfer */
    if (scan_type == SCAN_OUT || scan_type == SCAN_IO) {
        for (i = 0; i < scan_size_bytes; i++)
            cmd->payload_out[i + 5] = tdi[i];
    }
 
    /* Setup payload_in pointers for types with IN transfer */
    if (scan_type == SCAN_IN || scan_type == SCAN_IO) {
        cmd->payload_in_start = tdo_start;
        cmd->payload_in = tdo;
        cmd->payload_in_size = scan_size_bytes;
    }
 
    cmd->needs_postprocessing = postprocess;
    cmd->cmd_origin = origin;
 
    /* For scan commands, we free payload_in_start only when the command is
     * the last in a series of split commands or a stand-alone command */
    cmd->free_payload_in_start = postprocess;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_clock_tms_cmd(struct angie *device, uint8_t count,
    uint8_t sequence)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    if (device->delay_clock_tms < 0)
        cmd->id = CMD_CLOCK_TMS;
    else
        cmd->id = CMD_SLOW_CLOCK_TMS;
 
    /* CMD_CLOCK_TMS has two OUT payload bytes and zero IN payload bytes */
    ret = angie_allocate_payload(cmd, 2, PAYLOAD_DIRECTION_OUT);
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    cmd->payload_out[0] = count;
    cmd->payload_out[1] = sequence;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_clock_tck_cmd(struct angie *device, uint16_t count)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    if (device->delay_clock_tck < 0)
        cmd->id = CMD_CLOCK_TCK;
    else
        cmd->id = CMD_SLOW_CLOCK_TCK;
 
    /* CMD_CLOCK_TCK has two OUT payload bytes and zero IN payload bytes */
    ret = angie_allocate_payload(cmd, 2, PAYLOAD_DIRECTION_OUT);
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    cmd->payload_out[0] = count & 0xff;
    cmd->payload_out[1] = (count >> 8) & 0xff;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_get_signals_cmd(struct angie *device)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    cmd->id = CMD_GET_SIGNALS;
    cmd->needs_postprocessing = true;
 
    /* CMD_GET_SIGNALS has two IN payload bytes */
    ret = angie_allocate_payload(cmd, 2, PAYLOAD_DIRECTION_IN);
 
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_set_signals_cmd(struct angie *device, uint8_t low,
    uint8_t high)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    cmd->id = CMD_SET_SIGNALS;
 
    /* CMD_SET_SIGNALS has two OUT payload bytes and zero IN payload bytes */
    ret = angie_allocate_payload(cmd, 2, PAYLOAD_DIRECTION_OUT);
 
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    cmd->payload_out[0] = low;
    cmd->payload_out[1] = high;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_sleep_cmd(struct angie *device, uint32_t us)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    cmd->id = CMD_SLEEP_US;
 
    /* CMD_SLEEP_US has two OUT payload bytes and zero IN payload bytes */
    ret = angie_allocate_payload(cmd, 2, PAYLOAD_DIRECTION_OUT);
 
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    cmd->payload_out[0] = us & 0x00ff;
    cmd->payload_out[1] = (us >> 8) & 0x00ff;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_configure_tck_cmd(struct angie *device, int delay_scan_in,
    int delay_scan_out, int delay_scan_io, int delay_tck, int delay_tms)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    cmd->id = CMD_CONFIGURE_TCK_FREQ;
 
    /* CMD_CONFIGURE_TCK_FREQ has five OUT payload bytes and zero
     * IN payload bytes */
    ret = angie_allocate_payload(cmd, 5, PAYLOAD_DIRECTION_OUT);
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    if (delay_scan_in < 0)
        cmd->payload_out[0] = 0;
    else
        cmd->payload_out[0] = (uint8_t)delay_scan_in;
 
    if (delay_scan_out < 0)
        cmd->payload_out[1] = 0;
    else
        cmd->payload_out[1] = (uint8_t)delay_scan_out;
 
    if (delay_scan_io < 0)
        cmd->payload_out[2] = 0;
    else
        cmd->payload_out[2] = (uint8_t)delay_scan_io;
 
    if (delay_tck < 0)
        cmd->payload_out[3] = 0;
    else
        cmd->payload_out[3] = (uint8_t)delay_tck;
 
    if (delay_tms < 0)
        cmd->payload_out[4] = 0;
    else
        cmd->payload_out[4] = (uint8_t)delay_tms;
 
    return angie_append_queue(device, cmd);
}
 
static int angie_append_test_cmd(struct angie *device)
{
    struct angie_cmd *cmd = calloc(1, sizeof(struct angie_cmd));
    int ret;
 
    if (!cmd) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    cmd->id = CMD_TEST;
 
    /* CMD_TEST has one OUT payload byte and zero IN payload bytes */
    ret = angie_allocate_payload(cmd, 1, PAYLOAD_DIRECTION_OUT);
    if (ret != ERROR_OK) {
        free(cmd);
        return ret;
    }
 
    cmd->payload_out[0] = 0xAA;
 
    return angie_append_queue(device, cmd);
}
 
/****************** ANGIE TCK frequency helper functions ******************/
 
static int angie_calculate_delay(enum angie_delay_type type, long f, int *delay)
{
    float t_us, x, x_ceil;
 
    /* Calculate period of requested TCK frequency */
    t_us = 1000000.0 / f;
 
    switch (type) {
        case DELAY_CLOCK_TCK:
            x = (t_us - 6.0) / 4;
            break;
        case DELAY_CLOCK_TMS:
            x = (t_us - 8.5) / 4;
            break;
        case DELAY_SCAN_IN:
            x = (t_us - 8.8308) / 4;
            break;
        case DELAY_SCAN_OUT:
            x = (t_us - 10.527) / 4;
            break;
        case DELAY_SCAN_IO:
            x = (t_us - 13.132) / 4;
            break;
        default:
            return ERROR_FAIL;
            break;
    }
 
    /* Check if the delay value is negative. This happens when a frequency is
     * requested that is too high for the delay loop implementation. In this
     * case, set delay value to zero. */
    if (x < 0)
        x = 0;
 
    /* We need to convert the exact delay value to an integer. Therefore, we
     * round the exact value UP to ensure that the resulting frequency is NOT
     * higher than the requested frequency. */
    x_ceil = ceilf(x);
 
    /* Check if the value is within limits */
    if (x_ceil > 255)
        return ERROR_FAIL;
 
    *delay = (int)x_ceil;
 
    return ERROR_OK;
}
 
static long angie_calculate_frequency(enum angie_delay_type type, int delay)
{
    float t_us, f_float;
 
    if (delay > 255)
        return 0;
 
    switch (type) {
        case DELAY_CLOCK_TCK:
            if (delay < 0)
                t_us = 2.666;
            else
                t_us = (4.0 * delay) + 6.0;
            break;
        case DELAY_CLOCK_TMS:
            if (delay < 0)
                t_us = 5.666;
            else
                t_us = (4.0 * delay) + 8.5;
            break;
        case DELAY_SCAN_IN:
            if (delay < 0)
                t_us = 5.5;
            else
                t_us = (4.0 * delay) + 8.8308;
            break;
        case DELAY_SCAN_OUT:
            if (delay < 0)
                t_us = 7.0;
            else
                t_us = (4.0 * delay) + 10.527;
            break;
        case DELAY_SCAN_IO:
            if (delay < 0)
                t_us = 9.926;
            else
                t_us = (4.0 * delay) + 13.132;
            break;
        default:
            return 0;
    }
 
    f_float = 1000000.0 / t_us;
    return roundf(f_float);
}
 
/******************* Interface between ANGIE and OpenOCD ******************/
 
static void angie_set_end_state(tap_state_t endstate)
{
    if (tap_is_state_stable(endstate))
        tap_set_end_state(endstate);
    else
        LOG_ERROR("BUG: %s is not a valid end state", tap_state_name(endstate));
}
 
static int angie_queue_statemove(struct angie *device)
{
    uint8_t tms_sequence, tms_count;
    int ret;
 
    if (tap_get_state() == tap_get_end_state()) {
        /* Do nothing if we are already there */
        return ERROR_OK;
    }
 
    tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
    tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
 
    ret = angie_append_clock_tms_cmd(device, tms_count, tms_sequence);
 
    if (ret == ERROR_OK)
        tap_set_state(tap_get_end_state());
 
    return ret;
}
 
static int angie_queue_scan(struct angie *device, struct jtag_command *cmd)
{
    uint32_t scan_size_bits, scan_size_bytes, bits_last_scan;
    uint32_t scans_max_payload, bytecount;
    uint8_t *tdi_buffer_start = NULL, *tdi_buffer = NULL;
    uint8_t *tdo_buffer_start = NULL, *tdo_buffer = NULL;
 
    uint8_t first_tms_count, first_tms_sequence;
    uint8_t last_tms_count, last_tms_sequence;
 
    uint8_t tms_count_pause, tms_sequence_pause;
    uint8_t tms_count_resume, tms_sequence_resume;
 
    uint8_t tms_count_start, tms_sequence_start;
    uint8_t tms_count_end, tms_sequence_end;
 
    enum scan_type type;
    int ret;
 
    /* Determine scan size */
    scan_size_bits = jtag_scan_size(cmd->cmd.scan);
    scan_size_bytes = DIV_ROUND_UP(scan_size_bits, 8);
 
    /* Determine scan type (IN/OUT/IO) */
    type = jtag_scan_type(cmd->cmd.scan);
 
    /* Determine number of scan commands with maximum payload */
    scans_max_payload = scan_size_bytes / 58;
 
    /* Determine size of last shift command */
    bits_last_scan = scan_size_bits - (scans_max_payload * 58 * 8);
 
    /* Allocate TDO buffer if required */
    if (type == SCAN_IN || type == SCAN_IO) {
        tdo_buffer_start = calloc(scan_size_bytes, sizeof(uint8_t));
 
        if (!tdo_buffer_start)
            return ERROR_FAIL;
 
        tdo_buffer = tdo_buffer_start;
    }
 
    /* Fill TDI buffer if required */
    if (type == SCAN_OUT || type == SCAN_IO) {
        jtag_build_buffer(cmd->cmd.scan, &tdi_buffer_start);
        tdi_buffer = tdi_buffer_start;
    }
 
    /* Get TAP state transitions */
    if (cmd->cmd.scan->ir_scan) {
        angie_set_end_state(TAP_IRSHIFT);
        first_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
        first_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 
        tap_set_state(TAP_IRSHIFT);
        tap_set_end_state(cmd->cmd.scan->end_state);
        last_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
        last_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 
        /* TAP state transitions for split scans */
        tms_count_pause = tap_get_tms_path_len(TAP_IRSHIFT, TAP_IRPAUSE);
        tms_sequence_pause = tap_get_tms_path(TAP_IRSHIFT, TAP_IRPAUSE);
        tms_count_resume = tap_get_tms_path_len(TAP_IRPAUSE, TAP_IRSHIFT);
        tms_sequence_resume = tap_get_tms_path(TAP_IRPAUSE, TAP_IRSHIFT);
    } else {
        angie_set_end_state(TAP_DRSHIFT);
        first_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
        first_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 
        tap_set_state(TAP_DRSHIFT);
        tap_set_end_state(cmd->cmd.scan->end_state);
        last_tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());
        last_tms_sequence = tap_get_tms_path(tap_get_state(), tap_get_end_state());
 
        /* TAP state transitions for split scans */
        tms_count_pause = tap_get_tms_path_len(TAP_DRSHIFT, TAP_DRPAUSE);
        tms_sequence_pause = tap_get_tms_path(TAP_DRSHIFT, TAP_DRPAUSE);
        tms_count_resume = tap_get_tms_path_len(TAP_DRPAUSE, TAP_DRSHIFT);
        tms_sequence_resume = tap_get_tms_path(TAP_DRPAUSE, TAP_DRSHIFT);
    }
 
    /* Generate scan commands */
    bytecount = scan_size_bytes;
    while (bytecount > 0) {
        if (bytecount == scan_size_bytes) {
            /* This is the first scan */
            tms_count_start = first_tms_count;
            tms_sequence_start = first_tms_sequence;
        } else {
            /* Resume from previous scan */
            tms_count_start = tms_count_resume;
            tms_sequence_start = tms_sequence_resume;
        }
 
        if (bytecount > 58) {   /* Full scan, at least one scan will follow */
            tms_count_end = tms_count_pause;
            tms_sequence_end = tms_sequence_pause;
 
            ret = angie_append_scan_cmd(device,
                    type,
                    58 * 8,
                    tdi_buffer,
                    tdo_buffer_start,
                    tdo_buffer,
                    tms_count_start,
                    tms_sequence_start,
                    tms_count_end,
                    tms_sequence_end,
                    cmd,
                    false);
 
            bytecount -= 58;
 
            /* Update TDI and TDO buffer pointers */
            if (tdi_buffer_start)
                tdi_buffer += 58;
            if (tdo_buffer_start)
                tdo_buffer += 58;
        } else if (bytecount == 58) {   /* Full scan, no further scans */
            tms_count_end = last_tms_count;
            tms_sequence_end = last_tms_sequence;
 
            ret = angie_append_scan_cmd(device,
                    type,
                    58 * 8,
                    tdi_buffer,
                    tdo_buffer_start,
                    tdo_buffer,
                    tms_count_start,
                    tms_sequence_start,
                    tms_count_end,
                    tms_sequence_end,
                    cmd,
                    true);
 
            bytecount = 0;
        } else {/* Scan with less than maximum payload, no further scans */
            tms_count_end = last_tms_count;
            tms_sequence_end = last_tms_sequence;
 
            ret = angie_append_scan_cmd(device,
                    type,
                    bits_last_scan,
                    tdi_buffer,
                    tdo_buffer_start,
                    tdo_buffer,
                    tms_count_start,
                    tms_sequence_start,
                    tms_count_end,
                    tms_sequence_end,
                    cmd,
                    true);
 
            bytecount = 0;
        }
 
        if (ret != ERROR_OK) {
            free(tdi_buffer_start);
            free(tdo_buffer_start);
            return ret;
        }
    }
 
    free(tdi_buffer_start);
 
    /* Set current state to the end state requested by the command */
    tap_set_state(cmd->cmd.scan->end_state);
 
    return ERROR_OK;
}
 
static int angie_queue_tlr_reset(struct angie *device, struct jtag_command *cmd)
{
    int ret = angie_append_clock_tms_cmd(device, 5, 0xff);
 
    if (ret == ERROR_OK)
        tap_set_state(TAP_RESET);
 
    return ret;
}
 
static int angie_queue_runtest(struct angie *device, struct jtag_command *cmd)
{
    int ret;
 
    /* Only perform statemove if the TAP currently isn't in the TAP_IDLE state */
    if (tap_get_state() != TAP_IDLE) {
        angie_set_end_state(TAP_IDLE);
        angie_queue_statemove(device);
    }
 
    /* Generate the clock cycles */
    ret = angie_append_clock_tck_cmd(device, cmd->cmd.runtest->num_cycles);
    if (ret != ERROR_OK)
        return ret;
 
    /* Move to end state specified in command */
    if (cmd->cmd.runtest->end_state != tap_get_state()) {
        tap_set_end_state(cmd->cmd.runtest->end_state);
        angie_queue_statemove(device);
    }
 
    return ERROR_OK;
}
 
static int angie_reset(int trst, int srst)
{
    struct angie *device = angie_handle;
    uint8_t low = 0, high = 0;
 
    if (trst) {
        tap_set_state(TAP_RESET);
        low |= SIGNAL_TRST;
    } else {
        high |= SIGNAL_TRST;
    }
 
    if (srst)
        low |= SIGNAL_SRST;
    else
        high |= SIGNAL_SRST;
 
    int ret = angie_append_set_signals_cmd(device, low, high);
    if (ret != ERROR_OK)
        return ret;
 
    ret = angie_execute_queued_commands(device, LIBUSB_TIMEOUT_MS);
    if (ret != ERROR_OK)
        return ret;
 
    angie_clear_queue(device);
 
    return ERROR_OK;
}
 
static int angie_queue_pathmove(struct angie *device, struct jtag_command *cmd)
{
    int ret, state_count;
    tap_state_t *path;
    uint8_t tms_sequence;
 
    unsigned int num_states = cmd->cmd.pathmove->num_states;
    path = cmd->cmd.pathmove->path;
    state_count = 0;
 
    while (num_states > 0) {
        unsigned int batch_size;
 
        tms_sequence = 0;
 
        /* Determine batch size */
        if (num_states >= 8)
            batch_size = 8;
        else
            batch_size = num_states;
 
        for (unsigned int i = 0; i < batch_size; i++) {
            if (tap_state_transition(tap_get_state(), false) == path[state_count]) {
                /* Append '0' transition: clear bit 'i' in tms_sequence */
                buf_set_u32(&tms_sequence, i, 1, 0x0);
            } else if (tap_state_transition(tap_get_state(), true)
                   == path[state_count]) {
                /* Append '1' transition: set bit 'i' in tms_sequence */
                buf_set_u32(&tms_sequence, i, 1, 0x1);
            } else {
                /* Invalid state transition */
                LOG_ERROR("BUG: %s -> %s isn't a valid TAP state transition",
                    tap_state_name(tap_get_state()),
                    tap_state_name(path[state_count]));
                return ERROR_FAIL;
            }
 
            tap_set_state(path[state_count]);
            state_count++;
            num_states--;
        }
 
        /* Append CLOCK_TMS command to ANGIE command queue */
        LOG_INFO("pathmove batch: count = %i, sequence = 0x%" PRIx8 "", batch_size, tms_sequence);
        ret = angie_append_clock_tms_cmd(angie_handle, batch_size, tms_sequence);
        if (ret != ERROR_OK)
            return ret;
    }
 
    return ERROR_OK;
}
 
static int angie_queue_sleep(struct angie *device, struct jtag_command *cmd)
{
    /* IMPORTANT! Due to the time offset in command execution introduced by
     * command queueing, this needs to be implemented in the ANGIE device */
    return angie_append_sleep_cmd(device, cmd->cmd.sleep->us);
}
 
static int angie_queue_stableclocks(struct angie *device, struct jtag_command *cmd)
{
    int ret;
 
    if (!tap_is_state_stable(tap_get_state())) {
        LOG_ERROR("JTAG_STABLECLOCKS: state not stable");
        return ERROR_FAIL;
    }
 
    unsigned int num_cycles = cmd->cmd.stableclocks->num_cycles;
 
    /* TMS stays either high (Test Logic Reset state) or low (all other states) */
    if (tap_get_state() == TAP_RESET)
        ret = angie_append_set_signals_cmd(device, 0, SIGNAL_TMS);
    else
        ret = angie_append_set_signals_cmd(device, SIGNAL_TMS, 0);
 
    if (ret != ERROR_OK)
        return ret;
 
    while (num_cycles > 0) {
        if (num_cycles > 0xFFFF) {
            /* ANGIE CMD_CLOCK_TCK can generate up to 0xFFFF (uint16_t) cycles */
            ret = angie_append_clock_tck_cmd(device, 0xFFFF);
            num_cycles -= 0xFFFF;
        } else {
            ret = angie_append_clock_tck_cmd(device, num_cycles);
            num_cycles = 0;
        }
 
        if (ret != ERROR_OK)
            return ret;
    }
 
    return ERROR_OK;
}
 
static int angie_post_process_scan(struct angie_cmd *angie_cmd)
{
    struct jtag_command *cmd = angie_cmd->cmd_origin;
    int ret;
 
    switch (jtag_scan_type(cmd->cmd.scan)) {
        case SCAN_IN:
        case SCAN_IO:
            ret = jtag_read_buffer(angie_cmd->payload_in_start, cmd->cmd.scan);
            break;
        case SCAN_OUT:
            /* Nothing to do for OUT scans */
            ret = ERROR_OK;
            break;
        default:
            LOG_ERROR("BUG: angie post process scan encountered an unknown JTAG scan type");
            ret = ERROR_FAIL;
            break;
    }
 
    return ret;
}
 
static int angie_post_process_queue(struct angie *device)
{
    struct angie_cmd *current;
    struct jtag_command *openocd_cmd;
    int ret;
 
    current = device->queue_start;
 
    while (current) {
        openocd_cmd = current->cmd_origin;
 
        /* Check if a corresponding OpenOCD command is stored for this
         * ANGIE command */
        if (current->needs_postprocessing && openocd_cmd) {
            switch (openocd_cmd->type) {
                case JTAG_SCAN:
                    ret = angie_post_process_scan(current);
                    break;
                case JTAG_TLR_RESET:
                case JTAG_RUNTEST:
                case JTAG_PATHMOVE:
                case JTAG_SLEEP:
                case JTAG_STABLECLOCKS:
                    /* Nothing to do for these commands */
                    ret = ERROR_OK;
                    break;
                default:
                    ret = ERROR_FAIL;
                    LOG_ERROR("BUG: angie post process queue encountered unknown JTAG "
                    "command type");
                    break;
            }
 
            if (ret != ERROR_OK)
                return ret;
        }
 
        current = current->next;
    }
 
    return ERROR_OK;
}
 
/**************************** JTAG driver functions ***************************/
 
static int angie_execute_queue(struct jtag_command *cmd_queue)
{
    struct jtag_command *cmd = cmd_queue;
    int ret;
 
    while (cmd) {
        switch (cmd->type) {
            case JTAG_SCAN:
                ret = angie_queue_scan(angie_handle, cmd);
                break;
            case JTAG_TLR_RESET:
                ret = angie_queue_tlr_reset(angie_handle, cmd);
                break;
            case JTAG_RUNTEST:
                ret = angie_queue_runtest(angie_handle, cmd);
                break;
            case JTAG_PATHMOVE:
                ret = angie_queue_pathmove(angie_handle, cmd);
                break;
            case JTAG_SLEEP:
                ret = angie_queue_sleep(angie_handle, cmd);
                break;
            case JTAG_STABLECLOCKS:
                ret = angie_queue_stableclocks(angie_handle, cmd);
                break;
            default:
                ret = ERROR_FAIL;
                LOG_ERROR("BUG: encountered unknown JTAG command type");
                break;
        }
 
        if (ret != ERROR_OK)
            return ret;
 
        cmd = cmd->next;
    }
 
    if (angie_handle->commands_in_queue > 0) {
        ret = angie_execute_queued_commands(angie_handle, LIBUSB_TIMEOUT_MS);
        if (ret != ERROR_OK)
            return ret;
 
        ret = angie_post_process_queue(angie_handle);
        if (ret != ERROR_OK)
            return ret;
 
        angie_clear_queue(angie_handle);
    }
 
    return ERROR_OK;
}
 
static int angie_khz(int khz, int *jtag_speed)
{
    int ret;
 
    if (khz == 0) {
        LOG_ERROR("RCLK not supported");
        return ERROR_FAIL;
    }
 
    /* CLOCK_TCK commands are decoupled from others. Therefore, the frequency
     * setting can be done independently from all other commands. */
    if (khz >= 375) {
        angie_handle->delay_clock_tck = -1;
    } else {
        ret = angie_calculate_delay(DELAY_CLOCK_TCK, khz * 1000,
                &angie_handle->delay_clock_tck);
        if (ret != ERROR_OK)
            return ret;
    }
 
    /* SCAN_{IN,OUT,IO} commands invoke CLOCK_TMS commands. Therefore, if the
     * requested frequency goes below the maximum frequency for SLOW_CLOCK_TMS
     * commands, all SCAN commands MUST also use the variable frequency
     * implementation! */
    if (khz >= 176) {
        angie_handle->delay_clock_tms = -1;
        angie_handle->delay_scan_in = -1;
        angie_handle->delay_scan_out = -1;
        angie_handle->delay_scan_io = -1;
    } else {
        ret = angie_calculate_delay(DELAY_CLOCK_TMS, khz * 1000,
                &angie_handle->delay_clock_tms);
        if (ret != ERROR_OK)
            return ret;
 
        ret = angie_calculate_delay(DELAY_SCAN_IN, khz * 1000,
                &angie_handle->delay_scan_in);
        if (ret != ERROR_OK)
            return ret;
 
        ret = angie_calculate_delay(DELAY_SCAN_OUT, khz * 1000,
                &angie_handle->delay_scan_out);
        if (ret != ERROR_OK)
            return ret;
 
        ret = angie_calculate_delay(DELAY_SCAN_IO, khz * 1000,
                &angie_handle->delay_scan_io);
        if (ret != ERROR_OK)
            return ret;
    }
 
    LOG_DEBUG_IO("ANGIE TCK setup: delay_tck      = %i (%li Hz),",
        angie_handle->delay_clock_tck,
        angie_calculate_frequency(DELAY_CLOCK_TCK, angie_handle->delay_clock_tck));
    LOG_DEBUG_IO("                 delay_tms      = %i (%li Hz),",
        angie_handle->delay_clock_tms,
        angie_calculate_frequency(DELAY_CLOCK_TMS, angie_handle->delay_clock_tms));
    LOG_DEBUG_IO("                 delay_scan_in  = %i (%li Hz),",
        angie_handle->delay_scan_in,
        angie_calculate_frequency(DELAY_SCAN_IN, angie_handle->delay_scan_in));
    LOG_DEBUG_IO("                 delay_scan_out = %i (%li Hz),",
        angie_handle->delay_scan_out,
        angie_calculate_frequency(DELAY_SCAN_OUT, angie_handle->delay_scan_out));
    LOG_DEBUG_IO("                 delay_scan_io  = %i (%li Hz),",
        angie_handle->delay_scan_io,
        angie_calculate_frequency(DELAY_SCAN_IO, angie_handle->delay_scan_io));
 
    /* Configure the ANGIE device with the new delay values */
    ret = angie_append_configure_tck_cmd(angie_handle,
            angie_handle->delay_scan_in,
            angie_handle->delay_scan_out,
            angie_handle->delay_scan_io,
            angie_handle->delay_clock_tck,
            angie_handle->delay_clock_tms);
 
    if (ret != ERROR_OK)
        return ret;
 
    *jtag_speed = khz;
 
    return ERROR_OK;
}
 
static int angie_speed(int speed)
{
    int dummy;
 
    return angie_khz(speed, &dummy);
}
 
static int angie_speed_div(int speed, int *khz)
{
    *khz = speed;
 
    return ERROR_OK;
}
 
static int angie_init(void)
{
    int ret, transferred;
    char str_manufacturer[20];
    bool download_firmware = false;
    char dummy[64];
    uint8_t input_signals, output_signals;
 
    angie_handle = calloc(1, sizeof(struct angie));
 
    if (!angie_handle) {
        LOG_ERROR("Out of memory");
        return ERROR_FAIL;
    }
 
    ret = angie_usb_open(angie_handle);
    if (ret != ERROR_OK) {
        free(angie_handle);
        angie_handle = NULL;
        return ret;
    }
 
    /* Get String Descriptor to determine if firmware needs to be loaded */
    ret = libusb_get_string_descriptor_ascii(angie_handle->usb_device_handle, 1, (unsigned char *)str_manufacturer, 20);
    if (ret < 0) {
        /* Could not get descriptor -> Unconfigured or original Keil firmware */
        download_firmware = true;
    } else {
        /* We got a String Descriptor, check if it is the correct one */
        if (strncmp(str_manufacturer, "NanoXplore, SAS.", 16) != 0)
            download_firmware = true;
    }
 
    if (download_firmware) {
        LOG_INFO("Loading ANGIE firmware. This is reversible by power-cycling ANGIE device.");
        if (libusb_claim_interface(angie_handle->usb_device_handle, 0) != LIBUSB_SUCCESS) {
            LOG_ERROR("Could not claim interface 0");
            return ERROR_FAIL;
        }
        ret = angie_load_firmware_and_renumerate(angie_handle,
                ANGIE_FIRMWARE_FILE, ANGIE_RENUMERATION_DELAY_US);
        if (ret != ERROR_OK) {
            LOG_ERROR("Could not download firmware and re-numerate ANGIE");
            angie_quit();
            return ret;
        }
        ret = angie_load_bitstream(angie_handle, ANGIE_BITSTREAM_FILE);
        if (ret != ERROR_OK) {
            LOG_ERROR("Could not download bitstream");
            angie_quit();
            return ret;
        }
        if (libusb_release_interface(angie_handle->usb_device_handle, 0) != LIBUSB_SUCCESS) {
            LOG_ERROR("Fail release interface 0");
            return ERROR_FAIL;
        }
        if (libusb_claim_interface(angie_handle->usb_device_handle, 1) != LIBUSB_SUCCESS) {
            LOG_ERROR("Could not claim interface 1");
            return ERROR_FAIL;
        }
        /* Configure io extender 23: all input */
        ret = angie_io_extender_config(angie_handle, 0x23, 0xFF);
        if (ret != ERROR_OK) {
            LOG_ERROR("Could not configure io extender 23");
            return ret;
        }
        if (libusb_release_interface(angie_handle->usb_device_handle, 1) != LIBUSB_SUCCESS) {
            LOG_ERROR("Fail release interface 1");
            return ERROR_FAIL;
        }
    } else {
        LOG_INFO("ANGIE device is already running ANGIE firmware");
    }
 
    /* Get ANGIE USB IN/OUT endpoints and claim the interface 0 */
    ret = jtag_libusb_choose_interface(angie_handle->usb_device_handle,
        &angie_handle->ep_in, &angie_handle->ep_out, 0xFF, 0, 0, -1);
    if (ret != ERROR_OK) {
        LOG_ERROR("Choose and claim interface failed");
        angie_quit();
        return ret;
    }
 
    /* Initialize ANGIE command queue */
    angie_clear_queue(angie_handle);
 
    /* Issue one test command with short timeout */
    ret = angie_append_test_cmd(angie_handle);
    if (ret != ERROR_OK) {
        LOG_ERROR("Append test command failed.");
        angie_quit();
        return ret;
    }
 
    ret = angie_execute_queued_commands(angie_handle, 200);
    if (ret != ERROR_OK) {
        /* Sending test command failed. The ANGIE device may be forever waiting for
         * the host to fetch an USB Bulk IN packet (e. g. OpenOCD crashed or was
         * shut down by the user via Ctrl-C. Try to retrieve this Bulk IN packet. */
 
        ret = jtag_libusb_bulk_write(angie_handle->usb_device_handle, angie_handle->ep_in,
                dummy, 64, 200, &transferred);
 
        if (ret != ERROR_OK || transferred == 0) {
            /* Bulk IN transfer failed -> unrecoverable error condition */
            LOG_ERROR("Cannot communicate with ANGIE device. Disconnect ANGIE from "
                "the USB port and re-connect, then re-run OpenOCD");
            angie_quit();
            return ERROR_FAIL;
        }
        /* Successfully received Bulk IN packet -> continue */
        LOG_INFO("Recovered from lost Bulk IN packet");
    }
 
    angie_clear_queue(angie_handle);
 
    /* Execute get signals command */
    ret = angie_append_get_signals_cmd(angie_handle);
    if (ret != ERROR_OK) {
        LOG_ERROR("Append get signals command failed");
        angie_quit();
        return ret;
    }
    ret = angie_execute_queued_commands(angie_handle, 200);
    if (ret != ERROR_OK) {
        LOG_ERROR("Execute get signals command failed");
        angie_quit();
        return ret;
    }
 
    /* Post-process the single CMD_GET_SIGNALS command */
    input_signals = angie_handle->queue_start->payload_in[0];
    output_signals = angie_handle->queue_start->payload_in[1];
    angie_dump_signal_states(input_signals, output_signals);
 
    angie_clear_queue(angie_handle);
 
    return ERROR_OK;
}
 
static int angie_quit(void)
{
    int ret = angie_usb_close(angie_handle);
    free(angie_handle);
    angie_handle = NULL;
 
    return ret;
}
 
static struct jtag_interface angie_interface = {
    .execute_queue = angie_execute_queue,
};
 
struct adapter_driver angie_adapter_driver = {
    .name = "angie",
    .transports = jtag_only,
 
    .init = angie_init,
    .quit = angie_quit,
    .reset = angie_reset,
    .speed = angie_speed,
    .khz = angie_khz,
    .speed_div = angie_speed_div,
 
    .jtag_ops = &angie_interface,
};