static inline void dbg_save_state(int cpu)
{
int i, j;
i = cpu * MAX_DBG_REGS;
switch (dbg.arch) {
case ARM_DEBUG_ARCH_V8:
dbg_write(0x1, OSLAR_EL1);
isb();
dbg.state[i++] = (uint32_t)dbg_readl(MDSCR_EL1);
for (j = 0; j < dbg.nr_bp; j++)
i = dbg_read_arch64_bxr((uint64_t *)dbg.state, i, j);
for (j = 0; j < dbg.nr_wp; j++)
i = dbg_read_arch64_wxr((uint64_t *)dbg.state, i, j);
dbg.state[i++] = (uint32_t)dbg_readl(MDCCINT_EL1);
dbg.state[i++] = (uint32_t)dbg_readl(DBGCLAIMCLR_EL1);
dbg.state[i++] = (uint32_t)dbg_readl(OSECCR_EL1);
dbg.state[i++] = (uint32_t)dbg_readl(OSDTRRX_EL1);
dbg.state[i++] = (uint32_t)dbg_readl(OSDTRTX_EL1);
isb();
dbg_write(0x1, OSDLR_EL1);
isb();
break;
default:
pr_err_ratelimited("unsupported dbg arch %d in %s\n", dbg.arch,
__func__);
}
}
static inline void dbg_save_state(int cpu)
{
int i, j;
i = cpu * MAX_DBG_REGS;
switch (dbg.arch) {
case ARM_DEBUG_ARCH_V8:
dbg_write(OSLOCK_MAGIC, DBGOSLAR);
isb();
dbg.state[i++] = dbg_read(DBGDSCRext);
for (j = 0; j < dbg.nr_bp; j++)
i = dbg_read_arch32_bxr(dbg.state, i, j);
for (j = 0; j < dbg.nr_wp; j++)
i = dbg_read_arch32_wxr(dbg.state, i, j);
dbg.state[i++] = dbg_read(DBGDCCINT);
dbg.state[i++] = dbg_read(DBGCLAIMCLR);
dbg.state[i++] = dbg_read(DBGOSECCR);
dbg.state[i++] = dbg_read(DBGDTRRXext);
dbg.state[i++] = dbg_read(DBGDTRTXext);
isb();
dbg_write(0x1, DBGOSDLR);
isb();
break;
default:
pr_err_ratelimited("unsupported dbg arch %d in %s\n", dbg.arch,
__func__);
}
}
void dbg_write_u32(const unsigned long *val, long len)
{
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(4) | ((len & 0xffff) << 16));
while (len > 0)
{
dbg_write(*val);
val++;
len--;
}
}
void dbg_write_u16(const unsigned short *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(2) | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = val[0]
| ((len > 1) ? val[1] << 16: 0x0000);
dbg_write(dcc_data);
val += 2;
len -= 2;
}
}
void dbg_write_u8(const unsigned char *val, long len)
{
unsigned long dcc_data;
dbg_write(TARGET_REQ_DEBUGMSG_HEXMSG(1) | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = val[0]
| ((len > 1) ? val[1] << 8 : 0x00)
| ((len > 2) ? val[2] << 16 : 0x00)
| ((len > 3) ? val[3] << 24 : 0x00);
dbg_write(dcc_data);
val += 4;
len -= 4;
}
}
void dbg_write_str(const char *msg)
{
long len;
unsigned long dcc_data;
for (len = 0; msg[len] && (len < 65536); len++);
dbg_write(TARGET_REQ_DEBUGMSG_ASCII | ((len & 0xffff) << 16));
while (len > 0)
{
dcc_data = msg[0]
| ((len > 1) ? msg[1] << 8 : 0x00)
| ((len > 2) ? msg[2] << 16 : 0x00)
| ((len > 3) ? msg[3] << 24 : 0x00);
dbg_write(dcc_data);
msg += 4;
len -= 4;
}
}
static int dbg_write_arch32_wxr(uint32_t *state, int i, int j)
{
switch (j) {
case 0:
dbg_write(state[i++], DBGWVR0);
dbg_write(state[i++], DBGWCR0);
break;
case 1:
dbg_write(state[i++], DBGWVR1);
dbg_write(state[i++], DBGWCR1);
break;
case 2:
dbg_write(state[i++], DBGWVR2);
dbg_write(state[i++], DBGWCR2);
break;
case 3:
dbg_write(state[i++], DBGWVR3);
dbg_write(state[i++], DBGWCR3);
break;
case 4:
dbg_write(state[i++], DBGWVR4);
dbg_write(state[i++], DBGWCR4);
break;
case 5:
dbg_write(state[i++], DBGWVR5);
dbg_write(state[i++], DBGWCR5);
break;
case 6:
dbg_write(state[i++], DBGWVR6);
dbg_write(state[i++], DBGWCR6);
break;
case 7:
dbg_write(state[i++], DBGWVR7);
dbg_write(state[i++], DBGWCR7);
break;
case 8:
dbg_write(state[i++], DBGWVR8);
dbg_write(state[i++], DBGWCR8);
break;
case 9:
dbg_write(state[i++], DBGWVR9);
dbg_write(state[i++], DBGWCR9);
break;
case 10:
dbg_write(state[i++], DBGWVR10);
dbg_write(state[i++], DBGWCR10);
break;
case 11:
dbg_write(state[i++], DBGWVR11);
dbg_write(state[i++], DBGWCR11);
break;
case 12:
dbg_write(state[i++], DBGWVR12);
dbg_write(state[i++], DBGWCR12);
break;
case 13:
dbg_write(state[i++], DBGWVR13);
dbg_write(state[i++], DBGWCR13);
break;
case 14:
dbg_write(state[i++], DBGWVR14);
dbg_write(state[i++], DBGWCR14);
break;
case 15:
dbg_write(state[i++], DBGWVR15);
dbg_write(state[i++], DBGWCR15);
break;
default:
pr_err_ratelimited("idx %d out of bounds in %s\n", j, __func__);
}
return i;
}
static int dbg_write_arch64_wxr(uint64_t *state, int i, int j)
{
switch (j) {
case 0:
dbg_write(state[i++], DBGWVR0_EL1);
dbg_write(state[i++], DBGWCR0_EL1);
break;
case 1:
dbg_write(state[i++], DBGWVR1_EL1);
dbg_write(state[i++], DBGWCR1_EL1);
break;
case 2:
dbg_write(state[i++], DBGWVR2_EL1);
dbg_write(state[i++], DBGWCR2_EL1);
break;
case 3:
dbg_write(state[i++], DBGWVR3_EL1);
dbg_write(state[i++], DBGWCR3_EL1);
break;
case 4:
dbg_write(state[i++], DBGWVR4_EL1);
dbg_write(state[i++], DBGWCR4_EL1);
break;
case 5:
dbg_write(state[i++], DBGWVR5_EL1);
dbg_write(state[i++], DBGWCR5_EL1);
break;
case 6:
dbg_write(state[i++], DBGWVR0_EL1);
dbg_write(state[i++], DBGWCR6_EL1);
break;
case 7:
dbg_write(state[i++], DBGWVR7_EL1);
dbg_write(state[i++], DBGWCR7_EL1);
break;
case 8:
dbg_write(state[i++], DBGWVR8_EL1);
dbg_write(state[i++], DBGWCR8_EL1);
break;
case 9:
dbg_write(state[i++], DBGWVR9_EL1);
dbg_write(state[i++], DBGWCR9_EL1);
break;
case 10:
dbg_write(state[i++], DBGWVR10_EL1);
dbg_write(state[i++], DBGWCR10_EL1);
break;
case 11:
dbg_write(state[i++], DBGWVR11_EL1);
dbg_write(state[i++], DBGWCR11_EL1);
break;
case 12:
dbg_write(state[i++], DBGWVR12_EL1);
dbg_write(state[i++], DBGWCR12_EL1);
break;
case 13:
dbg_write(state[i++], DBGWVR13_EL1);
dbg_write(state[i++], DBGWCR13_EL1);
break;
case 14:
dbg_write(state[i++], DBGWVR14_EL1);
dbg_write(state[i++], DBGWCR14_EL1);
break;
case 15:
dbg_write(state[i++], DBGWVR15_EL1);
dbg_write(state[i++], DBGWCR15_EL1);
break;
default:
pr_err_ratelimited("idx %d out of bounds in %s\n", j, __func__);
}
return i;
}
/*
* dbg_sh_writec
*
* Write one or more charracters to the clients' memory
*
* Usage:
* writeb <address> [-c <sid>] -d string
* -c sid Use a special sid (if not defined the current client will be used)
*/
int dbg_sh_writec(void)
{
dbg_shell_t *l__shell = *dbg_tls_shellptr;
int l__infopar = -1;
size_t l__datas = 0;
sid_t l__sid = 0;
uintptr_t l__adr = 0;
/* Right count of parameters? */
if (l__shell->n_pars < 2)
{
dbg_iprintf(l__shell->terminal, "Missing parameters. Try \"help writec\" for more informations.\n");
return -1;
}
/* Get the destination address */
if (dbglib_atoul(l__shell->pars[1], &l__adr, 16))
{
dbg_iprintf(l__shell->terminal, "Illegal address - \"%s\".\n", l__shell->pars[1]);
return NULL;
}
/* Was a SID defined? */
l__sid = dbg_get_sidpar("writea", SIDTYPE_THREAD);
if (l__sid == SID_INVALID) return -1;
/* Get the data parameter */
l__infopar = dbg_test_par(0, "-d");
if (l__infopar == -1)
{
dbg_iprintf(l__shell->terminal, "Missing data parameter. Try \"help writed\" for more informations.\n");
return -1;
}
/* Store the datas to a buffer */
if (((unsigned)l__infopar + 1) >= l__shell->n_pars)
{
dbg_iprintf(l__shell->terminal, "Missing datas. Try \"help writed\" for more informations.\n");
return -1;
}
/* Copy without zero termination */
l__datas = str_len(l__shell->pars[l__infopar + 1], DBGSHELL_CMDBUFFER_SIZE);
if (*tls_errno)
{
return -1;
}
/* Write it to the client memory */
dbg_iprintf(l__shell->terminal, "Writing %i bytes of datas to client 0x%X at 0x%X...", l__datas * sizeof(uint8_t), l__sid, l__adr);
if (dbg_write(l__sid, l__adr, l__shell->pars[l__infopar + 1], l__datas))
{
*tls_errno = 0;
dbg_iprintf(l__shell->terminal, "( FAILED )\nCan't write datas to client. Please look at the system terminal for further informations.\n");
return -1;
}
dbg_iprintf(l__shell->terminal, "( DONE )\n");
return 0;
}
/*
* dbg_sh_writeb
*
* Write one or more bytes to the clients' memory
*
* Usage:
* writeb <address> [-c <sid>] -d <data1> [<data-2> <data-3> ... <data-n>]
* -c sid Use a special sid (if not defined the current client will be used)
*/
int dbg_sh_writeb(void)
{
dbg_shell_t *l__shell = *dbg_tls_shellptr;
int l__infopar = -1;
unsigned l__datas = 0;
sid_t l__sid = 0;
uintptr_t l__adr = 0;
uint8_t *l__buf = NULL;
/* Right count of parameters? */
if (l__shell->n_pars < 2)
{
dbg_iprintf(l__shell->terminal, "Missing parameters. Try \"help writeb\" for more informations.\n");
return -1;
}
/* Get the destination address */
if (dbglib_atoul(l__shell->pars[1], &l__adr, 16))
{
dbg_iprintf(l__shell->terminal, "Illegal address - \"%s\".\n", l__shell->pars[1]);
return NULL;
}
/* Was a SID defined? */
l__sid = dbg_get_sidpar("writeb", SIDTYPE_THREAD);
if (l__sid == SID_INVALID) return -1;
/* Get the data parameter */
l__infopar = dbg_test_par(0, "-d");
if (l__infopar == -1)
{
dbg_iprintf(l__shell->terminal, "Missing data parameter. Try \"help writeb\" for more informations.\n");
return -1;
}
/* Store the datas to a buffer */
l__datas = l__shell->n_pars - (l__infopar + 1);
if (l__datas < 1)
{
dbg_iprintf(l__shell->terminal, "Missing datas. Try \"help writeb\" for more informations.\n");
return -1;
}
l__buf = mem_alloc(l__datas * sizeof(uint8_t));
if (l__buf == NULL)
{
dbg_iprintf(l__shell->terminal, "Can't allocate outbuf buffer, because of %i.\n", *tls_errno);
return -1;
}
for (unsigned l__i = 0; l__i < l__datas; l__i ++)
{
uint32_t l__din;
if (dbglib_atoul(l__shell->pars[l__infopar + l__i + 1], &l__din, 16))
{
dbg_iprintf(l__shell->terminal, "Invalid data paremter - %s. Value ignored.\n", l__shell->pars[l__infopar + l__i + 1]);
}
if (l__din > 255)
{
dbg_iprintf(l__shell->terminal, "Parameter 0x%X is to big for 8-bit output. Value shortened\n", l__din);
}
l__buf[l__i] = l__din;
}
/* Write it to the client memory */
dbg_iprintf(l__shell->terminal, "Writing %i bytes of datas to client 0x%X at 0x%X...", l__datas * sizeof(uint8_t), l__sid, l__adr);
if (dbg_write(l__sid, l__adr, l__buf, l__datas * sizeof(uint8_t)))
{
*tls_errno = 0;
dbg_iprintf(l__shell->terminal, "( FAILED )\nCan't write datas to client. Please look at the system terminal for further informations.\n");
return -1;
}
dbg_iprintf(l__shell->terminal, "( DONE )\n");
mem_free(l__buf);
return 0;
}
void dbg_trace_point(unsigned long number)
{
dbg_write(TARGET_REQ_TRACEMSG | (number << 8));
}
void dbg_write_char(char msg)
{
dbg_write(TARGET_REQ_DEBUGCHAR | ((msg & 0xff) << 16));
}
void printf_handler(void* param, const char *const buf, unsigned int size)
{
dbg_write(buf, size);
}
