#include "rsp_lemm.h"
#include "utils.h"

#include <string.h> /* memset(), strncmp() */
#include <stdio.h> /* printf() */

int rsp_lemm_init(struct rsp_state *rsp, char *ds_si) {
	int rv;
	int sockfd;

	memset(ds_si,'X',10);
	ds_si[9]='\0';

	rv = tcp_client_init(HOST, PORT, &sockfd);
	if ( rv != 0 ) return rv;

	rsp_init(sockfd, MAXDATASIZE-1, rsp);

/*
	rsp_query(rsp, "qRcmd,666c61745f6569702c30"); //flat_eip,0
	if ( rsp_check_and_clear(rsp, "OK") != 0 ) printf("Bug\n");
*/
	rsp_query(rsp, "?");
	if  ( rsp_check_and_clear(rsp, "S05") != 0 ) {
		// The program is not stopped
		rsp_send_break(rsp); // Request to freeze the program
		if ( rsp->replied != 1 ) return 10;
	}

	rsp_query(rsp, "Z0,38C4,1"); // Set execution breakpoint at 0208:1844 (0x38C4)
	if ( rsp_check_and_clear(rsp, "OK") != 0 ) return 11;

	do {
		rsp_query(rsp, "c"); // Continue
		if ( rsp->replied != 1 ) continue; //return 12;
		rsp_recv_full(rsp);
		if ( rsp_check_and_clear(rsp, "S05") != 0 ) continue; //return 13;
//		(void) rsp_check_and_clear(rsp, "S05");

		rsp_query(rsp, "p8"); // Read $eip (/!\ byte order. ex : $e4b0* !#76 )
//		if ( rsp_check_and_clear(rsp, "c4380000") != 0 ) return 14;
	} while ( rsp_check_and_clear(rsp, "c4380000") != 0 );

	rsp_query(rsp, "pc"); // Read $ds
	if ( rsp_decode(rsp) <8 ) return 15;
	ds_si[0]=rsp->decoded[2];
	ds_si[1]=rsp->decoded[3];
	ds_si[2]=rsp->decoded[0];
	ds_si[3]=rsp->decoded[1];
	ds_si[4]=':';

	rsp_query(rsp, "p6"); // Read $si
	if ( rsp_decode(rsp) <8 ) return 16;
	ds_si[5]=rsp->decoded[2];
	ds_si[6]=rsp->decoded[3];
	ds_si[7]=rsp->decoded[0];
	ds_si[8]=rsp->decoded[1];

	printf("ds:si == %s\n", ds_si);
	flatten(ds_si);
	printf("$ds_si == %s\n", ds_si);

	return 0;
}