///////////////////////////////////////////////////////////////////////////////// 
//
// modbus.cc is part of TestModbus-Server.
//
///////////////////////////////////////////////////////////////////////////////// 

#include <errno.h>
#include <glib.h>
#include <sys/time.h>

#if defined(_WIN32) || defined(_WIN64) || defined(__CYGWIN__)
#else
  #include <unistd.h> /* close() */
#endif

#include "gui.h"
#include "modbussrv.h"
#include "mbsconfig.h"
#include "config.h"


std::string to_hex16 (int v) {
	char HEX[] = "0123456789ABCDEF";
	int i = v;
	int n;
	std::string txt = "";

	for (n = 0; n < 4; n++) {
		txt = HEX[i%16]+ txt;
		i = i / 16;
	}

	return txt;
}


float get_cycletime(struct timeval *t) {
	struct timeval t1;
	float f = 0.0;

	t1 = *t;
	gettimeofday(t, NULL);
	f = (float)(t->tv_sec - t1.tv_sec) + ((t->tv_usec - t1.tv_usec) / 1000000.0);

	return f;
}



//
// C / C++ Wrapper
gpointer _ServerThread (gpointer data) {
	modbussrv.ServerThread ();
	return NULL;
};

//
//

ModbusSrv::ModbusSrv () {
	onchangecallback = NULL;
	ModbusRegister r = {0, false, 0, false};
	port = 502;
	g_mutex_init (&servermutex);
	serverthread = NULL;
	for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
		clients[i] = NULL;
	}

	for (int i = 0; i < MODBUS_IOBUFFER; i++) {
		mbarray[FC1][i] = r;
		mbarray[FC2][i] = r;
		mbarray[FC3][i] = r;
		mbarray[FC4][i] = r;
	}
};

ModbusSrv::~ModbusSrv () {
	for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
		if (clients[i] != NULL) {
			delete clients[i];
			clients[i] = NULL;
		}
	}
};


int ModbusSrv::isRunning() {
	return tcpserver.IsListen();
};


int ModbusSrv::Start(int serverport) {

	port = serverport;
	serverthread = g_thread_new("network thread", _ServerThread, NULL);

	return 1;
};

void ModbusSrv::Stop() {
	g_mutex_lock(&servermutex);
	for (int i = 0; i < MODBUS_MAXCLIENTS; i++) {
		if (clients[i] != NULL) {
			delete clients[i];
			clients[i] = NULL;
		}
	}
	tcpserver.Close();
	g_mutex_unlock(&servermutex);
};


void ModbusSrv::CloseConnection(int slot) {
	if (slot < 0 || slot >= MODBUS_MAXCLIENTS) return;	// slot out of bound
	if (clients[slot] != NULL)
		delete clients[slot];
	clients[slot] = NULL;
};


void ModbusSrv::SetCallback (gboolean (*callback_func)(gpointer data)) {
	onchangecallback = callback_func;
}


void ModbusSrv::ServerThread() {
	int keep_running = 1;
	int ret;
	struct timeval cycletimestamp = { 0 };
	float cycletime = 0.0;
	string s;

	g_mutex_lock(&servermutex);
	ret = tcpserver.Listen(port);
	if (ret != 1) {
		printf ("error:%s\n", strerror(errno));
		keep_running = 0;
	}
	g_mutex_unlock(&servermutex);

	//
	// primary network loop
	//
	while (keep_running) {
		TCP *tcp = NULL;
		int slot;
		int len;

		cycletime = get_cycletime (&cycletimestamp);

		//
		// accept new connection?
		g_mutex_lock(&servermutex);
		if ((tcp = tcpserver.Accept()) != NULL) {
			for (slot = 0; slot < MODBUS_MAXCLIENTS; slot++) {
				if (clients[slot] == NULL) break;
			}

			if (slot < MODBUS_MAXCLIENTS && tcp) {
				char *msg;
				clients[slot] = tcp;
				msg = (char*) malloc (255);
				snprintf (msg, 255, "new connection from %s, use slot:%d\n", clients[slot]->GetRemoteAddr().c_str(), slot);
				gdk_threads_add_idle(cb_thread_network_text_add, msg);
			}
			else {
				// no free slot, accept and close right away.
				if (tcp) {
					char *msg;
					msg = (char*) malloc (255);
					snprintf (msg, 255, "no free slot, close connection.\n");
					gdk_threads_add_idle(cb_thread_network_text_add, msg);
					delete tcp;
				}
			}
		}

		//
		//
		// loop through all clients
		for (slot = 0; slot < MODBUS_MAXCLIENTS; slot++) {
			if (clients[slot] != NULL) {
				len = clients[slot]->ReadTimeout(inbuffer, MODBUS_IOBUFFER, 0);
				if (len < 0) {
					char *msg;
					msg = (char*) malloc (255);
					snprintf (msg, 255, "connection on slot %d closed.\n", slot);
					gdk_threads_add_idle(cb_thread_network_text_add, msg);
					delete clients[slot];
					clients[slot] = NULL;
				}
				else if (len > 0) {
					//
					// incomming data
					//
					struct modbus_data *mbindata = (struct modbus_data*) malloc(sizeof (struct modbus_data));

					if (len > 0x10000) {
						printf ("%s:%d out of bound inbuffer? len:%d max %d\n", __FILE__, __LINE__, len, 0x10000);
						exit (1);
					}

					//
					// reset and copy data to default values
					strncpy (mbindata->hostname, clients[slot]->GetRemoteAddr().c_str(), TEXT_LEN);
					memcpy (mbindata->buffer, inbuffer, len);
					mbindata->bufferlen = len;
					mbindata->fc = 0;
					mbindata->regcnt = -1;
					mbindata->regstart = -1;
					mbindata->unitid = 0;
					mbindata->length = 0;
					mbindata->transactionid = 0;
					mbindata->protoolid = 0;

					if (Decode(mbindata) == 0) {
						mbindata->direction = 2;	// mark as error
						gdk_threads_add_idle(cb_thread_network_data_add, mbindata);
						continue;
					}
					gdk_threads_add_idle(cb_thread_network_data_add, mbindata);
					//
					// fill in outdata
					//

					//
					// reply data - outbuffer
					//
					struct modbus_data *mboutdata = (struct modbus_data*) malloc(sizeof (struct modbus_data));
					strncpy (mboutdata->hostname, clients[slot]->GetRemoteAddr().c_str(), TEXT_LEN);
					memset (mboutdata->buffer, 0x0, 0x10000);
					mboutdata->bufferlen = 0;
					mboutdata->fc = 0;
					mboutdata->regcnt = -1;
					mboutdata->regstart = -1;
					mboutdata->unitid = 0;
					mboutdata->length = 0;
					mboutdata->transactionid = 0;
					mboutdata->protoolid = 0;
					if (mbindata->fc >= 1 && mbindata->fc <= 4) {
						if (WorkerAndEncodeRead(mbindata, mboutdata)) {
							clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
						}
						else {
							char *txt = (char*)malloc(255);
							snprintf (txt, 255, "error on processing message, close connection\n");
							gdk_threads_add_idle(cb_thread_network_text_add, txt);
							delete clients[slot];
							clients[slot] = NULL;

						}
					}
					else if (mbindata->fc == 5 || mbindata->fc == 6) {
						if (WorkerAndEncodeWriteSingle(mbindata, mboutdata)) {
							clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
							delete clients[slot];
							clients[slot] = NULL;
						}
						else {
							char *txt = (char*)malloc(255);
							snprintf (txt, 255, "error on processing message, close connection\n");
							gdk_threads_add_idle(cb_thread_network_text_add, txt);
							delete clients[slot];
							clients[slot] = NULL;
						}
					}
					else if (mbindata->fc == 15 || mbindata->fc == 16) {
						if (WorkerAndEncodeWriteMulti(mbindata, mboutdata)) {
							clients[slot]->Write(mboutdata->buffer, mboutdata->bufferlen);
						}
						else {
							char *txt = (char*)malloc(255);
							snprintf (txt, 255, "error on processing message, close connection\n");
							gdk_threads_add_idle(cb_thread_network_text_add, txt);
							delete clients[slot];
							clients[slot] = NULL;
						}
					}
					gdk_threads_add_idle(cb_thread_network_data_add, mboutdata);
				}
			}
		}
		// unlock servermutex
		cycletime = get_cycletime (&cycletimestamp);
		s = "modbus cycletime: " + std::to_string(1000.0 * cycletime) + "ms";
		gdk_threads_add_idle(cb_thread_status, &s);

		g_mutex_unlock(&servermutex);

		// wait some time (10ms)
		usleep (10000);

		g_mutex_lock(&servermutex);
		keep_running = tcpserver.IsListen();
		g_mutex_unlock(&servermutex);
	}
	s = "modbus server stoped";
	gdk_threads_add_idle(cb_thread_status, &s);  // not thread save we wait 100ms to be sure
	usleep (10000);
}


// return 0 on error
int ModbusSrv::WorkerAndEncodeRead(struct modbus_data *mbin, struct modbus_data *mbout) {
	//
	// to prevent race condition and invalid data: servermutex must be already locked
	// buffer on outdata must be of size 0x10000
	uint16_t i16;
	uint8_t	 i8;
	int pos = 0;
	int i;
	int c;
	int error = 0;

	// transaction
	mbout->transactionid = mbin->transactionid;
	i16 = htons((uint16_t)mbin->transactionid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// protocolid
	mbout->protoolid = mbin->protoolid;
	i16 = htons((uint16_t)mbin->protoolid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// length  3 + number of registers
	mbout->length = 3;
	if (mbin->fc == 1 || mbin->fc == 2) {
		mbout->length += mbin->regcnt / 8;
		if ((mbin->regcnt % 8) > 0) mbout->length += 1;
	}
	else if (mbin->fc == 4 || mbin->fc == 3) {
		mbout->length += mbin->regcnt * 2;		// 16bit = 2Bytes
	}
	if (mbout->length > 0x1000-6) {
		printf ("outbuffer to small? mbout-length:%d\n", mbout->length);
		return 0;
	}
	i16 = htons((uint16_t)mbout->length);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// device id
	mbout->unitid = mbin->unitid;
	memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	// fc
	mbout->fc = mbin->fc;
	memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	// length of data in bytes??????
	i8 = 0;
	if (mbin->fc == 1 || mbin->fc == 2) {
		i8 += mbin->regcnt / 8;
		if ((mbin->regcnt % 8) > 0) i8 += 1;
	}
	else if (mbin->fc == 4 || mbin->fc == 3) {
		i8 += mbin->regcnt * 2;		// 16bit = 2Bytes
	}
	mbout->regcnt = i8;
	memcpy (mbout->buffer+pos, &i8, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	mbout->direction = 1;

	//
	// fill in the buffer
	if (mbin->fc == 1 || mbin->fc == 2) {
		uint8_t u8 = 0;
		for (c= 0, i = mbin->regstart; i < mbin->regstart + mbin->regcnt; i++, c++) {
			if (c != 0 && c%8 == 0) {
				memcpy (&mbout->buffer[pos], &u8, 1);
				u8 = 0;
				pos++;
			}

			if (mbin->fc == 1) {
				if (!mbarray[FC1][i].enabled) error = 1;			// Error
				mbarray[FC1][i].requested |= 1;
				if (mbarray[FC1][i].value) 			u8 |= (1<<(c%8));
			}
			else if (mbin->fc == 2) {
				if (!mbarray[FC2][i].enabled) error = 1;			// Error
				mbarray[FC2][i].requested |= 1;
				if (mbarray[FC2][i].value) 			u8 |= (1<<(c%8));
			}
			else {
				printf ("unknown fc code?  fc:%d\n", mbin->fc);
				error = 1;
			}
		}
		memcpy (&mbout->buffer[pos], &u8, 1);
		pos++;
	}
	else if (mbin->fc == 4 || mbin->fc == 3) {
		for (i = mbin->regstart; i < mbin->regstart + mbin->regcnt; i++) {
			if (mbin->fc == 3) {
				if (!mbarray[FC3][i].enabled) error = 1;			// return nothing
				mbarray[FC3][i].requested |= 1;
				i16 = htons(mbarray[FC3][i].value);
			}
			if (mbin->fc == 4) {
				if (!mbarray[FC4][i].enabled) error = 1;			// return nothing
				mbarray[FC4][i].requested |= 1;
				i16 = htons(mbarray[FC4][i].value);
			}
			memcpy (mbout->buffer+pos, &i16, sizeof(i16));
			pos += sizeof (i16);
		}
	}

	//
	// inform the application about the modbus values change
	if (onchangecallback != NULL && mbin->regcnt > 0) {
		struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));

		mdata->r = (ModbusRegister *) malloc (sizeof (ModbusRegister) * mbin->regcnt);
		int fc = mbin->fc;
		if (fc == 5)  fc = 1;
		if (fc == 6)  fc = 3;
		if (fc == 15)  fc = 1;
		if (fc == 16)  fc = 3;

		if (fc == 1)
			memcpy (mdata->r, &mbarray[FC1][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 2)
			memcpy (mdata->r, &mbarray[FC2][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 3)
			memcpy (mdata->r, &mbarray[FC3][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 4)
			memcpy (mdata->r, &mbarray[FC4][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);

		g_mutex_unlock(&servermutex);
		mdata->fc = fc;
		mdata->regstart = mbin->regstart;
		mdata->count = mbin->regcnt;
		gdk_threads_add_idle(onchangecallback, mdata);  // not thread save we wait 100ms to be sure;
		g_mutex_lock(&servermutex);
	}

	mbout->bufferlen = pos;

	if (error) return 0;
	else return 1;
}


int ModbusSrv::WorkerAndEncodeWriteSingle(struct modbus_data *mbin, struct modbus_data *mbout) {
	uint16_t i16;
	int pos = 0;
	int error = 0;

	// transaction
	mbout->transactionid = mbin->transactionid;
	i16 = htons((uint16_t)mbin->transactionid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// protocolid
	mbout->protoolid = mbin->protoolid;
	i16 = htons((uint16_t)mbin->protoolid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// length  3 + number of registers
	mbout->length = 3;
	mbout->length += mbin->regcnt * 2;		// 16bit = 2Bytes
	i16 = htons((uint16_t)mbout->length);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// device id
	mbout->unitid = mbin->unitid;
	memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	// fc
	mbout->fc = mbin->fc;
	memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	mbout->direction = 1;

	if (mbin->fc == 5) {
		mbarray[FC1][mbin->regstart].requested |= 2;
		if (mbarray[FC1][mbin->regstart].enabled) {
			if (mbin->regcnt == 0xFF00)		mbarray[FC1][mbin->regstart].value = 1;
			else 							mbarray[FC1][mbin->regstart].value = 0;
		}
		else error = 1;

		// return register and value
		i16 = htons((uint16_t)mbin->regstart);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);

		i16 = htons((uint16_t)mbin->regcnt);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);
	}
	if (mbin->fc == 6) {
		mbarray[FC3][mbin->regstart].requested |= 2;
		if (mbarray[FC3][mbin->regstart].enabled)
			mbarray[FC3][mbin->regstart].value = mbin->regcnt;
		else error = 1;

		// return register and value
		i16 = htons((uint16_t)mbin->regstart);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);

		i16 = htons((uint16_t)mbin->regcnt);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);
	}

	mbout->bufferlen = pos;

	//
	// inform the application about the modbus values change
	if (onchangecallback != NULL) {
		struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
		mdata->r = (ModbusRegister*) malloc(sizeof(ModbusRegister));

		int fc = mbin->fc;
		if (fc == 5)  fc = 1;
		if (fc == 6)  fc = 3;
		if (fc == 15)  fc = 1;
		if (fc == 16)  fc = 3;
		*mdata->r = mbarray[fc-1][mbin->regstart];
		mdata->fc = fc;
		mdata->regstart = mbin->regstart;
		mdata->count = 1;
		g_mutex_unlock(&servermutex);
		gdk_threads_add_idle(onchangecallback, mdata);  // not thread save we wait 100ms to be sure;
		g_mutex_lock(&servermutex);
	}

	if (error) return 0;
	else return 1;
}


int ModbusSrv::WorkerAndEncodeWriteMulti(struct modbus_data *mbin, struct modbus_data *mbout) {
	uint16_t i16;
	int pos = 0;
	int i;
	int c;
	int error = 0;

	// transaction
	mbout->transactionid = mbin->transactionid;
	i16 = htons((uint16_t)mbin->transactionid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// protocolid
	mbout->protoolid = mbin->protoolid;
	i16 = htons((uint16_t)mbin->protoolid);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// length  3 + number of registers
	mbout->length = 3;
	mbout->length += mbin->regcnt * 2;		// 16bit = 2Bytes
	i16 = htons((uint16_t)mbout->length);
	memcpy (mbout->buffer+pos, &i16, sizeof(i16));
	pos += sizeof (i16);

	// device id
	mbout->unitid = mbin->unitid;
	memcpy (mbout->buffer+pos, &mbout->unitid, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	// fc
	mbout->fc = mbin->fc;
	memcpy (mbout->buffer+pos, &mbout->fc, sizeof(uint8_t));
	pos += sizeof (uint8_t);

	mbout->direction = 1;

	if (mbin->fc == 15) {
		for (i = 0; mbin->regstart+i < 0x10000 && i < mbin->regcnt; i++) {
			mbarray[FC1][mbin->regstart+i].requested |= 2;
			if (mbarray[FC1][mbin->regstart+i].enabled) {
				if (mbin->buffer[13+(i/8)] & (1<<(i%8)))	mbarray[FC1][mbin->regstart+i].value = 1;
				else			 							mbarray[FC1][mbin->regstart+i].value = 0;
			}
			else error = 1;
		}

		// return register and value
		i16 = htons((uint16_t)mbin->regstart);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);

		i16 = htons((uint16_t)mbin->regcnt);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);
	}
	if (mbin->fc == 16) {
		for (c = 13, i = 0; i < mbin->regcnt && mbin->regstart+i < 0x10000; i++, c += sizeof(uint16_t)) {
			memcpy (&i16, mbin->buffer+c, sizeof(i16));
			mbarray[FC3][mbin->regstart+i].requested |= 2;
			if (mbarray[FC3][mbin->regstart+i].enabled)
				mbarray[FC3][mbin->regstart+i].value = ntohs(i16);
			else error = 1;
		}

		// return register and value
		i16 = htons((uint16_t)mbin->regstart);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);

		i16 = htons((uint16_t)mbin->regcnt);
		memcpy (mbout->buffer+pos, &i16, sizeof(i16));
		pos += sizeof (i16);
	}

	mbout->bufferlen = pos;

	//
	// inform the application about the modbus values change
	if (onchangecallback != NULL && mbin->regcnt > 0 && mbin->regstart > 0 && mbin->regstart < 0x10000) {
		struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
		mdata->r = (ModbusRegister*) malloc (sizeof (ModbusRegister) * mbin->regcnt);;

		int fc = mbin->fc;
		if (fc == 5)  fc = 1;
		if (fc == 6)  fc = 3;
		if (fc == 15)  fc = 1;
		if (fc == 16)  fc = 3;

		if (mbin->regstart+mbin->regcnt > 0xFFFF) {
			printf ("%s:%d ERROR: mbin->regstart+mbin->regcnt > 0xFFFF   limit:%x \n", __FILE__, __LINE__, mbin->regstart+mbin->regcnt);
			exit (1);
		}

		if (fc == 1)
			memcpy (mdata->r, &mbarray[FC1][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 2)
			memcpy (mdata->r, &mbarray[FC2][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 3)
			memcpy (mdata->r, &mbarray[FC3][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);
		else if (fc == 4)
			memcpy (mdata->r, &mbarray[FC4][mbin->regstart], sizeof (ModbusRegister) * mbin->regcnt);

		g_mutex_unlock(&servermutex);
		mdata->fc = fc;
		mdata->regstart = mbin->regstart;
		mdata->count = mbin->regcnt;
		gdk_threads_add_idle(onchangecallback, mdata);  // not thread save we wait 100ms to be sure;
		g_mutex_lock(&servermutex);
	}

	if (error) return 0;
	else return 1;
}


//
// return 1 on success, 0 on error
int ModbusSrv::Decode(struct modbus_data *mbdata) {
	//
	uint16_t i16;
	uint8_t	 i8;
	int pos = 0;
	int ret = 1;

	mbdata->fc = 0;
	mbdata->regcnt = -1;
	mbdata->regstart = -1;
	mbdata->unitid = 0;
	mbdata->length = 0;
	mbdata->direction = 0;
	mbdata->transactionid = 0;
	mbdata->protoolid = 0;
	mbdata->bytecnt = 0;

	if (mbdata->bufferlen < 8) return 0;

	// Transaction
	memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
	pos += sizeof(i16);
	mbdata->transactionid = ntohs(i16);

	// protocol
	memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
	pos += sizeof(i16);
	mbdata->protoolid = ntohs(i16);

	// length
	memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
	pos += sizeof(i16);
	mbdata->length = ntohs(i16);

	// unitid
	if (mbdata->length < pos-6) ret = 0;
	memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
	pos += sizeof(i8);
	mbdata->unitid = i8;

	// function code
	if (mbdata->length < pos-6) ret = 0;
	memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
	pos += sizeof(i8);
	mbdata->fc = i8;

	// register
	if (mbdata->length < pos-6) ret = 0;
	memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
	pos += sizeof(i16);
	mbdata->regstart = ntohs(i16);

	// number of registers
	if (mbdata->length < pos-6) ret = 0;
	memcpy (&i16, mbdata->buffer+pos, sizeof(i16));
	pos += sizeof(i16);
	mbdata->regcnt = ntohs(i16);

	// bytecnt
	// if (mbdata->length < pos-6) ret = 0;
	memcpy (&i8, mbdata->buffer+pos, sizeof(i8));
	pos += sizeof(i8);
	mbdata->bytecnt = i8;

	return ret;
};

int ModbusSrv::GetRegister(int fc, int regnum, ModbusRegister *r) {
	 r->enabled = false;
	 r->requested = 0;
	 r->updated = false;
	 r->value = 0;

	 if (regnum < 0 || regnum >= 0x10000) return -1;

	 g_mutex_lock(&servermutex);

	 if (fc == 1) *r = mbarray[FC1][regnum];
	 else if (fc == 2) *r = mbarray[FC2][regnum];
	 else if (fc == 3) *r = mbarray[FC3][regnum];
	 else if (fc == 4) *r = mbarray[FC4][regnum];

	 g_mutex_unlock(&servermutex);

	 return 0;
};


int ModbusSrv::GetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
	 r->enabled = false;
	 r->requested = 0;
	 r->updated = false;
	 r->value = 0;

	 if (regnum < 0 || regnum >= 0x10000) return -1;

	 g_mutex_lock(&servermutex);

	 if (fc == 1) *r = mbarray[FC1][regnum];
	 else if (fc == 2) *r = mbarray[FC2][regnum];
	 else if (fc == 3) *r = mbarray[FC3][regnum];
	 else if (fc == 4) *r = mbarray[FC4][regnum];

	 g_mutex_unlock(&servermutex);

	 return 0;
};


int ModbusSrv::SetRegister(int fc, int regnum, ModbusRegister *r) {
	 if (regnum < 0 || regnum >= 0x10000) return -1;

	 g_mutex_lock(&servermutex);

	 if (fc == 1) mbarray[FC1][regnum] = *r;
	 else if (fc == 2) mbarray[FC2][regnum] = *r;
	 else if (fc == 3) mbarray[FC3][regnum] = *r;
	 else if (fc == 4) mbarray[FC4][regnum] = *r;

	 g_mutex_unlock(&servermutex);

	 return 0;
}


int ModbusSrv::SetRegisters(int fc, int regnum, int count, ModbusRegister *r) {
	int reg = regnum;
	int regend = regnum+count;

	g_mutex_lock(&servermutex);
	for (reg = regnum; reg < regend; regnum++) {
		if (reg < 0 && reg >= 0x10000) {
			g_mutex_unlock(&servermutex);
			return -1;
		}

		if (fc == 1) mbarray[FC1][reg] = *r;
		else if (fc == 2) mbarray[FC2][reg] = *r;
		else if (fc == 3) mbarray[FC3][reg] = *r;
		else if (fc == 4) mbarray[FC4][reg] = *r;
		r++;
	}
	g_mutex_unlock(&servermutex);

	return 0;
}


void ModbusSrv::Enable(int fc, int regstart, int count, int onoff) {
	int i;
	if (fc < 1 || fc > 4) return;
	if (regstart < 0) return;
	fc--;

	g_mutex_lock(&servermutex);
	for (i = 0; i < count && regstart < MODBUS_IOBUFFER; i++, regstart++) {
		mbarray[fc][regstart].enabled = onoff;
	}
	g_mutex_unlock(&servermutex);
};


void ModbusSrv::RequestsClear() {
	int fc, reg;

	g_mutex_lock(&servermutex);
	for (fc = 0; fc < 4; fc++) for (reg = 0; reg < MODBUS_IOBUFFER; reg++) {
		mbarray[fc][reg].requested = 0;
	}
	g_mutex_unlock(&servermutex);
};


void ModbusSrv::EnableAll(int onoff) {
	int fc, reg;

	g_mutex_lock(&servermutex);
	for (fc = 0; fc < 4; fc++) for (reg = 0; reg < MODBUS_IOBUFFER; reg++) {
		mbarray[fc][reg].enabled = onoff;
	}
	g_mutex_unlock(&servermutex);
};


void ModbusSrv::SetRegValue(int fc, int regstart, int count, uint16_t *values) {
	int reg;

	if (fc <= 0 || fc > 4) {
		printf ("%s:%d fc(%d) is set out of range\n", __FILE__, __LINE__, fc);
		return;
	}

	if (regstart < 0) {
		printf ("%s:%d regstart(%d) is set out of range\n", __FILE__, __LINE__, regstart);
		return;
	}

	if (regstart + count >= MODBUS_IOBUFFER || regstart < 0) {
		printf ("%s:%d regstart(%d) or count(%d) are set out of range\n", __FILE__, __LINE__, regstart, count);
		return;
	}

	g_mutex_lock(&servermutex);
	for (int i = 0, reg = regstart; i < count && reg < 0x10000; i++, reg++) {
		mbarray[fc-1][reg].value =  values[i];
	}

	//
	// inform the application about the modbus values change
	if (onchangecallback != NULL && count > 0 && regstart >= 0 && regstart < 0x10000) {
		struct modbus_callback_data *mdata = (struct modbus_callback_data*) malloc(sizeof(struct modbus_callback_data));
		mdata->r = (ModbusRegister*) malloc (sizeof (ModbusRegister) * count);

		if (fc == 5)  fc = 1;
		if (fc == 6)  fc = 3;
		if (fc == 15)  fc = 1;
		if (fc == 16)  fc = 3;

		if (regstart+count > 0xFFFF) {
			printf ("%s:%d ERROR: regstart+count > 0xFFFF   limit:%x \n", __FILE__, __LINE__, regstart+count);
			exit (1);
		}

		if (fc == 1)
			memcpy (mdata->r, &mbarray[FC1][regstart], sizeof (ModbusRegister) * count);
		else if (fc == 2)
			memcpy (mdata->r, &mbarray[FC2][regstart], sizeof (ModbusRegister) * count);
		else if (fc == 3)
			memcpy (mdata->r, &mbarray[FC3][regstart], sizeof (ModbusRegister) * count);
		else if (fc == 4)
			memcpy (mdata->r, &mbarray[FC4][regstart], sizeof (ModbusRegister) * count);

		g_mutex_unlock(&servermutex);
		mdata->fc = fc;
		mdata->regstart = regstart;
		mdata->count = count;
		gdk_threads_add_idle(onchangecallback, mdata);  // not thread save we wait 100ms to be sure;
		g_mutex_lock(&servermutex);
	}

	g_mutex_unlock(&servermutex);
};