- #include <stdio.h>
- #include <string.h>
- #include <stdlib.h>
- #include <stdarg.h>
- #include "ctype.h"
- #include "w5500.h"
- #include "socket.h"
- #include "config.h"
- #include "mcu_int.h"
- #include "snmpLib.h"
- #include "snmpDemo.h"
- struct messageStruct request_msg;
- struct messageStruct response_msg;
- uint8 errorStatus, errorIndex;
- #define COPY_SEGMENT_TEMP20110929(x) \
- { \
- request_msg.index += seglen; \
- memcpy(&response_msg.buffer[response_msg.index], &request_msg.buffer[x.start], seglen ); \
- response_msg.index += seglen; \
- }
- void WDEBUG(char *fmt, ...)
- {
- char zlog_string[100];
- va_list ap;
- va_start(ap, fmt);
- vsprintf(zlog_string, fmt, ap);
- strcat(zlog_string, "\r\n");
- printf(zlog_string);
- va_end(ap);
- }
- int32 findEntry(uint8 *oid, int32 len)
- {
- int32 i;
- for (i = 0 ; i < maxData ; i++)
- {
- if (len == snmpData[i].oidlen)
- {
- if (!memcmp(snmpData[i].oid, oid, len)) return(i);
- }
- }
- return OID_NOT_FOUND;
- }
- int32 getOID(int32 id, uint8 *oid, uint8 *len)
- {
- int32 j;
- if (!((id >= 0) && (id < maxData))) return INVALID_ENTRY_ID;
- *len = snmpData[id].oidlen;
- for (j = 0 ; j < *len ; j++)
- {
- oid[j] = snmpData[id].oid[j];
- }
- return SUCCESS;
- }
- int32 getValue( uint8 *vptr, int32 vlen)
- {
- int32 index = 0;
- int32 value = 0;
- while (index < vlen)
- {
- if (index != 0) value <<= 8;
- value |= vptr[index++];
- }
- return value;
- }
- int32 getEntry(int32 id, uint8 *dataType, void *ptr, int32 *len)
- {
- if (!((id >= 0) && (id < maxData))) return INVALID_ENTRY_ID;
- *dataType = snmpData[id].dataType;
- switch(*dataType)
- {
- case SNMPDTYPE_OCTET_STRING :
- case SNMPDTYPE_OBJ_ID :
- {
- uint8 *string = ptr;
- int32 j;
- if (snmpData[id].getfunction != NULL)
- {
- snmpData[id].getfunction( (void *)&snmpData[id].u.octetstring, &snmpData[id].dataLen );
- }
- if ( (*dataType)==SNMPDTYPE_OCTET_STRING )
- {
- snmpData[id].dataLen = (uint8)strlen((int8*)&snmpData[id].u.octetstring);
- }
- *len = snmpData[id].dataLen;
- for (j = 0 ; j < *len ; j++)
- {
- string[j] = snmpData[id].u.octetstring[j];
- }
- }
- break;
- case SNMPDTYPE_INTEGER :
- case SNMPDTYPE_TIME_TICKS :
- case SNMPDTYPE_COUNTER :
- case SNMPDTYPE_GAUGE :
- {
- int32 *value = ( int32 * )ptr;
- if (snmpData[id].getfunction != NULL)
- {
- snmpData[id].getfunction( (void *)&snmpData[id].u.intval, &snmpData[id].dataLen );
- }
- *len = sizeof(uint32);
- *value = HTONL(snmpData[id].u.intval);
- }
- break;
- default :
- return INVALID_DATA_TYPE;
- }
- return SUCCESS;
- }
- int32 setEntry(int32 id, void *val, int32 vlen, uint8 dataType, int32 index)
- {
- int32 retStatus=OID_NOT_FOUND;
- int32 j;
- if (snmpData[id].dataType != dataType)
- {
- errorStatus = BAD_VALUE;
- errorIndex = index;
- return INVALID_DATA_TYPE;
- }
- switch(snmpData[id].dataType)
- {
- case SNMPDTYPE_OCTET_STRING :
- case SNMPDTYPE_OBJ_ID :
- {
- uint8 *string = val;
- for (j = 0 ; j < vlen ; j++)
- {
- snmpData[id].u.octetstring[j] = string[j];
- }
- snmpData[id].dataLen = vlen;
- }
- retStatus = SUCCESS;
- break;
- case SNMPDTYPE_INTEGER :
- case SNMPDTYPE_TIME_TICKS :
- case SNMPDTYPE_COUNTER :
- case SNMPDTYPE_GAUGE :
- {
- snmpData[id].u.intval = getValue( (uint8 *)val, vlen);
- snmpData[id].dataLen = vlen;
- if (snmpData[id].setfunction != NULL)
- {
- snmpData[id].setfunction(snmpData[id].u.intval);
- }
- }
- retStatus = SUCCESS;
- break;
- default :
- retStatus = INVALID_DATA_TYPE;
- break;
- }
- return retStatus;
- }
- int32 parseLength(const uint8 *msg, int32 *len)
- {
- int32 i=1;
- if (msg[0] & 0x80)
- {
- int32 tlen = (msg[0] & 0x7f) - 1;
- *len = msg[i++];
- while (tlen--)
- {
- *len <<= 8;
- *len |= msg[i++];
- }
- }
- else
- {
- *len = msg[0];
- }
- return i;
- }
- int32 parseTLV(const uint8 *msg, int32 index, tlvStructType *tlv)
- {
- int32 Llen = 0;
- tlv->start = index;
- Llen = parseLength((const uint8 *)&msg[index+1], &tlv->len );
- tlv->vstart = index + Llen + 1;
- switch (msg[index])
- {
- case SNMPDTYPE_SEQUENCE:
- case GET_REQUEST:
- case GET_NEXT_REQUEST:
- case SET_REQUEST:
- tlv->nstart = tlv->vstart;
- break;
- default:
- tlv->nstart = tlv->vstart + tlv->len;
- break;
- }
- return 0;
- }
- void insertRespLen(int32 reqStart, int32 respStart, int32 size)
- {
- int32 indexStart, lenLength;
- uint32 mask = 0xff;
- int32 shift = 0;
- if (request_msg.buffer[reqStart+1] & 0x80)
- {
- lenLength = request_msg.buffer[reqStart+1] & 0x7f;
- indexStart = respStart+2;
- while (lenLength--)
- {
- response_msg.buffer[indexStart+lenLength] =
- (uint8)((size & mask) >> shift);
- shift+=8;
- mask <<= shift;
- }
- }
- else
- {
- response_msg.buffer[respStart+1] = (uint8)(size & 0xff);
- }
- }
- int32 parseVarBind(int32 reqType, int32 index)
- {
- int32 seglen = 0, id;
- tlvStructType name, value;
- int32 size = 0;
-
- extern const int32 maxData;
- parseTLV(request_msg.buffer, request_msg.index, &name);
- if ( request_msg.buffer[name.start] != SNMPDTYPE_OBJ_ID ) return -1;
- id = findEntry(&request_msg.buffer[name.vstart], name.len);
- if ((reqType == GET_REQUEST) || (reqType == SET_REQUEST))
- {
- seglen = name.nstart - name.start;
- COPY_SEGMENT_TEMP20110929(name);
- size = seglen;
- }
- else if (reqType == GET_NEXT_REQUEST)
- {
- response_msg.buffer[response_msg.index] = request_msg.buffer[name.start];
- if (++id >= maxData)
- {
- id = OID_NOT_FOUND;
- seglen = name.nstart - name.start;
- COPY_SEGMENT_TEMP20110929(name);
- size = seglen;
- }
- else
- {
- request_msg.index += name.nstart - name.start;
- getOID(id, &response_msg.buffer[response_msg.index+2], &response_msg.buffer[response_msg.index+1]);
- seglen = response_msg.buffer[response_msg.index+1]+2;
- response_msg.index += seglen ;
- size = seglen;
- }
- }
- parseTLV(request_msg.buffer, request_msg.index, &value);
- if (id != OID_NOT_FOUND)
- {
- uint8 dataType;
- int32 len;
- if ((reqType == GET_REQUEST) || (reqType == GET_NEXT_REQUEST))
- {
- getEntry(id, &dataType, &response_msg.buffer[response_msg.index+2], &len);
- response_msg.buffer[response_msg.index] = dataType;
- response_msg.buffer[response_msg.index+1] = len;
- seglen = (2 + len);
- response_msg.index += seglen;
- request_msg.index += (value.nstart - value.start);
- }
- else if (reqType == SET_REQUEST)
- {
- setEntry(id, &request_msg.buffer[value.vstart], value.len, request_msg.buffer[value.start], index);
- seglen = value.nstart - value.start;
- COPY_SEGMENT_TEMP20110929(value);
- }
- }
- else
- {
- seglen = value.nstart - value.start;
- COPY_SEGMENT_TEMP20110929(value);
- errorIndex = index;
- errorStatus = NO_SUCH_NAME;
- }
- size += seglen;
- return size;
- }
- int32 parseSequence(int32 reqType, int32 index)
- {
- int32 seglen;
- tlvStructType seq;
- int32 size = 0, respLoc;
- parseTLV(request_msg.buffer, request_msg.index, &seq);
- if ( request_msg.buffer[seq.start] != SNMPDTYPE_SEQUENCE ) return -1;
- seglen = seq.vstart - seq.start;
- respLoc = response_msg.index;
- COPY_SEGMENT_TEMP20110929(seq);
- size = parseVarBind( reqType, index );
- insertRespLen(seq.start, respLoc, size);
- size += seglen;
- return size;
- }
- int32 parseSequenceOf(int32 reqType)
- {
- int32 seglen;
- tlvStructType seqof;
- int32 size = 0, respLoc;
- int32 index = 0;
- parseTLV(request_msg.buffer, request_msg.index, &seqof);
- if ( request_msg.buffer[seqof.start] != SNMPDTYPE_SEQUENCE_OF ) return -1;
- seglen = seqof.vstart - seqof.start;
- respLoc = response_msg.index;
- COPY_SEGMENT_TEMP20110929(seqof);
- while (request_msg.index < request_msg.len)
- {
- size += parseSequence( reqType, index++ );
- }
- insertRespLen(seqof.start, respLoc, size);
- return size;
- }
- int32 parseRequest()
- {
- int32 ret, seglen;
- tlvStructType snmpreq, requestid, errStatus, errIndex;
- int32 size = 0, respLoc, reqType;
- parseTLV(request_msg.buffer, request_msg.index, &snmpreq);
- reqType = request_msg.buffer[snmpreq.start];
- if ( !VALID_REQUEST(reqType) ) return -1;
- seglen = snmpreq.vstart - snmpreq.start;
- respLoc = snmpreq.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(snmpreq);
- response_msg.buffer[snmpreq.start] = GET_RESPONSE;
- parseTLV(request_msg.buffer, request_msg.index, &requestid);
- seglen = requestid.nstart - requestid.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(requestid);
- parseTLV(request_msg.buffer, request_msg.index, &errStatus);
- seglen = errStatus.nstart - errStatus.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(errStatus);
- parseTLV(request_msg.buffer, request_msg.index, &errIndex);
- seglen = errIndex.nstart - errIndex.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(errIndex);
- ret = parseSequenceOf(reqType);
- if (ret == -1) return -1;
- else size += ret;
- insertRespLen(snmpreq.start, respLoc, size);
- if (errorStatus)
- {
- response_msg.buffer[errStatus.vstart] = errorStatus;
- response_msg.buffer[errIndex.vstart] = errorIndex + 1;
- }
- return size;
- }
- int32 parseCommunity()
- {
- int32 seglen;
- tlvStructType community;
- int32 size=0;
- parseTLV(request_msg.buffer, request_msg.index, &community);
- if (!((request_msg.buffer[community.start] == SNMPDTYPE_OCTET_STRING) && (community.len == COMMUNITY_SIZE)))
- {
- return -1;
- }
- if (!memcmp(&request_msg.buffer[community.vstart], (int8 *)COMMUNITY, COMMUNITY_SIZE))
- {
- seglen = community.nstart - community.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(community);
- size += parseRequest();
- }
- else
- {
- return -1;
- }
- return size;
- }
- int32 parseVersion()
- {
- int32 size = 0, seglen;
- tlvStructType tlv;
- size = parseTLV(request_msg.buffer, request_msg.index, &tlv);
- if (!((request_msg.buffer[tlv.start] == SNMPDTYPE_INTEGER) && (request_msg.buffer[tlv.vstart] == SNMP_V1)))
- return -1;
- seglen = tlv.nstart - tlv.start;
- size += seglen;
- COPY_SEGMENT_TEMP20110929(tlv);
- size = parseCommunity();
- if (size == -1) return size;
- else return (size + seglen);
- }
- int32 parseSNMPMessage()
- {
- int32 size = 0, seglen, respLoc;
- tlvStructType tlv;
- parseTLV(request_msg.buffer, request_msg.index, &tlv);
- if (request_msg.buffer[tlv.start] != SNMPDTYPE_SEQUENCE_OF) return -1;
- seglen = tlv.vstart - tlv.start;
- respLoc = tlv.start;
- COPY_SEGMENT_TEMP20110929(tlv);
- size = parseVersion();
- if (size == -1) return -1;
- else size += seglen;
- insertRespLen(tlv.start, respLoc, size);
- return 0;
- }
- void dumpCode(int8* header, int8* tail, unsigned char *buff, int len)
- {
- int i;
- printf(header);
- for (i=0; i<len; i++)
- {
- if ( i%16==0 ) printf("0x%04x : ", i);
- printf("%02x ",buff[i]);
- if ( i%16-15==0 )
- {
- int j;
- printf(" ");
- for (j=i-15; j<=i; j++)
- {
- if ( isprint(buff[j]) ) printf("%c", buff[j]);
- else printf(".");
- }
- printf("\r\n");
- }
- }
- if ( i%16!=0 )
- {
- int j;
- int spaces=(len-i+16-i%16)*3+2;
- for (j=0; j<spaces; j++) printf(" ");
- for (j=i-i%16; j<len; j++)
- {
- if ( isprint(buff[j]) ) printf("%c", buff[j]);
- else printf(".");
- }
- }
- printf(tail);
- }
- void ipToByteArray(int8 *ip, uint8 *pDes)
- {
- uint32 i, ip1=0, ip2=0, ip3=0, ip4=0;
- int8 buff[32];
- uint32 len = (uint32)strlen(ip);
- strcpy(buff, ip);
- for (i=0; i<len; i++)
- {
- if ( buff[i]=='.' ) buff[i] = ' ';
- }
- sscanf(buff, "%u %u %u %u", &ip1, &ip2, &ip3, &ip4);
- pDes[0] = ip1; pDes[1] = ip2; pDes[2] = ip3; pDes[3] = ip4;
- }
- int32 makeTrapVariableBindings(dataEntryType *oid_data, void *ptr, uint32 *len)
- {
- uint32 j;
- ((uint8*)ptr)[0] = 0x30;
- ((uint8*)ptr)[1] = 0xff;
- ((uint8*)ptr)[2] = 0x06;
- ((uint8*)ptr)[3] = oid_data->oidlen;
- for (j = 0 ; j < oid_data->oidlen ; j++)
- {
- ((uint8*)ptr)[j+4] = oid_data->oid[j];
- }
- switch(oid_data->dataType)
- {
- case SNMPDTYPE_OCTET_STRING :
- case SNMPDTYPE_OBJ_ID :
- {
- uint8 *string = &((uint8*)ptr)[4+oid_data->oidlen+2];
- if ( oid_data->dataType==SNMPDTYPE_OCTET_STRING )
- {
- oid_data->dataLen = (uint8)strlen((int8*)&oid_data->u.octetstring);
- }
- for (j = 0 ; j < oid_data->dataLen ; j++)
- {
- string[j] = oid_data->u.octetstring[j];
- }
- ((uint8*)ptr)[4+oid_data->oidlen] = oid_data->dataType;
- ((uint8*)ptr)[4+oid_data->oidlen+1] = oid_data->dataLen;
- ((uint8*)ptr)[1] = 2 + oid_data->oidlen + 2 + oid_data->dataLen;
- *len = 4 + oid_data->oidlen + 2 + oid_data->dataLen;
- }
- break;
- case SNMPDTYPE_INTEGER :
- case SNMPDTYPE_TIME_TICKS :
- case SNMPDTYPE_COUNTER :
- case SNMPDTYPE_GAUGE :
- {
- oid_data->dataLen = 4;
- *(int32*)(&((uint8*)ptr)[4+oid_data->oidlen+2]) = HTONL(oid_data->u.intval);
- ((uint8*)ptr)[4+oid_data->oidlen] = oid_data->dataType;
- ((uint8*)ptr)[4+oid_data->oidlen+1] = oid_data->dataLen;
- ((uint8*)ptr)[1] = 2 + oid_data->oidlen + 2 + oid_data->dataLen;
- *len = 4 + oid_data->oidlen + 2 + oid_data->dataLen;
- }
- break;
- default :
- return INVALID_DATA_TYPE;
- }
- return SUCCESS;
- }
- int32 SnmpXInit()
- {
-
- // initTable();
- // LED3=0;
- return 0;
- }
- uint8 packet_trap[1024] = {0,};
- int32 SnmpXTrapSend(int8* managerIP, int8* agentIP, int8* community, dataEntryType enterprise_oid, uint32 genericTrap, uint32 specificTrap, uint32 va_count, ...)
- {
- uint32 i;
- int32 packet_index = 0;
- int32 packet_buff1 = 0;
- int32 packet_buff2 = 0;
- int32 packet_buff3 = 0;
- uint8 trap_agentip[4] = {0,};
-
- ipToByteArray(agentIP, trap_agentip);
- packet_trap[packet_index++] = 0x30; // ASN.1 Header
- packet_trap[packet_index] = 0xff; // pdu_length, temp
- packet_buff1 = packet_index++;
- packet_trap[packet_index++] = 0x02; // Version
- packet_trap[packet_index++] = 0x01;
- packet_trap[packet_index++] = 0x00;
-
- packet_trap[packet_index++] = 0x04; // Community
- packet_trap[packet_index++] = (uint8)strlen(community);
- memcpy(&(packet_trap[packet_index]), community, strlen(community));
- packet_index = packet_index + (uint8)strlen(community);
- packet_trap[packet_index++] = 0xa4; // trap
- packet_trap[packet_index] = 0xff; // length, temp
- packet_buff2 = packet_index++;
- packet_trap[packet_index++] = 0x06; // enterprise_oid
- packet_trap[packet_index++] = enterprise_oid.oidlen;
- for (i=0; i<enterprise_oid.oidlen; i++)
- {
- packet_trap[packet_index++] = enterprise_oid.oid[i];
- }
-
- packet_trap[packet_index++] = 0x40; // agent ip
- packet_trap[packet_index++] = 0x04;
- packet_trap[packet_index++] = trap_agentip[0];
- packet_trap[packet_index++] = trap_agentip[1];
- packet_trap[packet_index++] = trap_agentip[2];
- packet_trap[packet_index++] = trap_agentip[3];
- packet_trap[packet_index++] = 0x02; // Generic Trap
- packet_trap[packet_index++] = 0x01;
- packet_trap[packet_index++] = (uint8)genericTrap;
- packet_trap[packet_index++] = 0x02; // Specific Trap
- packet_trap[packet_index++] = 0x01;
- packet_trap[packet_index++] = (uint8)specificTrap;
- packet_trap[packet_index++] = 0x43; // Timestamp
- packet_trap[packet_index++] = 0x01;
- packet_trap[packet_index++] = 0x00;
- packet_trap[packet_index++] = 0x30; // Sequence of variable-bindings
- packet_trap[packet_index] = 0xff;
- packet_buff3 = packet_index++;
-
- // variable-bindings
- {
- va_list ap;
- uint32 length_var_bindings = 0;
- uint32 length_buff = 0;
- va_start (ap, va_count);
- for (i=0; i<va_count; i++)
- {
- dataEntryType* fff = va_arg(ap, dataEntryType*);
- makeTrapVariableBindings(fff, &(packet_trap[packet_index]), &length_buff);
- packet_index = packet_index + length_buff;
- length_var_bindings = length_var_bindings + length_buff;
- }
- packet_trap[packet_buff3] = length_var_bindings;
- va_end (ap);
- }
- packet_trap[packet_buff1] = packet_index - 2;
- packet_trap[packet_buff2] = packet_index - (9 + (uint8)strlen(community));
- // Send Packet
- {
- uint8 svr_addr[6];
- //UDPOpen(SOCK_SNMP, 162);
- socket(SOCK_SNMP,Sn_MR_UDP,162,0);
- ipToByteArray(managerIP, svr_addr);
- sendto(SOCK_SNMP, packet_trap, packet_index, svr_addr, 162);
-
- close(SOCK_SNMP);
- return 0;
- }
- }
- int32 SnmpXDaemon()
- {
- int32 snmpfd = 0;
- int32 fromlen = 0;
- int32 retStatus = 0;
- int32 len = 0;
- uint8 loopsnmpd = 1;
- uint8 svr_addr[6];
- uint16 svr_port;
- UNUSED(snmpfd);
- UNUSED(fromlen);
- UNUSED(retStatus);
- // UDPOpen(SOCK_SNMP, 161);
- socket(SOCK_SNMP,Sn_MR_UDP,161,0);
- WDEBUG("Start SNMP Daemon(Agent) ");
- while(loopsnmpd)
- {
- if ( (len = getSn_RX_RSR(SOCK_SNMP))>0 )
- {
- request_msg.len= recvfrom(SOCK_SNMP, (uint8 *)&request_msg.buffer[0], len, svr_addr, &svr_port);
- }
- else
- {
- request_msg.len = 0;
- continue;
- }
- if (request_msg.len > 0)
- {
- dumpCode("\r\n[Request]\r\n", "\r\n", request_msg.buffer, request_msg.len);
-
- request_msg.index = 0;
- response_msg.index = 0;
- errorStatus = errorIndex = 0;
-
- if (parseSNMPMessage() != -1)
- {
- sendto(SOCK_SNMP, response_msg.buffer, response_msg.index, svr_addr, svr_port);
- }
-
- dumpCode("\r\n[Response]\r\n", "\r\n", response_msg.buffer, response_msg.index);
- }
- }
- close(SOCK_SNMP);
- return(0);
- }