#include "stdafx.h"
#include "Helpers.h"

#ifndef _DEBUG
 #define printf //
#endif

//!erzeugt eine BOOTPAntwort nach RFC 951
int CreateBOOTPReply(BOOTP_MESSAGE * pMsg,unsigned int transactionID, unsigned int OwnIPaddress, unsigned int ClientIPAddress, unsigned int ClientSubnetMask, unsigned char ClientMACAddress[6])
{
	//all good?
	if(pMsg == NULL || ClientMACAddress == NULL)
		return -1;

	//set all Bytes to zero
	memset(pMsg,0,sizeof(BOOTP_MESSAGE));

	//fill the data
	pMsg->op	= 2;	//Reply
	pMsg->htype = 1;	//Ethernet
	pMsg->hlen	= 6;	//MAC 6 Bytes
	pMsg->hops	= 0;
	pMsg->xid	= transactionID;			//ACHTUNG! 32Bit
	pMsg->secs	= 0;
	pMsg->ciaddr= 0;
	pMsg->yiaddr= ClientIPAddress;			//ACHTUNG! 32Bit
	pMsg->siaddr= OwnIPaddress;				//ACHTUNG! 32Bit
	pMsg->giaddr= 0;
	pMsg->chaddr[0] = ClientMACAddress[0];
	pMsg->chaddr[1] = ClientMACAddress[1];
	pMsg->chaddr[2] = ClientMACAddress[2];
	pMsg->chaddr[3] = ClientMACAddress[3];
	pMsg->chaddr[4] = ClientMACAddress[4];
	pMsg->chaddr[5] = ClientMACAddress[5];

	// Put Subnet mask to vendor information (RFC1497)
	if (ClientSubnetMask != 0)
	{
		pMsg->vend[0] = 99;	// Magic Cookie
		pMsg->vend[1] = 130;// Magic Cookie
		pMsg->vend[2] = 83;	// Magic Cookie
		pMsg->vend[3] = 99;	// Magic Cookie
		pMsg->vend[4] = 1;	// Tag: Subnet Mask
		pMsg->vend[5] = 4;	// Length
		pMsg->vend[6] = ClientSubnetMask & 0xff;
		pMsg->vend[7] = (ClientSubnetMask>>8) & 0xff;
		pMsg->vend[8] = (ClientSubnetMask>>16) & 0xff;
		pMsg->vend[9] = (ClientSubnetMask>>24) & 0xff;
	}

	return 0;
}

int ParseSNMPPayload(unsigned char* pData, unsigned int Size, SNMP_UniversalPrimitiveItem ** ppResult)
{
	if(pData==NULL || ppResult == NULL)
		return -1;

	SNMP_UniversalPrimitiveItem * pItemCurrent = NULL;

	printf("\nParse SNMP Payload: (%i Bytes)\n",Size);

	unsigned int i = 0;
	//durch das Paket laufen und alle größen auslesen
	while(i <Size)
	{
		/*** 1. Byte Type ***/
		unsigned char Val = pData[i++];

		//Klasse ist Bit 7/6
		int Class = Val >> 6;
		
		//Wenn die Klasse "Universal"(00) ist, Type einlesen Bit 4-0, wenn größer 30 ->Fehler
		int Type = Val;
//		if(Type > 30){
		if((Val & 0x1F) > 30){
			printf("Invalid Type!\n");
			return -1;
		}
	
		//ist es ein primitive/constructed Bit 5
		bool isPrimitive = !((Val >> 5) & 0x1);

		/*** 2. Byte Size ***/		
		Val = pData[i++];
		unsigned int anzValue=0;
		//wie groß?, kurz Form (Bit 7=0, 6-0!=0)
		if( ((Val >> 7) ==0) && ((Val & 0x7F)!=0) )
		{
			anzValue = Val & 0x7F;
		}
		//unbestimmt Form (Bit 7=1, 6-0!=0)
		else if( ((Val >> 7) ==1) && ((Val & 0x7F)==0) )
		{
			printf("undefined length!\n");
			return -1;
		}
		//lange form (Bit 7=1, 6-0!=0)
		else if( ((Val >> 7) ==1) && ((Val & 0x7F)!=0) )
		{
			//Bit 6-0 sind die anz der Bytes der Länge
			unsigned char anzLength = Val & 0x7F;
			for(int i1=1; i1<= anzLength; i1++)
				anzValue += pData[i++];					
		}	

		printf("BER: \n\t Class: %s\n\t Type: %s (%i)\n\t isPrimitive: %s\n\t Size: %i\n",
				(Class==0)?("Universal"):( (Class==1)?("Application"):( (Class==2)?("Context-specific"):("Private") ) ),
				(Type==2)?("INTEGER"):( (Type==4)?("OCTET STRING"):( (Type==5)?("NULL"):( (Type==6)?("OBJECT IDENTIFIER"):(Type==16)?("SEQUENCE"):( "--- ??? ---") ) ) ), Type,
				(isPrimitive==true)?("primitive"):("constructed"),
			anzValue);


		/*** (3)...n Byte Daten ***/
		//wenn es nicht primitive dann überspringen
		if(isPrimitive  == false)
		{
			printf("\n");
		}
		else
		{
			/* neues item */				
			SNMP_UniversalPrimitiveItem *pTempItem =(SNMP_UniversalPrimitiveItem*) malloc(sizeof(SNMP_UniversalPrimitiveItem));
			if(pTempItem == NULL)
				return -1;								
			pTempItem->Type = (SNMP_UNIVERSAL_TYPE)Type;
			pTempItem->pNext = NULL;

			//1. Item, in ppResult setzen 
			if(pItemCurrent!=NULL)
				pItemCurrent->pNext=pTempItem;
			else
				(*ppResult)=pTempItem;
			pItemCurrent = pTempItem;

			/* echte Daten */
			//nur wenn Class = Universal dann ist es genormt was in Type steht
			if(Class == 0)
			{
				//die möglichen Daten
				int iData=0;
				char stText[512];
				char stOID[128];
				
				/* was ist es für ein DatenTyp? */
				switch(Type)
				{
				case SNMP_INTEGER: //Int
					//darf nicht länger als 4Byte sein
					if(anzValue>4)
						printf("Data: \n\t INTEGER to long!\n\n");
					else
					{
						for(unsigned int i1=1; i1<= anzValue; i1++)
							iData |= pData[i++] << (8*(anzValue-i));
						printf("Data: \n\t INTEGER: %i\n\n",iData);						
						
						pTempItem->Size = sizeof(int);
						pTempItem->pValue = malloc(pTempItem->Size);						
						if(pTempItem->pValue == NULL)
							return -1;
						(*(int*)pTempItem->pValue) = iData;
					}
					break;

				case SNMP_OCTET_STRING: //OCTET STRING
					//darf nicht zu lang sein
					if(anzValue>=sizeof(stText) )
						printf("Data: \n\t OCTET STRING to long!\n\n");
					else
					{
						memset(stText,0,sizeof(stText));
						for(unsigned int i1=1; i1<= anzValue; i1++)
							stText[i1-1] = pData[i++];
						printf("Data: \n\t OCTET STRING: %s\n\n",stText);

						pTempItem->Size = anzValue+1;
						pTempItem->pValue = malloc(pTempItem->Size);
						if(pTempItem->pValue == NULL)
							return -1;
						memcpy(pTempItem->pValue,stText,pTempItem->Size);						
					}
					break;

				case SNMP_NULL: //NULL
					printf("Data: \n\t NULL\n\n");
					break;

				case SNMP_OBJECT_IDENTIFIER: //OBJECT IDENTIFIER
					//darf nicht zu lang sein
					if( (anzValue+1)>=sizeof(stOID) )
						printf("Data: \n\t OBJECT IDENTIFIER to long!\n\n");
					else
					{						
						memset(stOID,0,sizeof(stOID));
						unsigned int iTemp = 0;
						char stTemp[32];
						bool isFirstVal = false;
						//Bit 7 =1 -> Zahl besteht aus mehr als aus einem Byte
						//1. und 2. -> (X*40) + Y						
						for(unsigned int i1=1; i1<= anzValue; i1++)
						{
							Val = pData[i++];
							//7Bit[6-0] (an/ein)fügen
							iTemp = iTemp <<8;
							iTemp |= (Val & 0x7F);

							if( (Val >> 7) == 0){
								if(isFirstVal == false){
									isFirstVal = true;
									sprintf_s(stTemp,sizeof(stTemp), "%i.%i.",iTemp/40, iTemp ^40 );
								}else
									sprintf_s(stTemp,sizeof(stTemp), "%i.",iTemp);
								strncat_s(stOID,sizeof(stOID),stTemp,strlen(stTemp) );
								iTemp = 0;
							}							
						}
						printf("Data: \n\t OBJECT IDENTIFIER: %s\n\n",stOID);
						
						pTempItem->Size =(int) strlen(stOID)+1;
						pTempItem->pValue = malloc(pTempItem->Size);
						if(pTempItem->pValue == NULL)
							return -1;
						memcpy(pTempItem->pValue,stOID,pTempItem->Size);						
					}
					break;

				default:

					printf("Data: \n\t --- ??? ---\n\n");
				}//case Datatype

			}//if Class = 0 
			else
			{
				printf("Data: \n\t no Universal DataTyp!\n\n");
				pTempItem->Size = anzValue;
				pTempItem->pValue = malloc(pTempItem->Size);
				if(pTempItem->pValue == NULL)
					return -1;
				memcpy(pTempItem->pValue, pData+i, pTempItem->Size);
				i += pTempItem->Size;
			}
		}//if isPrimitive
	
		printf("\n");

	}
	

	return 0;
}

#ifdef printf
	#undef printf
#endif 