ESP8266 mDNS problem

_parsePacket with extra debug prints

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);
}

serial output

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