gseEncodePacket.c

00001 /**
00002  * Rapid-prototyping protection schemes with IEC 61850
00003  *
00004  * Copyright (c) 2011 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 gseData *gseData) {
00027     int size = 0;
00028     int len = 0;
00029 
00030     size = strlen((const char *) gseData->gocbRef);
00031     len += size + getLengthBytes(size) + 1;
00032 
00033     size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->timeAllowedToLive);
00034     len += size + getLengthBytes(size) + 1;
00035 
00036     size = strlen((const char *) gseData->datSet);
00037     len += size + getLengthBytes(size) + 1;
00038 
00039     size = strlen((const char *) gseData->goID);
00040     len += size + getLengthBytes(size) + 1;
00041 
00042     size = BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t);
00043     len += size + getLengthBytes(size) + 1;
00044 
00045     size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->stNum);
00046     len += size + getLengthBytes(size) + 1;
00047 
00048     size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->sqNum);
00049     len += size + getLengthBytes(size) + 1;
00050 
00051     size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->test);
00052     len += size + getLengthBytes(size) + 1;
00053 
00054     size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->confRev);
00055     len += size + getLengthBytes(size) + 1;
00056 
00057     size = BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->ndsCom);
00058     len += size + getLengthBytes(size) + 1;
00059 
00060     size = BER_GET_LENGTH_CTYPE_INT32U(&gseData->numDatSetEntries);
00061     len += size + getLengthBytes(size) + 1;
00062 
00063     size = (gseData->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 gseData *gseData, unsigned char *buf) {
00071     int offset = 0;
00072     int size = 0;
00073     int ADPULength = getGseHeaderLength(gseData);
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], gseData->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], &gseData->ethHeaderData.VLAN_ID, 2);    // TCI
00088     buf[offset] |= (gseData->ethHeaderData.VLAN_PRIORITY << 5);
00089     offset += 2;
00090 
00091     buf[offset++] = 0x88;    // EtherType
00092     buf[offset++] = 0xB8;
00093 
00094     netmemcpy(&buf[offset], &gseData->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++] = 0x61;
00106     offset += encodeLength(&buf[offset], ADPULength /*+ getLengthBytes(ADPULength) + 1*/);
00107 
00108     buf[offset++] = 0x80;
00109     size = strlen((const char *) gseData->gocbRef);
00110     buf[offset++] = size;
00111     memcpy(&buf[offset], gseData->gocbRef, size);
00112     offset += size;
00113 
00114     buf[offset++] = 0x81;
00115     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->timeAllowedToLive));
00116     //offset += BER_ENCODE_CTYPE_INT32U(&buf[offset], &gseData->timeAllowedToLive);
00117     offset += ber_encode_integer(&buf[offset], &gseData->timeAllowedToLive, SV_GET_LENGTH_INT32U);
00118 
00119     buf[offset++] = 0x82;
00120     size = strlen((const char *) gseData->datSet);
00121     buf[offset++] = size;
00122     memcpy(&buf[offset], gseData->datSet, size);
00123     offset += size;
00124 
00125     buf[offset++] = 0x83;
00126     size = strlen((const char *) gseData->goID);
00127     buf[offset++] = size;
00128     memcpy(&buf[offset], gseData->goID, size);
00129     offset += size;
00130 
00131     buf[offset++] = 0x84;
00132     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t));
00133     setTimestamp(&gseData->t);
00134     memcpy(&buf[offset], &gseData->t, BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t));
00135     offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&gseData->t);
00136 
00137     buf[offset++] = 0x85;
00138     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->stNum));
00139     offset += ber_encode_integer(&buf[offset], &gseData->stNum, SV_GET_LENGTH_INT32U);
00140 
00141     buf[offset++] = 0x86;
00142     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->sqNum));
00143     offset += ber_encode_integer(&buf[offset], &gseData->sqNum, SV_GET_LENGTH_INT32U);
00144 
00145     buf[offset++] = 0x87;
00146     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->test));
00147     offset += ber_encode_integer(&buf[offset], &gseData->test, SV_GET_LENGTH_BOOLEAN);
00148 
00149     buf[offset++] = 0x88;
00150     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->confRev));
00151     offset += ber_encode_integer(&buf[offset], &gseData->confRev, SV_GET_LENGTH_INT32U);
00152 
00153     buf[offset++] = 0x89;
00154     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_BOOLEAN(&gseData->ndsCom));
00155     offset += ber_encode_integer(&buf[offset], &gseData->ndsCom, SV_GET_LENGTH_BOOLEAN);
00156 
00157     buf[offset++] = 0x8A;
00158     offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&gseData->numDatSetEntries));
00159     offset += ber_encode_integer(&buf[offset], &gseData->numDatSetEntries, SV_GET_LENGTH_INT32U);
00160 
00161     buf[offset++] = 0xAB;
00162     offset += encodeLength(&buf[offset], (gseData->getDatasetLength)());
00163     offset += (gseData->encodeDataset)(&buf[offset]);
00164 
00165     // assume network interface, such as WinPcap, generates CRC bytes
00166 
00167     return offset;
00168 }