Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
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
You can’t perform that action at this time.