[netwatch.git] / gdb / proto.c

/* proto.c
 * GDB remote serial protocol implementation
 * NetWatch system management mode administration console
 *
 * Copyright 2009, Google Inc.
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 
 *     * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above
 * copyright notice, this list of conditions and the following disclaimer
 * in the documentation and/or other materials provided with the
 * distribution.
 *     * Neither the name of Google Inc. nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,           
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY           
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#define GDB_BUF_SIZE		1024

#include <minilib.h>
#include <demap.h>
#include <tables.h>

#include <lwip/tcp.h>

#include "stub.h"

#define GDB_PORT	2159

enum fsm_result { NEEDMORE, OK, FAIL };

static struct tcp_pcb * last_conn = NULL;

void enhexificate (void * ibuf, char * obuf, int len) {
	unsigned char * cibuf = ibuf;
	while (len > 0) {
		btohex(obuf, *(cibuf++));
		obuf += 2;
		len--;
	}

	*obuf = '\0';
}

void rle (char * buf) {
	char * inptr = buf;
	char * outptr = buf;
	int lastchar = buf[0];
	int repcount = 0;

	do {
		if (*inptr == lastchar && repcount < 97) {
			repcount += 1;
		} else {
			*(outptr++) = lastchar;

			if (repcount == 2)
				*(outptr++) = lastchar;

			while (repcount == 6 || repcount == 7) {
				*(outptr++) = lastchar;
				repcount--;
			}

			if (repcount > 2) {
				*(outptr++) = '*';
				*(outptr++) = repcount + 29;
			}

			repcount = 1;
			lastchar = *inptr;
		}

		inptr++;
	} while (lastchar);

	*(outptr) = 0;
}


struct gdb_state {
	char data[128];
	int writepos;
	int readpos;
};


static void close_conn(struct tcp_pcb *pcb, struct gdb_state *state) {
	outputf("close_conn: bailing");
	set_run_mode(RM_UNENCUMBERED);
	last_conn = NULL;
	tcp_arg(pcb, NULL);
	tcp_sent(pcb, NULL);
	tcp_recv(pcb, NULL);
	mem_free(state);
	tcp_close(pcb);
	outputf("close_conn: done");
}

static int dehexbyte (char * p) {
	int v0, v1;
	v0 = dehexit (p[0]);
	v1 = dehexit (p[1]);
	if (v0 < 0 || v1 < 0) return -1;
	return v0 << 4 | v1;
}

static void finish_and_send(struct tcp_pcb *pcb, char * output_buf, int datalength) {
	char * p;
	unsigned char checksum = 0;

	output_buf[0] = '+';
	output_buf[1] = '$';

	for (p = output_buf + 2; p < output_buf + datalength + 2; p++)
		checksum += *(unsigned char *)p;

	output_buf[datalength + 2] = '#';

	btohex(output_buf + datalength + 3, checksum);
	tcp_write(pcb, output_buf, datalength + 5, TCP_WRITE_FLAG_COPY);
}

void send_stop_packet() {
	if (!last_conn) return;
	tcp_write(last_conn, "$T05thread:01;#07", 17, 0);
}

static enum fsm_result recv_fsm(struct tcp_pcb *pcb, struct gdb_state *state) {
	char * endp;
	char * p;
	unsigned char checksum = 0;
	int i;
	uint64_t addr;
	uint64_t length;

	char output_buf[256];

	/* Make sure we have at least 4 bytes (the size of the smallest legal
	   packet), and that the packet does in fact start with $. */

	if ((state->data[0] == '+') || (state->data[0] == '-')) {
		outputf("GDB: chomp");
		state->readpos++;
		return OK;
	}

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

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

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

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

	if ((!endp) || (endp - state->data > state->writepos - 3))
		return NEEDMORE;

	/* Checksum. */

	for (p = state->data + 1; p < endp; p++)
		checksum += *(unsigned char *)p;

	if (checksum != dehexbyte(endp + 1))
	{
		outputf("GDB: bad checksum: %d vs %d", checksum , dehexbyte(endp + 1));
		return FAIL;
	}

	/* Null-terminate, for processing convenience */
	*endp = '\0';

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

	/* OK, process the packet */

	switch (state->data[1]) {
	case '?':
		tcp_write(pcb, "+$T05thread:01;#07", 18, 0);
		break;
	case 'g':
		read_registers_32(output_buf + 2);
		finish_and_send(pcb, output_buf, 128);
		break;

	case 'm':
		/* Parse the address */
		p = memchr(state->data, ',', endp - state->data);
		if (!p) return FAIL;
		*p = '\0';
		addr = 0;
		length = 0;

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

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

		if (length > 120) length = 120;

		for (i = 0; i < length; i++) {
			p = demap(addr++);
			if (!p) break;
			btohex(output_buf + 2 + (2*i), *(char *)p);
		}

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

	case 's':
		/* Step. */
		set_run_mode(RM_STEPPING);
		tcp_write(pcb, "+", 1, 0);
		break;
		
	default:
		tcp_write(pcb, "+$#00", 5, 0);
		break;
	}

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

	return OK;
}

err_t
gdb_recv(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err) {
	struct gdb_state *state = arg;
	uint16_t copylen;

	if (err != ERR_OK) {
		outputf("GDB: recv err %d", err);
		/* FIXME do something better here? */
		return ERR_OK;
	}

	if (p == NULL) {
		outputf("GDB: Connection closed");
		close_conn(pcb, state);
		return ERR_OK;
	}

	if (p->tot_len > (GDB_BUF_SIZE - state->writepos)) {
		/* Overflow! */
		outputf("GDB: Overflow!");
		close_conn(pcb, state);
		return ERR_OK;
	}

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

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

	state->writepos += p->tot_len;

	tcp_recved(pcb, p->tot_len);
	pbuf_free(p);

	while (1) {
		switch (recv_fsm(pcb, state)) {
		case NEEDMORE:
			outputf("GDB FSM: blocking");
			goto doneprocessing;

		case OK:
			if (state->readpos == state->writepos) {
				state->readpos = 0;
				state->writepos = 0;
				goto doneprocessing;
			} else {
				memmove(state->data,
					state->data + state->readpos,
					state->writepos - state->readpos);
				state->writepos -= state->readpos;
				state->readpos = 0;
			}
			break;
		case FAIL:
			/* Shit */
			outputf("GDB: Protocol error");
			close_conn(pcb, state);
			return ERR_OK;
		}
	}

doneprocessing:
	return ERR_OK;
}

static err_t gdb_sent(void *arg, struct tcp_pcb *pcb, uint16_t len) {
/*
	struct gdb_state *state = arg;
	send_fsm(pcb, state);
*/
	return ERR_OK;
}

static err_t gdb_poll(void *arg, struct tcp_pcb *pcb) {
	/* Nothing? */
	return ERR_OK;
}


static err_t gdb_accept(void *arg, struct tcp_pcb *pcb, err_t err) {
	struct gdb_state *state;

	LWIP_UNUSED_ARG(arg);
	LWIP_UNUSED_ARG(err);

	outputf("GDB: accept");

	last_conn = pcb;

	set_run_mode(RM_STOPPED);

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

	if (!state)
	{
		outputf("gdb_accept: out of memory\n");
		return ERR_MEM;
	}

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

	tcp_arg(pcb, state);
	tcp_recv(pcb, gdb_recv);
	tcp_sent(pcb, gdb_sent);
	tcp_poll(pcb, gdb_poll, 1);
/*
	tcp_err(pcb, gdb_err);
*/
	return ERR_OK;
}


void gdb_init() {
	struct tcp_pcb *pcb;
	pcb = tcp_new();
	tcp_bind(pcb, IP_ADDR_ANY, GDB_PORT);
	pcb = tcp_listen(pcb);
	tcp_accept(pcb, gdb_accept);
} 

PROTOCOL(gdb_init);
Commit	Line	Data
4e3ef36b JW	1	/* proto.c
	2	* GDB remote serial protocol implementation
	3	* NetWatch system management mode administration console
	4	*
	5	* Copyright 2009, Google Inc.
	6	* All rights reserved.
	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	*/
	34
	35	#define GDB_BUF_SIZE 1024
	36
	37	#include <minilib.h>
	38	#include <demap.h>
	39	#include <tables.h>
	40
	41	#include <lwip/tcp.h>
	42
	43	#include "stub.h"
	44
	45	#define GDB_PORT 2159
	46
	47	enum fsm_result { NEEDMORE, OK, FAIL };
	48
	49	static struct tcp_pcb * last_conn = NULL;
	50
	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	}
	58
	59	*obuf = '\0';
	60	}
	61
	62	void rle (char * buf) {
	63	char * inptr = buf;
	64	char * outptr = buf;
65	int lastchar = buf[0];
66	int repcount = 0;
67
68	do {
69	if (*inptr == lastchar && repcount < 97) {
70	repcount += 1;
71	} else {
72	*(outptr++) = lastchar;
73
74	if (repcount == 2)
75	*(outptr++) = lastchar;
76
77	while (repcount == 6 \|\| repcount == 7) {
78	*(outptr++) = lastchar;
79	repcount--;
80	}
81
82	if (repcount > 2) {
83	(outptr++) = '';
84	*(outptr++) = repcount + 29;
85	}
86
87	repcount = 1;
88	lastchar = *inptr;
89	}
90
91	inptr++;
92	} while (lastchar);
93
94	*(outptr) = 0;
95	}
96
97
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 + (2i), (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);