gseDecodePacket.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 
00022 #include "gseDecodeBasic.h"
00023 #include "ied.h"
00024 #include "gseDecode.h"
00025 #include "gsePacketData.h"
00026 #include "decodePacket.h"
00027 #include <stddef.h>
00028 
00029 void gseDecodePDU(unsigned char *buf) {
00030     unsigned char    tag = 0;
00031     CTYPE_INT16U    lengthFieldSize = 0;
00032     CTYPE_INT16U    lengthValue = 0;
00033     CTYPE_INT16U    offsetForSequence = 0;
00034     CTYPE_INT16U    offsetForNonSequence = 0;
00035     unsigned char    *gocbRef = NULL;
00036     CTYPE_INT16U    gocbRefLength = 0;
00037     CTYPE_INT32U    timeAllowedToLive = 0;
00038     CTYPE_TIMESTAMP    T = 0;
00039     CTYPE_INT32U    sqNum = 0;
00040     CTYPE_INT32U    stNum = 0;
00041 
00042     while (1) {
00043         tag = (unsigned char) buf[0];    // assumes only one byte is used
00044         lengthFieldSize = getLengthFieldSize((unsigned char) buf[1]);
00045         lengthValue = decodeLength((unsigned char *) &buf[1]);
00046         offsetForSequence = 1 + lengthFieldSize;
00047         offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
00048 
00049         switch (tag) {
00050         case GSE_TAG_GOCBREF:
00051             // save pointer to gocbRef name
00052             gocbRef = &buf[offsetForSequence];
00053             gocbRefLength = lengthValue;
00054 
00055             buf = &buf[offsetForNonSequence];
00056             break;
00057         case GSE_TAG_TIME_ALLOWED_TO_LIVE:
00058             ber_decode_integer(&buf[offsetForSequence], lengthValue, &timeAllowedToLive, SV_GET_LENGTH_INT32U);
00059             buf = &buf[offsetForNonSequence];
00060             break;
00061         case ASN1_TAG_SEQUENCE:
00062             buf = &buf[offsetForSequence];
00063             break;
00064         case GSE_TAG_DATSET:
00065             buf = &buf[offsetForNonSequence];
00066             break;
00067         case GSE_TAG_T:
00068             memcpy(&T, &buf[offsetForSequence], BER_GET_LENGTH_CTYPE_TIMESTAMP(&T));
00069             buf = &buf[offsetForNonSequence];
00070             break;
00071         case GSE_TAG_STNUM:
00072             ber_decode_integer(&buf[offsetForSequence], lengthValue, &stNum, SV_GET_LENGTH_INT32U);
00073             buf = &buf[offsetForNonSequence];
00074             break;
00075         case GSE_TAG_SQNUM:
00076             ber_decode_integer(&buf[offsetForSequence], lengthValue, &sqNum, SV_GET_LENGTH_INT32U);
00077             buf = &buf[offsetForNonSequence];
00078             break;
00079         case GSE_TAG_ALLDATA:
00080             gseDecodeDataset(&buf[offsetForSequence], lengthValue, gocbRef, gocbRefLength, timeAllowedToLive, T, stNum, sqNum);
00081             return;
00082         default:
00083             buf = &buf[offsetForNonSequence];
00084             break;
00085         }
00086     }
00087 }
00088 
00089 void gseDecode(unsigned char *buf, int len) {
00090     int offset = 22;
00091 
00092     // check for VLAN tag
00093     if (buf[12] == 0x81 && buf[13] == 0x00) {
00094         offset = 26;
00095     }
00096 
00097     gseDecodePDU(&buf[offset]);    // cuts out frame header
00098 }