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