void setTimestamp(CTYPE_TIMESTAMP *dest) {
#if TIMESTAMP_SUPPORTED == 1
unsigned char *buf = (unsigned char *) dest;
struct timeval tv;
CTYPE_INT32U frac = 0;
gettimeofday(&tv, NULL);
frac = (CTYPE_INT32U) ((float) tv.tv_usec * 4294.967296); // * 2^32 / 1000000;
netmemcpy(&buf[0], &tv.tv_sec, 4);
netmemcpy(&buf[4], &frac, 4);
buf[7] = 0x18; // quality: 24 bits of accuracy
#endif
}
int ber_encode_integer_fixed_size(unsigned char *bufDst, void *value, int maxLength) {
unsigned char *firstByte = (unsigned char*) value;
unsigned char padding = (firstByte[0] & 0x80) ? 0xFF : 0x00;
bufDst[0] = padding;
netmemcpy(&bufDst[1], value, maxLength);
return maxLength + 1;
}
int BER_DECODE_CTYPE_FLOAT64(unsigned char *buf, CTYPE_FLOAT64 *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset++] == 0x87) {
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
// check for fixed-length GOOSE. If not, check for 11 bits for exponent
if (len == 9 && buf[offset] == 0x0B) {
netmemcpy(value, &buf[offset + 1], len - 1);
}
else if (len == 8) {
netmemcpy(value, &buf[offset], len);
}
}
return offset + len - 1;
}
int BER_ENCODE_CTYPE_DBPOS(unsigned char *buf, CTYPE_DBPOS *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = BER_GET_LENGTH_CTYPE_DBPOS(value);
buf[offset++] = 0x85;
offset += encodeLength(&buf[offset], len);
netmemcpy(&buf[offset], value, len);
return offset + len;
}
int BER_ENCODE_CTYPE_BOOLEAN(unsigned char *buf, CTYPE_BOOLEAN *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = BER_GET_LENGTH_CTYPE_BOOLEAN(value);
buf[offset++] = ASN1_TAG_BOOLEAN;
offset += encodeLength(&buf[offset], len);
netmemcpy(&buf[offset], value, len);
return offset + len;
}
int BER_ENCODE_CTYPE_QUALITY(unsigned char *buf, CTYPE_QUALITY *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = BER_GET_LENGTH_CTYPE_QUALITY(value);
buf[offset++] = ASN1_TAG_BIT_STRING;
offset += encodeLength(&buf[offset], len);
buf[offset++] = QUALITY_UNUSED_BITS; // number of unused bits
netmemcpy(&buf[offset], value, len - 1);
return offset + len - 1;
}
int BER_ENCODE_CTYPE_FLOAT64(unsigned char *buf, CTYPE_FLOAT64 *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = BER_GET_LENGTH_CTYPE_FLOAT64(value);
buf[offset++] = ASN1_TAG_FLOATING_POINT;
offset += encodeLength(&buf[offset], len);
buf[offset++] = 0x0B; // bits for exponent
netmemcpy(&buf[offset], value, len - 1);
return offset + len - 1;
}
int BER_DECODE_CTYPE_TIMESTAMP(unsigned char *buf, CTYPE_TIMESTAMP *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset++] == 0x89) {
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
netmemcpy(value, &buf[offset], len);
}
return offset + len;
}
int BER_DECODE_CTYPE_BOOLEAN(unsigned char *buf, CTYPE_BOOLEAN *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset++] == ASN1_TAG_BOOLEAN) {
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
netmemcpy(value, &buf[offset], len);
}
return offset + len;
}
int BER_DECODE_CTYPE_QUALITY(unsigned char *buf, CTYPE_QUALITY *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset] == ASN1_TAG_BIT_STRING) {
offset++;
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
netmemcpy(value, &buf[offset + 1], len - 1); // skip over one byte (which contains number of unused bits)
}
return offset + len;
}
int ber_encode_integer(unsigned char *bufDst, void *value, int maxLength) {
unsigned char endian_buf[ENDIAN_BUFFER_SIZE] = {0};
netmemcpy(endian_buf, value, maxLength); // ensure bytes are in big-endian order
unsigned char *buf = endian_buf;
unsigned char *end1 = buf + maxLength - 1;
int shift = 0;
/* Compute the number of superfluous leading bytes */
for(; buf < end1; buf++) {
/*
* If the contents octets of an integer value encoding
* consist of more than one octet, then the bits of the
* first octet and bit 8 of the second octet:
* a) shall not all be ones; and
* b) shall not all be zero.
*/
switch(*buf) {
case 0x00:
if((buf[1] & 0x80) == 0) {
continue;
}
break;
case 0xff:
if((buf[1] & 0x80)) {
continue;
}
break;
}
break;
}
shift = buf - endian_buf;
unsigned char *nb = endian_buf;
unsigned char *end;
maxLength -= shift; /* New size, minus bad bytes */
end = nb + maxLength;
int i = 0;
for(; nb < end; nb++, buf++, i++) {
//*nb = *buf;
bufDst[i] = *buf;
}
return maxLength;
}
//#if GOOSE_FIXED_SIZE == 1
//void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
// ;
//}
//#else
void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
unsigned char endian_buf[ENDIAN_BUFFER_SIZE] = {0};
unsigned char padding = (buf[0] & 0x80) ? 0xFF : 0x00;
int i = 0;
unsigned char *dest = (unsigned char *) value;
for (i = maxLength - 1; i >= 0; i--) {
if ((i + length) < maxLength) {
endian_buf[i] = padding;
}
else {
endian_buf[i] = buf[i - (maxLength - length)];
}
}
netmemcpy(dest, endian_buf, maxLength);
}
int BER_DECODE_CTYPE_DBPOS(unsigned char *buf, CTYPE_DBPOS *value) {
netmemcpy(value, buf, SV_GET_LENGTH_DBPOS);
return SV_GET_LENGTH_DBPOS;
}
int BER_DECODE_CTYPE_VISSTRING255(unsigned char *buf, CTYPE_VISSTRING255 *value) {
netmemcpy(value, buf, SV_GET_LENGTH_VISSTRING255);
return SV_GET_LENGTH_VISSTRING255;
}
// creates a GSE packet, including frame header. returns 0 on fail; number of bytes on success
int gseEncodePacket(struct gseControl *gseControl, unsigned char *buf) {
int offset = 0;
int size = 0;
int ADPULength = getGseHeaderLength(gseControl);
int len = ADPULength + 9 + getLengthBytes(ADPULength); // APDU tag size (1 byte), plus 8 "header" bytes
//printf("ADPULength: %i, len: %i\n", ADPULength, len);
// frame header
memcpy(&buf[offset], gseControl->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], &gseControl->ethHeaderData.VLAN_ID, 2); // TCI
buf[offset] |= (gseControl->ethHeaderData.VLAN_PRIORITY << 5);
offset += 2;
buf[offset++] = 0x88; // EtherType
buf[offset++] = 0xB8;
netmemcpy(&buf[offset], &gseControl->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++] = ASN1_TAG_SEQUENCE;
offset += encodeLength(&buf[offset], ADPULength /*+ getLengthBytes(ADPULength) + 1*/);
buf[offset++] = GSE_TAG_GOCBREF;
size = strlen((const char *) gseControl->gocbRef);
buf[offset++] = size;
memcpy(&buf[offset], gseControl->gocbRef, size);
offset += size;
buf[offset++] = GSE_TAG_TIME_ALLOWED_TO_LIVE;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive));
#if GOOSE_FIXED_SIZE == 1
offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U);
#else
offset += ber_encode_integer(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U);
#endif
buf[offset++] = GSE_TAG_DATSET;
size = strlen((const char *) gseControl->datSet);
buf[offset++] = size;
memcpy(&buf[offset], gseControl->datSet, size);
offset += size;
buf[offset++] = GSE_TAG_GOID;
size = strlen((const char *) gseControl->goID);
buf[offset++] = size;
memcpy(&buf[offset], gseControl->goID, size);
offset += size;
buf[offset++] = GSE_TAG_T;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t));
setTimestamp(&gseControl->t);
memcpy(&buf[offset], &gseControl->t, BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t));
offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t);
buf[offset++] = GSE_TAG_STNUM;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum));
#if GOOSE_FIXED_SIZE == 1
offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U);
#else
offset += ber_encode_integer(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U);
#endif
buf[offset++] = GSE_TAG_SQNUM;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum));
#if GOOSE_FIXED_SIZE == 1
offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U);
#else
offset += ber_encode_integer(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U);
#endif
buf[offset++] = GSE_TAG_SIMULATION;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test));
offset += ber_encode_integer(&buf[offset], &gseControl->test, SV_GET_LENGTH_BOOLEAN);
buf[offset++] = GSE_TAG_CONFREV;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev));
#if GOOSE_FIXED_SIZE == 1
offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U);
#else
offset += ber_encode_integer(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U);
#endif
buf[offset++] = GSE_TAG_NDSCOM;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom));
offset += ber_encode_integer(&buf[offset], &gseControl->ndsCom, SV_GET_LENGTH_BOOLEAN);
buf[offset++] = GSE_TAG_NUMDATSETENTRIES;
offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries));
#if GOOSE_FIXED_SIZE == 1
offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U);
#else
offset += ber_encode_integer(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U);
#endif
buf[offset++] = GSE_TAG_ALLDATA;
offset += encodeLength(&buf[offset], (gseControl->getDatasetLength)());
offset += (gseControl->encodeDataset)(&buf[offset]);
// assume network interface, such as WinPcap, generates CRC bytes
return offset;
}
int BER_ENCODE_CTYPE_VISSTRING255(unsigned char *buf, CTYPE_VISSTRING255 *value) {
netmemcpy(buf, value, BER_GET_LENGTH_CTYPE_VISSTRING255(value));
return BER_GET_LENGTH_CTYPE_VISSTRING255(value);
}
