in common/recipes-core/asd_1.4.7/files/daemon/asd_msg.c [904:1411]
STATUS process_jtag_message(ASD_MSG* state, struct asd_message* s_message)
{
u_int32_t response_cnt = 0;
enum jtag_states end_state;
STATUS status = ST_OK;
int size = get_message_size(s_message);
struct packet_data packet;
unsigned char* data_ptr;
uint8_t cmd = 0;
if (size == -1)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK, ASD_LogOption_None,
"Failed to process jtag message because "
"get message size failed.");
return ST_ERR;
}
explicit_bzero(&state->out_msg.header, sizeof(struct message_header));
explicit_bzero(&state->out_msg.buffer, MAX_DATA_SIZE);
#ifdef ENABLE_DEBUG_LOGGING
ASD_log(ASD_LogLevel_Debug, ASD_LogStream_Network, ASD_LogOption_No_Remote,
"NetReq tag: %d size: %d", s_message->header.tag, size);
ASD_log_buffer(ASD_LogLevel_Debug, ASD_LogStream_Network,
ASD_LogOption_No_Remote, s_message->buffer, (size_t)size,
"NetReq");
#endif
packet.next_data = s_message->buffer;
packet.used = 0;
packet.total = (unsigned int)size;
while (packet.used < packet.total)
{
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK, ASD_LogOption_None,
"Failed to read data for cmd, short packet");
status = ST_ERR;
break;
}
else
{
cmd = *data_ptr;
}
if (cmd == WRITE_EVENT_CONFIG)
{
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(
ASD_LogLevel_Error, ASD_LogStream_SDK, ASD_LogOption_None,
"Failed to read data for WRITE_EVENT_CONFIG, short packet");
status = ST_ERR;
break;
}
status = write_event_config(state, *data_ptr);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "write_event_config failed, %d",
status);
break;
}
}
else if (cmd >= WRITE_CFG_MIN && cmd <= WRITE_CFG_MAX)
{
status = write_cfg(state, (writeCfg)cmd, &packet);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "write_cfg failed, %d", status);
break;
}
}
else if (cmd == WRITE_PINS)
{
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data for WRITE_PINS, short packet");
status = ST_ERR;
break;
}
uint8_t data = *data_ptr;
bool assert = (data >> 7) == 1;
uint8_t index = data & WRITE_PIN_MASK;
if (index < PIN_MAX)
{
Pin pin = (Pin)index;
status = target_write(state->target_handler, pin, assert);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "target_write failed, %d",
status);
break;
}
#ifdef SKIP_WAIT_CYCLES_DURING_RESET
if (pin == PIN_RESET_BUTTON && assert == true)
{
while (packet.used < packet.total)
{
data_ptr = get_packet_data(&packet, 2);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read WAIT_CYCLES data");
status = ST_ERR;
break;
}
data = *data_ptr;
if (data != WAIT_CYCLES_TCK_DISABLE)
{
packet.next_data -= 2;
packet.used -= 2;
break;
}
}
if (status == ST_ERR)
break;
}
#endif
}
else if ((index & SCAN_CHAIN_SELECT) == SCAN_CHAIN_SELECT)
{
if (state->jtag_chain_mode == JTAG_CHAIN_SELECT_MODE_SINGLE)
{
uint8_t scan_chain = (index & SCAN_CHAIN_SELECT_MASK);
if (scan_chain >= MAX_SCAN_CHAINS)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Unexpected scan chain: 0x%02x", scan_chain);
status = ST_ERR;
break;
}
status =
asd_write_set_active_chain_event(state, scan_chain);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Target set active chain failed, %d", status);
break;
}
}
else
{
uint8_t chain_bytes_length =
(index & SCAN_CHAIN_SELECT_MASK) + 1;
uint8_t chain_bytes[MAX_MULTICHAINS];
// e.g.: chain_bytes_length = 1
// chain_bytes[0] = b'11111111 = Chain
// 0-7 are selected chain_bytes[1] =
// b'00000011 = Chain 8 (bit 0) and 9
// (bit 1) are selected Notes: support
// up to 16 chain_bytes = 128 jtag
// chains
for (int i = 0; i < chain_bytes_length; i++)
{
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data for chain_bytes");
status = ST_ERR;
break;
}
chain_bytes[i] = *data_ptr;
}
// Implementing specific code for
// multi-jtag chain select add here:
#ifdef ENABLE_DEBUG_LOGGING
ASD_log(
ASD_LogLevel_Debug, ASD_LogStream_SDK,
ASD_LogOption_None,
"Multi-jtag chain select command not yet implemented");
#endif
// e.g. < Chain 21 > Received chain
// bytes_length: Chain: 0x20 0000
char line[MAX_MULTICHAINS * CHARS_PER_CHAIN];
uint8_t pos = 0;
explicit_bzero(line, sizeof(line));
for (int i = chain_bytes_length - 1; i >= 0; i--)
{
snprintf(&line[pos], sizeof(line), "%02X",
chain_bytes[i]);
pos = pos + 2;
if (i % 2 == 0 && i != 0)
{
snprintf(&line[pos], sizeof(line), " ");
pos++;
}
}
#ifdef ENABLE_DEBUG_LOGGING
ASD_log(ASD_LogLevel_Debug, ASD_LogStream_SDK,
ASD_LogOption_None, "Chain : 0x%s", line);
#endif
}
}
else
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Unexpected WRITE_PINS index: 0x%02x", index);
status = ST_ERR;
break;
}
}
else if (cmd >= READ_STATUS_MIN && cmd <= READ_STATUS_MAX)
{
int bytes_written = 0;
uint8_t index = (cmd & READ_STATUS_MASK);
ReadType readStatusTypeIndex = (ReadType)index;
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data for Read Status, short packet");
status = ST_ERR;
break;
}
if (response_cnt + 2 > MAX_DATA_SIZE)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to process READ_STATUS. "
"Response buffer already full");
status = ST_ERR;
break;
}
uint8_t pin = (*data_ptr & READ_STATUS_PIN_MASK);
status = read_status(state, readStatusTypeIndex, pin,
&(state->out_msg.buffer[response_cnt]),
&bytes_written);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "read_status failed, %d", status);
break;
}
response_cnt += bytes_written;
}
else if (cmd == WAIT_CYCLES_TCK_DISABLE ||
cmd == WAIT_CYCLES_TCK_ENABLE)
{
data_ptr = get_packet_data(&packet, 1);
if (data_ptr == NULL)
{
ASD_log(
ASD_LogLevel_Error, ASD_LogStream_SDK, ASD_LogOption_None,
"Failed to read data for WAIT_CYCLES_TCK, short packet");
status = ST_ERR;
break;
}
unsigned int number_of_cycles = *data_ptr;
if (number_of_cycles == 0)
number_of_cycles = 256;
status = JTAG_wait_cycles(state->jtag_handler, number_of_cycles);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_wait_cycles failed, %d",
status);
break;
}
}
else if (cmd == WAIT_PRDY)
{
status =
target_wait_PRDY(state->target_handler, state->prdy_timeout);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, " wait for PRDY failed, %d",
status);
break;
}
}
else if (cmd == CLEAR_TIMEOUT)
{
// This command does not apply to JTAG
// so we will likely not implement it.
ASD_log(ASD_LogLevel_Info, ASD_LogStream_SDK, ASD_LogOption_None,
"CLEAR_TIMEOUT not yet implemented.");
}
else if (cmd == TAP_RESET)
{
status = JTAG_tap_reset(state->jtag_handler);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_tap_reset failed, %d",
status);
break;
}
}
else if (cmd == WAIT_SYNC)
{
uint16_t timeout = 0;
uint16_t delay = 0;
data_ptr = get_packet_data(&packet, WAIT_SYNC_CMD_LENGTH);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data for the WaitSync command, short "
"packet");
status = ST_ERR;
break;
}
// read timout and delay. 2 bytes each, LSB first
timeout = data_ptr[0] + (data_ptr[1] << 8);
delay = data_ptr[2] + (data_ptr[3] << 8);
status = target_wait_sync(state->target_handler, timeout, delay);
if (status == ST_TIMEOUT)
{
ASD_log(ASD_LogLevel_Warning, ASD_LogStream_SDK,
ASD_LogOption_None, "target_wait_sync timed out");
status = ST_OK;
}
else if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "target_wait_sync failed: %d",
status);
break;
}
}
else if (cmd >= TAP_STATE_MIN && cmd <= TAP_STATE_MAX)
{
status = JTAG_set_tap_state(
state->jtag_handler,
(enum jtag_states)(cmd & (uint8_t)TAP_STATE_MASK));
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_set_tap_state failed, %d",
status);
break;
}
}
else if (cmd >= WRITE_SCAN_MIN && cmd <= WRITE_SCAN_MAX)
{
uint8_t num_of_bits = 0;
uint8_t num_of_bytes = 0;
get_scan_length(cmd, &num_of_bits, &num_of_bytes);
data_ptr = get_packet_data(&packet, num_of_bytes);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data from buffer: %d", num_of_bytes);
status = ST_ERR;
break;
}
status = determine_shift_end_state(state, ScanType_Write, &packet,
&end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"determine_shift_end_state failed, %d", status);
break;
}
status = JTAG_shift(state->jtag_handler, num_of_bits, num_of_bytes,
data_ptr, 0, NULL, end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_shift failed, %d", status);
break;
}
}
else if (cmd >= READ_SCAN_MIN && cmd <= READ_SCAN_MAX)
{
uint8_t num_of_bits = 0;
uint8_t num_of_bytes = 0;
get_scan_length(cmd, &num_of_bits, &num_of_bytes);
if (response_cnt + sizeof(char) + num_of_bytes > MAX_DATA_SIZE)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to process READ_SCAN. "
"Response buffer already full");
status = ST_ERR;
break;
}
state->out_msg.buffer[response_cnt++] = cmd;
status = determine_shift_end_state(state, ScanType_Read, &packet,
&end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"determine_shift_end_state failed, %d", status);
break;
}
status = JTAG_shift(
state->jtag_handler, num_of_bits, 0, NULL, num_of_bytes,
&(state->out_msg.buffer[response_cnt]), end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_shift failed, %d", status);
break;
}
response_cnt += num_of_bytes;
}
else if (cmd >= READ_WRITE_SCAN_MIN)
{
uint8_t num_of_bits = 0;
uint8_t num_of_bytes = 0;
get_scan_length(cmd, &num_of_bits, &num_of_bytes);
if (response_cnt + sizeof(char) + num_of_bytes > MAX_DATA_SIZE)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to process READ_WRITE_SCAN. "
"Response buffer already full");
status = ST_ERR;
break;
}
state->out_msg.buffer[response_cnt++] = cmd;
data_ptr = get_packet_data(&packet, num_of_bytes);
if (data_ptr == NULL)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"Failed to read data from buffer: %d", num_of_bytes);
status = ST_ERR;
break;
}
status = determine_shift_end_state(state, ScanType_ReadWrite,
&packet, &end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None,
"determine_shift_end_state failed, %d", status);
break;
}
status =
JTAG_shift(state->jtag_handler, num_of_bits, num_of_bytes,
data_ptr, num_of_bytes,
&(state->out_msg.buffer[response_cnt]), end_state);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_None, "JTAG_shift failed, %d", status);
break;
}
response_cnt += num_of_bytes;
}
else
{
// Unknown Command
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK, ASD_LogOption_None,
"Encountered unknown command 0x%02x", (int)cmd);
status = ST_ERR;
break;
}
}
if (status == ST_OK)
{
if (memcpy_s(&state->out_msg.header, sizeof(struct message_header),
&s_message->header, sizeof(struct message_header)))
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_JTAG, ASD_LogOption_None,
"memcpy_s: message header to out msg header copy failed.");
status = ST_ERR;
}
state->out_msg.header.size_lsb = lsb_from_msg_size(response_cnt);
state->out_msg.header.size_msb = msb_from_msg_size(response_cnt);
state->out_msg.header.cmd_stat = ASD_SUCCESS;
status = send_response(state, &state->out_msg);
if (status != ST_OK)
{
ASD_log(ASD_LogLevel_Error, ASD_LogStream_SDK,
ASD_LogOption_No_Remote,
"Failed to send message back on the socket");
}
}
return status;
}