Steven Blair
The goal of this software is to automatically generate C/C++ code which reads and writes GOOSE and Sampled Value packets. Any valid IEC 61850 Substation Configuration Description (SCD) file, describing GOOSE and/or SV communications, can be used as the input. The output code is lightweight and platform-independent, so it can run on a variety of devices, including low-cost microcontrollers. It\'s ideal for rapid-prototyping new protection and control systems that require communications. This mbed project is a simple example of this functionality. Other code: https://github.com/stevenblair/rapid61850 Project homepage: http://personal.strath.ac.uk/steven.m.blair/
gseEncodePacket.c@1:9399d44c2b1a, 2011-10-07 (annotated)
- Committer:
- sblair
- Date:
- Fri Oct 07 13:48:18 2011 +0000
- Revision:
- 1:9399d44c2b1a
- Parent:
- 0:230c10b228ea
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| sblair | 1:9399d44c2b1a | 1 | /** |
| sblair | 1:9399d44c2b1a | 2 | * Rapid-prototyping protection schemes with IEC 61850 |
| sblair | 1:9399d44c2b1a | 3 | * |
| sblair | 1:9399d44c2b1a | 4 | * Copyright (c) 2011 Steven Blair |
| sblair | 1:9399d44c2b1a | 5 | * |
| sblair | 1:9399d44c2b1a | 6 | * This program is free software; you can redistribute it and/or |
| sblair | 1:9399d44c2b1a | 7 | * modify it under the terms of the GNU General Public License |
| sblair | 1:9399d44c2b1a | 8 | * as published by the Free Software Foundation; either version 2 |
| sblair | 1:9399d44c2b1a | 9 | * of the License, or (at your option) any later version. |
| sblair | 1:9399d44c2b1a | 10 | |
| sblair | 1:9399d44c2b1a | 11 | * This program is distributed in the hope that it will be useful, |
| sblair | 1:9399d44c2b1a | 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| sblair | 1:9399d44c2b1a | 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| sblair | 1:9399d44c2b1a | 14 | * GNU General Public License for more details. |
| sblair | 1:9399d44c2b1a | 15 | |
| sblair | 1:9399d44c2b1a | 16 | * You should have received a copy of the GNU General Public License |
| sblair | 1:9399d44c2b1a | 17 | * along with this program; if not, write to the Free Software |
| sblair | 1:9399d44c2b1a | 18 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
| sblair | 1:9399d44c2b1a | 19 | */ |
| sblair | 1:9399d44c2b1a | 20 | |
| sblair | 0:230c10b228ea | 21 | #include "encodePacket.h" |
| sblair | 0:230c10b228ea | 22 | #include "gsePacketData.h" |
| sblair | 0:230c10b228ea | 23 | #include "gseEncode.h" |
| sblair | 0:230c10b228ea | 24 | #include "svEncode.h" |
| sblair | 0:230c10b228ea | 25 | |
| sblair | 0:230c10b228ea | 26 | int getGseHeaderLength(struct gseData *gseData) { |
| sblair | 1:9399d44c2b1a | 27 | int size = 0; |
| sblair | 1:9399d44c2b1a | 28 | int len = 0; |
| sblair | 0:230c10b228ea | 29 | |
| sblair | 1:9399d44c2b1a | 30 | size = strlen((const char *) gseData->gocbRef); |
| sblair | 1:9399d44c2b1a | 31 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 32 | |
| sblair | 1:9399d44c2b1a | 33 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->timeAllowedToLive); |
| sblair | 1:9399d44c2b1a | 34 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 35 | |
| sblair | 1:9399d44c2b1a | 36 | size = strlen((const char *) gseData->datSet); |
| sblair | 1:9399d44c2b1a | 37 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 38 | |
| sblair | 1:9399d44c2b1a | 39 | size = strlen((const char *) gseData->goID); |
| sblair | 1:9399d44c2b1a | 40 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 41 | |
| sblair | 1:9399d44c2b1a | 42 | size = BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t); |
| sblair | 1:9399d44c2b1a | 43 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 44 | |
| sblair | 1:9399d44c2b1a | 45 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->stNum); |
| sblair | 1:9399d44c2b1a | 46 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 47 | |
| sblair | 1:9399d44c2b1a | 48 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->sqNum); |
| sblair | 1:9399d44c2b1a | 49 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 50 | |
| sblair | 1:9399d44c2b1a | 51 | size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->test); |
| sblair | 1:9399d44c2b1a | 52 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 53 | |
| sblair | 1:9399d44c2b1a | 54 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->confRev); |
| sblair | 1:9399d44c2b1a | 55 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 56 | |
| sblair | 1:9399d44c2b1a | 57 | size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->ndsCom); |
| sblair | 1:9399d44c2b1a | 58 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 59 | |
| sblair | 1:9399d44c2b1a | 60 | size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->numDatSetEntries); |
| sblair | 1:9399d44c2b1a | 61 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 62 | |
| sblair | 1:9399d44c2b1a | 63 | size = (gseData->getDatasetLength)(); |
| sblair | 1:9399d44c2b1a | 64 | len += size + getLengthBytes(size) + 1; |
| sblair | 0:230c10b228ea | 65 | |
| sblair | 1:9399d44c2b1a | 66 | return len; |
| sblair | 0:230c10b228ea | 67 | } |
| sblair | 0:230c10b228ea | 68 | |
| sblair | 0:230c10b228ea | 69 | // creates a GSE packet, including frame header. returns 0 on fail; number of bytes on success |
| sblair | 0:230c10b228ea | 70 | int gseEncodePacket(struct gseData *gseData, unsigned char *buf) { |
| sblair | 1:9399d44c2b1a | 71 | int offset = 0; |
| sblair | 1:9399d44c2b1a | 72 | int size = 0; |
| sblair | 1:9399d44c2b1a | 73 | int ADPULength = getGseHeaderLength(gseData); |
| sblair | 1:9399d44c2b1a | 74 | int len = ADPULength + 9 + getLengthBytes(ADPULength); // APDU tag size (1 byte), plus 8 "header" bytes |
| sblair | 0:230c10b228ea | 75 | |
| sblair | 1:9399d44c2b1a | 76 | //printf("ADPULength: %i, len: %i\n", ADPULength, len); |
| sblair | 0:230c10b228ea | 77 | |
| sblair | 1:9399d44c2b1a | 78 | // frame header |
| sblair | 1:9399d44c2b1a | 79 | memcpy(&buf[offset], gseData->ethHeaderData.destMACAddress, 6); // destination MAC addresses |
| sblair | 1:9399d44c2b1a | 80 | offset += 6; |
| sblair | 1:9399d44c2b1a | 81 | memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6); // source MAC addresses |
| sblair | 1:9399d44c2b1a | 82 | offset += 6; |
| sblair | 0:230c10b228ea | 83 | |
| sblair | 1:9399d44c2b1a | 84 | buf[offset++] = 0x81; // TPID |
| sblair | 1:9399d44c2b1a | 85 | buf[offset++] = 0x00; |
| sblair | 0:230c10b228ea | 86 | |
| sblair | 1:9399d44c2b1a | 87 | netmemcpy(&buf[offset], &gseData->ethHeaderData.VLAN_ID, 2); // TCI |
| sblair | 1:9399d44c2b1a | 88 | buf[offset] |= (gseData->ethHeaderData.VLAN_PRIORITY << 5); |
| sblair | 1:9399d44c2b1a | 89 | offset += 2; |
| sblair | 0:230c10b228ea | 90 | |
| sblair | 1:9399d44c2b1a | 91 | buf[offset++] = 0x88; // EtherType |
| sblair | 1:9399d44c2b1a | 92 | buf[offset++] = 0xB8; |
| sblair | 0:230c10b228ea | 93 | |
| sblair | 1:9399d44c2b1a | 94 | netmemcpy(&buf[offset], &gseData->ethHeaderData.APPID, 2); // APPID |
| sblair | 1:9399d44c2b1a | 95 | offset += 2; |
| sblair | 0:230c10b228ea | 96 | |
| sblair | 1:9399d44c2b1a | 97 | netmemcpy(&buf[offset], &len, 2); // length |
| sblair | 1:9399d44c2b1a | 98 | offset += 2; |
| sblair | 0:230c10b228ea | 99 | |
| sblair | 1:9399d44c2b1a | 100 | buf[offset++] = 0x00; // reserved 1 |
| sblair | 1:9399d44c2b1a | 101 | buf[offset++] = 0x00; |
| sblair | 1:9399d44c2b1a | 102 | buf[offset++] = 0x00; // reserved 2 |
| sblair | 1:9399d44c2b1a | 103 | buf[offset++] = 0x00; |
| sblair | 0:230c10b228ea | 104 | |
| sblair | 1:9399d44c2b1a | 105 | buf[offset++] = 0x61; |
| sblair | 1:9399d44c2b1a | 106 | offset += encodeLength(&buf[offset], ADPULength /*+ getLengthBytes(ADPULength) + 1*/); |
| sblair | 0:230c10b228ea | 107 | |
| sblair | 1:9399d44c2b1a | 108 | buf[offset++] = 0x80; |
| sblair | 1:9399d44c2b1a | 109 | size = strlen((const char *) gseData->gocbRef); |
| sblair | 1:9399d44c2b1a | 110 | buf[offset++] = size; |
| sblair | 1:9399d44c2b1a | 111 | memcpy(&buf[offset], gseData->gocbRef, size); |
| sblair | 1:9399d44c2b1a | 112 | offset += size; |
| sblair | 0:230c10b228ea | 113 | |
| sblair | 1:9399d44c2b1a | 114 | buf[offset++] = 0x81; |
| sblair | 1:9399d44c2b1a | 115 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->timeAllowedToLive)); |
| sblair | 1:9399d44c2b1a | 116 | //offset += BER_ENCODE_CTYPE_INT32U(&buf[offset], &gseData->timeAllowedToLive); |
| sblair | 1:9399d44c2b1a | 117 | offset += ber_encode_integer(&buf[offset], &gseData->timeAllowedToLive, SV_GET_LENGTH_INT32U); |
| sblair | 0:230c10b228ea | 118 | |
| sblair | 1:9399d44c2b1a | 119 | buf[offset++] = 0x82; |
| sblair | 1:9399d44c2b1a | 120 | size = strlen((const char *) gseData->datSet); |
| sblair | 1:9399d44c2b1a | 121 | buf[offset++] = size; |
| sblair | 1:9399d44c2b1a | 122 | memcpy(&buf[offset], gseData->datSet, size); |
| sblair | 1:9399d44c2b1a | 123 | offset += size; |
| sblair | 0:230c10b228ea | 124 | |
| sblair | 1:9399d44c2b1a | 125 | buf[offset++] = 0x83; |
| sblair | 1:9399d44c2b1a | 126 | size = strlen((const char *) gseData->goID); |
| sblair | 1:9399d44c2b1a | 127 | buf[offset++] = size; |
| sblair | 1:9399d44c2b1a | 128 | memcpy(&buf[offset], gseData->goID, size); |
| sblair | 1:9399d44c2b1a | 129 | offset += size; |
| sblair | 0:230c10b228ea | 130 | |
| sblair | 1:9399d44c2b1a | 131 | buf[offset++] = 0x84; |
| sblair | 1:9399d44c2b1a | 132 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t)); |
| sblair | 1:9399d44c2b1a | 133 | setTimestamp(&gseData->t); |
| sblair | 1:9399d44c2b1a | 134 | memcpy(&buf[offset], &gseData->t, BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t)); |
| sblair | 1:9399d44c2b1a | 135 | offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t); |
| sblair | 0:230c10b228ea | 136 | |
| sblair | 1:9399d44c2b1a | 137 | buf[offset++] = 0x85; |
| sblair | 1:9399d44c2b1a | 138 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->stNum)); |
| sblair | 1:9399d44c2b1a | 139 | offset += ber_encode_integer(&buf[offset], &gseData->stNum, SV_GET_LENGTH_INT32U); |
| sblair | 0:230c10b228ea | 140 | |
| sblair | 1:9399d44c2b1a | 141 | buf[offset++] = 0x86; |
| sblair | 1:9399d44c2b1a | 142 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->sqNum)); |
| sblair | 1:9399d44c2b1a | 143 | offset += ber_encode_integer(&buf[offset], &gseData->sqNum, SV_GET_LENGTH_INT32U); |
| sblair | 0:230c10b228ea | 144 | |
| sblair | 1:9399d44c2b1a | 145 | buf[offset++] = 0x87; |
| sblair | 1:9399d44c2b1a | 146 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->test)); |
| sblair | 1:9399d44c2b1a | 147 | offset += ber_encode_integer(&buf[offset], &gseData->test, SV_GET_LENGTH_BOOLEAN); |
| sblair | 0:230c10b228ea | 148 | |
| sblair | 1:9399d44c2b1a | 149 | buf[offset++] = 0x88; |
| sblair | 1:9399d44c2b1a | 150 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->confRev)); |
| sblair | 1:9399d44c2b1a | 151 | offset += ber_encode_integer(&buf[offset], &gseData->confRev, SV_GET_LENGTH_INT32U); |
| sblair | 0:230c10b228ea | 152 | |
| sblair | 1:9399d44c2b1a | 153 | buf[offset++] = 0x89; |
| sblair | 1:9399d44c2b1a | 154 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->ndsCom)); |
| sblair | 1:9399d44c2b1a | 155 | offset += ber_encode_integer(&buf[offset], &gseData->ndsCom, SV_GET_LENGTH_BOOLEAN); |
| sblair | 0:230c10b228ea | 156 | |
| sblair | 1:9399d44c2b1a | 157 | buf[offset++] = 0x8A; |
| sblair | 1:9399d44c2b1a | 158 | offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->numDatSetEntries)); |
| sblair | 1:9399d44c2b1a | 159 | offset += ber_encode_integer(&buf[offset], &gseData->numDatSetEntries, SV_GET_LENGTH_INT32U); |
| sblair | 0:230c10b228ea | 160 | |
| sblair | 1:9399d44c2b1a | 161 | buf[offset++] = 0xAB; |
| sblair | 1:9399d44c2b1a | 162 | offset += encodeLength(&buf[offset], (gseData->getDatasetLength)()); |
| sblair | 1:9399d44c2b1a | 163 | offset += (gseData->encodeDataset)(&buf[offset]); |
| sblair | 0:230c10b228ea | 164 | |
| sblair | 1:9399d44c2b1a | 165 | // assume network interface, such as WinPcap, generates CRC bytes |
| sblair | 0:230c10b228ea | 166 | |
| sblair | 1:9399d44c2b1a | 167 | return offset; |
| sblair | 0:230c10b228ea | 168 | } |