176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* 276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * Copyright (C) 2008 Stefan Hajnoczi <stefanha@gmail.com>. 376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * This program is free software; you can redistribute it and/or 576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * modify it under the terms of the GNU General Public License as 676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * published by the Free Software Foundation; either version 2 of the 776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * License, or any later version. 876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * This program is distributed in the hope that it will be useful, but 1076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * WITHOUT ANY WARRANTY; without even the implied warranty of 1176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 1276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * General Public License for more details. 1376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 1476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * You should have received a copy of the GNU General Public License 1576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * along with this program; if not, write to the Free Software 1676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 1776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman */ 1876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 1976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/** 2076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * @file 2176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 2276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * GDB stub for remote debugging 2376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 2476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman */ 2576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 2676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman#include <stdlib.h> 2776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman#include <stdint.h> 2876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman#include "serial.h" 2976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 3076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmantypedef uint32_t gdbreg_t; 3176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 3276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanenum { 3376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman POSIX_EINVAL = 0x1c, /* used to report bad arguments to GDB */ 3476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman SIZEOF_PAYLOAD = 256, /* buffer size of GDB payload data */ 3576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman DR7_CLEAR = 0x00000400, /* disable hardware breakpoints */ 3676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman DR6_CLEAR = 0xffff0ff0, /* clear breakpoint status */ 3776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman}; 3876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 3976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* The register snapshot, this must be in sync with interrupt handler and the 4076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * GDB protocol. */ 4176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanenum { 4276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EAX, 4376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_ECX, 4476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EDX, 4576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EBX, 4676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_ESP, 4776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EBP, 4876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_ESI, 4976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EDI, 5076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EIP, 5176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_EFLAGS, 5276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_CS, 5376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_SS, 5476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_DS, 5576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_ES, 5676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_FS, 5776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_GS, 5876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_NREGS, 5976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_SIZEOF_REGS = GDBMACH_NREGS * sizeof(gdbreg_t) 6076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman}; 6176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 6276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* Breakpoint types */ 6376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanenum { 6476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_BPMEM, 6576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_BPHW, 6676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_WATCH, 6776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_RWATCH, 6876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_AWATCH, 6976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman}; 7076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 7176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstruct gdbstub { 7276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int exit_handler; /* leave interrupt handler */ 7376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 7476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int signo; 7576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbreg_t *regs; 7676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 7776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman void (*parse) (struct gdbstub * stub, char ch); 7876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint8_t cksum1; 7976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 8076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Buffer for payload data when parsing a packet. Once the 8176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * packet has been received, this buffer is used to hold 8276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * the reply payload. */ 8376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman char buf[SIZEOF_PAYLOAD + 4]; /* $...PAYLOAD...#XX */ 8476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman char *payload; /* start of payload */ 8576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int len; /* length of payload */ 8676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman}; 8776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 8876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/** Hardware breakpoint, fields stored in x86 bit pattern form */ 8976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstruct hwbp { 9076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int type; /* type (1=write watchpoint, 3=access watchpoint) */ 9176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned long addr; /* linear address */ 9276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman size_t len; /* length (0=1-byte, 1=2-byte, 3=4-byte) */ 9376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int enabled; 9476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman}; 9576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 9676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic struct hwbp hwbps[4]; 9776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic gdbreg_t dr7 = DR7_CLEAR; 9876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 9976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic inline void gdbmach_set_pc(gdbreg_t * regs, gdbreg_t pc) 10076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 10176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman regs[GDBMACH_EIP] = pc; 10276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 10376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 10476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic inline void gdbmach_set_single_step(gdbreg_t * regs, int step) 10576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 10676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman regs[GDBMACH_EFLAGS] &= ~(1 << 8); /* Trace Flag (TF) */ 10776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman regs[GDBMACH_EFLAGS] |= (step << 8); 10876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 10976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 11076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic inline void gdbmach_breakpoint(void) 11176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 11276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman __asm__ __volatile__("int $3\n"); 11376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 11476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 11576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic struct hwbp *gdbmach_find_hwbp(int type, unsigned long addr, size_t len) 11676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 11776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman struct hwbp *available = NULL; 11876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned int i; 11976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman for (i = 0; i < sizeof hwbps / sizeof hwbps[0]; i++) { 12076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (hwbps[i].type == type && hwbps[i].addr == addr 12176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman && hwbps[i].len == len) { 12276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return &hwbps[i]; 12376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 12476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (!hwbps[i].enabled) { 12576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman available = &hwbps[i]; 12676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 12776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 12876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return available; 12976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 13076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 13176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbmach_commit_hwbp(struct hwbp *bp) 13276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 13376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int regnum = bp - hwbps; 13476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 13576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Set breakpoint address */ 13676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman switch (regnum) { 13776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 0: 13876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman__asm__ __volatile__("movl %0, %%dr0\n": :"r"(bp->addr)); 13976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 14076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 1: 14176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman__asm__ __volatile__("movl %0, %%dr1\n": :"r"(bp->addr)); 14276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 14376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 2: 14476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman__asm__ __volatile__("movl %0, %%dr2\n": :"r"(bp->addr)); 14576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 14676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 3: 14776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman__asm__ __volatile__("movl %0, %%dr3\n": :"r"(bp->addr)); 14876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 14976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 15076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 15176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Set type */ 15276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 &= ~(0x3 << (16 + 4 * regnum)); 15376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 |= bp->type << (16 + 4 * regnum); 15476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 15576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Set length */ 15676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 &= ~(0x3 << (18 + 4 * regnum)); 15776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 |= bp->len << (18 + 4 * regnum); 15876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 15976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Set/clear local enable bit */ 16076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 &= ~(0x3 << 2 * regnum); 16176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dr7 |= bp->enabled << 2 * regnum; 16276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 16376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 16476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanint gdbmach_set_breakpoint(int type, unsigned long addr, size_t len, int enable) 16576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 16676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman struct hwbp *bp; 16776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 16876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Check and convert breakpoint type to x86 type */ 16976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman switch (type) { 17076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case GDBMACH_WATCH: 17176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman type = 0x1; 17276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 17376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case GDBMACH_AWATCH: 17476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman type = 0x3; 17576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 17676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman default: 17776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return 0; /* unsupported breakpoint type */ 17876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 17976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 18076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Only lengths 1, 2, and 4 are supported */ 18176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (len != 2 && len != 4) { 18276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman len = 1; 18376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 18476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman len--; /* convert to x86 breakpoint length bit pattern */ 18576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 18676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Set up the breakpoint */ 18776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman bp = gdbmach_find_hwbp(type, addr, len); 18876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (!bp) { 18976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return 0; /* ran out of hardware breakpoints */ 19076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 19176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman bp->type = type; 19276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman bp->addr = addr; 19376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman bp->len = len; 19476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman bp->enabled = enable; 19576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbmach_commit_hwbp(bp); 19676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return 1; 19776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 19876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 19976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbmach_disable_hwbps(void) 20076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 20176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Store and clear hardware breakpoints */ 20276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman __asm__ __volatile__("movl %0, %%dr7\n"::"r"(DR7_CLEAR)); 20376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 20476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 20576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbmach_enable_hwbps(void) 20676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 20776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Clear breakpoint status register */ 20876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman __asm__ __volatile__("movl %0, %%dr6\n"::"r"(DR6_CLEAR)); 20976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 21076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Restore hardware breakpoints */ 21176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman __asm__ __volatile__("movl %0, %%dr7\n"::"r"(dr7)); 21276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 21376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 21476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* Packet parser states */ 21576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_new(struct gdbstub *stub, char ch); 21676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_data(struct gdbstub *stub, char ch); 21776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_cksum1(struct gdbstub *stub, char ch); 21876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_cksum2(struct gdbstub *stub, char ch); 21976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_wait_ack(struct gdbstub *stub, char ch); 22076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 22176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void serial_write(void *buf, size_t len) 22276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 22376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman char *p = buf; 22476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman while (len-- > 0) 22576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman serial_putc(*p++); 22676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 22776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 22876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic uint8_t gdbstub_from_hex_digit(char ch) 22976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 23076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (ch >= '0' && ch <= '9') 23176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return ch - '0'; 23276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman else if (ch >= 'A' && ch <= 'F') 23376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return ch - 'A' + 0xa; 23476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman else 23576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return (ch - 'a' + 0xa) & 0xf; 23676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 23776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 23876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic uint8_t gdbstub_to_hex_digit(uint8_t b) 23976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 24076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman b &= 0xf; 24176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return (b < 0xa ? '0' : 'a' - 0xa) + b; 24276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 24376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 24476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* 24576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * To make reading/writing device memory atomic, we check for 24676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * 2- or 4-byte aligned operations and handle them specially. 24776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman */ 24876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 24976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_from_hex_buf(char *dst, char *src, int lenbytes) 25076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 25176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (lenbytes == 2 && ((unsigned long)dst & 0x1) == 0) { 25276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint16_t i = gdbstub_from_hex_digit(src[2]) << 12 | 25376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[3]) << 8 | 25476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[0]) << 4 | 25576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[1]); 25676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *(uint16_t *) dst = i; 25776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else if (lenbytes == 4 && ((unsigned long)dst & 0x3) == 0) { 25876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint32_t i = gdbstub_from_hex_digit(src[6]) << 28 | 25976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[7]) << 24 | 26076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[4]) << 20 | 26176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[5]) << 16 | 26276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[2]) << 12 | 26376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[3]) << 8 | 26476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[0]) << 4 | 26576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[1]); 26676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *(uint32_t *) dst = i; 26776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 26876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman while (lenbytes-- > 0) { 26976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *dst++ = gdbstub_from_hex_digit(src[0]) << 4 | 27076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_digit(src[1]); 27176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman src += 2; 27276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 27376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 27476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 27576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 27676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_to_hex_buf(char *dst, char *src, int lenbytes) 27776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 27876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (lenbytes == 2 && ((unsigned long)src & 0x1) == 0) { 27976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint16_t i = *(uint16_t *) src; 28076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[0] = gdbstub_to_hex_digit(i >> 4); 28176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[1] = gdbstub_to_hex_digit(i); 28276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[2] = gdbstub_to_hex_digit(i >> 12); 28376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[3] = gdbstub_to_hex_digit(i >> 8); 28476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else if (lenbytes == 4 && ((unsigned long)src & 0x3) == 0) { 28576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint32_t i = *(uint32_t *) src; 28676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[0] = gdbstub_to_hex_digit(i >> 4); 28776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[1] = gdbstub_to_hex_digit(i); 28876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[2] = gdbstub_to_hex_digit(i >> 12); 28976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[3] = gdbstub_to_hex_digit(i >> 8); 29076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[4] = gdbstub_to_hex_digit(i >> 20); 29176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[5] = gdbstub_to_hex_digit(i >> 16); 29276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[6] = gdbstub_to_hex_digit(i >> 28); 29376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman dst[7] = gdbstub_to_hex_digit(i >> 24); 29476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 29576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman while (lenbytes-- > 0) { 29676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *dst++ = gdbstub_to_hex_digit(*src >> 4); 29776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *dst++ = gdbstub_to_hex_digit(*src); 29876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman src++; 29976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 30076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 30176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 30276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 30376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic uint8_t gdbstub_cksum(char *data, int len) 30476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 30576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint8_t cksum = 0; 30676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman while (len-- > 0) { 30776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman cksum += (uint8_t) * data++; 30876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 30976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return cksum; 31076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 31176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 31276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_tx_packet(struct gdbstub *stub) 31376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 31476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint8_t cksum = gdbstub_cksum(stub->payload, stub->len); 31576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->buf[0] = '$'; 31676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->buf[stub->len + 1] = '#'; 31776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->buf[stub->len + 2] = gdbstub_to_hex_digit(cksum >> 4); 31876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->buf[stub->len + 3] = gdbstub_to_hex_digit(cksum); 31976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman serial_write(stub->buf, stub->len + 4); 32076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_wait_ack; 32176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 32276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 32376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* GDB commands */ 32476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_send_ok(struct gdbstub *stub) 32576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 32676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[0] = 'O'; 32776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[1] = 'K'; 32876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 2; 32976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 33076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 33176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 33276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_send_num_packet(struct gdbstub *stub, char reply, int num) 33376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 33476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[0] = reply; 33576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[1] = gdbstub_to_hex_digit((char)num >> 4); 33676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[2] = gdbstub_to_hex_digit((char)num); 33776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 3; 33876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 33976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 34076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 34176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* Format is arg1,arg2,...,argn:data where argn are hex integers and data is not an argument */ 34276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic int gdbstub_get_packet_args(struct gdbstub *stub, unsigned long *args, 34376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int nargs, int *stop_idx) 34476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 34576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int i; 34676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman char ch = 0; 34776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int argc = 0; 34876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned long val = 0; 34976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman for (i = 1; i < stub->len && argc < nargs; i++) { 35076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman ch = stub->payload[i]; 35176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (ch == ':') { 35276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 35376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else if (ch == ',') { 35476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman args[argc++] = val; 35576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman val = 0; 35676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 35776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman val = (val << 4) | gdbstub_from_hex_digit(ch); 35876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 35976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 36076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stop_idx) { 36176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman *stop_idx = i; 36276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 36376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (argc < nargs) { 36476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman args[argc++] = val; 36576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 36676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return ((i == stub->len || ch == ':') && argc == nargs); 36776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 36876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 36976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_send_errno(struct gdbstub *stub, int errno) 37076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 37176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_num_packet(stub, 'E', errno); 37276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 37376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 37476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_report_signal(struct gdbstub *stub) 37576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 37676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_num_packet(stub, 'S', stub->signo); 37776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 37876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 37976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_read_regs(struct gdbstub *stub) 38076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 38176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_to_hex_buf(stub->payload, (char *)stub->regs, GDBMACH_SIZEOF_REGS); 38276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = GDBMACH_SIZEOF_REGS * 2; 38376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 38476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 38576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 38676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_write_regs(struct gdbstub *stub) 38776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 38876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stub->len != 1 + GDBMACH_SIZEOF_REGS * 2) { 38976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_errno(stub, POSIX_EINVAL); 39076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return; 39176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 39276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_buf((char *)stub->regs, &stub->payload[1], 39376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman GDBMACH_SIZEOF_REGS); 39476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_ok(stub); 39576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 39676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 39776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_read_mem(struct gdbstub *stub) 39876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 39976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned long args[2]; 40076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (!gdbstub_get_packet_args 40176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman (stub, args, sizeof args / sizeof args[0], NULL)) { 40276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_errno(stub, POSIX_EINVAL); 40376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return; 40476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 40576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman args[1] = (args[1] < SIZEOF_PAYLOAD / 2) ? args[1] : SIZEOF_PAYLOAD / 2; 40676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_to_hex_buf(stub->payload, (char *)args[0], args[1]); 40776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = args[1] * 2; 40876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 40976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 41076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 41176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_write_mem(struct gdbstub *stub) 41276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 41376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned long args[2]; 41476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int colon; 41576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (!gdbstub_get_packet_args 41676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman (stub, args, sizeof args / sizeof args[0], &colon) || colon >= stub->len 41776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman || stub->payload[colon] != ':' || (stub->len - colon - 1) % 2 != 0) { 41876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_errno(stub, POSIX_EINVAL); 41976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return; 42076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 42176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_from_hex_buf((char *)args[0], &stub->payload[colon + 1], 42276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman (stub->len - colon - 1) / 2); 42376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_ok(stub); 42476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 42576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 42676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_continue(struct gdbstub *stub, int single_step) 42776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 42876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbreg_t pc; 42976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stub->len > 1 43076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman && gdbstub_get_packet_args(stub, (unsigned long *)&pc, 1, NULL)) { 43176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbmach_set_pc(stub->regs, pc); 43276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 43376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbmach_set_single_step(stub->regs, single_step); 43476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->exit_handler = 1; 43576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Reply will be sent when we hit the next breakpoint or interrupt */ 43676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 43776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 43876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_breakpoint(struct gdbstub *stub) 43976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 44076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman unsigned long args[3]; 44176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman int enable = stub->payload[0] == 'Z' ? 1 : 0; 44276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (!gdbstub_get_packet_args 44376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman (stub, args, sizeof args / sizeof args[0], NULL)) { 44476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_errno(stub, POSIX_EINVAL); 44576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman return; 44676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 44776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (gdbmach_set_breakpoint(args[0], args[1], args[2], enable)) { 44876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_ok(stub); 44976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 45076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* Not supported */ 45176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 0; 45276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 45376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 45476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 45576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 45676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_rx_packet(struct gdbstub *stub) 45776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 45876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman switch (stub->payload[0]) { 45976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case '?': 46076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_report_signal(stub); 46176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 46276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'g': 46376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_read_regs(stub); 46476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 46576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'G': 46676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_write_regs(stub); 46776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 46876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'm': 46976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_read_mem(stub); 47076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 47176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'M': 47276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_write_mem(stub); 47376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 47476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'c': /* Continue */ 47576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'k': /* Kill */ 47676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 's': /* Step */ 47776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'D': /* Detach */ 47876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_continue(stub, stub->payload[0] == 's'); 47976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stub->payload[0] == 'D') { 48076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_send_ok(stub); 48176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 48276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 48376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'Z': /* Insert breakpoint */ 48476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman case 'z': /* Remove breakpoint */ 48576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_breakpoint(stub); 48676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 48776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman default: 48876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 0; 48976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 49076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman break; 49176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 49276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 49376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 49476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman/* GDB packet parser */ 49576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_new(struct gdbstub *stub, char ch) 49676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 49776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (ch == '$') { 49876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 0; 49976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_data; 50076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 50176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 50276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 50376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_data(struct gdbstub *stub, char ch) 50476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 50576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (ch == '#') { 50676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_cksum1; 50776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else if (ch == '$') { 50876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->len = 0; /* retry new packet */ 50976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 51076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* If the length exceeds our buffer, let the checksum fail */ 51176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stub->len < SIZEOF_PAYLOAD) { 51276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->payload[stub->len++] = ch; 51376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 51476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 51576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 51676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 51776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_cksum1(struct gdbstub *stub, char ch) 51876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 51976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->cksum1 = gdbstub_from_hex_digit(ch) << 4; 52076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_cksum2; 52176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 52276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 52376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_cksum2(struct gdbstub *stub, char ch) 52476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 52576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint8_t their_cksum; 52676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman uint8_t our_cksum; 52776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 52876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_new; 52976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman their_cksum = stub->cksum1 + gdbstub_from_hex_digit(ch); 53076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman our_cksum = gdbstub_cksum(stub->payload, stub->len); 53176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 53276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (their_cksum == our_cksum) { 53376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman serial_write("+", 1); 53476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (stub->len > 0) { 53576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_rx_packet(stub); 53676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 53776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 53876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman serial_write("-", 1); 53976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 54076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 54176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 54276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanstatic void gdbstub_state_wait_ack(struct gdbstub *stub, char ch) 54376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 54476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman if (ch == '+') { 54576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub->parse = gdbstub_state_new; 54676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } else { 54776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman /* This retransmit is very aggressive but necessary to keep 54876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman * in sync with GDB. */ 54976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_tx_packet(stub); 55076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman } 55176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 55276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 55376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartmanvoid gdbstub_handler(int signo, gdbreg_t * regs) 55476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman{ 55576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman struct gdbstub stub; 55676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 55776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbmach_disable_hwbps(); 55876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 55976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.parse = gdbstub_state_new; 56076d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.payload = &stub.buf[1]; 56176d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.signo = signo; 56276d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.regs = regs; 56376d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.exit_handler = 0; 56476d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbstub_report_signal(&stub); 56576d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman while (!stub.exit_handler) 56676d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman stub.parse(&stub, serial_getc()); 56776d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman 56876d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman gdbmach_enable_hwbps(); 56976d05dc695b06c4e987bb8078f78032441e1430cGreg Hartman} 570