void _stlink_usb_read_all_regs(stlink_t *sl, reg *regp) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const cmd = sl->c_buf;
unsigned char* const data = sl->q_buf;
ssize_t size;
uint32_t rep_len = 84;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_READALLREGS;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
sl->q_len = (size_t) size;
stlink_print_data(sl);
for(i=0; i<16; i++)
regp->r[i]= read_uint32(sl->q_buf, i*4);
regp->xpsr = read_uint32(sl->q_buf, 64);
regp->main_sp = read_uint32(sl->q_buf, 68);
regp->process_sp = read_uint32(sl->q_buf, 72);
regp->rw = read_uint32(sl->q_buf, 76);
regp->rw2 = read_uint32(sl->q_buf, 80);
if (sl->verbose < 2)
return;
DLOG("xpsr = 0x%08x\n", read_uint32(sl->q_buf, 64));
DLOG("main_sp = 0x%08x\n", read_uint32(sl->q_buf, 68));
DLOG("process_sp = 0x%08x\n", read_uint32(sl->q_buf, 72));
DLOG("rw = 0x%08x\n", read_uint32(sl->q_buf, 76));
DLOG("rw2 = 0x%08x\n", read_uint32(sl->q_buf, 80));
}
void _stlink_usb_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
int i = fill_command(sl, SG_DXFER_TO_DEV, 0);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_WRITEMEM_8BIT;
write_uint32(&cmd[i], addr);
write_uint16(&cmd[i + 4], len);
send_only(slu, 0, cmd, slu->cmd_len);
send_only(slu, 1, data, len);
}
void _stlink_usb_exit_debug_mode(stlink_t *sl) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
int i = fill_command(sl, SG_DXFER_FROM_DEV, 0);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_EXIT;
size = send_only(slu, 1, cmd, slu->cmd_len);
if (size == -1) {
printf("[!] send_only\n");
return;
}
}
void _stlink_usb_force_debug(stlink_t *sl) {
struct stlink_libusb *slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
int rep_len = 2;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_FORCEDEBUG;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
}
int _stlink_usb_current_mode(stlink_t * sl) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const cmd = sl->c_buf;
unsigned char* const data = sl->q_buf;
ssize_t size;
int rep_len = 2;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_GET_CURRENT_MODE;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return -1;
}
return sl->q_buf[0];
}
void _stlink_usb_version(stlink_t *sl) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
uint32_t rep_len = 6;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_GET_VERSION;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
}
void _stlink_usb_enter_swd_mode(stlink_t * sl) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
const int rep_len = 0;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_ENTER;
cmd[i++] = STLINK_DEBUG_ENTER_SWD;
size = send_only(slu, 1, cmd, slu->cmd_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
}
void _stlink_usb_core_id(stlink_t * sl) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const cmd = sl->c_buf;
unsigned char* const data = sl->q_buf;
ssize_t size;
int rep_len = 4;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_READCOREID;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
sl->core_id = read_uint32(data, 0);
}
void _stlink_usb_read_reg(stlink_t *sl, int r_idx, reg *regp) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
uint32_t r;
uint32_t rep_len = 4;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_READREG;
cmd[i++] = (uint8_t) r_idx;
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
sl->q_len = (size_t) size;
stlink_print_data(sl);
r = read_uint32(sl->q_buf, 0);
DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
switch (r_idx) {
case 16:
regp->xpsr = r;
break;
case 17:
regp->main_sp = r;
break;
case 18:
regp->process_sp = r;
break;
case 19:
regp->rw = r; /* XXX ?(primask, basemask etc.) */
break;
case 20:
regp->rw2 = r; /* XXX ?(primask, basemask etc.) */
break;
default:
regp->r[r_idx] = r;
}
}
void _stlink_usb_write_reg(stlink_t *sl, uint32_t reg, int idx) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
uint32_t rep_len = 2;
int i = fill_command(sl, SG_DXFER_FROM_DEV, rep_len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_WRITEREG;
cmd[i++] = idx;
write_uint32(&cmd[i], reg);
size = send_recv(slu, 1, cmd, slu->cmd_len, data, rep_len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
sl->q_len = (size_t) size;
stlink_print_data(sl);
}
void _stlink_usb_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libusb * const slu = sl->backend_data;
unsigned char* const data = sl->q_buf;
unsigned char* const cmd = sl->c_buf;
ssize_t size;
int i = fill_command(sl, SG_DXFER_FROM_DEV, len);
cmd[i++] = STLINK_DEBUG_COMMAND;
cmd[i++] = STLINK_DEBUG_READMEM_32BIT;
write_uint32(&cmd[i], addr);
write_uint16(&cmd[i + 4], len);
size = send_recv(slu, 1, cmd, slu->cmd_len, data, len);
if (size == -1) {
printf("[!] send_recv\n");
return;
}
sl->q_len = (size_t) size;
stlink_print_data(sl);
}
/*
* cmyth_proginfo_fill(cmyth_conn_t control, cmyth_proginfo_t prog)
*
* Scope: PRIVATE (static)
*
* Description
*
* Fill out a (possibly incomplete) program info. Incomplete program
* info comes from program listings. Since this modifies the contents of
* the supplied program info, it must never be called with a program info
* that has more than one reference).
*
* Return Value:
*
* Success: 0
*
* Failure: a negative error code.
*/
static int
cmyth_proginfo_fill(cmyth_conn_t control, cmyth_proginfo_t prog)
{
int err = 0;
int count;
int ret;
long long length = 0;
if (!control) {
cmyth_dbg(CMYTH_DBG_ERROR, "%s: no connection\n",
__FUNCTION__);
return -EINVAL;
}
if (!prog) {
cmyth_dbg(CMYTH_DBG_ERROR, "%s: no program info\n",
__FUNCTION__);
return -EINVAL;
}
pthread_mutex_lock(&control->conn_mutex);
length = prog->proginfo_Length;
if ((ret=fill_command(control, prog, "FILL_PROGRAM_INFO") != 0))
goto out;
count = cmyth_rcv_length(control);
if (count < 0) {
cmyth_dbg(CMYTH_DBG_ERROR,
"%s: cmyth_rcv_length() failed (%d)\n",
__FUNCTION__, count);
ret = count;
goto out;
}
if (cmyth_rcv_proginfo(control, &err, prog, count) != count) {
cmyth_dbg(CMYTH_DBG_ERROR,
"%s: cmyth_rcv_proginfo() < count\n", __FUNCTION__);
ret = err;
goto out;
}
/*
* Myth seems to cache the program length, rather than call stat()
* every time it needs to know. Using FILL_PROGRAM_INFO has worked
* to force mythbackend to call stat() and return the correct length.
*
* However, some users are reporting that FILL_PROGRAM_INFO is
* returning 0 for the program length. In that case, the original
* number is still probably wrong, but it's better than 0.
*/
if (prog->proginfo_Length == 0) {
prog->proginfo_Length = length;
ret = -1;
goto out;
}
ret = 0;
out:
pthread_mutex_unlock(&control->conn_mutex);
return ret;
}
