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); }