void AgentuinoClass::listen(void) {
// if bytes are available in receive buffer
// and pointer to a function (delegate function)
// isn't null, trigger the function
Udp.parsePacket();
if (Udp.available() && _callback != NULL) (*_callback)();
}
unsigned long getNtpTime()
{
// We need to call the begin to reset the socket.
// Because localClient.connect() may occupy the same socket.
EthernetUDP theUDP;
theUDP.begin(localPort);
Serial.println("Sending time sync request to NTP server.");
sendNTPpacket(timeServer); // send an NTP packet to a time server
unsigned long startMillis = millis();
int tryCounter = 1;
while( millis() - startMillis < 1000) // wait up to one second for the response
{ // wait to see if a reply is available
if ( theUDP.available() )
{
theUDP.readPacket(packetBuffer,NTP_PACKET_SIZE); // read the packet into the buffer
//the timestamp starts at byte 40 of the received packet and is four bytes,
// or two words, long. First, esxtract the two words:
unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
// combine the four bytes (two words) into a long integer
// this is NTP time (seconds since Jan 1 1900):
unsigned long secsSince1900 = highWord << 16 | lowWord;
// now convert NTP time into Arduino Time format:
// Time starts on Jan 1 1970. In seconds, that's 2208988800:
const unsigned long seventyYears = 2208988800UL;
// subtract seventy years:
unsigned long epoch = secsSince1900 - seventyYears;
Serial.println("Time sync successfully.");
return epoch;
}
Serial.print("No date data available. Try counter: .");
Serial.println(tryCounter++);
delay(100);
}
Serial.println("Time sync failed.");
return 0; // return 0 if unable to get the time
}
boolean SNMPClass::listen(void)
{
// if bytes are available in receive buffer
// and pointer to a function (delegate function)
// isn't null, trigger the function
if(Udp.parsePacket() > 1024){
_udp_extra_data_packet = true;
}else{
_udp_extra_data_packet = false;
}
if (Udp.available()){
if(_callback != NULL){
(*_callback)();
}else{
return true;
}
}
return false;
}
void commandInterfaceTick() {
int packetSize = cmdsock.parsePacket();
if(cmdsock.available()) {
// read the packet into packetBufffer
cmdsock.read(udpPacketBuffer, PACKET_SIZE);
if(memcmp("INGV\0", udpPacketBuffer, 5) != 0) {
return;
}
bool reboot = false;
unsigned long unixTimeM = getUNIXTime();
unsigned long uptime = getUNIXTime() - getBootTime();
byte macaddress[6] = { 0 };
getMACAddress(macaddress);
uint32_t probeSpeed = getProbeSpeedStatistic();
uint32_t freeramkb = freeMemory();
float latency = 0;
if(udpPacketBuffer[5] == PKTTYPE_GETINFO) {
latency = tcpLatency();
}
float longitude = 0;
float latitude = 0;
switch(udpPacketBuffer[5]) {
case PKTTYPE_DISCOVERY:
// Reply to discovery
udpPacketBuffer[5] = PKTTYPE_DISCOVERY_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 12, getVersionAsString().c_str(), 4);
memcpy(udpPacketBuffer + 16, "uno", 3);
break;
case PKYTYPE_PING:
// Reply to ping
udpPacketBuffer[5] = PKYTYPE_PONG;
break;
case PKTTYPE_SENDGPS:
// Get coords
udpPacketBuffer[5] = PKTTYPE_OK;
memcpy(&latitude, udpPacketBuffer + 12, 4);
memcpy(&longitude, udpPacketBuffer + 16, 4);
reverse4bytes((byte*)&latitude);
reverse4bytes((byte*)&longitude);
break;
case PKTTYPE_REBOOT:
// Reboot
// Reply with OK
udpPacketBuffer[5] = PKTTYPE_OK;
reboot = true;
break;
case PKTTYPE_GETINFO:
udpPacketBuffer[5] = PKTTYPE_GETINFO_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 28, &uptime, 4);
memcpy(udpPacketBuffer + 32, &unixTimeM, 4);
memcpy(udpPacketBuffer + 36, VERSION, 4);
memcpy(udpPacketBuffer + 40, &freeramkb, 4);
memcpy(udpPacketBuffer + 44, &latency, 4);
memcpy(udpPacketBuffer + 53, "uno", 3);
memcpy(udpPacketBuffer + 57, "MMA7361", 7);
memcpy(udpPacketBuffer + 65, &probeSpeed, 4);
break;
#ifdef RESET_ENABLED
case PKTTYPE_RESET:
initEEPROM();
reboot = true;
break;
#endif
default:
// Unknown packet or invalid command
return;
}
if(longitude != 0 && latitude != 0) {
setLongitude(longitude);
setLatitude(latitude);
}
cmdsock.beginPacket(cmdsock.remoteIP(), cmdsock.remotePort());
cmdsock.write(udpPacketBuffer, PACKET_SIZE);
cmdsock.endPacket();
cmdsock.flush();
if(reboot) {
soft_restart();
}
}
}
uint64_t getNTPtime()
{
// send an NTP packet to a time server
sendNTPpacket(timeServer);
// wait to see if a reply is available
delay(1000);
if ( Udp.available() )
{
// read the packet into the buffer
Udp.read(pb, NTP_PACKET_SIZE); // New from IDE 1.0,
// NTP contains four timestamps with an integer part and a fraction part
// we only use the integer part here
unsigned long t1, t2, t3, t4;
t1 = t2 = t3 = t4 = 0;
for (int i=0; i< 4; i++)
{
t1 = t1 << 8 | pb[16+i];
t2 = t2 << 8 | pb[24+i];
t3 = t3 << 8 | pb[32+i];
t4 = t4 << 8 | pb[40+i];
}
// part of the fractional part
// could be 4 bytes but this is more precise than the 1307 RTC
// which has a precision of ONE second
// in fact one byte is sufficient for 1307
float f1,f2,f3,f4;
f1 = ((long)pb[20] * 256 + pb[21]) / 65536.0;
f2 = ((long)pb[28] * 256 + pb[29]) / 65536.0;
f3 = ((long)pb[36] * 256 + pb[37]) / 65536.0;
f4 = ((long)pb[44] * 256 + pb[45]) / 65536.0;
// NOTE:
// one could use the fractional part to set the RTC more precise
// 1) at the right (calculated) moment to the NEXT second!
// t4++;
// delay(1000 - f4*1000);
// RTC.adjust(DateTime(t4));
// keep in mind that the time in the packet was the time at
// the NTP server at sending time so one should take into account
// the network latency (try ping!) and the processing of the data
// ==> delay (850 - f4*1000);
// 2) simply use it to round up the second
// f > 0.5 => add 1 to the second before adjusting the RTC
// (or lower threshold eg 0.4 if one keeps network latency etc in mind)
// 3) a SW RTC might be more precise, => ardomic clock :)
// convert NTP to UNIX time, differs seventy years = 2208988800 seconds
// NTP starts Jan 1, 1900
// Unix time starts on Jan 1 1970.
const unsigned long seventyYears = 2208988800UL;
t1 -= seventyYears;
t2 -= seventyYears;
t3 -= seventyYears;
t4 -= seventyYears;
//Serial.println("T1 .. T4 && fractional parts");
//PrintDateTime(DateTime(t1)); Serial.println(f1,4);
//PrintDateTime(DateTime(t2)); Serial.println(f2,4);
//PrintDateTime(DateTime(t3)); Serial.println(f3,4);
//PrintDateTime(t4);
char ntpTime[21];
//GetDatetimeString(t4, ntpTime);
Serial.println(f4,4);
Serial.println();
// Adjust timezone and DST... in my case substract 4 hours for Chile Time
// or work in UTC?
t4 -= (3 * 3600L); // Notice the L for long calculations!!
t4 += 1; // adjust the delay(1000) at begin of loop!
if (f4 > 0.4) t4++; // adjust fractional part, see above
}
else
{
Serial.println("Failed to get NTP UDP packet!");
}
}
bool gatewayTransportSend(MyMessage &message)
{
bool ret = true;
char *_ethernetMsg = protocolFormat(message);
setIndication(INDICATION_GW_TX);
_w5100_spi_en(true);
#if defined(MY_CONTROLLER_IP_ADDRESS)
#if defined(MY_USE_UDP)
_ethernetServer.beginPacket(_ethernetControllerIP, MY_PORT);
_ethernetServer.write(_ethernetMsg, strlen(_ethernetMsg));
// returns 1 if the packet was sent successfully
ret = _ethernetServer.endPacket();
#else
EthernetClient client;
#if defined(MY_CONTROLLER_URL_ADDRESS)
if (client.connected() || client.connect(MY_CONTROLLER_URL_ADDRESS, MY_PORT)) {
#else
if (client.connected() || client.connect(_ethernetControllerIP, MY_PORT)) {
#endif
client.write(_ethernetMsg, strlen(_ethernetMsg));
}
else {
// connecting to the server failed!
ret = false;
}
#endif
#else
// Send message to connected clients
#if defined(MY_GATEWAY_ESP8266)
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++)
{
if (clients[i] && clients[i].connected())
{
clients[i].write((uint8_t*)_ethernetMsg, strlen(_ethernetMsg));
}
}
#else
_ethernetServer.write(_ethernetMsg);
#endif
#endif
_w5100_spi_en(false);
return ret;
}
#if defined(MY_GATEWAY_ESP8266)
bool _readFromClient(uint8_t i) {
while (clients[i].connected() && clients[i].available()) {
char inChar = clients[i].read();
if (inputString[i].idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString[i].string[inputString[i].idx] = 0;
debug(PSTR("Client %d: %s\n"), i, inputString[i].string);
inputString[i].idx = 0;
if (protocolParse(_ethernetMsg, inputString[i].string)) {
return true;
}
} else {
// add it to the inputString:
inputString[i].string[inputString[i].idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Client %d: Message too long\n"), i);
inputString[i].idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#else
bool _readFromClient() {
while (client.connected() && client.available()) {
char inChar = client.read();
if (inputString.idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString.string[inputString.idx] = 0;
debug(PSTR("Eth: %s\n"), inputString.string);
inputString.idx = 0;
if (protocolParse(_ethernetMsg, inputString.string)) {
return true;
}
} else {
// add it to the inputString:
inputString.string[inputString.idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Eth: Message too long\n"));
inputString.idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#endif
bool gatewayTransportAvailable()
{
_w5100_spi_en(true);
#if !defined(MY_IP_ADDRESS) && defined(MY_GATEWAY_W5100)
// renew IP address using DHCP
gatewayTransportRenewIP();
#endif
#ifdef MY_USE_UDP
int packet_size = _ethernetServer.parsePacket();
if (packet_size) {
//debug(PSTR("UDP packet available. Size:%d\n"), packet_size);
setIndication(INDICATION_GW_RX);
#if defined(MY_GATEWAY_ESP8266)
_ethernetServer.read(inputString[0].string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString[0].string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString[0].string);
return protocolParse(_ethernetMsg, inputString[0].string);
#else
_ethernetServer.read(inputString.string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString.string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString.string);
_w5100_spi_en(false);
return protocolParse(_ethernetMsg, inputString.string);
#endif
}
#else
#if defined(MY_GATEWAY_ESP8266)
// ESP8266: Go over list of clients and stop any that are no longer connected.
// If the server has a new client connection it will be assigned to a free slot.
bool allSlotsOccupied = true;
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (!clients[i].connected()) {
if (clientsConnected[i]) {
debug(PSTR("Client %d disconnected\n"), i);
clients[i].stop();
}
//check if there are any new clients
if (_ethernetServer.hasClient()) {
clients[i] = _ethernetServer.available();
inputString[i].idx = 0;
debug(PSTR("Client %d connected\n"), i);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
if (presentation)
presentation();
}
}
bool connected = clients[i].connected();
clientsConnected[i] = connected;
allSlotsOccupied &= connected;
}
if (allSlotsOccupied && _ethernetServer.hasClient()) {
//no free/disconnected spot so reject
debug(PSTR("No free slot available\n"));
EthernetClient c = _ethernetServer.available();
c.stop();
}
// Loop over clients connect and read available data
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (_readFromClient(i)) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
#else
// W5100/ENC module does not have hasClient-method. We can only serve one client at the time.
EthernetClient newclient = _ethernetServer.available();
// if a new client connects make sure to dispose any previous existing sockets
if (newclient) {
if (client != newclient) {
client.stop();
client = newclient;
debug(PSTR("Eth: connect\n"));
_w5100_spi_en(false);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
_w5100_spi_en(true);
if (presentation)
presentation();
}
}
if (client) {
if (!client.connected()) {
debug(PSTR("Eth: disconnect\n"));
client.stop();
} else {
if (_readFromClient()) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
}
#endif
#endif
_w5100_spi_en(false);
return false;
}
SNMP_API_STAT_CODES AgentuinoClass::requestPdu(SNMP_PDU *pdu)
{
char *community;
// sequence length
byte seqLen;
// version
byte verLen, verEnd;
// community string
byte comLen, comEnd;
// pdu
byte pduTyp, pduLen;
byte ridLen, ridEnd;
byte errLen, errEnd;
byte eriLen, eriEnd;
byte vblTyp, vblLen;
byte vbiTyp, vbiLen;
byte obiLen, obiEnd;
byte valTyp, valLen, valEnd;
byte i;
//
// set packet packet size (skip UDP header)
_packetSize = Udp.available();
//
// reset packet array
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
//
// validate packet
if ( _packetSize != 0 && _packetSize > SNMP_MAX_PACKET_LEN ) {
//
//SNMP_FREE(_packet);
return SNMP_API_STAT_PACKET_TOO_BIG;
}
//
// get UDP packet
//Udp.parsePacket();
Udp.read(_packet, _packetSize);
// Udp.readPacket(_packet, _packetSize, _dstIp, &_dstPort);
//
// packet check 1
if ( _packet[0] != 0x30 ) {
//
//SNMP_FREE(_packet);
return SNMP_API_STAT_PACKET_INVALID;
}
//
// sequence length
seqLen = _packet[1];
// version
verLen = _packet[3];
verEnd = 3 + verLen;
// community string
comLen = _packet[verEnd + 2];
comEnd = verEnd + 2 + comLen;
// pdu
pduTyp = _packet[comEnd + 1];
pduLen = _packet[comEnd + 2];
ridLen = _packet[comEnd + 4];
ridEnd = comEnd + 4 + ridLen;
errLen = _packet[ridEnd + 2];
errEnd = ridEnd + 2 + errLen;
eriLen = _packet[errEnd + 2];
eriEnd = errEnd + 2 + eriLen;
vblTyp = _packet[eriEnd + 1];
vblLen = _packet[eriEnd + 2];
vbiTyp = _packet[eriEnd + 3];
vbiLen = _packet[eriEnd + 4];
obiLen = _packet[eriEnd + 6];
obiEnd = eriEnd + obiLen + 6;
valTyp = _packet[obiEnd + 1];
valLen = _packet[obiEnd + 2];
valEnd = obiEnd + 2 + valLen;
//
// extract version
pdu->version = 0;
for ( i = 0; i < verLen; i++ ) {
pdu->version = (pdu->version << 8) | _packet[5 + i];
}
//
// validate version
//
// pdu-type
pdu->type = (SNMP_PDU_TYPES)pduTyp;
_dstType = pdu->type;
//
// validate community size
if ( comLen > SNMP_MAX_NAME_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
//
return SNMP_API_STAT_NAME_TOO_BIG;
}
//
//
// validate community name
if ( pdu->type == SNMP_PDU_SET && comLen == _setSize ) {
//
for ( i = 0; i < _setSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_setCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else if ( pdu->type == SNMP_PDU_GET && comLen == _getSize ) {
//
for ( i = 0; i < _getSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_getCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else if ( pdu->type == SNMP_PDU_GET_NEXT && comLen == _getSize ) {
//
for ( i = 0; i < _getSize; i++ ) {
if( _packet[verEnd + 3 + i] != (byte)_getCommName[i] ) {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
} else {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
//
//
// extract reqiest-id 0x00 0x00 0x00 0x01 (4-byte int aka int32)
pdu->requestId = 0;
for ( i = 0; i < ridLen; i++ ) {
pdu->requestId = (pdu->requestId << 8) | _packet[comEnd + 5 + i];
}
//
// extract error
pdu->error = SNMP_ERR_NO_ERROR;
int32_t err = 0;
for ( i = 0; i < errLen; i++ ) {
err = (err << 8) | _packet[ridEnd + 3 + i];
}
pdu->error = (SNMP_ERR_CODES)err;
//
// extract error-index
pdu->errorIndex = 0;
for ( i = 0; i < eriLen; i++ ) {
pdu->errorIndex = (pdu->errorIndex << 8) | _packet[errEnd + 3 + i];
}
//
//
// validate object-identifier size
if ( obiLen > SNMP_MAX_OID_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_OID_TOO_BIG;
}
//
// extract and contruct object-identifier
memset(pdu->OID.data, 0, SNMP_MAX_OID_LEN);
pdu->OID.size = obiLen;
for ( i = 0; i < obiLen; i++ ) {
pdu->OID.data[i] = _packet[eriEnd + 7 + i];
}
//
// value-type
pdu->VALUE.syntax = (SNMP_SYNTAXES)valTyp;
//
// validate value size
if ( obiLen > SNMP_MAX_VALUE_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_VALUE_TOO_BIG;
}
//
// value-size
pdu->VALUE.size = valLen;
//
// extract value
memset(pdu->VALUE.data, 0, SNMP_MAX_VALUE_LEN);
for ( i = 0; i < valLen; i++ ) {
pdu->VALUE.data[i] = _packet[obiEnd + 3 + i];
}
//
return SNMP_API_STAT_SUCCESS;
}
/**
* Parses incoming SNMP messages.
*
* Original Auther: Rex Park
* Updated: November 7, 2015 (Added support for Inform responses (SNMP_PDU_RESPONSE)
*/
SNMP_API_STAT_CODES SNMPClass::requestPdu(SNMP_PDU *pdu, char *extra_data, int extra_data_max_size)
{
// sequence length
uint16_t seqLen, valLen, vblLen, pduLen;
// version
byte verLen, verEnd;
// community string
byte comLen, comEnd;
// pdu
byte pduTyp, pduEnd;
byte ridLen, ridEnd;
byte errLen, errEnd;
byte eriLen, eriEnd;
byte vblTyp;
byte vbiTyp, vbiLen;
byte obiLen, obiEnd;
byte valTyp, valEnd;
int i;
// set packet packet size (skip UDP header)
_packetSize = Udp.available();
_packetPos = 0;
// reset packet array
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
//
// validate packet
if ( _packetSize <= 0) {
return SNMP_API_STAT_PACKET_TOO_BIG;
}
// get UDP packet
if(_udp_extra_data_packet == true){
if(extra_data != NULL){
memset(extra_data, 0, extra_data_max_size);
}
Udp.read(_packet, _packetSize - extra_data_max_size);
if(extra_data != NULL){
Udp.read((byte*)extra_data, extra_data_max_size);
}
}else{
Udp.read(_packet, _packetSize);
}
// Serial.println("Incomming: ");
// for(int i = 0; i < _packetSize; i++){
// Serial.print(_packet[i],HEX);
// Serial.print(" ");
// }
// Serial.println();
// packet check 1
if ( _packet[0] != 0x30 ) {
return SNMP_API_STAT_PACKET_INVALID;
}
// sequence length
if(_packet[1] >= 0x82){
seqLen = combine_msb_lsb(_packet[2], _packet[3]);
_packetPos = 4;
}
else if(_packet[1] == 0x81){
seqLen = _packet[2];
_packetPos = 3;
}else{
seqLen = _packet[1];
_packetPos = 2;
}
// version
if(_packet[_packetPos] != 0x02){
return SNMP_API_STAT_PACKET_INVALID;
}
verLen = _packet[_packetPos+1];//used to be hard coded as index 3
verEnd = _packetPos+1 + verLen;
// community string
comLen = _packet[verEnd + 2];
comEnd = verEnd + 2 + comLen;
// pdu
pduTyp = _packet[comEnd + 1];
if(_packet[comEnd + 2] >= 0x82){
pduLen = combine_msb_lsb(_packet[comEnd +3], _packet[comEnd +4]);
pduEnd = comEnd + 2 + pduLen + 2;
_packetPos = comEnd + 2 + 2;
}
else if(_packet[comEnd + 2] == 0x81){
pduLen = _packet[comEnd +3];
pduEnd = comEnd + 2 + pduLen + 1;
_packetPos = comEnd + 2 + 1;
}else{
pduLen = _packet[comEnd + 2];
pduEnd = comEnd + 2 + pduLen;
_packetPos = comEnd + 2;
}
//request id
ridLen = _packet[_packetPos + 2];
ridEnd = _packetPos + 2 + ridLen;
//error
errLen = _packet[ridEnd + 2];
errEnd = ridEnd + 2 + errLen;
//error index
eriLen = _packet[errEnd + 2];
eriEnd = errEnd + 2 + eriLen;
//variable bindings
vblTyp = _packet[eriEnd + 1];
if(_packet[eriEnd + 2] >= 0x82){
vblLen = combine_msb_lsb(_packet[eriEnd +3], _packet[eriEnd +4]);
_packetPos = eriEnd + 2 + 2;
}
else if(_packet[eriEnd + 2] == 0x81){
vblLen = _packet[eriEnd +3];
_packetPos = eriEnd + 2 + 1;
}else{
vblLen = _packet[eriEnd + 2];
_packetPos = eriEnd + 2;
}
//variable bindings id
vbiTyp = _packet[_packetPos + 1];
if(_packet[_packetPos + 2] > 0x80){
vbiLen = combine_msb_lsb(_packet[_packetPos +3], _packet[_packetPos +4]);
_packetPos = _packetPos + 2 + 2;
}else{
vbiLen = _packet[_packetPos + 2];
_packetPos = _packetPos + 2;
}
//object identifier
obiLen = _packet[_packetPos + 2];
obiEnd = _packetPos + 2 + obiLen;
//unknown
valTyp = _packet[obiEnd + 1];
if(_packet[obiEnd + 2] >= 0x82){
valLen = combine_msb_lsb(_packet[obiEnd +3], _packet[obiEnd +4]);
valEnd = obiEnd + 2 + valLen + 2;
}
else if(_packet[obiEnd + 2] == 0x81){
valLen = _packet[obiEnd + 3];
valEnd = obiEnd + 2 + valLen + 1;
}else{
valLen = _packet[obiEnd + 2];
valEnd = obiEnd + 2 + valLen;
}
// Serial.println(verEnd);
// Serial.println(comEnd);
// Serial.println(ridEnd);
// Serial.println(errEnd);
// Serial.println(eriEnd);
// Serial.println(obiEnd);
// Serial.println(pduEnd);
// Serial.println(pduTyp,HEX);
// extract version
pdu->version = _packet[verEnd];
// Serial.println(pdu->version,HEX);
// validate version
if(pdu->version != 0x0 && pdu->version != 0x1){
return SNMP_API_STAT_PACKET_INVALID;
}
// pdu-type
pdu->type = (SNMP_PDU_TYPES)pduTyp;
_dstType = pdu->type;
// validate community size
if ( comLen > SNMP_MAX_NAME_LEN ) {
// set pdu error
pdu->error = SNMP_ERR_TOO_BIG;
//
return SNMP_API_STAT_NAME_TOO_BIG;
}
// validate community name
if ( pdu->type == SNMP_PDU_SET && comLen == _setSize ) {
if(memcmp(_setCommName,_packet+verEnd+3,_setSize) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
} else if ( pdu->type == SNMP_PDU_GET) {
if(memcmp(_getCommName,_packet+verEnd+3,comLen) != 0 && memcmp(_setCommName,_packet+verEnd+3,comLen) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
} else if (pdu->type == SNMP_PDU_RESPONSE){
if(memcmp(_trapCommName,_packet+verEnd+3,comLen) != 0){
pdu->error = SNMP_ERR_NO_SUCH_NAME;
return SNMP_API_STAT_NO_SUCH_NAME;
}
}
else {
// set pdu error
pdu->error = SNMP_ERR_NO_SUCH_NAME;
//
return SNMP_API_STAT_NO_SUCH_NAME;
}
// extract reqiest-id 0x00 0x00 0x00 0x01 (4-byte int aka int32)
pdu->requestId = 0;
for ( i = 0; i < ridLen; i++ ) {
pdu->requestId = (pdu->requestId << 8) | _packet[ridEnd-ridLen+1 + i];
}
// extract error
pdu->error = SNMP_ERR_NO_ERROR;
int32_t err = 0;
for ( i = 0; i < errLen; i++ ) {
err = (err << 8) | _packet[errEnd-errLen+1 + i];
}
pdu->error = (SNMP_ERR_CODES)err;
// extract error-index
pdu->errorIndex = 0;
for ( i = 0; i < eriLen; i++ ) {
pdu->errorIndex = (pdu->errorIndex << 8) | _packet[eriEnd-eriLen+1 + i];
}
/**
* OID
*/
//validate length
if ( obiLen > SNMP_MAX_OID_LEN ) {
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_OID_TOO_BIG;
}
//decode OID
memset(pdu->value.OID.data, 0, SNMP_MAX_OID_LEN);
if(pdu->value.OID.decode(_packet + (obiEnd-obiLen+1), obiLen) != SNMP_API_STAT_SUCCESS){
return SNMP_API_STAT_MALLOC_ERR;
}
/**
* Value
*/
//syntax type
pdu->value.syntax = (SNMP_SYNTAXES)valTyp;
//check length of data
if ( valLen > SNMP_MAX_VALUE_LEN && _udp_extra_data_packet == false) {
pdu->error = SNMP_ERR_TOO_BIG;
return SNMP_API_STAT_VALUE_TOO_BIG;
}
//set value size
pdu->value.size = valLen;
if(_udp_extra_data_packet == true){
// memset(extra_data, '\0', extra_data_max_size);
// for ( i = 0; i < valLen; i++ ) {
// extra_data[i] = _packet[obiEnd+3 + i];
// }
}else{
memset(pdu->value.data, 0, SNMP_MAX_VALUE_LEN);
for ( i = 0; i < valLen; i++ ) {
pdu->value.data[i] = _packet[obiEnd+3 + i];
}
}
return SNMP_API_STAT_SUCCESS;
}