void gseDecodePDU(unsigned char *buf) {
unsigned char tag = 0;
CTYPE_INT16U lengthFieldSize = 0;
CTYPE_INT16U lengthValue = 0;
CTYPE_INT16U offsetForSequence = 0;
CTYPE_INT16U offsetForNonSequence = 0;
unsigned char *gocbRef = NULL;
CTYPE_INT16U gocbRefLength = 0;
CTYPE_INT32U timeAllowedToLive = 0;
CTYPE_TIMESTAMP T = 0;
CTYPE_INT32U sqNum = 0;
CTYPE_INT32U stNum = 0;
while (1) {
tag = (unsigned char) buf[0]; // assumes only one byte is used
lengthFieldSize = getLengthFieldSize((unsigned char) buf[1]);
lengthValue = decodeLength((unsigned char *) &buf[1]);
offsetForSequence = 1 + lengthFieldSize;
offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
switch (tag) {
case GSE_TAG_GOCBREF:
// save pointer to gocbRef name
gocbRef = &buf[offsetForSequence];
gocbRefLength = lengthValue;
buf = &buf[offsetForNonSequence];
break;
case GSE_TAG_TIME_ALLOWED_TO_LIVE:
ber_decode_integer(&buf[offsetForSequence], lengthValue, &timeAllowedToLive, SV_GET_LENGTH_INT32U);
buf = &buf[offsetForNonSequence];
break;
case ASN1_TAG_SEQUENCE:
buf = &buf[offsetForSequence];
break;
case GSE_TAG_DATSET:
buf = &buf[offsetForNonSequence];
break;
case GSE_TAG_T:
memcpy(&T, &buf[offsetForSequence], BER_GET_LENGTH_CTYPE_TIMESTAMP(&T));
buf = &buf[offsetForNonSequence];
break;
case GSE_TAG_STNUM:
ber_decode_integer(&buf[offsetForSequence], lengthValue, &stNum, SV_GET_LENGTH_INT32U);
buf = &buf[offsetForNonSequence];
break;
case GSE_TAG_SQNUM:
ber_decode_integer(&buf[offsetForSequence], lengthValue, &sqNum, SV_GET_LENGTH_INT32U);
buf = &buf[offsetForNonSequence];
break;
case GSE_TAG_ALLDATA:
gseDecodeDataset(&buf[offsetForSequence], lengthValue, gocbRef, gocbRefLength, timeAllowedToLive, T, stNum, sqNum);
return;
default:
buf = &buf[offsetForNonSequence];
break;
}
}
}
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_INT32U(unsigned char *buf, CTYPE_INT32U *value) {
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset++] == ASN1_TAG_UNSIGNED) {
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
ber_decode_integer(&buf[offset], len, value, SV_GET_LENGTH_INT32U);
}
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_DECODE_CTYPE_ENUM(unsigned char *buf, CTYPE_ENUM *value) { // assuming enum is an int - allows any enum type to be used
CTYPE_INT16U offset = 0;
CTYPE_INT16U len = 0;
if (buf[offset++] == ASN1_TAG_UNSIGNED) {
len += decodeLength(&buf[offset]);
offset += getLengthFieldSize(buf[offset]);
#if GOOSE_FIXED_SIZE == 1
ber_decode_integer(&buf[offset], len, value, SV_GET_LENGTH_INT8);
#else
ber_decode_integer(&buf[offset], len, value, SV_GET_LENGTH_INT32U);
#endif
}
return offset + len;
}
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 stats3_decompress_bits(range_coder *c,unsigned char m[1025],int *len_out)
{
int i;
*len_out=0;
/* Check if message is encoded naturally */
int notRawASCII=range_decode_equiprobable(c,2);
if (notRawASCII==0) {
/* raw bytes -- copy from input to output */
// printf("decoding raw bytes: bits_used=%d\n",c->bits_used);
for(i=0;c->bit_stream[i]&&i<1024&&i<(c->bit_stream_length>>3);i++) {
m[i]=c->bit_stream[i];
// printf("%d 0x%02x\n",i,c->bit_stream[i]);
}
// printf("%d 0x%02x\n",i,c->bit_stream[i]);
m[i]=0;
*len_out=i;
return 0;
}
int notPackedASCII=range_decode_symbol(c,&probPackedASCII,2);
int encodedLength=decodeLength(c);
for(i=0;i<encodedLength;i++) m[i]='?'; m[i]=0;
*len_out=encodedLength;
if (notPackedASCII==0) {
/* packed ASCII -- copy from input to output */
// printf("decoding packed ASCII\n");
decodePackedASCII(c,(char *)m,encodedLength);
return 0;
}
unsigned char nonAlphaValues[1024];
int nonAlphaPositions[1024];
int nonAlphaCount=0;
decodeNonAlpha(c,nonAlphaPositions,nonAlphaValues,&nonAlphaCount,encodedLength);
int alphaCount=(*len_out)-nonAlphaCount;
// printf("message contains %d non-alpha characters, %d alpha chars.\n",nonAlphaCount,alphaCount);
unsigned char lowerCaseAlphaChars[1025];
decodeLCAlphaSpace(c,lowerCaseAlphaChars,alphaCount);
lowerCaseAlphaChars[alphaCount]=0;
decodeCaseModel1(c,lowerCaseAlphaChars);
mungeCase((char *)lowerCaseAlphaChars);
/* reintegrate alpha and non-alpha characters */
int nonAlphaPointer=0;
int alphaPointer=0;
for(i=0;i<(*len_out);i++)
{
if (nonAlphaPointer<nonAlphaCount
&&nonAlphaPositions[nonAlphaPointer]==i) {
m[i]=nonAlphaValues[nonAlphaPointer++];
} else {
m[i]=lowerCaseAlphaChars[alphaPointer++];
}
}
m[i]=0;
return 0;
}
bool SRUP_MSG_OBS_BASE::DeSerialize(const uint8_t *serial_data)
{
uint16_t x;
uint32_t p=0;
uint8_t bytes[2];
// We need to unmarshall the data to reconstruct the object...
// We can start with the two bytes for the header.
// One for the version - and one for the message type.
std::memcpy(m_version, (uint8_t*) serial_data, 1);
p+=1;
std::memcpy(m_msgtype, (uint8_t*) serial_data + p, 1);
p+=1;
// Now we have to unmarshall the sequence ID...
uint8_t sid_bytes[8];
for (int i=0;i<8;i++)
{
std::memcpy(&sid_bytes[i], (uint8_t*) serial_data + p, 1);
++p;
}
// ... then we copy them into m_sequence_ID
if (m_sequence_ID != nullptr)
delete(m_sequence_ID);
m_sequence_ID = new uint64_t;
std::memcpy(m_sequence_ID, sid_bytes, 8);
// Next we have to unmarshall the sender ID...
uint8_t snd_bytes[8];
for (int i=0;i<8;i++)
{
std::memcpy(&snd_bytes[i], (uint8_t*) serial_data + p, 1);
++p;
}
// ... then we copy them into the sender ID
if (m_sender_ID != nullptr)
delete(m_sender_ID);
m_sender_ID = new uint64_t;
std::memcpy(m_sender_ID, snd_bytes, 8);
// Now we have two-bytes for the size of the token ... and x bytes for the token
std::memcpy(bytes, serial_data + p, 2);
x = decodeLength(bytes);
p+=2;
if(m_token != nullptr)
delete(m_token);
m_token = new uint8_t[x+1];
std::memcpy(m_token, (uint8_t *) serial_data + p, x);
m_token_len = x;
p+=x;
// The next two bytes are the size of the signature...
std::memcpy(bytes, serial_data + p, 2);
x = decodeLength(bytes);
p+=2;
m_sig_len = x;
// The next x bytes are the value of the signature.
if(m_signature != nullptr)
delete(m_signature);
m_signature = new uint8_t[x];
std::memcpy(m_signature, serial_data + p, x);
p+=x;
// Lastly we have the encrypted data.
std::memcpy(bytes, serial_data + p, 2);
x = decodeLength(bytes);
p+=2;
auto* encrypted_data = new uint8_t[x];
std::memcpy(encrypted_data, (uint8_t *) serial_data + p, x);
m_crypto->crypt(encrypted_data, x);
delete[] encrypted_data;
return true;
}
