File content as of revision 0:230c10b228ea:

#include "svPacketData.h"
#include "svEncode.h"
#include "gseEncode.h"
#include "encodePacket.h"


int svASDULength(struct svData *svData) {
	int len = 0;

	len += strlen((const char *) svData->ASDU[0].svID) + 2;
	//printf("%i, %s\n", strlen(svData->ASDU[0].svID), svData->ASDU[0].svID);
	//len += strlen((const char *) svData->ASDU[0].datset) + 2;
	len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpCnt) + 2;
	len += BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[0].confRev) + 2;
	len += SV_GET_LENGTH_BOOLEAN + 2;
	//len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpRate) + 2;
	len += svData->ASDU[0].data.size + getLengthBytes(svData->ASDU[0].data.size);
	len++;

	return len;
}

int svSeqLength(struct svData *svData) {
	int len = svASDULength(svData);
	len += getLengthBytes(len);
	len++;
	len = len * svData->noASDU;	// assume all ASDUs are the same size

	return len;
}

int svAPDULength(struct svData *svData) {
	int len = svSeqLength(svData);
	len += getLengthBytes(len);
	len++;

	len += 3;

	return len;
}


//TODO: convert to proper BER sizes
// creates an SV packet, including frame header. returns 0 on fail; number of bytes on success
int svEncodePacket(struct svData *svData, unsigned char *buf) {
	int offset = 0;
	int len = svAPDULength(svData);
	len += getLengthBytes(len);
	len += 9;		// savPdu tag size (1 byte), plus 8 "header" bytes

	// frame header
	memcpy(&buf[offset], svData->ethHeaderData.destMACAddress, 6);	// destination MAC addresses
	offset += 6;
	memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6);						// source MAC addresses
	offset += 6;

	buf[offset++] = 0x81;	// TPID
	buf[offset++] = 0x00;

	netmemcpy(&buf[offset], &svData->ethHeaderData.VLAN_ID, 2);	// TCI
	buf[offset] |= (svData->ethHeaderData.VLAN_PRIORITY << 5);
	offset += 2;

	buf[offset++] = 0x88;	// EtherType
	buf[offset++] = 0xBA;

	netmemcpy(&buf[offset], &svData->ethHeaderData.APPID, 2);	// APPID
	offset += 2;

	netmemcpy(&buf[offset], &len, 2);	// length
	offset += 2;

	buf[offset++] = 0x00;	// reserved 1
	buf[offset++] = 0x00;
	buf[offset++] = 0x00;	// reserved 2
	buf[offset++] = 0x00;

	buf[offset++] = 0x60;
	offset += encodeLength(&buf[offset], svAPDULength(svData));

	//TODO noASDU may be > 126?
	buf[offset++] = 0x80;
	buf[offset++] = 1;
	buf[offset++] = (unsigned char) svData->noASDU;

	buf[offset++] = 0xA2;
	offset += encodeLength(&buf[offset], svSeqLength(svData));

	int i = 0;
	int size = 0;
	for (i = 0; i < svData->noASDU; i++) {
		buf[offset++] = 0x30;
		offset += encodeLength(&buf[offset], svASDULength(svData));

		size = strlen((const char *) svData->ASDU[i].svID);
		buf[offset++] = 0x80;
		buf[offset++] = size;
		memcpy(&buf[offset], svData->ASDU[i].svID, size);
		offset += size;

		/*size = strlen(svData->ASDU[i].datset);
		buf[offset++] = 0x81;
		buf[offset++] = size;
		memcpy(&buf[offset], svData->ASDU[i].datset, size);
		offset += size;*/

		buf[offset++] = 0x82;
		offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt));
		offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
		//buf[offset++] = BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt);
		//offset += BER_ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpCnt);

		buf[offset++] = 0x83;
		offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev));
		offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
		//buf[offset++] = BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev);
		//offset += BER_ENCODE_CTYPE_INT32U(&buf[offset], &svData->ASDU[i].confRev);

		buf[offset++] = 0x85;
		buf[offset++] = SV_GET_LENGTH_BOOLEAN;
		offset += ENCODE_CTYPE_BOOLEAN(&buf[offset], &svData->ASDU[i].smpSynch);

		/*buf[offset++] = 0x86;
		buf[offset++] = SV_GET_LENGTH_INT16U;
		offset += ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpRate);*/

		buf[offset++] = 0x87;
		offset += encodeLength(&buf[offset], svData->ASDU[i].data.size);
		memcpy(&buf[offset], svData->ASDU[i].data.data, svData->ASDU[i].data.size);
		offset += svData->ASDU[i].data.size;
	}

	// assume network interface, such as WinPcap, generates CRC bytes

	return offset;
}