Steven Blair / Mbed 2 deprecated rapid61850-SV-LE

An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.

Dependencies:   mbed

An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.

rapid61850/svEncodePacket.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?

UserRevisionLine numberNew 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 "svPacketData.h"
sblair 0:f09b7bb8bcce 22 #include "svEncode.h"
sblair 0:f09b7bb8bcce 23 #include "gseEncode.h"
sblair 0:f09b7bb8bcce 24 #include "encodePacket.h"
sblair 0:f09b7bb8bcce 25
sblair 0:f09b7bb8bcce 26
sblair 0:f09b7bb8bcce 27 int svASDULength(struct svControl *svControl) {
sblair 0:f09b7bb8bcce 28 int len = 0;
sblair 0:f09b7bb8bcce 29
sblair 0:f09b7bb8bcce 30 len += strlen((const char *) svControl->ASDU[0].svID) + 2;
sblair 0:f09b7bb8bcce 31 //printf("%i, %s\n", strlen(svControl->ASDU[0].svID), svControl->ASDU[0].svID);
sblair 0:f09b7bb8bcce 32 //len += strlen((const char *) svControl->ASDU[0].datset) + 2;
sblair 0:f09b7bb8bcce 33
sblair 0:f09b7bb8bcce 34 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
sblair 0:f09b7bb8bcce 35 len += SV_GET_LENGTH_INT16U + 2; // smpCnt
sblair 0:f09b7bb8bcce 36 len += SV_GET_LENGTH_INT32U + 2; // confRev
sblair 0:f09b7bb8bcce 37 #else
sblair 0:f09b7bb8bcce 38 len += ber_integer_length((&svControl->ASDU[0].smpCnt), SV_GET_LENGTH_INT16U)/*BER_GET_LENGTH_CTYPE_INT16U(&svControl->ASDU[0].smpCnt)*/ + 2;
sblair 0:f09b7bb8bcce 39 len += ber_integer_length((&svControl->ASDU[0].confRev), SV_GET_LENGTH_INT32U)/*BER_GET_LENGTH_CTYPE_INT32U(&svControl->ASDU[0].confRev)*/ + 2;
sblair 0:f09b7bb8bcce 40 #endif
sblair 0:f09b7bb8bcce 41 len += SV_GET_LENGTH_BOOLEAN + 2;
sblair 0:f09b7bb8bcce 42
sblair 0:f09b7bb8bcce 43
sblair 0:f09b7bb8bcce 44
sblair 0:f09b7bb8bcce 45
sblair 0:f09b7bb8bcce 46 //len += BER_GET_LENGTH_CTYPE_INT16U(&svControl->ASDU[0].smpRate) + 2;
sblair 0:f09b7bb8bcce 47 len += svControl->ASDU[0].data.size + getLengthBytes(svControl->ASDU[0].data.size);
sblair 0:f09b7bb8bcce 48 len++;
sblair 0:f09b7bb8bcce 49
sblair 0:f09b7bb8bcce 50 return len;
sblair 0:f09b7bb8bcce 51 }
sblair 0:f09b7bb8bcce 52
sblair 0:f09b7bb8bcce 53 int svSeqLength(struct svControl *svControl) {
sblair 0:f09b7bb8bcce 54 int len = svASDULength(svControl);
sblair 0:f09b7bb8bcce 55 len += getLengthBytes(len);
sblair 0:f09b7bb8bcce 56 len++;
sblair 0:f09b7bb8bcce 57 len = len * svControl->noASDU; // assume all ASDUs are the same size
sblair 0:f09b7bb8bcce 58
sblair 0:f09b7bb8bcce 59 return len;
sblair 0:f09b7bb8bcce 60 }
sblair 0:f09b7bb8bcce 61
sblair 0:f09b7bb8bcce 62 int svAPDULength(struct svControl *svControl) {
sblair 0:f09b7bb8bcce 63 int len = svSeqLength(svControl);
sblair 0:f09b7bb8bcce 64 len += getLengthBytes(len);
sblair 0:f09b7bb8bcce 65 len++;
sblair 0:f09b7bb8bcce 66
sblair 0:f09b7bb8bcce 67 len += 3;
sblair 0:f09b7bb8bcce 68
sblair 0:f09b7bb8bcce 69 return len;
sblair 0:f09b7bb8bcce 70 }
sblair 0:f09b7bb8bcce 71
sblair 0:f09b7bb8bcce 72 // creates an SV packet, including frame header. returns 0 on fail; number of bytes on success
sblair 0:f09b7bb8bcce 73 int svEncodePacket(struct svControl *svControl, unsigned char *buf) {
sblair 0:f09b7bb8bcce 74 int offset = 0;
sblair 0:f09b7bb8bcce 75 int len = svAPDULength(svControl);
sblair 0:f09b7bb8bcce 76 len += getLengthBytes(len);
sblair 0:f09b7bb8bcce 77 len += 9; // savPdu tag size (1 byte), plus 8 "header" bytes
sblair 0:f09b7bb8bcce 78
sblair 0:f09b7bb8bcce 79 // frame header
sblair 0:f09b7bb8bcce 80 memcpy(&buf[offset], svControl->ethHeaderData.destMACAddress, 6); // destination MAC addresses
sblair 0:f09b7bb8bcce 81 offset += 6;
sblair 0:f09b7bb8bcce 82 memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6); // source MAC addresses
sblair 0:f09b7bb8bcce 83 offset += 6;
sblair 0:f09b7bb8bcce 84
sblair 0:f09b7bb8bcce 85 #if SV_USE_VLAN == 1
sblair 0:f09b7bb8bcce 86 buf[offset++] = 0x81; // TPID
sblair 0:f09b7bb8bcce 87 buf[offset++] = 0x00;
sblair 0:f09b7bb8bcce 88
sblair 0:f09b7bb8bcce 89 netmemcpy(&buf[offset], &svControl->ethHeaderData.VLAN_ID, 2); // TCI
sblair 0:f09b7bb8bcce 90 buf[offset] |= (svControl->ethHeaderData.VLAN_PRIORITY << 5);
sblair 0:f09b7bb8bcce 91 offset += 2;
sblair 0:f09b7bb8bcce 92 #endif
sblair 0:f09b7bb8bcce 93
sblair 0:f09b7bb8bcce 94 buf[offset++] = 0x88; // EtherType
sblair 0:f09b7bb8bcce 95 buf[offset++] = 0xBA;
sblair 0:f09b7bb8bcce 96
sblair 0:f09b7bb8bcce 97 netmemcpy(&buf[offset], &svControl->ethHeaderData.APPID, 2); // APPID
sblair 0:f09b7bb8bcce 98 offset += 2;
sblair 0:f09b7bb8bcce 99
sblair 0:f09b7bb8bcce 100 netmemcpy(&buf[offset], &len, 2); // length
sblair 0:f09b7bb8bcce 101 offset += 2;
sblair 0:f09b7bb8bcce 102
sblair 0:f09b7bb8bcce 103 buf[offset++] = 0x00; // reserved 1
sblair 0:f09b7bb8bcce 104 buf[offset++] = 0x00;
sblair 0:f09b7bb8bcce 105 buf[offset++] = 0x00; // reserved 2
sblair 0:f09b7bb8bcce 106 buf[offset++] = 0x00;
sblair 0:f09b7bb8bcce 107
sblair 0:f09b7bb8bcce 108 buf[offset++] = SV_TAG_SAVPDU;
sblair 0:f09b7bb8bcce 109 offset += encodeLength(&buf[offset], svAPDULength(svControl));
sblair 0:f09b7bb8bcce 110
sblair 0:f09b7bb8bcce 111 buf[offset++] = SV_TAG_NOASDU;
sblair 0:f09b7bb8bcce 112 offset += encodeLength(&buf[offset], ber_integer_length(&svControl->noASDU, SV_GET_LENGTH_INT16U));
sblair 0:f09b7bb8bcce 113 offset += ber_encode_integer(&buf[offset], &svControl->noASDU, ber_integer_length(&svControl->noASDU, SV_GET_LENGTH_INT16U));
sblair 0:f09b7bb8bcce 114
sblair 0:f09b7bb8bcce 115 buf[offset++] = SV_TAG_SEQUENCEOFASDU;
sblair 0:f09b7bb8bcce 116 offset += encodeLength(&buf[offset], svSeqLength(svControl));
sblair 0:f09b7bb8bcce 117
sblair 0:f09b7bb8bcce 118 int i = 0;
sblair 0:f09b7bb8bcce 119 int size = 0;
sblair 0:f09b7bb8bcce 120 for (i = 0; i < svControl->noASDU; i++) {
sblair 0:f09b7bb8bcce 121 buf[offset++] = SV_TAG_ASDU;
sblair 0:f09b7bb8bcce 122 offset += encodeLength(&buf[offset], svASDULength(svControl));
sblair 0:f09b7bb8bcce 123
sblair 0:f09b7bb8bcce 124 size = strlen((const char *) svControl->ASDU[i].svID);
sblair 0:f09b7bb8bcce 125 buf[offset++] = SV_TAG_SVID;
sblair 0:f09b7bb8bcce 126 buf[offset++] = size;
sblair 0:f09b7bb8bcce 127 memcpy(&buf[offset], svControl->ASDU[i].svID, size);
sblair 0:f09b7bb8bcce 128 offset += size;
sblair 0:f09b7bb8bcce 129
sblair 0:f09b7bb8bcce 130 #if SV_OPTIONAL_SUPPORTED == 1
sblair 0:f09b7bb8bcce 131 if (svControl->ASDU[i].showDatset) {
sblair 0:f09b7bb8bcce 132 size = strlen(svControl->ASDU[i].datset);
sblair 0:f09b7bb8bcce 133 buf[offset++] = SV_TAG_DATSET;
sblair 0:f09b7bb8bcce 134 buf[offset++] = size;
sblair 0:f09b7bb8bcce 135 memcpy(&buf[offset], svControl->ASDU[i].datset, size);
sblair 0:f09b7bb8bcce 136 offset += size;
sblair 0:f09b7bb8bcce 137 }
sblair 0:f09b7bb8bcce 138 #endif
sblair 0:f09b7bb8bcce 139
sblair 0:f09b7bb8bcce 140 buf[offset++] = SV_TAG_SMPCNT;
sblair 0:f09b7bb8bcce 141 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
sblair 0:f09b7bb8bcce 142 buf[offset++] = SV_GET_LENGTH_INT16U;
sblair 0:f09b7bb8bcce 143 netmemcpy(&buf[offset], &svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
sblair 0:f09b7bb8bcce 144 offset += SV_GET_LENGTH_INT16U;
sblair 0:f09b7bb8bcce 145 #else
sblair 0:f09b7bb8bcce 146 offset += encodeLength(&buf[offset], ber_integer_length(&svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U));
sblair 0:f09b7bb8bcce 147 offset += ber_encode_integer(&buf[offset], &svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
sblair 0:f09b7bb8bcce 148 #endif
sblair 0:f09b7bb8bcce 149
sblair 0:f09b7bb8bcce 150 buf[offset++] = SV_TAG_CONFREV;
sblair 0:f09b7bb8bcce 151 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
sblair 0:f09b7bb8bcce 152 buf[offset++] = SV_GET_LENGTH_INT32U;
sblair 0:f09b7bb8bcce 153 netmemcpy(&buf[offset], &svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
sblair 0:f09b7bb8bcce 154 offset += SV_GET_LENGTH_INT32U;
sblair 0:f09b7bb8bcce 155 #else
sblair 0:f09b7bb8bcce 156 offset += encodeLength(&buf[offset], ber_integer_length(&svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U));
sblair 0:f09b7bb8bcce 157 offset += ber_encode_integer(&buf[offset], &svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
sblair 0:f09b7bb8bcce 158 #endif
sblair 0:f09b7bb8bcce 159
sblair 0:f09b7bb8bcce 160 #if SV_OPTIONAL_SUPPORTED == 1
sblair 0:f09b7bb8bcce 161 if (svControl->ASDU[i].showRefrTm) {
sblair 0:f09b7bb8bcce 162 buf[offset++] = SV_TAG_REFRTM;
sblair 0:f09b7bb8bcce 163 offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm));
sblair 0:f09b7bb8bcce 164 setTimestamp(&svControl->ASDU[i].refrTm);
sblair 0:f09b7bb8bcce 165 memcpy(&buf[offset], &svControl->ASDU[i].refrTm, BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm));
sblair 0:f09b7bb8bcce 166 offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm);
sblair 0:f09b7bb8bcce 167 }
sblair 0:f09b7bb8bcce 168 #endif
sblair 0:f09b7bb8bcce 169
sblair 0:f09b7bb8bcce 170 buf[offset++] = SV_TAG_SMPSYNCH;
sblair 0:f09b7bb8bcce 171 buf[offset++] = SV_GET_LENGTH_BOOLEAN;
sblair 0:f09b7bb8bcce 172 offset += ENCODE_CTYPE_BOOLEAN(&buf[offset], &svControl->ASDU[i].smpSynch);
sblair 0:f09b7bb8bcce 173
sblair 0:f09b7bb8bcce 174 #if SV_OPTIONAL_SUPPORTED == 1
sblair 0:f09b7bb8bcce 175 if (svControl->ASDU[i].showSmpRate) {
sblair 0:f09b7bb8bcce 176 buf[offset++] = SV_TAG_SMPRATE;
sblair 0:f09b7bb8bcce 177 buf[offset++] = SV_GET_LENGTH_INT16U;
sblair 0:f09b7bb8bcce 178 offset += ENCODE_CTYPE_INT16U(&buf[offset], &svControl->ASDU[i].smpRate);
sblair 0:f09b7bb8bcce 179 }
sblair 0:f09b7bb8bcce 180 #endif
sblair 0:f09b7bb8bcce 181
sblair 0:f09b7bb8bcce 182 buf[offset++] = SV_TAG_SEQUENCEOFDATA;
sblair 0:f09b7bb8bcce 183 offset += encodeLength(&buf[offset], svControl->ASDU[i].data.size);
sblair 0:f09b7bb8bcce 184 memcpy(&buf[offset], svControl->ASDU[i].data.data, svControl->ASDU[i].data.size);
sblair 0:f09b7bb8bcce 185 offset += svControl->ASDU[i].data.size;
sblair 0:f09b7bb8bcce 186 }
sblair 0:f09b7bb8bcce 187
sblair 0:f09b7bb8bcce 188 // assume network interface, such as WinPcap, generates CRC bytes
sblair 0:f09b7bb8bcce 189
sblair 0:f09b7bb8bcce 190 return offset;
sblair 0:f09b7bb8bcce 191 }