gseEncodePacket.c

00001 /**
00002  * Rapid-prototyping protection schemes with IEC 61850
00003  *
00004  * Copyright (c) 2012 Steven Blair
00005  *
00006  * This program is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU General Public License
00008  * as published by the Free Software Foundation; either version 2
00009  * of the License, or (at your option) any later version.
00010 
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015 
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00019  */
00020 
00021 #include "encodePacket.h"
00022 #include "gsePacketData.h"
00023 #include "gseEncode.h"
00024 #include "svEncode.h"
00025 
00026 int getGseHeaderLength(struct gseControl *gseControl) {
00027     int size = 0;
00028     int len = 0;
00029 
00030     size = strlen((const char *) gseControl->gocbRef);
00031     len += size + getLengthBytes(size) + 1;
00032 
00033     size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive);
00034     len += size + getLengthBytes(size) + 1;
00035 
00036     size = strlen((const char *) gseControl->datSet);
00037     len += size + getLengthBytes(size) + 1;
00038 
00039     size = strlen((const char *) gseControl->goID);
00040     len += size + getLengthBytes(size) + 1;
00041 
00042     size = BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t);
00043     len += size + getLengthBytes(size) + 1;
00044 
00045     size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum);
00046     len += size + getLengthBytes(size) + 1;
00047 
00048     size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum);
00049     len += size + getLengthBytes(size) + 1;
00050 
00051     size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test);
00052     len += size + getLengthBytes(size) + 1;
00053 
00054     size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev);
00055     len += size + getLengthBytes(size) + 1;
00056 
00057     size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom);
00058     len += size + getLengthBytes(size) + 1;
00059 
00060     size = BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries);
00061     len += size + getLengthBytes(size) + 1;
00062 
00063     size = (gseControl->getDatasetLength)();
00064     len += size + getLengthBytes(size) + 1;
00065 
00066     return len;
00067 }
00068 
00069 // creates a GSE packet, including frame header. returns 0 on fail; number of bytes on success
00070 int gseEncodePacket(struct gseControl *gseControl, unsigned char *buf) {
00071     int offset = 0;
00072     int size = 0;
00073     int ADPULength = getGseHeaderLength(gseControl);
00074     int len = ADPULength + 9 + getLengthBytes(ADPULength);    // APDU tag size (1 byte), plus 8 "header" bytes
00075 
00076     //printf("ADPULength: %i, len: %i\n", ADPULength, len);
00077 
00078     // frame header
00079     memcpy(&buf[offset], gseControl->ethHeaderData.destMACAddress, 6);    // destination MAC addresses
00080     offset += 6;
00081     memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6);                        // source MAC addresses
00082     offset += 6;
00083 
00084     buf[offset++] = 0x81;    // TPID
00085     buf[offset++] = 0x00;
00086 
00087     netmemcpy(&buf[offset], &gseControl->ethHeaderData.VLAN_ID, 2);    // TCI
00088     buf[offset] |= (gseControl->ethHeaderData.VLAN_PRIORITY << 5);
00089     offset += 2;
00090 
00091     buf[offset++] = 0x88;    // EtherType
00092     buf[offset++] = 0xB8;
00093 
00094     netmemcpy(&buf[offset], &gseControl->ethHeaderData.APPID, 2);    // APPID
00095     offset += 2;
00096 
00097     netmemcpy(&buf[offset], &len, 2);    // length
00098     offset += 2;
00099 
00100     buf[offset++] = 0x00;    // reserved 1
00101     buf[offset++] = 0x00;
00102     buf[offset++] = 0x00;    // reserved 2
00103     buf[offset++] = 0x00;
00104 
00105     buf[offset++] = ASN1_TAG_SEQUENCE;
00106     offset += encodeLength(&buf[offset], ADPULength /*+ getLengthBytes(ADPULength) + 1*/);
00107 
00108     buf[offset++] = GSE_TAG_GOCBREF;
00109     size = strlen((const char *) gseControl->gocbRef);
00110     buf[offset++] = size;
00111     memcpy(&buf[offset], gseControl->gocbRef, size);
00112     offset += size;
00113 
00114     buf[offset++] = GSE_TAG_TIME_ALLOWED_TO_LIVE;
00115     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->timeAllowedToLive));
00116 #if GOOSE_FIXED_SIZE == 1
00117     offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U);
00118 #else
00119     offset += ber_encode_integer(&buf[offset], &gseControl->timeAllowedToLive, SV_GET_LENGTH_INT32U);
00120 #endif
00121 
00122     buf[offset++] = GSE_TAG_DATSET;
00123     size = strlen((const char *) gseControl->datSet);
00124     buf[offset++] = size;
00125     memcpy(&buf[offset], gseControl->datSet, size);
00126     offset += size;
00127 
00128     buf[offset++] = GSE_TAG_GOID;
00129     size = strlen((const char *) gseControl->goID);
00130     buf[offset++] = size;
00131     memcpy(&buf[offset], gseControl->goID, size);
00132     offset += size;
00133 
00134     buf[offset++] = GSE_TAG_T;
00135     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t));
00136     setTimestamp(&gseControl->t);
00137     memcpy(&buf[offset], &gseControl->t, BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t));
00138     offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseControl->t);
00139 
00140     buf[offset++] = GSE_TAG_STNUM;
00141     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->stNum));
00142 #if GOOSE_FIXED_SIZE == 1
00143     offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U);
00144 #else
00145     offset += ber_encode_integer(&buf[offset], &gseControl->stNum, SV_GET_LENGTH_INT32U);
00146 #endif
00147 
00148     buf[offset++] = GSE_TAG_SQNUM;
00149     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->sqNum));
00150 #if GOOSE_FIXED_SIZE == 1
00151     offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U);
00152 #else
00153     offset += ber_encode_integer(&buf[offset], &gseControl->sqNum, SV_GET_LENGTH_INT32U);
00154 #endif
00155 
00156     buf[offset++] = GSE_TAG_SIMULATION;
00157     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->test));
00158     offset += ber_encode_integer(&buf[offset], &gseControl->test, SV_GET_LENGTH_BOOLEAN);
00159 
00160     buf[offset++] = GSE_TAG_CONFREV;
00161     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->confRev));
00162 #if GOOSE_FIXED_SIZE == 1
00163     offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U);
00164 #else
00165     offset += ber_encode_integer(&buf[offset], &gseControl->confRev, SV_GET_LENGTH_INT32U);
00166 #endif
00167 
00168     buf[offset++] = GSE_TAG_NDSCOM;
00169     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseControl->ndsCom));
00170     offset += ber_encode_integer(&buf[offset], &gseControl->ndsCom, SV_GET_LENGTH_BOOLEAN);
00171 
00172     buf[offset++] = GSE_TAG_NUMDATSETENTRIES;
00173     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseControl->numDatSetEntries));
00174 #if GOOSE_FIXED_SIZE == 1
00175     offset += ber_encode_integer_fixed_size(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U);
00176 #else
00177     offset += ber_encode_integer(&buf[offset], &gseControl->numDatSetEntries, SV_GET_LENGTH_INT32U);
00178 #endif
00179 
00180     buf[offset++] = GSE_TAG_ALLDATA;
00181     offset += encodeLength(&buf[offset], (gseControl->getDatasetLength)());
00182     offset += (gseControl->encodeDataset)(&buf[offset]);
00183 
00184     // assume network interface, such as WinPcap, generates CRC bytes
00185 
00186     return offset;
00187 }