1 files changed, 243 insertions, 0 deletions
diff --git a/reverse-engineering/dosbox_snif/rsp.c b/reverse-engineering/dosbox_snif/rsp.c
new file mode 100644
index 0000000..c9957b7
--- /dev/null
+++ b/reverse-engineering/dosbox_snif/rsp.c
@@ -0,0 +1,243 @@
+#include "rsp.h"
+
+/* recv() */
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include <string.h> /* memset(), strncmp() */
+#include <stdio.h> /* perror(), fprintf(), snprintf(), sscanf() */
+#include <stdlib.h> /* malloc(), free() */
+#include <unistd.h> /* close() */
+#include <errno.h> /* EAGAIN... */
+
+void _rsp_sniff_garbage(struct rsp_state *rsp) {
+	int recvbytes, rv;
+
+	// Sniff garbages before sending command
+	recvbytes = recv(rsp->sockfd, rsp->response, rsp->data_maxlen, MSG_DONTWAIT);
+	if ( recvbytes < 0 ) {
+		rv = errno;
+		if ( rv == EAGAIN || rv == EWOULDBLOCK ) {
+			/* No garbage found, that's a good news */
+		} else {
+			LOG_DEBUG("! <-(error %i)\n", rv);
+		}
+		recvbytes=0;
+	}
+	rsp->response[recvbytes] = '\0';
+	if ( recvbytes > 0 ) {
+		LOG_DEBUG("! <-'%s'\n",rsp->response);
+	}
+}
+
+int rsp_recv_full(struct rsp_state *rsp) {
+
+	int rv, error=0, complete=0, full=0, sentbytes;
+	ssize_t bufoldpos=0, bufpos=0;
+	char *bom=NULL, *eom=NULL, *ptr;
+	unsigned char c, cack='+';
+	unsigned int computed_checksum, read_checksum;
+
+	do {
+		rv = recv(rsp->sockfd, rsp->response + bufpos, rsp->data_maxlen - bufpos, 0);
+		if ( rv < 0 ) {
+			switch (errno) {
+				case EAGAIN:
+					continue;
+				default:
+					error=1;
+					continue;
+			}
+		}
+		if ( rv == 0 ) {
+			error=1;
+			continue;
+		}
+
+		bufoldpos = bufpos;
+		bufpos += rv;
+
+		if ( bom == NULL ) {
+			bom = memchr(rsp->response + bufoldpos, '$', bufpos - bufoldpos);
+		}
+
+		if ( bom != NULL && eom == NULL ) {
+			eom = memchr(bom, '#', bufpos - (bom - rsp->response) );
+		}
+
+		complete = bom && eom && (eom+2) <= (rsp->response + bufpos); // 2 char checksum after EOM
+		full = (bufpos >= rsp->data_maxlen);
+
+	} while (!error && !complete && !full);
+
+	if (error) return 1;
+	if (full) return 2;
+
+	rsp->response[bufpos]='\0';
+
+	c=eom[3];
+	eom[3]='\0';
+	LOG_DEBUG("  <- '%s'\n", rsp->response);
+
+	if ( (eom+3) < (rsp->response + bufpos - 1)) {
+		eom[3]=c;
+		LOG_DEBUG("? <- '%s'\n", eom+3);
+	}
+
+
+	rsp->response_len = eom - bom - 1;
+	rsp->response_bom = bom;
+	rsp->response_eom = eom;
+
+	computed_checksum=0;
+	for (ptr = bom+1; ptr < eom; ptr++) {
+		computed_checksum = (computed_checksum + ((unsigned char)*ptr) ) % 256;
+	}
+
+	rv = sscanf(eom + 1, "%02x", &read_checksum);
+	if ( rv != 1 || computed_checksum != read_checksum) return 3;
+
+	rsp->replied = 1;
+
+	if (rsp->noackmode !=1) {
+		sentbytes = send(rsp->sockfd, &cack, 1, 0);
+		if ( sentbytes < 1 ) return 4;
+		LOG_DEBUG("-> '+'\n");
+	}
+
+	return 0;
+}
+
+int rsp_init(int sockfd, int data_maxlen, struct rsp_state *rsp) {
+	memset(rsp,0,sizeof(struct rsp_state));
+	rsp->sockfd = sockfd;
+	rsp->data_maxlen = data_maxlen;
+
+	rsp->command = malloc(data_maxlen+1);
+	if ( rsp->command == NULL ) return 1;
+
+	rsp->response = malloc(data_maxlen+1);
+	if ( rsp->response == NULL ) return 1;
+	
+	rsp->decoded = malloc(data_maxlen+1);
+	if ( rsp->decoded == NULL ) return 1;
+	
+/*
+	rsp_query(rsp, "QStartNoAckMode");
+	if ( rsp_check_and_clear(rsp, "OK") == 0 ) rsp->noackmode=1;
+*/
+	return 0;
+}
+
+void rsp_quit(struct rsp_state *rsp) {
+	//rsp_query(rsp, "k");
+	if ( rsp->command != NULL ) free(rsp->command);
+	if ( rsp->response != NULL ) free(rsp->response);
+	if ( rsp->decoded != NULL ) free(rsp->decoded);
+	close(rsp->sockfd);
+	memset(rsp,0,sizeof(struct rsp_state));
+}
+
+void rsp_send_break(struct rsp_state *rsp) {
+	int sentbytes;
+	char cbreak=3;
+
+	rsp->replied = 0;
+	rsp->response_len = 0;
+
+	_rsp_sniff_garbage(rsp);
+
+	sentbytes = send(rsp->sockfd, &cbreak, 1, 0);
+	if ( sentbytes < 1 ) return;
+	LOG_DEBUG("-> '^C'\n");
+
+	rsp_recv_full(rsp);
+}
+
+void rsp_query(struct rsp_state *rsp, char command[]) {
+	int sentbytes;
+	unsigned int checksum, i;
+	unsigned char c;
+
+
+	rsp->replied = 0;
+	rsp->response_len = 0;
+
+	i=0; checksum=0;
+	while ( (c=command[i++]) != '\0' ) {
+		checksum = (checksum + c ) % 256;
+	}
+
+	rsp->command_len = snprintf(rsp->command, rsp->data_maxlen, "$%s#%02x", command, checksum);
+	if (rsp->command_len < 5) return;
+
+	_rsp_sniff_garbage(rsp);
+
+	sentbytes = send(rsp->sockfd, rsp->command, rsp->command_len, 0);
+	if ( sentbytes < rsp->command_len ) return;
+	LOG_DEBUG("-> '%s'\n", rsp->command);
+
+
+	rsp_recv_full(rsp);
+}
+
+int rsp_check_and_clear(struct rsp_state *rsp, char expected_response[]) {
+
+	int expected_responselen = strlen(expected_response);
+
+	if ( rsp->replied != 1 ) return 1;
+	rsp->replied = 0;
+
+	if ( rsp->response_len < expected_responselen ) return 2;
+	if ( strncmp(rsp->response_bom + 1, expected_response, expected_responselen) != 0 ) return 3;
+
+	return 0;
+}
+
+int rsp_decode(struct rsp_state *rsp) {
+/*
+Response data can be run-length encoded to save space. Run-length encoding replaces runs of identical characters with one instance of the repeated character, followed by a ‘*’ and a repeat count. The repeat count is itself sent encoded, to avoid binary characters in data: a value of n is sent as n+29. For a repeat count greater or equal to 3, this produces a printable ascii character, e.g. a space (ascii code 32) for a repeat count of 3. (This is because run-length encoding starts to win for counts 3 or more.) Thus, for example, ‘0* ’ is a run-length encoding of “0000”: the space character after ‘*’ means repeat the leading 0 32 - 29 = 3 more times.
+
+The printable characters ‘#’ and ‘$’ or with a numeric value greater than 126 must not be used. Runs of six repeats (‘#’) or seven repeats (‘$’) can be expanded using a repeat count of only five (‘"’). For example, ‘00000000’ can be encoded as ‘0*"00’. 
+*/
+	char src, *srcptr, *dstptr;
+	int repe, max;
+	
+	if ( rsp->replied != 1 ) return -1;
+
+	dstptr = rsp->decoded;
+	for ( srcptr = rsp->response_bom+1; srcptr < rsp->response_eom; srcptr++ ) {
+		// FIXME : implemnt RLE decoding
+		src = *srcptr;
+		switch (src) {
+			case '*':
+				srcptr++;
+				repe=(*srcptr)-29;
+				if (repe < 2 ) return -1;
+				max = rsp->decoded + rsp->data_maxlen - 1 - dstptr;
+				if ( repe > max ) { repe = max; }
+				memset(dstptr, *(srcptr-2), repe);
+				dstptr += repe;
+				break;
+			default:
+				*(dstptr++) = *srcptr;
+				break;
+		}
+	}
+	*dstptr='\0';
+
+	return dstptr - rsp->decoded;
+}
+
+int rsp_poke(struct rsp_state *rsp, unsigned int poke_addr, unsigned int poke_value) {
+	char command[16];
+	printf("DEBUG : rsp_poke()\n");
+	if ( snprintf(command, 15, "M %4x,2:%2x", poke_addr & 0xffff, poke_value & 0xff) < 11 ) return 1;
+	printf("DEBUG : command == '%s'\n", command);
+	rsp_query(rsp, command); // Send the poke command to GDB stub 
+	printf("DEBUG : response == '%s'\n", rsp->response);
+	if ( rsp_check_and_clear(rsp, "OK") != 0 ) return 2;
+	
+	return 0;
+}
+