/_parsePacket with extra debug prints
Created Jun 8, 2017
ESP8266 mDNS problem
| void MDNSResponder::_parsePacket(){ | |
| int i; | |
| char tmp; | |
| bool serviceParsed = false; | |
| bool protoParsed = false; | |
| bool localParsed = false; | |
| char hostName[255]; | |
| uint8_t hostNameLen; | |
| char serviceName[32]; | |
| uint8_t serviceNameLen; | |
| uint16_t servicePort = 0; | |
| char protoName[32]; | |
| protoName[0] = 0; | |
| uint8_t protoNameLen = 0; | |
| uint16_t packetHeader[6]; | |
| for(i=0; i<6; i++) packetHeader[i] = _conn_read16(); | |
| if ((packetHeader[1] & 0x8000) != 0) { // Read answers | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("Reading answers RX: REQ, ID:%u, Q:%u, A:%u, NS:%u, ADD:%u\n", packetHeader[0], packetHeader[2], packetHeader[3], packetHeader[4], packetHeader[5]); | |
| #endif | |
| if (!_waitingForAnswers) { | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.println("Not expecting any answers right now, returning"); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| int numAnswers = packetHeader[3]; | |
| // Assume that the PTR answer always comes first and that it is always accompanied by a TXT, SRV, AAAA (optional) and A answer in the same packet. | |
| if (numAnswers < 4) { | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.println("Expected a packet with 4 answers, returning"); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| uint8_t tmp8; | |
| uint16_t answerPort = 0; | |
| uint8_t answerIp[4] = { 0,0,0,0 }; | |
| char answerHostName[255]; | |
| bool serviceMatch = false; | |
| MDNSAnswer *answer; | |
| uint8_t partsCollected = 0; | |
| // Clear answer list | |
| if (_newQuery) { | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.println("New query, clearing old data"); | |
| #endif | |
| int numAnswers = _getNumAnswers(); | |
| for (int n = numAnswers - 1; n >= 0; n--) { | |
| answer = _getAnswerFromIdx(n); | |
| os_free(answer->hostname); | |
| os_free(answer); | |
| answer = 0; | |
| } | |
| _answers = 0; | |
| _newQuery = false; | |
| } | |
| while (numAnswers--) { | |
| // Read name | |
| do { | |
| tmp8 = _conn_read8(); | |
| if (tmp8 & 0xC0) { // Compressed pointer (not supported) | |
| tmp8 = _conn_read8(); | |
| break; | |
| } | |
| if (tmp8 == 0x00) { // Énd of name | |
| break; | |
| } | |
| _conn_readS(serviceName, tmp8); | |
| serviceName[tmp8] = '\0'; | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf(" %d ", tmp8); | |
| for (int n = 0; n < tmp8; n++) { | |
| Serial.printf("%02x ", serviceName[n]); | |
| } | |
| Serial.println(); | |
| #endif | |
| if (serviceName[0] == '_') { | |
| if (strcmp(&serviceName[1], _query->_service) == 0) { | |
| serviceMatch = true; | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("found matching service: %s\n", _query->_service); | |
| #endif | |
| } | |
| } | |
| } while (true); | |
| uint16_t answerType = _conn_read16(); // Read type | |
| uint16_t answerClass = _conn_read16(); // Read class | |
| uint32_t answerTtl = _conn_read32(); // Read ttl | |
| uint16_t answerRdlength = _conn_read16(); // Read rdlength | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("type: %04x rdlength: %d\n", answerType, answerRdlength); | |
| #endif | |
| if (answerType == MDNS_TYPE_PTR) { | |
| partsCollected |= 0x01; | |
| _conn_readS(hostName, answerRdlength); // Read rdata | |
| #ifdef MDNS_DEBUG_RX | |
| for (int n = 0; n < answerRdlength; n++) { | |
| Serial.printf("%02x ", hostName[n]); | |
| } | |
| Serial.println(); | |
| #endif | |
| } | |
| else if (answerType == MDNS_TYPE_TXT) { | |
| partsCollected |= 0x02; | |
| _conn_readS(hostName, answerRdlength); // Read rdata | |
| #ifdef MDNS_DEBUG_RX | |
| for (int n = 0; n < answerRdlength; n++) { | |
| Serial.printf("%02x ", hostName[n]); | |
| } | |
| Serial.println(); | |
| #endif | |
| } | |
| else if (answerType == MDNS_TYPE_SRV) { | |
| partsCollected |= 0x04; | |
| uint16_t answerPrio = _conn_read16(); // Read priority | |
| uint16_t answerWeight = _conn_read16(); // Read weight | |
| answerPort = _conn_read16(); // Read port | |
| // Read hostname | |
| tmp8 = _conn_read8(); | |
| if (tmp8 & 0xC0) { // Compressed pointer (not supported) | |
| Serial.println("Skipping compressed pointer"); | |
| tmp8 = _conn_read8(); | |
| } | |
| else { | |
| _conn_readS(answerHostName, tmp8); | |
| answerHostName[tmp8] = '\0'; | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf(" %d ", tmp8); | |
| for (int n = 0; n < tmp8; n++) { | |
| Serial.printf("%02x ", answerHostName[n]); | |
| } | |
| Serial.printf("\n%s\n", answerHostName); | |
| #endif | |
| if (answerRdlength - (6 + 1 + tmp8) > 0) { // Skip any remaining rdata | |
| _conn_readS(hostName, answerRdlength - (6 + 1 + tmp8)); | |
| } | |
| } | |
| } | |
| else if (answerType == MDNS_TYPE_A) { | |
| partsCollected |= 0x08; | |
| for (int i = 0; i < 4; i++) { | |
| answerIp[i] = _conn_read8(); | |
| } | |
| } | |
| else { | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("Ignoring unsupported type %d\n", tmp8); | |
| #endif | |
| for (int n = 0; n < answerRdlength; n++) | |
| (void)_conn_read8(); | |
| } | |
| if ((partsCollected == 0x0F) && serviceMatch) { | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.println("All answers parsed, adding to _answers list.."); | |
| #endif | |
| // Add new answer to answer list | |
| if (_answers == 0) { | |
| Serial.println("_answers == 0"); | |
| _answers = (struct MDNSAnswer*)(os_malloc(sizeof(struct MDNSAnswer))); | |
| answer = _answers; | |
| } | |
| else { | |
| Serial.println("else"); | |
| Serial.printf("heap size: %u\n", ESP.getFreeHeap()); | |
| answer = _answers; | |
| while (answer->next != 0) { | |
| Serial.print("next "); | |
| Serial.println((unsigned long int)(answer->next)); | |
| answer = _answers->next; | |
| } | |
| answer->next = (struct MDNSAnswer*)(os_malloc(sizeof(struct MDNSAnswer))); | |
| answer = answer->next; | |
| } | |
| answer->next = 0; | |
| answer->hostname = 0; | |
| // Populate new answer | |
| answer->port = answerPort; | |
| for (int i = 0; i < 4; i++) { | |
| answer->ip[i] = answerIp[i]; | |
| } | |
| Serial.print("strlen(answerHostName): "); | |
| Serial.println(strlen(answerHostName)); | |
| answer->hostname = (char *)os_malloc(strlen(answerHostName) + 1); | |
| os_strcpy(answer->hostname, answerHostName); | |
| Serial.print("done "); | |
| Serial.print((unsigned long int)(answer)); | |
| Serial.print(" -> "); | |
| Serial.println((unsigned long int)(answer->next)); | |
| } | |
| } | |
| Serial.println("All requests parsed, _parsePacket returning.."); | |
| _conn->flush(); | |
| return; | |
| } | |
| Serial.println("PARSE REQUEST NAME.."); | |
| // PARSE REQUEST NAME | |
| hostNameLen = _conn_read8(); | |
| _conn_readS(hostName, hostNameLen); | |
| hostName[hostNameLen] = '\0'; | |
| if(hostName[0] == '_'){ | |
| serviceParsed = true; | |
| memcpy(serviceName, hostName+1, hostNameLen); | |
| serviceNameLen = hostNameLen-1; | |
| hostNameLen = 0; | |
| } | |
| if(hostNameLen > 0 && !_hostName.equals(hostName) && !_instanceName.equals(hostName)){ | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_NO_HOST: %s\n", hostName); | |
| Serial.printf("hostname: %s\n", _hostName.c_str() ); | |
| Serial.printf("instance: %s\n", _instanceName.c_str() ); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| if(!serviceParsed){ | |
| serviceNameLen = _conn_read8(); | |
| _conn_readS(serviceName, serviceNameLen); | |
| serviceName[serviceNameLen] = '\0'; | |
| if(serviceName[0] == '_'){ | |
| memmove(serviceName, serviceName+1, serviceNameLen); | |
| serviceNameLen--; | |
| serviceParsed = true; | |
| } else if(serviceNameLen == 5 && strcmp("local", serviceName) == 0){ | |
| tmp = _conn_read8(); | |
| if(tmp == 0){ | |
| serviceParsed = true; | |
| serviceNameLen = 0; | |
| protoParsed = true; | |
| protoNameLen = 0; | |
| localParsed = true; | |
| } else { | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_FQDN: %s\n", serviceName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| } else { | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_SERVICE: %s\n", serviceName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| } | |
| if(!protoParsed){ | |
| protoNameLen = _conn_read8(); | |
| _conn_readS(protoName, protoNameLen); | |
| protoName[protoNameLen] = '\0'; | |
| if(protoNameLen == 4 && protoName[0] == '_'){ | |
| memmove(protoName, protoName+1, protoNameLen); | |
| protoNameLen--; | |
| protoParsed = true; | |
| } else if(strcmp("services", serviceName) == 0 && strcmp("_dns-sd", protoName) == 0){ | |
| _conn->flush(); | |
| advertiseServices(); | |
| return; | |
| } else { | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_PROTO: %s\n", protoName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| } | |
| if(!localParsed){ | |
| char localName[32]; | |
| uint8_t localNameLen = _conn_read8(); | |
| _conn_readS(localName, localNameLen); | |
| localName[localNameLen] = '\0'; | |
| tmp = _conn_read8(); | |
| if(localNameLen == 5 && strcmp("local", localName) == 0 && tmp == 0){ | |
| localParsed = true; | |
| } else { | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_FQDN: %s\n", localName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| } | |
| if(serviceNameLen > 0 && protoNameLen > 0){ | |
| servicePort = _getServicePort(serviceName, protoName); | |
| if(servicePort == 0){ | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_NO_SERVICE: %s\n", serviceName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| } else if(serviceNameLen > 0 || protoNameLen > 0){ | |
| #ifdef MDNS_DEBUG_ERR | |
| Serial.printf("ERR_SERVICE_PROTO: %s\n", serviceName); | |
| #endif | |
| _conn->flush(); | |
| return; | |
| } | |
| // RESPOND | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("RX: REQ, ID:%u, Q:%u, A:%u, NS:%u, ADD:%u\n", packetHeader[0], packetHeader[2], packetHeader[3], packetHeader[4], packetHeader[5]); | |
| #endif | |
| uint16_t currentType; | |
| uint16_t currentClass; | |
| int numQuestions = packetHeader[2]; | |
| if(numQuestions > 4) numQuestions = 4; | |
| uint16_t questions[4]; | |
| int question = 0; | |
| while(numQuestions--){ | |
| currentType = _conn_read16(); | |
| if(currentType & MDNS_NAME_REF){ //new header handle it better! | |
| currentType = _conn_read16(); | |
| } | |
| currentClass = _conn_read16(); | |
| if(currentClass & MDNS_CLASS_IN) questions[question++] = currentType; | |
| if(numQuestions > 0){ | |
| if(_conn_read16() != 0xC00C){//new question but for another host/service | |
| _conn->flush(); | |
| numQuestions = 0; | |
| } | |
| } | |
| #ifdef MDNS_DEBUG_RX | |
| Serial.printf("REQ: "); | |
| if(hostNameLen > 0) Serial.printf("%s.", hostName); | |
| if(serviceNameLen > 0) Serial.printf("_%s.", serviceName); | |
| if(protoNameLen > 0) Serial.printf("_%s.", protoName); | |
| Serial.printf("local. "); | |
| if(currentType == MDNS_TYPE_AAAA) Serial.printf(" AAAA "); | |
| else if(currentType == MDNS_TYPE_A) Serial.printf(" A "); | |
| else if(currentType == MDNS_TYPE_PTR) Serial.printf(" PTR "); | |
| else if(currentType == MDNS_TYPE_SRV) Serial.printf(" SRV "); | |
| else if(currentType == MDNS_TYPE_TXT) Serial.printf(" TXT "); | |
| else Serial.printf(" 0x%04X ", currentType); | |
| if(currentClass == MDNS_CLASS_IN) Serial.printf(" IN "); | |
| else if(currentClass == MDNS_CLASS_IN_FLUSH_CACHE) Serial.printf(" IN[F] "); | |
| else Serial.printf(" 0x%04X ", currentClass); | |
| Serial.printf("\n"); | |
| #endif | |
| } | |
| uint8_t responseMask = 0; | |
| for(i=0;i<question;i++){ | |
| if(questions[i] == MDNS_TYPE_A) responseMask |= 0x1; | |
| else if(questions[i] == MDNS_TYPE_SRV) responseMask |= 0x3; | |
| else if(questions[i] == MDNS_TYPE_TXT) responseMask |= 0x4; | |
| else if(questions[i] == MDNS_TYPE_PTR) responseMask |= 0xF; | |
| } | |
| return _reply(responseMask, serviceName, protoName, servicePort); | |
| } |
| mDNS query for _esp._tcp.. | |
| queryService esp tcp | |
| Waiting for answers.. | |
| Reading answers RX: REQ, ID:0, Q:0, A:4, NS:0, ADD:0 | |
| New query, clearing old data | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 000c rdlength: 6 | |
| 03 70 69 32 c0 0c | |
| type: 0010 rdlength: 1 | |
| 00 | |
| type: 0021 rdlength: 12 | |
| 3 70 69 32 | |
| pi2 | |
| type: 0001 rdlength: 4 | |
| All answers parsed, adding to _answers list.. | |
| _answers == 0 | |
| strlen(answerHostName): 3 | |
| done 1073691884 -> 0 | |
| All requests parsed, _parsePacket returning.. | |
| Reading answers RX: REQ, ID:0, Q:0, A:4, NS:0, ADD:0 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 000c rdlength: 28 | |
| 0a 65 73 70 5f 31 39 35 32 32 31 04 5f 65 73 70 04 5f 74 63 70 05 6c 6f 63 61 6c 00 | |
| 10 65 73 70 5f 31 39 35 32 32 31 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0010 rdlength: 0 | |
| 10 65 73 70 5f 31 39 35 32 32 31 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0021 rdlength: 24 | |
| 10 65 73 70 5f 31 39 35 32 32 31 | |
| esp_195221 | |
| 10 65 73 70 5f 31 39 35 32 32 31 | |
| 5 6c 6f 63 61 6c | |
| type: 0001 rdlength: 4 | |
| All answers parsed, adding to _answers list.. | |
| else | |
| heap size: 28256 | |
| strlen(answerHostName): 10 | |
| done 1073691372 -> 0 | |
| All requests parsed, _parsePacket returning.. | |
| Reading answers RX: REQ, ID:0, Q:0, A:4, NS:0, ADD:0 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 000c rdlength: 28 | |
| 0a 65 73 70 5f 38 31 43 43 34 37 04 5f 65 73 70 04 5f 74 63 70 05 6c 6f 63 61 6c 00 | |
| 10 65 73 70 5f 38 31 43 43 34 37 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0010 rdlength: 0 | |
| 10 65 73 70 5f 38 31 43 43 34 37 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0021 rdlength: 24 | |
| 10 65 73 70 5f 38 31 63 63 34 37 | |
| esp_81cc47 | |
| 10 65 73 70 5f 38 31 63 63 34 37 | |
| 5 6c 6f 63 61 6c | |
| type: 0001 rdlength: 4 | |
| All answers parsed, adding to _answers list.. | |
| else | |
| heap size: 28168 | |
| next 1073691372 | |
| strlen(answerHostName): 10 | |
| done 1073691908 -> 0 | |
| All requests parsed, _parsePacket returning.. | |
| Reading answers RX: REQ, ID:0, Q:0, A:4, NS:0, ADD:0 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 000c rdlength: 28 | |
| 0a 65 73 70 5f 44 41 33 35 45 41 04 5f 65 73 70 04 5f 74 63 70 05 6c 6f 63 61 6c 00 | |
| 10 65 73 70 5f 44 41 33 35 45 41 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0010 rdlength: 0 | |
| 10 65 73 70 5f 44 41 33 35 45 41 | |
| 4 5f 65 73 70 | |
| found matching service: esp | |
| 4 5f 74 63 70 | |
| 5 6c 6f 63 61 6c | |
| type: 0021 rdlength: 24 | |
| 10 65 73 70 5f 64 61 33 35 65 61 | |
| esp_da35ea | |
| 10 65 73 70 5f 64 61 33 35 65 61 | |
| 5 6c 6f 63 61 6c | |
| type: 0001 rdlength: 4 | |
| All answers parsed, adding to _answers list.. | |
| else | |
| heap size: 28152 | |
| next 1073691372 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 | |
| next 1073691908 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment