void MDNSResponder::addService(char *name, char *proto, uint16_t port){ if(_getServicePort(name, proto) != 0) return; if(os_strlen(name) > 32 || os_strlen(proto) != 3) return; //bad arguments struct MDNSService *srv = (struct MDNSService*)(os_malloc(sizeof(struct MDNSService))); os_strcpy(srv->_name, name); os_strcpy(srv->_proto, proto); srv->_port = port; srv->_next = 0; if(_services) _services->_next = srv; else _services = srv; }
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 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) { 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 } 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 } 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)); } } } if (answerType == MDNS_TYPE_A) { partsCollected |= 0x08; for (int i = 0; i < 4; i++) { answerIp[i] = _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) { _answers = (struct MDNSAnswer*)(os_malloc(sizeof(struct MDNSAnswer))); answer = _answers; } else { answer = _answers; while (answer->next != 0) { 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]; } answer->hostname = (char *)os_malloc(strlen(answerHostName) + 1); os_strcpy(answer->hostname, answerHostName); } } _conn->flush(); return; } // 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); }
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; char protoName[32]; uint8_t protoNameLen; uint16_t packetHeader[6]; for(i=0; i<6; i++) packetHeader[i] = _conn_read16(); if((packetHeader[1] & 0x8000) != 0){ //not parsing responses yet _conn->flush(); return; } // 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 && strcmp(_hostName, hostName) != 0){ #ifdef MDNS_DEBUG_ERR os_printf("ERR_NO_HOST: %s\n", hostName); #endif _conn->flush(); return; } if(!serviceParsed){ serviceNameLen = _conn_read8(); _conn_readS(serviceName, serviceNameLen); serviceName[serviceNameLen] = '\0'; if(serviceName[0] == '_'){ memcpy(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 os_printf("ERR_FQDN: %s\n", serviceName); #endif _conn->flush(); return; } } else { #ifdef MDNS_DEBUG_ERR os_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] == '_'){ memcpy(protoName, protoName+1, protoNameLen); protoNameLen--; protoParsed = true; } else { #ifdef MDNS_DEBUG_ERR os_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 os_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 os_printf("ERR_NO_SERVICE: %s\n", serviceName); #endif _conn->flush(); return; } } else if(serviceNameLen > 0 || protoNameLen > 0){ #ifdef MDNS_DEBUG_ERR os_printf("ERR_SERVICE_PROTO: %s\n", serviceName); #endif _conn->flush(); return; } // RESPOND #ifdef MDNS_DEBUG_RX os_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 os_printf("REQ: "); if(hostNameLen > 0) os_printf("%s.", hostName); if(serviceNameLen > 0) os_printf("_%s.", serviceName); if(protoNameLen > 0) os_printf("_%s.", protoName); os_printf("local. "); if(currentType == MDNS_TYPE_AAAA) os_printf(" AAAA "); else if(currentType == MDNS_TYPE_A) os_printf(" A "); else if(currentType == MDNS_TYPE_PTR) os_printf(" PTR "); else if(currentType == MDNS_TYPE_SRV) os_printf(" SRV "); else if(currentType == MDNS_TYPE_TXT) os_printf(" TXT "); else os_printf(" 0x%04X ", currentType); if(currentClass == MDNS_CLASS_IN) os_printf(" IN "); else if(currentClass == MDNS_CLASS_IN_FLUSH_CACHE) os_printf(" IN[F] "); else os_printf(" 0x%04X ", currentClass); os_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); }