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_ENCODE_CTYPE_TIMESTAMP(unsigned char *buf, CTYPE_TIMESTAMP *value) { CTYPE_INT16U offset = 0; CTYPE_INT16U len = BER_GET_LENGTH_CTYPE_TIMESTAMP(value); buf[offset++] = ASN1_TAG_OCTET_STRING; offset += encodeLength(&buf[offset], len); memcpy(&buf[offset], value, len); return offset + len; }
int ber_get_length_myCMV(struct myCMV *myCMV) { int total = 0; int len = 0; len = ber_get_length_myVector(&myCMV->cVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&myCMV->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&myCMV->t); total += len + getLengthBytes(len) + 1; return total; }
int ber_get_length_mySPS(struct mySPS *mySPS) { int total = 0; int len = 0; len = BER_GET_LENGTH_CTYPE_INT32(&mySPS->stVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&mySPS->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&mySPS->t); total += len + getLengthBytes(len) + 1; return total; }
int ber_get_length_simpleMV(struct simpleMV *simpleMV) { int total = 0; int len = 0; len = BER_GET_LENGTH_CTYPE_FLOAT32(&simpleMV->mag); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&simpleMV->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&simpleMV->t); total += len + getLengthBytes(len) + 1; len = ber_get_length_ScaledValueConfig(&simpleMV->sVC); total += len + getLengthBytes(len) + 1; return total; }
int ber_get_length_myPos(struct myPos *myPos) { int total = 0; int len = 0; len = BER_GET_LENGTH_CTYPE_DBPOS(&myPos->stVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&myPos->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&myPos->t); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_BOOLEAN(&myPos->ctlVal); total += len + getLengthBytes(len) + 1; return total; }
int ber_get_length_myMod(struct myMod *myMod) { int total = 0; int len = 0; len = BER_GET_LENGTH_CTYPE_ENUM((CTYPE_ENUM *) &myMod->ctlVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_ENUM((CTYPE_ENUM *) &myMod->stVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&myMod->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&myMod->t); total += len + getLengthBytes(len) + 1; return total; }
int ber_get_length_simpleCMV(struct simpleCMV *simpleCMV) { int total = 0; int len = 0; len = ber_get_length_simpleVector(&simpleCMV->cVal); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&simpleCMV->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&simpleCMV->t); total += len + getLengthBytes(len) + 1; len = ber_get_length_mySPS(&simpleCMV->testSecondLayerSDO); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_INT32(&simpleCMV->testInteger); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_BOOLEAN(&simpleCMV->testBoolean); total += len + getLengthBytes(len) + 1; return total; }
int getGseHeaderLength(struct gseControl *gseControl) { int size = 0; int len = 0; size = strlen((const char *) gseControl->gocbRef); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive); len += size + getLengthBytes(size) + 1; size = strlen((const char *) gseControl->datSet); len += size + getLengthBytes(size) + 1; size = strlen((const char *) gseControl->goID); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom); len += size + getLengthBytes(size) + 1; size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries); len += size + getLengthBytes(size) + 1; size = (gseControl->getDatasetLength)(); len += size + getLengthBytes(size) + 1; return len; }
int ber_get_length_myMV(struct myMV *myMV) { int total = 0; int len = 0; len = ber_get_length_myAnalogValue(&myMV->mag); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_QUALITY(&myMV->q); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_TIMESTAMP(&myMV->t); total += len + getLengthBytes(len) + 1; len = ber_get_length_ScaledValueConfig(&myMV->sVC); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_INT32(&myMV->int1); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_INT32(&myMV->int2); total += len + getLengthBytes(len) + 1; len = BER_GET_LENGTH_CTYPE_INT32(&myMV->int3); total += len + getLengthBytes(len) + 1; return total; }
// 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; }