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

#include <string.h>
#include <errno.h>
#include "modbus.h"

#define PACK_INT8(__dstptr, __srci8)					\
	memcpy(__dstptr, &__srci8, 1); 						\
	__dstptr += 1

#define UNPACK_INT8(__dsti8, __srcptr)					\
	memcpy(&__dsti8, __srcptr, 1); 						\
	__srcptr += 1

#define PACK_INT16(__tmpi16, __dstptr, __srci16) 		\
	__tmpi16 = htons((uint16_t)__srci16); 				\
	memcpy(__dstptr, &__tmpi16, 2); 					\
	__dstptr += 2

#define UNPACK_INT16(__tmpi16, __dsti16, __srcptr) 		\
	memcpy(&__tmpi16, __srcptr, 2);						\
	__srcptr += 2; 										\
	__dsti16 = ntohs((uint16_t)__tmpi16)

/*
 * packs the header into the buffer data and returns the lenght of the header
 * returns NULL on error or points to end of data
 */
char * modbustcp_pkt_header (char *destbuffer, int bufsize, modbustcp_header *srcmbh) {
	uint16_t tmp;

	// destination memory valid
	if (destbuffer == NULL) {
		errno = EFAULT;
		return NULL;
	}
	if (bufsize < 12) {
		errno = EOVERFLOW;
		return NULL;
	}

	char *pos = destbuffer;

	PACK_INT16(tmp, pos, srcmbh->transid);
	PACK_INT16(tmp, pos, srcmbh->protid);
	PACK_INT16(tmp, pos, srcmbh->length);
	PACK_INT8(pos, srcmbh->uid);
	PACK_INT8(pos, srcmbh->fc);
	PACK_INT16(tmp, pos, srcmbh->offset);
	PACK_INT16(tmp, pos, srcmbh->number);

	return pos;
}


/*
 * unpacks the buffer into the header
 * returns NULL on error or points to end of data
 */
char * modbustcp_unpkt_header (modbustcp_header *destmbh, char *srcbuffer, int bufsize) {
	uint16_t tmp;

	// source memory valid
	if (srcbuffer == NULL) {
		errno = EFAULT;
		return NULL;
	}
	if (bufsize < 12) {
		errno = EOVERFLOW;
		return NULL;
	}

	char *src = srcbuffer;

	UNPACK_INT16(tmp, destmbh->transid, src);
	UNPACK_INT16(tmp, destmbh->protid, src);
	UNPACK_INT16(tmp, destmbh->length, src);
	UNPACK_INT8(destmbh->uid, src);
	UNPACK_INT8(destmbh->fc, src);
	UNPACK_INT16(tmp, destmbh->offset, src);
	UNPACK_INT16(tmp, destmbh->number, src);

	return src;
}