An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.
An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.
rapid61850/gseEncodePacket.c@0:f09b7bb8bcce, 2012-10-02 (annotated)
- Committer:
- sblair
- Date:
- Tue Oct 02 21:31:05 2012 +0000
- Revision:
- 0:f09b7bb8bcce
converted library to folder
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sblair | 0:f09b7bb8bcce | 1 | /** |
sblair | 0:f09b7bb8bcce | 2 | * Rapid-prototyping protection schemes with IEC 61850 |
sblair | 0:f09b7bb8bcce | 3 | * |
sblair | 0:f09b7bb8bcce | 4 | * Copyright (c) 2012 Steven Blair |
sblair | 0:f09b7bb8bcce | 5 | * |
sblair | 0:f09b7bb8bcce | 6 | * This program is free software; you can redistribute it and/or |
sblair | 0:f09b7bb8bcce | 7 | * modify it under the terms of the GNU General Public License |
sblair | 0:f09b7bb8bcce | 8 | * as published by the Free Software Foundation; either version 2 |
sblair | 0:f09b7bb8bcce | 9 | * of the License, or (at your option) any later version. |
sblair | 0:f09b7bb8bcce | 10 | |
sblair | 0:f09b7bb8bcce | 11 | * This program is distributed in the hope that it will be useful, |
sblair | 0:f09b7bb8bcce | 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
sblair | 0:f09b7bb8bcce | 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
sblair | 0:f09b7bb8bcce | 14 | * GNU General Public License for more details. |
sblair | 0:f09b7bb8bcce | 15 | |
sblair | 0:f09b7bb8bcce | 16 | * You should have received a copy of the GNU General Public License |
sblair | 0:f09b7bb8bcce | 17 | * along with this program; if not, write to the Free Software |
sblair | 0:f09b7bb8bcce | 18 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
sblair | 0:f09b7bb8bcce | 19 | */ |
sblair | 0:f09b7bb8bcce | 20 | |
sblair | 0:f09b7bb8bcce | 21 | #include "encodePacket.h" |
sblair | 0:f09b7bb8bcce | 22 | #include "gsePacketData.h" |
sblair | 0:f09b7bb8bcce | 23 | #include "gseEncode.h" |
sblair | 0:f09b7bb8bcce | 24 | #include "svEncode.h" |
sblair | 0:f09b7bb8bcce | 25 | |
sblair | 0:f09b7bb8bcce | 26 | int getGseHeaderLength(struct gseControl *gseControl) { |
sblair | 0:f09b7bb8bcce | 27 | int size = 0; |
sblair | 0:f09b7bb8bcce | 28 | int len = 0; |
sblair | 0:f09b7bb8bcce | 29 | |
sblair | 0:f09b7bb8bcce | 30 | size = strlen((const char *) gseControl->gocbRef); |
sblair | 0:f09b7bb8bcce | 31 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 32 | |
sblair | 0:f09b7bb8bcce | 33 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive); |
sblair | 0:f09b7bb8bcce | 34 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 35 | |
sblair | 0:f09b7bb8bcce | 36 | size = strlen((const char *) gseControl->datSet); |
sblair | 0:f09b7bb8bcce | 37 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 38 | |
sblair | 0:f09b7bb8bcce | 39 | size = strlen((const char *) gseControl->goID); |
sblair | 0:f09b7bb8bcce | 40 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 41 | |
sblair | 0:f09b7bb8bcce | 42 | size = BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t); |
sblair | 0:f09b7bb8bcce | 43 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 44 | |
sblair | 0:f09b7bb8bcce | 45 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum); |
sblair | 0:f09b7bb8bcce | 46 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 47 | |
sblair | 0:f09b7bb8bcce | 48 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum); |
sblair | 0:f09b7bb8bcce | 49 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 50 | |
sblair | 0:f09b7bb8bcce | 51 | size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test); |
sblair | 0:f09b7bb8bcce | 52 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 53 | |
sblair | 0:f09b7bb8bcce | 54 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev); |
sblair | 0:f09b7bb8bcce | 55 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 56 | |
sblair | 0:f09b7bb8bcce | 57 | size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom); |
sblair | 0:f09b7bb8bcce | 58 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 59 | |
sblair | 0:f09b7bb8bcce | 60 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries); |
sblair | 0:f09b7bb8bcce | 61 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 62 | |
sblair | 0:f09b7bb8bcce | 63 | size = (gseControl->getDatasetLength)(); |
sblair | 0:f09b7bb8bcce | 64 | len += size + getLengthBytes(size) + 1; |
sblair | 0:f09b7bb8bcce | 65 | |
sblair | 0:f09b7bb8bcce | 66 | return len; |
sblair | 0:f09b7bb8bcce | 67 | } |
sblair | 0:f09b7bb8bcce | 68 | |
sblair | 0:f09b7bb8bcce | 69 | // creates a GSE packet, including frame header. returns 0 on fail; number of bytes on success |
sblair | 0:f09b7bb8bcce | 70 | int gseEncodePacket(struct gseControl *gseControl, unsigned char *buf) { |
sblair | 0:f09b7bb8bcce | 71 | int offset = 0; |
sblair | 0:f09b7bb8bcce | 72 | int size = 0; |
sblair | 0:f09b7bb8bcce | 73 | int ADPULength = getGseHeaderLength(gseControl); |
sblair | 0:f09b7bb8bcce | 74 | int len = ADPULength + 9 + getLengthBytes(ADPULength); // APDU tag size (1 byte), plus 8 "header" bytes |
sblair | 0:f09b7bb8bcce | 75 | |
sblair | 0:f09b7bb8bcce | 76 | //printf("ADPULength: %i, len: %i\n", ADPULength, len); |
sblair | 0:f09b7bb8bcce | 77 | |
sblair | 0:f09b7bb8bcce | 78 | // frame header |
sblair | 0:f09b7bb8bcce | 79 | memcpy(&buf[offset], gseControl->ethHeaderData.destMACAddress, 6); // destination MAC addresses |
sblair | 0:f09b7bb8bcce | 80 | offset += 6; |
sblair | 0:f09b7bb8bcce | 81 | memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6); // source MAC addresses |
sblair | 0:f09b7bb8bcce | 82 | offset += 6; |
sblair | 0:f09b7bb8bcce | 83 | |
sblair | 0:f09b7bb8bcce | 84 | buf[offset++] = 0x81; // TPID |
sblair | 0:f09b7bb8bcce | 85 | buf[offset++] = 0x00; |
sblair | 0:f09b7bb8bcce | 86 | |
sblair | 0:f09b7bb8bcce | 87 | netmemcpy(&buf[offset], &gseControl->ethHeaderData.VLAN_ID, 2); // TCI |
sblair | 0:f09b7bb8bcce | 88 | buf[offset] |= (gseControl->ethHeaderData.VLAN_PRIORITY << 5); |
sblair | 0:f09b7bb8bcce | 89 | offset += 2; |
sblair | 0:f09b7bb8bcce | 90 | |
sblair | 0:f09b7bb8bcce | 91 | buf[offset++] = 0x88; // EtherType |
sblair | 0:f09b7bb8bcce | 92 | buf[offset++] = 0xB8; |
sblair | 0:f09b7bb8bcce | 93 | |
sblair | 0:f09b7bb8bcce | 94 | netmemcpy(&buf[offset], &gseControl->ethHeaderData.APPID, 2); // APPID |
sblair | 0:f09b7bb8bcce | 95 | offset += 2; |
sblair | 0:f09b7bb8bcce | 96 | |
sblair | 0:f09b7bb8bcce | 97 | netmemcpy(&buf[offset], &len, 2); // length |
sblair | 0:f09b7bb8bcce | 98 | offset += 2; |
sblair | 0:f09b7bb8bcce | 99 | |
sblair | 0:f09b7bb8bcce | 100 | buf[offset++] = 0x00; // reserved 1 |
sblair | 0:f09b7bb8bcce | 101 | buf[offset++] = 0x00; |
sblair | 0:f09b7bb8bcce | 102 | buf[offset++] = 0x00; // reserved 2 |
sblair | 0:f09b7bb8bcce | 103 | buf[offset++] = 0x00; |
sblair | 0:f09b7bb8bcce | 104 | |
sblair | 0:f09b7bb8bcce | 105 | buf[offset++] = ASN1_TAG_SEQUENCE; |
sblair | 0:f09b7bb8bcce | 106 | offset += encodeLength(&buf[offset], ADPULength /*+ getLengthBytes(ADPULength) + 1*/); |
sblair | 0:f09b7bb8bcce | 107 | |
sblair | 0:f09b7bb8bcce | 108 | buf[offset++] = GSE_TAG_GOCBREF; |
sblair | 0:f09b7bb8bcce | 109 | size = strlen((const char *) gseControl->gocbRef); |
sblair | 0:f09b7bb8bcce | 110 | buf[offset++] = size; |
sblair | 0:f09b7bb8bcce | 111 | memcpy(&buf[offset], gseControl->gocbRef, size); |
sblair | 0:f09b7bb8bcce | 112 | offset += size; |
sblair | 0:f09b7bb8bcce | 113 | |
sblair | 0:f09b7bb8bcce | 114 | buf[offset++] = GSE_TAG_TIME_ALLOWED_TO_LIVE; |
sblair | 0:f09b7bb8bcce | 115 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive)); |
sblair | 0:f09b7bb8bcce | 116 | #if GOOSE_FIXED_SIZE == 1 |
sblair | 0:f09b7bb8bcce | 117 | offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 118 | #else |
sblair | 0:f09b7bb8bcce | 119 | offset += ber_encode_integer(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 120 | #endif |
sblair | 0:f09b7bb8bcce | 121 | |
sblair | 0:f09b7bb8bcce | 122 | buf[offset++] = GSE_TAG_DATSET; |
sblair | 0:f09b7bb8bcce | 123 | size = strlen((const char *) gseControl->datSet); |
sblair | 0:f09b7bb8bcce | 124 | buf[offset++] = size; |
sblair | 0:f09b7bb8bcce | 125 | memcpy(&buf[offset], gseControl->datSet, size); |
sblair | 0:f09b7bb8bcce | 126 | offset += size; |
sblair | 0:f09b7bb8bcce | 127 | |
sblair | 0:f09b7bb8bcce | 128 | buf[offset++] = GSE_TAG_GOID; |
sblair | 0:f09b7bb8bcce | 129 | size = strlen((const char *) gseControl->goID); |
sblair | 0:f09b7bb8bcce | 130 | buf[offset++] = size; |
sblair | 0:f09b7bb8bcce | 131 | memcpy(&buf[offset], gseControl->goID, size); |
sblair | 0:f09b7bb8bcce | 132 | offset += size; |
sblair | 0:f09b7bb8bcce | 133 | |
sblair | 0:f09b7bb8bcce | 134 | buf[offset++] = GSE_TAG_T; |
sblair | 0:f09b7bb8bcce | 135 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t)); |
sblair | 0:f09b7bb8bcce | 136 | setTimestamp(&gseControl->t); |
sblair | 0:f09b7bb8bcce | 137 | memcpy(&buf[offset], &gseControl->t, BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t)); |
sblair | 0:f09b7bb8bcce | 138 | offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t); |
sblair | 0:f09b7bb8bcce | 139 | |
sblair | 0:f09b7bb8bcce | 140 | buf[offset++] = GSE_TAG_STNUM; |
sblair | 0:f09b7bb8bcce | 141 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum)); |
sblair | 0:f09b7bb8bcce | 142 | #if GOOSE_FIXED_SIZE == 1 |
sblair | 0:f09b7bb8bcce | 143 | offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 144 | #else |
sblair | 0:f09b7bb8bcce | 145 | offset += ber_encode_integer(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 146 | #endif |
sblair | 0:f09b7bb8bcce | 147 | |
sblair | 0:f09b7bb8bcce | 148 | buf[offset++] = GSE_TAG_SQNUM; |
sblair | 0:f09b7bb8bcce | 149 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum)); |
sblair | 0:f09b7bb8bcce | 150 | #if GOOSE_FIXED_SIZE == 1 |
sblair | 0:f09b7bb8bcce | 151 | offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 152 | #else |
sblair | 0:f09b7bb8bcce | 153 | offset += ber_encode_integer(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 154 | #endif |
sblair | 0:f09b7bb8bcce | 155 | |
sblair | 0:f09b7bb8bcce | 156 | buf[offset++] = GSE_TAG_SIMULATION; |
sblair | 0:f09b7bb8bcce | 157 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test)); |
sblair | 0:f09b7bb8bcce | 158 | offset += ber_encode_integer(&buf[offset], &gseControl->test, SV_GET_LENGTH_BOOLEAN); |
sblair | 0:f09b7bb8bcce | 159 | |
sblair | 0:f09b7bb8bcce | 160 | buf[offset++] = GSE_TAG_CONFREV; |
sblair | 0:f09b7bb8bcce | 161 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev)); |
sblair | 0:f09b7bb8bcce | 162 | #if GOOSE_FIXED_SIZE == 1 |
sblair | 0:f09b7bb8bcce | 163 | offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 164 | #else |
sblair | 0:f09b7bb8bcce | 165 | offset += ber_encode_integer(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 166 | #endif |
sblair | 0:f09b7bb8bcce | 167 | |
sblair | 0:f09b7bb8bcce | 168 | buf[offset++] = GSE_TAG_NDSCOM; |
sblair | 0:f09b7bb8bcce | 169 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom)); |
sblair | 0:f09b7bb8bcce | 170 | offset += ber_encode_integer(&buf[offset], &gseControl->ndsCom, SV_GET_LENGTH_BOOLEAN); |
sblair | 0:f09b7bb8bcce | 171 | |
sblair | 0:f09b7bb8bcce | 172 | buf[offset++] = GSE_TAG_NUMDATSETENTRIES; |
sblair | 0:f09b7bb8bcce | 173 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries)); |
sblair | 0:f09b7bb8bcce | 174 | #if GOOSE_FIXED_SIZE == 1 |
sblair | 0:f09b7bb8bcce | 175 | offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 176 | #else |
sblair | 0:f09b7bb8bcce | 177 | offset += ber_encode_integer(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U); |
sblair | 0:f09b7bb8bcce | 178 | #endif |
sblair | 0:f09b7bb8bcce | 179 | |
sblair | 0:f09b7bb8bcce | 180 | buf[offset++] = GSE_TAG_ALLDATA; |
sblair | 0:f09b7bb8bcce | 181 | offset += encodeLength(&buf[offset], (gseControl->getDatasetLength)()); |
sblair | 0:f09b7bb8bcce | 182 | offset += (gseControl->encodeDataset)(&buf[offset]); |
sblair | 0:f09b7bb8bcce | 183 | |
sblair | 0:f09b7bb8bcce | 184 | // assume network interface, such as WinPcap, generates CRC bytes |
sblair | 0:f09b7bb8bcce | 185 | |
sblair | 0:f09b7bb8bcce | 186 | return offset; |
sblair | 0:f09b7bb8bcce | 187 | } |