/***************************************************************************
 *   Copyright (C) 2004 by EVER Sp. z o.o.                                 *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "csnmp.h"

// Function name	: CSNMP::Init
// Description		: 
// Return type		: int 
// Argument         : unsigned char ipAddress[16]
// Argument         : u_short Port
int CSNMP::Init(unsigned char ipAddress[16], u_short Port)
{
	struct hostent* lphe;

	memset( &sSai, 0, sizeof(sSai) );
	memset( &sSaiLocal, 0, sizeof(sSaiLocal) );
	
	if (strlen((const char *)ipAddress) < 7)
		return SNMP_ERROR_BADPARAM;

	//if ((iError = WSAStartup(MAKEWORD(1, 1), &wsa)) != 0)
	//	return SNMP_ERROR;

/*	lphe = gethostbyname(getenv("HOSTNAME"));
	if ( !lphe )
		return SNMP_ERROR;
*/
	if ((sSnmp = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == INVALID_SOCKET) {
		iError = errno;
		return SNMP_ERROR_SOCKET_GENERAL;
	}

	sSaiLocal.sin_addr.s_addr = INADDR_ANY;//*((in_addr*)*lphe->h_addr_list);
	sSaiLocal.sin_family = AF_INET;
	sSaiLocal.sin_port = htons(Port);

	if ((bind(sSnmp, (struct sockaddr*)&sSaiLocal, sizeof(sSaiLocal))) == SOCKET_ERROR)
	{
		iError = errno;
		return SNMP_ERROR_SOCKET_BINDING;
	}

	sSai.sin_addr.s_addr = inet_addr( (char *)ipAddress );
	if (sSai.sin_addr.s_addr == INADDR_NONE)
		return SNMP_ERROR;
	sSai.sin_family = AF_INET;
	sSai.sin_port = htons(SNMP_PORT);
	
//	if (connect(sSnmp, (struct sockaddr *)&sSai, sizeof(sockaddr_in)) == SOCKET_ERROR)
//	{
//		iError = errno;
//		return SNMP_ERROR_SOCKET_CONNECT;
//	}

	if (!(sOid.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	if (!(sPdu.OID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	if (!(sPduRec.OID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;

	if (!(sTrapPdu.EnterpriseOID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	for (int i = 0; i < SNMP_MAX_TRAP_OBJECTS; i++)
	{
		if (!(sTrapPdu.sObjOID[i].lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
			return SNMP_ERROR_MEMALLOC;
	}
	
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::Init
// Description		: Inicjalizacja parametrów i połączenia dla trybu nasłuchu
// Return type		: int 
int CSNMP::Init()
{
	if (!(sOid.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	if (!(sPdu.OID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	if (!(sPduRec.OID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;

	if (!(sTrapPdu.EnterpriseOID.lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;
	for (int i = 0; i < SNMP_MAX_TRAP_OBJECTS; i++)
	{
		if (!(sTrapPdu.sObjOID[i].lpbSnmpData = (LPBYTE)malloc(255 * sizeof(BYTE))))
			return SNMP_ERROR_MEMALLOC;
	}
	
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::UnInit
// Description		: 
// Return type		: int 
int CSNMP::UnInit()
{
	if (sOid.lpbSnmpData != NULL)
		free(sOid.lpbSnmpData);
	if (sPdu.OID.lpbSnmpData != NULL)
		free(sPdu.OID.lpbSnmpData);
	if (sPduRec.OID.lpbSnmpData != NULL)
		free(sPduRec.OID.lpbSnmpData);

	if (sTrapPdu.EnterpriseOID.lpbSnmpData != NULL)
		free(sTrapPdu.EnterpriseOID.lpbSnmpData);
	for (int i = 0; i < SNMP_MAX_TRAP_OBJECTS; i++) {
		if (sTrapPdu.sObjOID[i].lpbSnmpData != NULL)
			free(sTrapPdu.sObjOID[i].lpbSnmpData);
	}

	if (sSnmp) {
		shutdown(sSnmp, SHUT_RDWR);
		close(sSnmp);
		return SNMP_ERROR_SUCCESS;
	} else {
		return SNMP_ERROR;
	}
}

// Function name	: CSNMP::CreateOID
// Description		: 
// Return type		: int 
// Argument         : LPSNMP_OID lpOid
// Argument         : int iBegin
// Argument         : ...
int CSNMP::CreateOID(LPSNMP_OID lpOid, int iLen, ...)
{
	int		i		= 0,
			iCnt	= 0;
	BYTE	b1		= 0,
			b2		= 0;
	va_list	set;

	memset(lpOid->lpbSnmpData, 0, sizeof(lpOid->lpbSnmpData));

	va_start(set, iLen);
	while(iLen > iCnt)
	{
		i = va_arg(set, int);
		if (i < 128)
			lpOid->lpbSnmpData[iCnt] = (BYTE)i;
		else
		{
			if (EncodeBER(
				(unsigned long)i, (int *)&b1, 
				(LPBYTE)(lpOid->lpbSnmpData+iCnt)) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR;
			iCnt += b1;
		}
		iCnt++;
	}
	va_end(set);
	
	lpOid->ulLength = iCnt;

	return iCnt;
}

// Function name	: CSNMP::CreateOIDFromTable
// Description		: Tworzy OID z tablicy o długości iCount
// Return type		: int 
// Argument         : LPSNMP_OID lpOid
// Argument         : int iCount
// Argument         : int *lpiTable
int CSNMP::CreateOIDFromTable(LPSNMP_OID lpOid, int iCount, int *lpiTable)
{
	int		i		= 0,
			iCnt	= 0;
	BYTE	b1		= 0,
			b2		= 0;
	
	memset(lpOid->lpbSnmpData, 0, sizeof(lpOid->lpbSnmpData));

	while(iCount > iCnt)
	{
		i = lpiTable[iCnt];
		if (i < 128)
			lpOid->lpbSnmpData[iCnt] = (BYTE)i;
		else
		{
			if (EncodeBER(
				(unsigned long)i, (int *)&b1, 
				(LPBYTE)(lpOid->lpbSnmpData+iCnt)) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR;
			iCnt += b1;
		}
		iCnt++;
	}
	
	lpOid->ulLength = iCnt;

	return iCnt;
}

// Function name	: CSNMP::CreateOID
// Description		: 
// Return type		: int 
// Argument         : int iLen
// Argument         : ...
int CSNMP::CreateOID(int iLen, ...)
{
	int		i		= 0,
			iCnt	= 0;
	BYTE	b1		= 0,
			b2		= 0;
	va_list	set;
	
	memset(sOid.lpbSnmpData, 0, sizeof(sOid.lpbSnmpData));

	va_start(set, iLen);
	while(iLen > iCnt)
	{
		i = va_arg(set, int);
		if (i < 128)
			sOid.lpbSnmpData[iCnt] = (BYTE)i;
		else
		{
			if (EncodeBER((unsigned long)i, (int *)&b1, (LPBYTE)(sOid.lpbSnmpData+iCnt)) == -1)
				return -1;
			iCnt += b1;
		}
		iCnt++;
	}
	va_end(set);
	
	sOid.ulLength = iCnt;

	return iCnt;
}

// Function name	: CSNMP::CreateOIDFromTable
// Description		: Tworzy OID z tablicy o długości iCount
// Return type		: int 
// Argument         : int iCount
// Argument         : int *lpiTable
int CSNMP::CreateOIDFromTable(int iCount, int *lpiTable)
{
	int		i		= 0,
			iCnt	= 0;
	BYTE	b1		= 0,
			b2		= 0;
	
	memset(sOid.lpbSnmpData, 0, sizeof(sOid.lpbSnmpData));

	while(iCount > iCnt)
	{
		i = lpiTable[iCnt];
		if (i < 128) {
			sOid.lpbSnmpData[iCnt] = (BYTE)i;
		} else {
			if (EncodeBER(
				(unsigned long)i, (int *)&b1, 
				(LPBYTE)(sOid.lpbSnmpData+iCnt)) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR;
			iCnt += b1;
		}
		iCnt++;
	}
	
	sOid.ulLength = iCnt;

	return iCnt;
}

// Function name	: CSNMP::EncodeBER
// Description		: Funkcja koduje liczbę za pomocą kodowania BER, zamieniając ją 
//					: na ciąg bajtów w tej notacji.
//					: Bufor iLen zwraca po pomyślnym wykonaniu ilość wyłuskanych bajtów.
//					: Bufor lpBuffer musi wskazywać początkowo na miejsce w którym mają być 
//					: zapisywane kolejne wyłuskane bajty. Długość tego bufora musi być 
//					: wystarczająca do tego aby zakodować liczbę, gdyż funkcja nie sprawdza
//					: alokacji miejsca na kolejne bajty.
// Return type		: int 
// Argument         : unsigned long ulNumber
// Argument         : int *lpiLen
// Argument         : LPBYTE lpBuffer
int CSNMP::EncodeBER(unsigned long ulNumber, int *lpiLen, LPBYTE lpBuffer)
{
	int i, ic = 0, id = 0;
	unsigned long ulin = 0, ulin2 = 0;
	BYTE bRest = 0;

	if (!lpBuffer)
		return SNMP_ERROR_BADPARAM;
	
	for (ic = 1; ic <= 4; ic++) {
		if ((ulNumber / pow(128, ic)) < 128) {
			*lpiLen = ic;
			break;
		}
	}

	ulin2 = ulNumber;
	for (i = 1; i <= ic; i++) {
		ulin = (int)(ulin2 / 128);
		lpBuffer[*lpiLen + 1 - i] = (BYTE)(ulin2 - (int)(ulin * 128));
		if (i != 1)
			lpBuffer[*lpiLen + 1 - i] += 128;
		ulin2 = (int)ulin2 / 128;
	}
	lpBuffer[*lpiLen + 1 - i] = (BYTE)(128 + ulin);

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::DecodeBER
// Description		: Funkcja dekoduje długość w BER na wartość liczbową i zwraca w buforze
//					: ilość bajtów przekonwertowanych oraz wartość
// Return type		: int 
// Argument         : LPBYTE lpBuffer
// Argument         : int *lpiLen
// Argument         : unsigned long *lpulDecoded
int CSNMP::DecodeBER(LPBYTE lpBuffer, int *lpiLen, unsigned long *lpulDecoded)
{
	int ic = 0;
	__int64 i64pow = 0, i64tmp = 0;

	if (!lpBuffer || !lpulDecoded)
		return SNMP_ERROR_BADPARAM;
	
	if (lpBuffer[0] < 128) {
		if (lpiLen != NULL)
			*lpiLen = 1;
		i64tmp = (__int64)lpBuffer[0];
	} else {
		ic = (int)lpBuffer[0] - 128;
		if (lpiLen != NULL)
			*lpiLen = (int)ic + 1;

		i64pow = (__int64)pow((double)256, (double)ic);
		for (int i = 0; i < ic; i++) {
			i64tmp += (unsigned long)(lpBuffer[i + 1] * (i64pow / 256));
		}
	}
	*lpulDecoded = (unsigned long)i64tmp;

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::DecodeInteger
// Description		: Funkcja dekoduje wartość integer
// Return type		: int 
// Argument         : LPBYTE lpBuffer
// Argument         : int *lpiLen
// Argument         : unsigned long *lpulDecoded
int CSNMP::DecodeInteger(LPBYTE lpBuffer, int *lpiLen, unsigned long *lpulDecoded)
{
	int			ic		= 0;
	__int64		i64pow	= 0,
				i64tmp	= 0;

	if (!lpBuffer || !lpulDecoded)
		return SNMP_ERROR_BADPARAM;
	
	ic = (int)lpBuffer[0];
	if (lpiLen != NULL)
		*lpiLen = (int)ic + 1;
	i64pow = (__int64)pow((double)256, (double)ic);
	for (int i = 0; i < ic; i++)
	{
		i64pow /= 256;
		i64tmp += (unsigned long)(lpBuffer[i + 1] * i64pow);
	}
	*lpulDecoded = (unsigned long)i64tmp;
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::SendPdu
// Description		: 
// Return type		: int 
// Argument         : LPSNMP_Pdu lpsp
// Argument         : int iType
// Argument         : int iSize
// Argument         : LPBYTE lpbData
int CSNMP::SendPdu(LPSNMP_Pdu lpsp, int iType, int iSize, LPBYTE lpbData)
{
	LPBYTE	lpb;
	int		icnt, idx, iSizeFrame, iSizePDU, iSizeSequence, iSizeSequence2;
	unsigned long	ulBuff;

	if (!lpsp)
		return SNMP_ERROR_BADPARAM;
	if (!(lpb = (LPBYTE)malloc(484 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;

	icnt = 0;
	// Message Header
	lpb[icnt++] = ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeFrame = icnt)] = 130; // Rozmiar ramki, (2 bajty rozmiaru)
	icnt++;
	// Version
	lpb[icnt++] = ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = (BYTE)lpsp->ulVersion - 1;
	// Community string
	lpb[icnt++] = ASN1_Primitive(DT_OCTET_STRING);
	lpb[icnt++] = strlen(lpsp->szCommunity);
	for (idx = icnt; idx < (icnt + (int)strlen(lpsp->szCommunity)); idx++)
		lpb[idx] = lpsp->szCommunity[idx - icnt];
	icnt = idx;
	// PDU
	lpb[icnt++] = (BYTE)lpsp->eCommand;
	lpb[(iSizePDU = icnt)] = 1; // Rozmiar PDU// Typ INTEGER
	icnt++;
	// RequestID
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	if (lpsp->ulRequestId > 127)
	{
		lpb[icnt++] = 2;
		lpb[icnt++] = (BYTE)(lpsp->ulRequestId / 256);
		lpb[icnt++] = (BYTE)(lpsp->ulRequestId - ((lpsp->ulRequestId / 256) * 256));
	}
	else
	{
		lpb[icnt++] = 1;
		lpb[icnt++] = (BYTE)lpsp->ulRequestId;
	}

	// Error status (No error)
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// Error index
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// PDU sequence
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	// PDU sequence internal
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence2 = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_OBJECT_IDENTIFIER);
	if (lpsp->OID.ulLength > 127)
	{
		lpb[icnt++] = 130;	// Rozmiar OID
		lpb[icnt++] = (BYTE)(lpsp->OID.ulLength / 256);
		lpb[icnt++] = (BYTE)(lpsp->OID.ulLength - ((lpsp->OID.ulLength / 256) * 256));
	}
	else
		lpb[icnt++] = (BYTE)lpsp->OID.ulLength;
	for (idx = icnt; idx < (int)(icnt + lpsp->OID.ulLength); idx++)
		lpb[idx] = lpsp->OID.lpbSnmpData[idx - icnt];
	icnt = idx;
	
	lpb[icnt++] = (BYTE)iType;
	switch((int)iType)
	{
	case ASN1_Primitive(DT_INTEGER):
		{
			memcpy(&idx, lpbData, iSize);
			lpb[icnt++] = 2;
			lpb[icnt++] = (BYTE)(idx / 256);
			lpb[icnt++] = (BYTE)(idx - ((idx / 256) * 256));
			break;
		}

	case ASN1_Primitive(DT_OCTET_STRING):
		{
			if (iSize > (484 - icnt))
				return SNMP_ERROR_BADPARAM;
			if (iSize > 127)
			{
				lpb[icnt++] = (BYTE)(127 + (iSize / 128));
				lpb[icnt++] = (BYTE)(iSize - ((iSize / 128) * 128));
			}
			else
				lpb[icnt++] = (BYTE)(iSize);
			for (idx = icnt; idx < (icnt + iSize); idx++)
				lpb[idx] = lpbData[idx - icnt];
			icnt = idx;
			break;
		}

	case SNMP_Primitive(PT_IpAddress):
		{
			memcpy(&ulBuff, lpbData, sizeof(unsigned long));
			lpb[icnt++] = sizeof(unsigned long);
			lpb[icnt++] = (BYTE)(HIBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(HIBYTE(LOWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(LOWORD(ulBuff)));
			break;
		}

	case SNMP_Primitive(PT_TimeTicks):
		{
			memcpy(&ulBuff, lpbData, sizeof(unsigned long));
			lpb[icnt++] = sizeof(unsigned long);
			lpb[icnt++] = (BYTE)(HIBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(HIBYTE(LOWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(LOWORD(ulBuff)));
			break;
		}

	case SNMP_Primitive(PT_Counter):
	case SNMP_Primitive(PT_Gauge):
		{
			memcpy(&ulBuff, lpbData, sizeof(unsigned long));
			lpb[icnt++] = sizeof(unsigned long);
			lpb[icnt++] = (BYTE)(HIBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(HIWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(HIBYTE(LOWORD(ulBuff)));
			lpb[icnt++] = (BYTE)(LOBYTE(LOWORD(ulBuff)));
			break;
		}

	case ASN1_Primitive(DT_OBJECT_IDENTIFIER):
		{
			break;
		}
	}

	lpsp->ulLength = icnt;
	
	//
	// Zakonczenie
	// Rozmiary ramek
	//
	icnt = (lpsp->ulLength - (iSizeSequence2 + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence2 + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeSequence2] = (BYTE)130;
		lpb[iSizeSequence2 + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence2 + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeSequence2] = (BYTE)icnt;
	icnt = (lpsp->ulLength - (iSizeSequence + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeSequence] = (BYTE)130;
		lpb[iSizeSequence + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
	{
		lpb[iSizeSequence] = (BYTE)icnt;
	}

	icnt = (lpsp->ulLength - (iSizePDU + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizePDU + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizePDU] = (BYTE)130;
		lpb[iSizePDU + 1] = (BYTE)(icnt / 256);
		lpb[iSizePDU + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizePDU] = (BYTE)icnt;
	
	icnt = (lpsp->ulLength - 2);
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeFrame + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeFrame] = (BYTE)130;
		lpb[iSizeFrame + 1] = (BYTE)(icnt / 256);
		lpb[iSizeFrame + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeFrame] = (BYTE)icnt;

	sendto(sSnmp, (const char*)lpb, lpsp->ulLength, 0, (struct sockaddr*)&sSai, sizeof(sSai));

	free(lpb);

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::SendPdu
// Description		: Sending data to agent
// Return type		: int 
// Argument         : LPSNMP_Pdu lpsp
int CSNMP::SendPdu(LPSNMP_Pdu lpsp)
{
	LPBYTE	lpb;
	int		icnt, idx, iSizeFrame, iSizePDU, iSizeSequence, iSizeSequence2;

	if (!lpsp)
		return SNMP_ERROR_BADPARAM;
	
	if (!(lpb = (LPBYTE)malloc(484 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;

	icnt = 0;
	// Message Header
	lpb[icnt++] = ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeFrame = icnt)] = 130; // Rozmiar ramki, (2 bajty rozmiaru)
	icnt++;
	// Version
	lpb[icnt++] = ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = (BYTE)lpsp->ulVersion - 1;
	// Community string
	lpb[icnt++] = ASN1_Primitive(DT_OCTET_STRING);
	lpb[icnt++] = strlen(lpsp->szCommunity);
	for (idx = icnt; idx < (icnt + (int)strlen(lpsp->szCommunity)); idx++)
		lpb[idx] = lpsp->szCommunity[idx - icnt];
	icnt = idx;
	// PDU
	lpb[icnt++] = (BYTE)lpsp->eCommand;
	lpb[(iSizePDU = icnt)] = 1; // Rozmiar PDU// Typ INTEGER
	icnt++;
	// RequestID
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	if (lpsp->ulRequestId > 127)
	{
		lpb[icnt++] = 2;
		lpb[icnt++] = (BYTE)(lpsp->ulRequestId / 256);
		lpb[icnt++] = (BYTE)(lpsp->ulRequestId - ((lpsp->ulRequestId / 256) * 256));
	}
	else
	{
		lpb[icnt++] = 1;
		lpb[icnt++] = (BYTE)lpsp->ulRequestId;
	}

	// Error status (No error)
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// Error index
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// PDU sequence
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	// PDU sequence internal
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence2 = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_OBJECT_IDENTIFIER);
	if (lpsp->OID.ulLength > 127)
	{
		lpb[icnt++] = 130;	// Rozmiar OID
		lpb[icnt++] = (BYTE)(lpsp->OID.ulLength / 256);
		lpb[icnt++] = (BYTE)(lpsp->OID.ulLength - ((lpsp->OID.ulLength / 256) * 256));
	}
	else
		lpb[icnt++] = (BYTE)lpsp->OID.ulLength;
	for (idx = icnt; idx < (int)(icnt + lpsp->OID.ulLength); idx++)
		lpb[idx] = lpsp->OID.lpbSnmpData[idx - icnt];
	icnt = idx;
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_NULL);
	lpb[icnt++] = 0;

	lpsp->ulLength = icnt;

	// Rozmiary ramek
	icnt = (lpsp->ulLength - (iSizeSequence2 + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence2 + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeSequence2] = (BYTE)130;
		lpb[iSizeSequence2 + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence2 + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeSequence2] = (BYTE)icnt;
	icnt = (lpsp->ulLength - (iSizeSequence + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeSequence] = (BYTE)130;
		lpb[iSizeSequence + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
	{
		lpb[iSizeSequence] = (BYTE)icnt;
	}

	icnt = (lpsp->ulLength - (iSizePDU + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizePDU + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizePDU] = (BYTE)130;
		lpb[iSizePDU + 1] = (BYTE)(icnt / 256);
		lpb[iSizePDU + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizePDU] = (BYTE)icnt;
	
	icnt = (lpsp->ulLength - 2);
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeFrame + 1], 2, icnt);
		lpsp->ulLength += 2;
		lpb[iSizeFrame] = (BYTE)130;
		lpb[iSizeFrame + 1] = (BYTE)(icnt / 256);
		lpb[iSizeFrame + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeFrame] = (BYTE)icnt;

	sendto(sSnmp, (const char*)lpb, lpsp->ulLength, 0, (struct sockaddr*)&sSai, sizeof(sSai));

	free(lpb);

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::SendPdu
// Description		: Wysyła ramkę z wewnątrz klasy
// Return type		: int 
int CSNMP::SendPdu()
{
	LPBYTE	lpb;
	int		icnt, idx, iSizeFrame, iSizePDU, iSizeSequence, iSizeSequence2;

	if (!(lpb = (LPBYTE)malloc(484 * sizeof(BYTE))))
		return SNMP_ERROR_MEMALLOC;

	icnt = 0;
	// Message Header
	lpb[icnt++] = ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeFrame = icnt)] = 130; // Rozmiar ramki, (2 bajty rozmiaru)
	icnt++;
	// Version
	lpb[icnt++] = ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = (BYTE)sPdu.ulVersion - 1;
	// Community string
	lpb[icnt++] = ASN1_Primitive(DT_OCTET_STRING);
	lpb[icnt++] = strlen(sPdu.szCommunity);
	for (idx = icnt; idx < (icnt + (int)strlen(sPdu.szCommunity)); idx++)
		lpb[idx] = sPdu.szCommunity[idx - icnt];
	icnt = idx;
	// PDU
	lpb[icnt++] = (BYTE)sPdu.eCommand;
	lpb[(iSizePDU = icnt)] = 1; // Rozmiar PDU// Typ INTEGER
	icnt++;
	// RequestID
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	if (sPdu.ulRequestId > 127)
	{
		lpb[icnt++] = 2;
		lpb[icnt++] = (BYTE)(sPdu.ulRequestId / 256);
		lpb[icnt++] = (BYTE)(sPdu.ulRequestId - ((sPdu.ulRequestId / 256) * 256));
	}
	else
	{
		lpb[icnt++] = 1;
		lpb[icnt++] = (BYTE)sPdu.ulRequestId;
	}

	// Error status (No error)
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// Error index
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_INTEGER);
	lpb[icnt++] = 1;
	lpb[icnt++] = 0;
	// PDU sequence
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	// PDU sequence internal
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_SEQUENCE_OF);
	lpb[(iSizeSequence2 = icnt)] = 130; // Rozmiar sekwencji PDU, (2 bajty)
	icnt++;
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_OBJECT_IDENTIFIER);
	if (sPdu.OID.ulLength > 127)
	{
		lpb[icnt++] = 130;	// Rozmiar OID
		lpb[icnt++] = (BYTE)(sPdu.OID.ulLength / 256);
		lpb[icnt++] = (BYTE)(sPdu.OID.ulLength - ((sPdu.OID.ulLength / 256) * 256));
	}
	else
		lpb[icnt++] = (BYTE)sPdu.OID.ulLength;
	for (idx = icnt; idx < (int)(icnt + sPdu.OID.ulLength); idx++)
		lpb[idx] = sPdu.OID.lpbSnmpData[idx - icnt];
	icnt = idx;
	lpb[icnt++] = (BYTE)ASN1_Primitive(DT_NULL);
	lpb[icnt++] = 0;

	sPdu.ulLength = icnt;

	// Rozmiary ramek
	icnt = (sPdu.ulLength - (iSizeSequence2 + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence2 + 1], 2, icnt);
		sPdu.ulLength += 2;
		lpb[iSizeSequence2] = (BYTE)130;
		lpb[iSizeSequence2 + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence2 + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeSequence2] = (BYTE)icnt;
	icnt = (sPdu.ulLength - (iSizeSequence + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeSequence + 1], 2, icnt);
		sPdu.ulLength += 2;
		lpb[iSizeSequence] = (BYTE)130;
		lpb[iSizeSequence + 1] = (BYTE)(icnt / 256);
		lpb[iSizeSequence + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
	{
		lpb[iSizeSequence] = (BYTE)icnt;
	}

	icnt = (sPdu.ulLength - (iSizePDU + 1));
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizePDU + 1], 2, icnt);
		sPdu.ulLength += 2;
		lpb[iSizePDU] = (BYTE)130;
		lpb[iSizePDU + 1] = (BYTE)(icnt / 256);
		lpb[iSizePDU + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizePDU] = (BYTE)icnt;
	
	icnt = (sPdu.ulLength - 2);
	if (icnt > 127)
	{
		icnt += 2;
		MoveMem(&lpb[iSizeFrame + 1], 2, icnt);
		sPdu.ulLength += 2;
		lpb[iSizeFrame] = (BYTE)130;
		lpb[iSizeFrame + 1] = (BYTE)(icnt / 256);
		lpb[iSizeFrame + 2] = (BYTE)(icnt - ((icnt / 256) * 256));
	}
	else
		lpb[iSizeFrame] = (BYTE)icnt;

	sendto(sSnmp, (const char*)lpb, sPdu.ulLength, 0, (struct sockaddr*)&sSai, sizeof(sSai));

	free(lpb);

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::MoveMem
// Description		: Funkcja przesuwa blok pamięci w pamięci w kierunku wyznacz. przez iDir.,
//					: o długość iDir;
// Return type		: int 
// Argument         : LPBYTE lpBuffer
// Argument         : int iDir
// Argument         : int iSize
int CSNMP::MoveMem(LPBYTE lpBuffer, int iDir, int iSize)
{
	if (iDir == 0)
		return SNMP_ERROR_SUCCESS;

	if (iDir > 0) {
		for (int ic = iSize; ic >= 0; ic--)
			lpBuffer[ic + abs(iDir)] = lpBuffer[ic];
	} else {
		for (int ic = 0; ic < iSize; ic++)
			lpBuffer[ic - abs(iDir)] = lpBuffer[ic];
	}
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::CreateFrame
// Description		: 
// Return type		: int 
// Argument         : int ulVersion
// Argument         : char *szCommunity
// Argument         : SNMP_Message eCommand
// Argument         : int ulRequestId
// Argument         : SNMP_OID sOid
int CSNMP::CreateFrame( int ulVersion, char *szCommunity, SNMP_Message eCommand, 
					  int ulRequestId, SNMP_OID soOid )
{
	if ((ulVersion < 1) || (!szCommunity))
		return SNMP_ERROR_BADPARAM;

	sPdu.ulVersion = ulVersion;
	strcpy(sPdu.szCommunity, szCommunity);
	sPdu.eCommand = eCommand;
	sPdu.ulRequestId = ulRequestId;
	
	sPdu.OID.ulLength = soOid.ulLength;
	for (int i = 0; i < (int)soOid.ulLength; i++)
		sPdu.OID.lpbSnmpData[i] = soOid.lpbSnmpData[i];

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::CreateFrame
// Description		: 
// Return type		: int 
// Argument         : int ulVersion
// Argument         : char *szCommunity
// Argument         : SNMP_Message eCommand
// Argument         : int ulRequestId
int CSNMP::CreateFrame(int ulVersion, char *szCommunity, SNMP_Message eCommand, int ulRequestId)
{
	if ((ulVersion < 1) || (!szCommunity))
		return SNMP_ERROR_BADPARAM;

	sPdu.ulVersion = ulVersion;
	strcpy(sPdu.szCommunity, szCommunity);
	sPdu.eCommand = eCommand;
	sPdu.ulRequestId = ulRequestId;

	sPdu.OID.ulLength = sOid.ulLength;
	for (int i = 0; i < (int)sOid.ulLength; i++)
		sPdu.OID.lpbSnmpData[i] = sOid.lpbSnmpData[i];

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::ReceiveInteger
// Description		: 
// Return type		: int 
// Argument         : unsigned long *lpInteger
int CSNMP::ReceiveInteger(unsigned long *lpInteger, int iTimeout)
{
	int iRet, icnt, idx, ic, nfds=0;
	fd_set frd;
	struct timeval tv;
	u_long ulfArg = 0;

	FD_ZERO(&frd);
	FD_SET(sSnmp, &frd);
	tv.tv_sec = iTimeout;
	tv.tv_usec = 0;
	nfds = max(nfds, sSnmp);
	
	iRet = select(nfds+1, &frd, NULL, NULL, &tv);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_DISCONNECT;	// Rozłączenie
	if (iRet == 0)
		return SNMP_ERROR_SOCKET_TIMEOUT;		// Timeout
	iRet = ioctl(sSnmp, FIONREAD, &ulfArg);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_GENERAL;		// Rozłączenie / Timeout

	if (ulfArg > sizeof(bFrameBuff))
		ulfArg = sizeof(bFrameBuff);
	
	icnt = sizeof(sockaddr_in);
	recvfrom(sSnmp, (char *)bFrameBuff, ulfArg, 0, (struct sockaddr*)&sSai, (socklen_t*)&icnt);

	icnt = 0;
	if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
	{
		// Length
		if (bFrameBuff[icnt] > 127)
		{
			sPduRec.ulLength = bFrameBuff[icnt++] * 128;
			sPduRec.ulLength += bFrameBuff[icnt++];
		}
		else
		{
			sPduRec.ulLength = bFrameBuff[icnt++];
		}
		if (sPduRec.ulLength != (int)(ulfArg - 2))
			return SNMP_ERROR_FRAME;
		// Version
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.ulVersion = bFrameBuff[icnt++] * 256;
				sPduRec.ulVersion += bFrameBuff[icnt++];
			}
			else
				sPduRec.ulVersion = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Community string
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OCTET_STRING))
		{
			ic = bFrameBuff[icnt++];
			for (idx = icnt; idx < icnt + ic; idx++)
				sPduRec.szCommunity[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Command
		if (bFrameBuff[icnt++] == SNMP_Message(GetResponse))
		{
			// PDU Length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// RequestId
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.ulRequestId = bFrameBuff[icnt++] * 256;
				sPduRec.ulRequestId += bFrameBuff[icnt++];
			}
			else
				sPduRec.ulRequestId = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Error status
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.iErrorStatus = bFrameBuff[icnt++] * 256;
				sPduRec.iErrorStatus += bFrameBuff[icnt++];
			}
			else
				sPduRec.iErrorStatus = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Error index
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.iErrorIndex = bFrameBuff[icnt++] * 256;
				sPduRec.iErrorIndex += bFrameBuff[icnt++];
			}
			else
				sPduRec.iErrorIndex = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// 1'st Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// 2'nd Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// Object indentifier
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OBJECT_IDENTIFIER))
		{
			if (bFrameBuff[icnt] > 127)
			{
				sPduRec.OID.ulLength = bFrameBuff[icnt++] * 128;
				sPduRec.OID.ulLength += bFrameBuff[icnt++];
			}
			else
				sPduRec.OID.ulLength = bFrameBuff[icnt++];

			for (idx = icnt; idx < (int)(icnt + sPduRec.OID.ulLength); idx++)
				sPduRec.OID.lpbSnmpData[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		// Value
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				*lpInteger = bFrameBuff[icnt++] * 256;
				*lpInteger += bFrameBuff[icnt++];
			}
			else
				*lpInteger = bFrameBuff[icnt++];

		}
		else
			return SNMP_ERROR_FRAME;
	}
	else
		return SNMP_ERROR_FRAME;

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::ReceiveOctetString
// Description		: 
// Return type		: int 
// Argument         : char *lpszOctetString
// Argument         : int iTimeout
int CSNMP::ReceiveOctetString(char *lpszOctetString, int iTimeout)
{
	int iRet, icnt, idx, ic, nfds=0;
	fd_set frd;
	struct timeval tv;
	u_long ulfArg = 0;

	FD_ZERO(&frd);
	FD_SET(sSnmp, &frd);
	tv.tv_sec = iTimeout;
	tv.tv_usec = 0;
	nfds = max(nfds, sSnmp);
	
	iRet = select(nfds+1, &frd, NULL, NULL, &tv);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_DISCONNECT;	// Rozłączenie
	if (iRet == 0)
		return SNMP_ERROR_SOCKET_TIMEOUT;		// Timeout
	iRet = ioctl(sSnmp, FIONREAD, &ulfArg);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_GENERAL;		// Rozłączenie / Timeout

	if (ulfArg > sizeof(bFrameBuff))
		ulfArg = sizeof(bFrameBuff);
	
	//recv(sSnmp, (char *)bFrameBuff, ulfArg, 0);
	icnt = sizeof(sockaddr_in);
	recvfrom(sSnmp, (char *)bFrameBuff, ulfArg, 0, (struct sockaddr *)&sSai, (socklen_t*)&icnt);

	icnt = 0;
	if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
	{
		// Length
		if (bFrameBuff[icnt] > 127)
		{
			sPduRec.ulLength = bFrameBuff[icnt++] * 128;
			sPduRec.ulLength += bFrameBuff[icnt++];
		}
		else
		{
			sPduRec.ulLength = bFrameBuff[icnt++];
		}
		if (sPduRec.ulLength != (int)(ulfArg - 2))
			return SNMP_ERROR_FRAME;
		// Version
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.ulVersion = bFrameBuff[icnt++] * 256;
				sPduRec.ulVersion += bFrameBuff[icnt++];
			}
			else
				sPduRec.ulVersion = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Community string
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OCTET_STRING))
		{
			if (bFrameBuff[icnt] > 127)
			{
				ic = bFrameBuff[icnt++] * 128;
				ic += bFrameBuff[icnt++];
			}
			else
				ic = bFrameBuff[icnt++];

			for (idx = icnt; idx < icnt + ic; idx++)
				sPduRec.szCommunity[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Command
		if (bFrameBuff[icnt++] == SNMP_Message(GetResponse))
		{
			// PDU Length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// RequestId
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.ulRequestId = bFrameBuff[icnt++] * 256;
				sPduRec.ulRequestId += bFrameBuff[icnt++];
			}
			else
				sPduRec.ulRequestId = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Error status
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.iErrorStatus = bFrameBuff[icnt++] * 256;
				sPduRec.iErrorStatus += bFrameBuff[icnt++];
			}
			else
				sPduRec.iErrorStatus = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// Error index
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (bFrameBuff[icnt++] > 1)
			{
				sPduRec.iErrorIndex = bFrameBuff[icnt++] * 256;
				sPduRec.iErrorIndex += bFrameBuff[icnt++];
			}
			else
				sPduRec.iErrorIndex = bFrameBuff[icnt++];
		}
		else
			return SNMP_ERROR_FRAME;
		// 1'st Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// 2'nd Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (bFrameBuff[icnt] > 127)
				icnt += (bFrameBuff[icnt] - 127);
			else
				icnt++;
		}
		else
			return SNMP_ERROR_FRAME;
		// Object indentifier
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OBJECT_IDENTIFIER))
		{
			if (bFrameBuff[icnt] > 127)
			{
				sPduRec.OID.ulLength = bFrameBuff[icnt++] * 128;
				sPduRec.OID.ulLength += bFrameBuff[icnt++];
			}
			else
				sPduRec.OID.ulLength = bFrameBuff[icnt++];

			for (idx = icnt; idx < (int)(icnt + sPduRec.OID.ulLength); idx++)
				sPduRec.OID.lpbSnmpData[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		// Value
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OCTET_STRING))
		{
			if (bFrameBuff[icnt] > 127)
			{
				ic = bFrameBuff[icnt++] * 128;
				ic += bFrameBuff[icnt++];
			}
			else
				ic = bFrameBuff[icnt++];

			for (idx = icnt; idx < icnt + ic; idx++)
				lpszOctetString[idx - icnt] = bFrameBuff[idx];
			lpszOctetString[idx - icnt] = '\0';
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
	}
	else
		return SNMP_ERROR_FRAME;

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::Receive
// Description		: Zwraca w buforze typ zmiennej, rozmiar, wartość zmiennej w zaalokowanej
//					: pamięci. 
// Return type		: int 
// Argument         : LPSNMP_Pdu lpsp
// Argument         : int *lpiType
// Argument         : int *lpiSize
// Argument         : LPBYTE lpbData
// Argument         : int iTimeout
int CSNMP::Receive(LPSNMP_Pdu lpsp, int *lpiType, int *lpiSize, LPBYTE lpbData, int iTimeout)
{
	int iRet, icnt, idx, ic, nfds;
	unsigned long ulres;
	fd_set frd;
	struct timeval tv;
	u_long ulfArg = 0;

	FD_ZERO(&frd);
	FD_SET(sSnmp, &frd);
	tv.tv_sec = iTimeout;
	tv.tv_usec = 0;
	nfds = max(nfds, sSnmp);
	
	iRet = select(nfds+1, &frd, NULL, NULL, &tv);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_DISCONNECT;	// Rozłączenie
	if (iRet == 0)
		return SNMP_ERROR_SOCKET_TIMEOUT;		// Timeout
	iRet = ioctl(sSnmp, FIONREAD, &ulfArg);
	if (iRet == SOCKET_ERROR)
		return SNMP_ERROR_SOCKET_GENERAL;		// Rozłączenie / Timeout

	if (ulfArg > sizeof(bFrameBuff))
		ulfArg = sizeof(bFrameBuff);
	
	//recv(sSnmp, (char *)bFrameBuff, ulfArg, 0);
	icnt = sizeof(sockaddr_in);
	recvfrom(sSnmp, (char *)bFrameBuff, ulfArg, 0, (struct sockaddr *)&sSai, (socklen_t*)&icnt);

	icnt = 0;
	if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
	{
		// Length
		if (DecodeBER(&bFrameBuff[icnt], &ic, &lpsp->ulLength) != SNMP_ERROR_SUCCESS)
			return SNMP_ERROR_FRAME_LENGTH;
		else
			icnt += ic;
		if (lpsp->ulLength != (int)(ulfArg - (1 + ic)))
			return SNMP_ERROR_FRAME_LENGTH;
		// Version
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&bFrameBuff[icnt], &ic, &lpsp->ulVersion) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// Community string
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OCTET_STRING))
		{
			if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;

			for (idx = icnt; idx < (int)(icnt + ulres); idx++)
				lpsp->szCommunity[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Command
		lpsp->eCommand = (SNMP_Message)bFrameBuff[icnt++];
		if (lpsp->eCommand == SNMP_Message(GetResponse))
		{
			// PDU Length - not needed right now
			if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// RequestId
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&bFrameBuff[icnt], &ic, &lpsp->ulRequestId) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// Error status
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
			lpsp->iErrorStatus = (int)ulres;
		}
		else
			return SNMP_ERROR_FRAME;
		// Error index
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
			lpsp->iErrorIndex = ulres;
		}
		else
			return SNMP_ERROR_FRAME;

		if (lpsp->iErrorStatus != 0)
		{
			// Błąd, więc przerywam i zwracam odwrócony kod błędu (* -1)
			return (lpsp->iErrorStatus);
		}

		// 1'st Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// 2'nd Sequence
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length - not needed right now
			if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// Object indentifier
		if (bFrameBuff[icnt++] == ASN1_Primitive(DT_OBJECT_IDENTIFIER))
		{
			if (DecodeBER(&bFrameBuff[icnt], &ic, &lpsp->OID.ulLength) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME_LENGTH;
			else
				icnt += ic;

			for (idx = icnt; idx < (int)(icnt + lpsp->OID.ulLength); idx++)
				lpsp->OID.lpbSnmpData[idx - icnt] = bFrameBuff[idx];
			icnt = idx;
		}
		// Value
		*lpiType = bFrameBuff[icnt++];
		switch(*lpiType)
		{
		// INTEGER
		case ASN1_Primitive(DT_INTEGER):
			{
				if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += *lpiSize;
				*lpiSize = ic - 1;
				memcpy(lpbData, &ulres, sizeof(unsigned long));
				break;
			}
		// OCTET STRING
		case ASN1_Primitive(DT_OCTET_STRING):
			{
				if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME_LENGTH;
				else
					icnt += ic;
				*lpiSize = (int)ulres;
				for (idx = icnt; idx < icnt + *lpiSize; idx++)
					lpbData[idx - icnt] = bFrameBuff[idx];
				lpbData[idx - icnt] = '\0';
				*lpiSize += 1;
				icnt = idx;
				break;
			}
		// Object identifier
		case ASN1_Primitive(DT_OBJECT_IDENTIFIER):
			{
				if (DecodeBER(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += ic;
				*lpiSize = ulres;
				memcpy(lpbData, &bFrameBuff[icnt], ulres);
				break;
			}
		// Gauge
		case SNMP_Primitive(PT_Gauge):
			{
				if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += *lpiSize;
				*lpiSize = ic - 1;
				memcpy(lpbData, &ulres, sizeof(unsigned long));
				break;
			}
		// INTEGER
		case SNMP_Primitive(PT_Counter):
			{
				if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += *lpiSize;
				*lpiSize = ic - 1;
				memcpy(lpbData, &ulres, sizeof(unsigned long));
				break;
			}
		// IpAddress
		case SNMP_Primitive(PT_IpAddress):
			{
				if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += ic;
				*lpiSize = ic - 1;
				memcpy(lpbData, &ulres, sizeof(unsigned long));
				break;
			}
		// TimeTicks
		case SNMP_Primitive(PT_TimeTicks):
			{
				if (DecodeInteger(&bFrameBuff[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += ic;
				*lpiSize = ic - 1;
				memcpy(lpbData, &ulres, sizeof(unsigned long));
				break;
			}
		default:
			return SNMP_ERROR_UNSUPPORTED_TYPE;
		}
	}
	else
		return SNMP_ERROR_FRAME;

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::CleanUp
// Description		: 
// Return type		: int 
// Argument         : LPSNMP_Pdu lpsp
int CSNMP::CleanUpPdu(LPSNMP_Pdu lpsp)
{
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::CleanUp
// Description		: 
// Return type		: int 
int CSNMP::CleanUpPdu()
{
	return SNMP_ERROR_SUCCESS;
}

// Function name	: *CSNMP::GetErrorString
// Description		: 
// Return type		: char 
// Argument         : int iErrorStatus
char *CSNMP::GetErrorString(int iErrorStatus)
{
	switch(iErrorStatus)
	{
	// SNMP errors
	case SNMP_ERROR_NOERROR:			return SNMP_ERROR_STR_NOERROR;		//"No error";
	case SNMP_ERROR_TOOBIG:				return SNMP_ERROR_STR_TOOBIG;		//"Too big";
	case SNMP_ERROR_NOSUCHNAME:			return SNMP_ERROR_STR_NOSUCHNAME;	//"No such name";
	case SNMP_ERROR_BADVALUE:			return SNMP_ERROR_STR_BADVALUE;		//"Bad value";
	case SNMP_ERROR_READONLY:			return SNMP_ERROR_STR_READONLY;		//"Read only";
	case SNMP_ERROR_GENERR:				return SNMP_ERROR_STR_GENERR;		//"Generic error";
	// Other errors
	case SNMP_ERROR_BADPARAM:			return SNMP_ERROR_STR_BADPARAM;
	case SNMP_ERROR_MEMALLOC:			return SNMP_ERROR_STR_MEMALLOC;
	case SNMP_ERROR_FRAME:				return SNMP_ERROR_STR_FRAME;
	case SNMP_ERROR_FRAME_LENGTH:		return SNMP_ERROR_STR_FRAME_LENGTH;
	case SNMP_ERROR_UNSUPPORTED_TYPE:	return SNMP_ERROR_STR_UNSUPPORTED_TYPE;
	case SNMP_ERROR_SOCKET_DISCONNECT:	return SNMP_ERROR_STR_SOCKET_DISCONNECT;
	case SNMP_ERROR_SOCKET_TIMEOUT:		return SNMP_ERROR_STR_SOCKET_TIMEOUT;
	case SNMP_ERROR_SOCKET_GENERAL:		return SNMP_ERROR_STR_SOCKET_GENERAL;
	case SNMP_ERROR_SOCKET_CONNECT:		return SNMP_ERROR_STR_SOCKET_CONNECT;
	case SNMP_ERROR_SOCKET_BINDING:		return SNMP_ERROR_STR_SOCKET_BINDING;
	case SNMP_ERROR_SOCKET_LISTEN:		return SNMP_ERROR_STR_SOCKET_LISTEN;

	default: return (char *)SNMP_ERROR_STR;	// unknown error
	}
}

// Function name	: CSNMP::StartTrapHandler
// Description		: Petla nasluchu dla Pulapek (Traps)
//					: Funkcja jest blokujaca.
// Return type		: int 
// Argument         : CBTRAPHANDLE lpcbt
// Argument         : int iPort
int CSNMP::StartTrapHandler(CBTRAPFN lpcbt, int iPort)
{
	int iRet=0, iLen=0, nfds=0;
	fd_set frd;
	u_long ulfArg=0;
	struct sockaddr_in sSaiSrv, sSaiCln;
	//struct hostent* lphe;

	if (!lpcbt)
		return SNMP_ERROR_BADPARAM;

	//lphe = gethostbyname(getenv("HOSTNAME"));
	//if ( !lphe )
	//	return SNMP_ERROR;

	memset(&sSaiSrv, 0, sizeof(struct sockaddr_in));
	sSaiSrv.sin_family = AF_INET;
	sSaiSrv.sin_addr.s_addr = INADDR_ANY;//*((in_addr*)*lphe->h_addr_list);
	sSaiSrv.sin_port = htons(iPort);

	if ((sSnmpTrap = socket(AF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) {
		iError = errno;
		return SNMP_ERROR_SOCKET_GENERAL;
	}
	if (bind(sSnmpTrap, (struct sockaddr *)&sSaiSrv, sizeof(sSaiSrv)) == SOCKET_ERROR) {
		iError = errno;
		return SNMP_ERROR_SOCKET_BINDING;
	}

	bQuitHandler = false;
	while(!bQuitHandler) {
		FD_ZERO(&frd);
		FD_SET(sSnmpTrap, &frd);
		nfds = max(nfds, sSnmpTrap);
		switch(iRet = select(nfds+1, &frd, NULL, NULL, NULL))
		{
		// Timeout
		case 0:
			continue;
		// Rozłączenie
		case SOCKET_ERROR:
			return SNMP_ERROR_SOCKET_DISCONNECT;
		}

		if ((iRet = ioctl(sSnmpTrap, FIONREAD, &ulfArg)) == SOCKET_ERROR)
			return SNMP_ERROR_SOCKET_GENERAL;		// Rozłączenie / Timeout

		if (ulfArg > sizeof(bFrameTrapBuff))
			ulfArg = sizeof(bFrameTrapBuff);
	
		iLen = sizeof(sockaddr_in);
		recvfrom(sSnmpTrap, (char*)bFrameTrapBuff, ulfArg, 0, (struct sockaddr*)&sSaiCln, (socklen_t*)&iLen);

		RecognizeTrapFrame(&sTrapPdu, bFrameTrapBuff, ulfArg);

		if (lpcbt != NULL)
			(CBTRAPFN *)lpcbt(&sTrapPdu, &sSaiCln);
	}
	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::StopTrapHandler
// Description		: 
// Return type		: int 
int CSNMP::StopTrapHandler()
{
	if (!bQuitHandler)
	{
		bQuitHandler = true;
		shutdown(sSnmpTrap, SHUT_RDWR);
		close(sSnmpTrap);
		return SNMP_ERROR_SUCCESS;
	}
	else
		return SNMP_ERROR;
}

// Function name	: CSNMP::RecognizeTrapFrame
// Description		: Rozpoznanie ramki Trap-Pdu
// Return type		: int 
// Argument         : LPSNMP_TrapPdu lpstp
// Argument         : LPBYTE lpbData
// Argument         : int iSize
int CSNMP::RecognizeTrapFrame(LPSNMP_TrapPdu lpstp, LPBYTE lpbData, int iSize)
{
	int				icnt, idx, ic;
	unsigned long	ulres;

	if (!lpbData || iSize <= 0)
		return SNMP_ERROR;

	icnt = 0;
	if (lpbData[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
	{
		// Length
		if (DecodeBER(&lpbData[icnt], &ic, &lpstp->ulLength) != SNMP_ERROR_SUCCESS)
			return SNMP_ERROR_FRAME;
		else
			icnt += ic;
			 
		if (lpstp->ulLength != (unsigned int)(iSize - 2))
			return SNMP_ERROR_FRAME;
		// Version
		if (lpbData[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
			{
				icnt += ic;
				lpstp->ulVersion = (int)ulres;
			}
		}
		else
			return SNMP_ERROR_FRAME;
		// Community string
		if (lpbData[icnt++] == ASN1_Primitive(DT_OCTET_STRING))
		{
			if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
			memset(lpstp->szCommunity, 0, sizeof(lpstp->szCommunity));
			for (idx = icnt; idx < (int)(icnt + ulres); idx++)
				lpstp->szCommunity[idx - icnt] = lpbData[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Command
		lpstp->eCommand = (SNMP_Message)lpbData[icnt++];
		if (lpstp->eCommand == SNMP_Message(Trap))
		{
			// PDU Length - not needed right now
			if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// Enterprise
		if (lpbData[icnt++] == ASN1_Primitive(DT_OBJECT_IDENTIFIER))
		{
			if (DecodeBER(&lpbData[icnt], &ic, &lpstp->EnterpriseOID.ulLength) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;

			for (idx = icnt; idx < (int)(icnt + lpstp->EnterpriseOID.ulLength); idx++)
				lpstp->EnterpriseOID.lpbSnmpData[idx - icnt] = lpbData[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Agent Address
		if (lpbData[icnt++] == SNMP_Primitive(PT_IpAddress))
		{
			if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
				 
			memset(lpstp->bAgentAddress, 0, sizeof(lpstp->bAgentAddress));
			if (ulres > 6)
				ulres = 6;
			for (idx = icnt; idx < (int)(icnt + ulres); idx++)
				lpstp->bAgentAddress[idx - icnt] = lpbData[idx];
			icnt = idx;
		}
		else
			return SNMP_ERROR_FRAME;
		// Generic Trap
		if (lpbData[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&lpbData[icnt], &ic, &lpstp->ulGenericTrap) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// Specific Trap
		if (lpbData[icnt++] == ASN1_Primitive(DT_INTEGER))
		{
			if (DecodeInteger(&lpbData[icnt], &ic, &lpstp->ulSpecificTrap) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		// TimeStamp
		if (lpbData[icnt++] == SNMP_Primitive(PT_TimeTicks))
		{
			if (DecodeInteger(&lpbData[icnt], &ic, &lpstp->ulTimeStamp) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		//
		// "Parent" Sequence of child sequences
		//
		if (lpbData[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
		{
			// Sequence length
			if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR_FRAME;
			else
				icnt += ic;
		}
		else
			return SNMP_ERROR_FRAME;
		//
		// From here are defined objects
		// Every object must start with SEQUENCE or SEQUENCE_OF data type
		//
		lpstp->iNumObjects = 0;
		while(icnt < iSize)
		{
			if (lpbData[icnt++] == ASN1_Primitive(DT_SEQUENCE_OF))
			{
				// Sequence length
				if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
					return SNMP_ERROR_FRAME;
				else
					icnt += ic;

				if (lpbData[icnt++] == ASN1_Primitive(DT_OBJECT_IDENTIFIER))
				{
					if (DecodeBER(&lpbData[icnt], &ic, &ulres) != SNMP_ERROR_SUCCESS)
						return SNMP_ERROR_FRAME;
					else
					{
						icnt += ic;
						lpstp->sObjOID[lpstp->iNumObjects].ulLength = ulres;
					}

					for (idx = icnt; idx < (int)(icnt + ulres); idx++)
						lpstp->sObjOID[lpstp->iNumObjects].lpbSnmpData[idx - icnt] = lpbData[idx];
					icnt = idx;
					lpstp->iNumObjects++;
				}
				else
					return SNMP_ERROR_FRAME;
			}
			else
				return SNMP_ERROR_FRAME;
		}
	}
	else
		return SNMP_ERROR_FRAME;

	return SNMP_ERROR_SUCCESS;
}

// Function name	: CSNMP::ConvertStringToOID
// Description		: Konwersja ciągu OID na obiekt OID
//					: Przerywnikiem każdej pozycji może być: Przecinek, Kropka, Spacja.
// Return type		: int 
// Argument         : LPSNMP_OID lpOid
// Argument         : char *lpszOID
int CSNMP::ConvertStringToOID(LPSNMP_OID lpOid, char *lpszOID)
{
	int i=0, iCnt=0, b1=0;
	BYTE b2=0;
	char szInBuff[512], *lpszToken;

	memset(lpOid->lpbSnmpData, 0, sizeof(lpOid->lpbSnmpData));
	strcpy(szInBuff, lpszOID);

	lpszToken = strtok(szInBuff, ",. ");
	while(lpszToken != NULL)
	{
		i = (int)strtol((const char *)lpszToken, NULL, 10);
		if (i < 128) {
			lpOid->lpbSnmpData[iCnt] = (BYTE)i;
		} else {
			if (EncodeBER((unsigned long)i, (int *)&b1, 
				(LPBYTE)(lpOid->lpbSnmpData+iCnt)) != SNMP_ERROR_SUCCESS)
				return SNMP_ERROR;
			iCnt += b1;
		}
		iCnt++;
		lpszToken = strtok(NULL, ",. ");
	}
	lpOid->ulLength = iCnt;

	return iCnt;
}

