static int icdi_usb_query(void *handle)
{
int result;
struct icdi_usb_handle_s *h = handle;
result = icdi_send_cmd(handle, "qSupported");
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("query supported failed: 0x%x", result);
return ERROR_FAIL;
}
/* from this we can get the max packet supported */
/* query packet buffer size */
char *offset = strstr(h->read_buffer, "PacketSize");
if (offset) {
char *separator;
int max_packet;
max_packet = strtol(offset + 11, &separator, 16);
if (!max_packet)
LOG_ERROR("invalid max packet, using defaults");
else
h->max_packet = max_packet;
LOG_DEBUG("max packet supported : %i bytes", h->max_packet);
}
/* if required re allocate packet buffer */
if (h->max_packet != ICDI_PACKET_SIZE) {
h->read_buffer = realloc(h->read_buffer, h->max_packet);
h->write_buffer = realloc(h->write_buffer, h->max_packet);
if (h->read_buffer == 0 || h->write_buffer == 0) {
LOG_ERROR("unable to reallocate memory");
return ERROR_FAIL;
}
}
/* set extended mode */
result = icdi_send_cmd(handle, "!");
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("unable to enable extended mode: 0x%x", result);
return ERROR_FAIL;
}
return ERROR_OK;
}
static int icdi_usb_read_mem(void *handle, uint32_t addr, uint32_t len, uint8_t *buffer)
{
int result;
struct icdi_usb_handle_s *h;
char cmd[20];
h = (struct icdi_usb_handle_s *)handle;
snprintf(cmd, sizeof(cmd), "x%x,%x", addr, len);
result = icdi_send_cmd(handle, cmd);
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("memory read failed: 0x%x", result);
return ERROR_FAIL;
}
/* unescape input */
int read_len = remote_unescape_input(h->read_buffer + 5, h->read_count - 8, (char *)buffer, len);
if (read_len != (int)len) {
LOG_ERROR("read more bytes than expected: actual 0x%" PRIx32 " expected 0x%" PRIx32, read_len, len);
return ERROR_FAIL;
}
return ERROR_OK;
}
static int icdi_usb_read_reg(void *handle, int num, uint32_t *val)
{
int result;
struct icdi_usb_handle_s *h;
char cmd[10];
h = (struct icdi_usb_handle_s *)handle;
snprintf(cmd, sizeof(cmd), "p%x", num);
result = icdi_send_cmd(handle, cmd);
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("register read failed: 0x%x", result);
return ERROR_FAIL;
}
/* convert result */
if (unhexify((char *)val, h->read_buffer + 2, 4) != 4) {
LOG_ERROR("failed to convert result");
return ERROR_FAIL;
}
return result;
}
static int icdi_usb_step(void *handle)
{
int result;
/* step target at current address */
result = icdi_send_cmd(handle, "s");
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("step failed: 0x%x", result);
return ERROR_FAIL;
}
return result;
}
static int icdi_usb_halt(void *handle)
{
int result;
/* this query halts the target ?? */
result = icdi_send_cmd(handle, "?");
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("halt failed: 0x%x", result);
return ERROR_FAIL;
}
return result;
}
static int icdi_usb_run(void *handle)
{
int result;
/* resume target at current address */
result = icdi_send_cmd(handle, "c");
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("continue failed: 0x%x", result);
return ERROR_FAIL;
}
return result;
}
static int icdi_usb_write_reg(void *handle, int num, uint32_t val)
{
int result;
char cmd[20];
int cmd_len = snprintf(cmd, sizeof(cmd), "P%x=", num);
hexify(cmd + cmd_len, (char *)&val, 4, sizeof(cmd));
result = icdi_send_cmd(handle, cmd);
if (result != ERROR_OK)
return result;
/* check result */
result = icdi_get_cmd_result(handle);
if (result != ERROR_OK) {
LOG_ERROR("register write failed: 0x%x", result);
return ERROR_FAIL;
}
return result;
}
