Joshua Wise's Git repositories - netwatch.git/blob

1 /* proto.c

2 * GDB remote serial protocol implementation

3 * NetWatch system management mode administration console

4 *

7 *

8 * Redistribution and use in source and binary forms, with or without

9 * modification, are permitted provided that the following conditions are

10 * met:

11 *

12 * * Redistributions of source code must retain the above copyright

13 * notice, this list of conditions and the following disclaimer.

14 * * Redistributions in binary form must reproduce the above

15 * copyright notice, this list of conditions and the following disclaimer

16 * in the documentation and/or other materials provided with the

17 * distribution.

18 * * Neither the name of Google Inc. nor the names of its

19 * contributors may be used to endorse or promote products derived from

20 * this software without specific prior written permission.

21 *

22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

25 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

26 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

27 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

28 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

29 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

30 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

31 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

32 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

33 */

35 #define GDB_BUF_SIZE 1024

37 #include <minilib.h>

38 #include <demap.h>

39 #include <tables.h>

41 #include <lwip/tcp.h>

43 #include "stub.h"

45 #define GDB_PORT 2159

47 enum fsm_result { NEEDMORE, OK, FAIL };

49 static struct tcp_pcb * last_conn = NULL;

51 void enhexificate (void * ibuf, char * obuf, int len) {

52 unsigned char * cibuf = ibuf;

53 while (len > 0) {

54 btohex(obuf, *(cibuf++));

55 obuf += 2;

56 len--;

57 }

59 *obuf = '\0';

60 }

62 void rle (char * buf) {

63 char * inptr = buf;

64 char * outptr = buf;

65 int lastchar = buf[0];

66 int repcount = 0;

68 do {

69 if (*inptr == lastchar && repcount < 97) {

70 repcount += 1;

71 } else {

72 *(outptr++) = lastchar;

74 if (repcount == 2)

75 *(outptr++) = lastchar;

77 while (repcount == 6 || repcount == 7) {

78 *(outptr++) = lastchar;

79 repcount--;

80 }

82 if (repcount > 2) {

83 *(outptr++) = '*';

84 *(outptr++) = repcount + 29;

85 }

87 repcount = 1;

88 lastchar = *inptr;

89 }

91 inptr++;

92 } while (lastchar);

94 *(outptr) = 0;

95 }

98 struct gdb_state {

99 char data[128];

100 int writepos;

101 int readpos;

102 };

103

104

105 static void close_conn(struct tcp_pcb *pcb, struct gdb_state *state) {

106 outputf("close_conn: bailing");

107 set_run_mode(RM_UNENCUMBERED);

108 last_conn = NULL;

109 tcp_arg(pcb, NULL);

110 tcp_sent(pcb, NULL);

111 tcp_recv(pcb, NULL);

112 mem_free(state);

113 tcp_close(pcb);

114 outputf("close_conn: done");

115 }

116

117 static int dehexbyte (char * p) {

118 int v0, v1;

119 v0 = dehexit (p[0]);

120 v1 = dehexit (p[1]);

121 if (v0 < 0 || v1 < 0) return -1;

122 return v0 << 4 | v1;

123 }

124

125 static void finish_and_send(struct tcp_pcb *pcb, char * output_buf, int datalength) {

126 char * p;

127 unsigned char checksum = 0;

128

129 output_buf[0] = '+';

130 output_buf[1] = '$';

131

132 for (p = output_buf + 2; p < output_buf + datalength + 2; p++)

133 checksum += *(unsigned char *)p;

134

135 output_buf[datalength + 2] = '#';

136

137 btohex(output_buf + datalength + 3, checksum);

138 tcp_write(pcb, output_buf, datalength + 5, TCP_WRITE_FLAG_COPY);

139 }

140

141 void send_stop_packet() {

142 if (!last_conn) return;

143 tcp_write(last_conn, "$T05thread:01;#07", 17, 0);

144 }

145

146 static enum fsm_result recv_fsm(struct tcp_pcb *pcb, struct gdb_state *state) {

147 char * endp;

148 char * p;

149 unsigned char checksum = 0;

150 int i;

151 uint64_t addr;

152 uint64_t length;

153

154 char output_buf[256];

155

156 /* Make sure we have at least 4 bytes (the size of the smallest legal

157 packet), and that the packet does in fact start with $. */

158

159 if ((state->data[0] == '+') || (state->data[0] == '-')) {

160 outputf("GDB: chomp");

161 state->readpos++;

162 return OK;

163 }

164

165 if (state->writepos < 4) return NEEDMORE;

166

167 if (state->data[0] != '$') return FAIL;

168

169 /* Find the end; make sure there's room for the checksum after. */

170

171 endp = memchr(state->data, '#', state->writepos);

172

173 if ((!endp) || (endp - state->data > state->writepos - 3))

174 return NEEDMORE;

175

176 /* Checksum. */

177

178 for (p = state->data + 1; p < endp; p++)

179 checksum += *(unsigned char *)p;

180

181 if (checksum != dehexbyte(endp + 1))

182 {

183 outputf("GDB: bad checksum: %d vs %d", checksum , dehexbyte(endp + 1));

184 return FAIL;

185 }

186

187 /* Null-terminate, for processing convenience */

188 *endp = '\0';

189

190 outputf("GDB: Got \'%s\'", state->data + 1);

191

192 /* OK, process the packet */

193

194 switch (state->data[1]) {

195 case '?':

196 tcp_write(pcb, "+$T05thread:01;#07", 18, 0);

197 break;

198 case 'g':

199 read_registers_32(output_buf + 2);

200 finish_and_send(pcb, output_buf, 128);

201 break;

202

203 case 'm':

204 /* Parse the address */

205 p = memchr(state->data, ',', endp - state->data);

206 if (!p) return FAIL;

207 *p = '\0';

208 addr = 0;

209 length = 0;

210

211 if (!dehexstring(&addr, state->data + 2, -1)) return FAIL;

212 if (!dehexstring(&length, p + 1, -1)) return FAIL;

213

214 outputf("GDB: read %d from %d", (uint32_t)length, (uint32_t)addr);

215

216 if (length > 120) length = 120;

217

218 for (i = 0; i < length; i++) {

219 p = demap(addr++);

220 if (!p) break;

221 btohex(output_buf + 2 + (2*i), *(char *)p);

222 }

223

224 finish_and_send(pcb, output_buf, 2*i);

225 break;

226

227 case 's':

228 /* Step. */

229 set_run_mode(RM_STEPPING);

230 tcp_write(pcb, "+", 1, 0);

231 break;

232

233 default:

234 tcp_write(pcb, "+$#00", 5, 0);

235 break;

236 }

237

238 state->readpos += (endp - state->data) + 3;

239

240 return OK;

241 }

242

243 err_t

244 gdb_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err) {

245 struct gdb_state *state = arg;

246 uint16_t copylen;

247

248 if (err != ERR_OK) {

249 outputf("GDB: recv err %d", err);

250 /* FIXME do something better here? */

251 return ERR_OK;

252 }

253

254 if (p == NULL) {

255 outputf("GDB: Connection closed");

256 close_conn(pcb, state);

257 return ERR_OK;

258 }

259

260 if (p->tot_len > (GDB_BUF_SIZE - state->writepos)) {

261 /* Overflow! */

262 outputf("GDB: Overflow!");

263 close_conn(pcb, state);

264 return ERR_OK;

265 }

266

267 copylen = pbuf_copy_partial(p, state->data + state->writepos, p->tot_len, 0);

268

269 outputf("GDB: Processing %d, wp %d, cp %d", p->tot_len, state->writepos, copylen);

270

271 state->writepos += p->tot_len;

272

273 tcp_recved(pcb, p->tot_len);

274 pbuf_free(p);

275

276 while (1) {

277 switch (recv_fsm(pcb, state)) {

278 case NEEDMORE:

279 outputf("GDB FSM: blocking");

280 goto doneprocessing;

281

282 case OK:

283 if (state->readpos == state->writepos) {

284 state->readpos = 0;

285 state->writepos = 0;

286 goto doneprocessing;

287 } else {

288 memmove(state->data,

289 state->data + state->readpos,

290 state->writepos - state->readpos);

291 state->writepos -= state->readpos;

292 state->readpos = 0;

293 }

294 break;

295 case FAIL:

296 /* Shit */

297 outputf("GDB: Protocol error");

298 close_conn(pcb, state);

299 return ERR_OK;

300 }

301 }

302

303 doneprocessing:

304 return ERR_OK;

305 }

306

307 static err_t gdb_sent(void *arg, struct tcp_pcb *pcb, uint16_t len) {

308 /*

309 struct gdb_state *state = arg;

310 send_fsm(pcb, state);

311 */

312 return ERR_OK;

313 }

314

315 static err_t gdb_poll(void *arg, struct tcp_pcb *pcb) {

316 /* Nothing? */

317 return ERR_OK;

318 }

319

320

321 static err_t gdb_accept(void *arg, struct tcp_pcb *pcb, err_t err) {

322 struct gdb_state *state;

323

324 LWIP_UNUSED_ARG(arg);

325 LWIP_UNUSED_ARG(err);

326

327 outputf("GDB: accept");

328

329 last_conn = pcb;

330

331 set_run_mode(RM_STOPPED);

332

333 state = (struct gdb_state *)mem_malloc(sizeof(struct gdb_state));

334

335 if (!state)

336 {

337 outputf("gdb_accept: out of memory\n");

338 return ERR_MEM;

339 }

340

341 memset(state, 0, sizeof(struct gdb_state));

342

343 tcp_arg(pcb, state);

344 tcp_recv(pcb, gdb_recv);

345 tcp_sent(pcb, gdb_sent);

346 tcp_poll(pcb, gdb_poll, 1);

347 /*

348 tcp_err(pcb, gdb_err);

349 */

350 return ERR_OK;

351 }

352

353

354 void gdb_init() {

355 struct tcp_pcb *pcb;

356 pcb = tcp_new();

357 tcp_bind(pcb, IP_ADDR_ANY, GDB_PORT);

358 pcb = tcp_listen(pcb);

359 tcp_accept(pcb, gdb_accept);

360 }

361

362 PROTOCOL(gdb_init);