svEncodePacket.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 "svPacketData.h"
00022 #include "svEncode.h"
00023 #include "gseEncode.h"
00024 #include "encodePacket.h"
00025 
00026 
00027 int svASDULength(struct svData *svData) {
00028     int len = 0;
00029 
00030     len += strlen((const char *) svData->ASDU[0].svID) + 2;
00031     //printf("%i, %s\n", strlen(svData->ASDU[0].svID), svData->ASDU[0].svID);
00032     //len += strlen((const char *) svData->ASDU[0].datset) + 2;
00033     len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpCnt) + 2;
00034     len += BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[0].confRev) + 2;
00035     len += SV_GET_LENGTH_BOOLEAN + 2;
00036     //len += BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[0].smpRate) + 2;
00037     len += svData->ASDU[0].data.size + getLengthBytes(svData->ASDU[0].data.size);
00038     len++;
00039 
00040     return len;
00041 }
00042 
00043 int svSeqLength(struct svData *svData) {
00044     int len = svASDULength(svData);
00045     len += getLengthBytes(len);
00046     len++;
00047     len = len * svData->noASDU;    // assume all ASDUs are the same size
00048 
00049     return len;
00050 }
00051 
00052 int svAPDULength(struct svData *svData) {
00053     int len = svSeqLength(svData);
00054     len += getLengthBytes(len);
00055     len++;
00056 
00057     len += 3;
00058 
00059     return len;
00060 }
00061 
00062 
00063 //TODO: convert to proper BER sizes
00064 // creates an SV packet, including frame header. returns 0 on fail; number of bytes on success
00065 int svEncodePacket(struct svData *svData, unsigned char *buf) {
00066     int offset = 0;
00067     int len = svAPDULength(svData);
00068     len += getLengthBytes(len);
00069     len += 9;        // savPdu tag size (1 byte), plus 8 "header" bytes
00070 
00071     // frame header
00072     memcpy(&buf[offset], svData->ethHeaderData.destMACAddress, 6);    // destination MAC addresses
00073     offset += 6;
00074     memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6);                        // source MAC addresses
00075     offset += 6;
00076 
00077     buf[offset++] = 0x81;    // TPID
00078     buf[offset++] = 0x00;
00079 
00080     netmemcpy(&buf[offset], &svData->ethHeaderData.VLAN_ID, 2);    // TCI
00081     buf[offset] |= (svData->ethHeaderData.VLAN_PRIORITY << 5);
00082     offset += 2;
00083 
00084     buf[offset++] = 0x88;    // EtherType
00085     buf[offset++] = 0xBA;
00086 
00087     netmemcpy(&buf[offset], &svData->ethHeaderData.APPID, 2);    // APPID
00088     offset += 2;
00089 
00090     netmemcpy(&buf[offset], &len, 2);    // length
00091     offset += 2;
00092 
00093     buf[offset++] = 0x00;    // reserved 1
00094     buf[offset++] = 0x00;
00095     buf[offset++] = 0x00;    // reserved 2
00096     buf[offset++] = 0x00;
00097 
00098     buf[offset++] = 0x60;
00099     offset += encodeLength(&buf[offset], svAPDULength(svData));
00100 
00101     //TODO noASDU may be > 126?
00102     buf[offset++] = 0x80;
00103     buf[offset++] = 1;
00104     buf[offset++] = (unsigned char) svData->noASDU;
00105 
00106     buf[offset++] = 0xA2;
00107     offset += encodeLength(&buf[offset], svSeqLength(svData));
00108 
00109     int i = 0;
00110     int size = 0;
00111     for (i = 0; i < svData->noASDU; i++) {
00112         buf[offset++] = 0x30;
00113         offset += encodeLength(&buf[offset], svASDULength(svData));
00114 
00115         size = strlen((const char *) svData->ASDU[i].svID);
00116         buf[offset++] = 0x80;
00117         buf[offset++] = size;
00118         memcpy(&buf[offset], svData->ASDU[i].svID, size);
00119         offset += size;
00120 
00121         /*size = strlen(svData->ASDU[i].datset);
00122         buf[offset++] = 0x81;
00123         buf[offset++] = size;
00124         memcpy(&buf[offset], svData->ASDU[i].datset, size);
00125         offset += size;*/
00126 
00127         buf[offset++] = 0x82;
00128         offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt));
00129         offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
00130         //buf[offset++] = BER_GET_LENGTH_CTYPE_INT16U(&svData->ASDU[i].smpCnt);
00131         //offset += BER_ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpCnt);
00132 
00133         buf[offset++] = 0x83;
00134         offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev));
00135         offset += ber_encode_integer(&buf[offset], &svData->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
00136         //buf[offset++] = BER_GET_LENGTH_CTYPE_INT32U(&svData->ASDU[i].confRev);
00137         //offset += BER_ENCODE_CTYPE_INT32U(&buf[offset], &svData->ASDU[i].confRev);
00138 
00139         buf[offset++] = 0x85;
00140         buf[offset++] = SV_GET_LENGTH_BOOLEAN;
00141         offset += ENCODE_CTYPE_BOOLEAN(&buf[offset], &svData->ASDU[i].smpSynch);
00142 
00143         /*buf[offset++] = 0x86;
00144         buf[offset++] = SV_GET_LENGTH_INT16U;
00145         offset += ENCODE_CTYPE_INT16U(&buf[offset], &svData->ASDU[i].smpRate);*/
00146 
00147         buf[offset++] = 0x87;
00148         offset += encodeLength(&buf[offset], svData->ASDU[i].data.size);
00149         memcpy(&buf[offset], svData->ASDU[i].data.data, svData->ASDU[i].data.size);
00150         offset += svData->ASDU[i].data.size;
00151     }
00152 
00153     // assume network interface, such as WinPcap, generates CRC bytes
00154 
00155     return offset;
00156 }