void LLMNRResponder::_process_packet() {
if (!_conn || !_conn->next())
return;
#ifdef LLMNR_DEBUG
Serial.println("LLMNR: RX'd packet");
#endif
uint16_t id = _conn_read16();
uint16_t flags = _conn_read16();
uint16_t qdcount = _conn_read16();
uint16_t ancount = _conn_read16();
uint16_t nscount = _conn_read16();
uint16_t arcount = _conn_read16();
#ifdef LLMNR_DEBUG
Serial.print("LLMNR: ID=");
Serial.println(id, HEX);
Serial.print("LLMNR: FLAGS=");
Serial.println(flags, HEX);
Serial.print("LLMNR: QDCOUNT=");
Serial.println(qdcount);
Serial.print("LLMNR: ANCOUNT=");
Serial.println(ancount);
Serial.print("LLMNR: NSCOUNT=");
Serial.println(nscount);
Serial.print("LLMNR: ARCOUNT=");
Serial.println(arcount);
#endif
#define BAD_FLAGS (FLAGS_QR | (FLAGS_OP_MASK << FLAGS_OP_SHIFT) | FLAGS_C)
if (flags & BAD_FLAGS) {
#ifdef LLMNR_DEBUG
Serial.println("Bad flags");
#endif
return;
}
if (qdcount != 1) {
#ifdef LLMNR_DEBUG
Serial.println("QDCOUNT != 1");
#endif
return;
}
if (ancount || nscount || arcount) {
#ifdef LLMNR_DEBUG
Serial.println("AN/NS/AR-COUNT != 0");
#endif
return;
}
uint8_t namelen = _conn_read8();
#ifdef LLMNR_DEBUG
Serial.print("QNAME len ");
Serial.println(namelen);
#endif
if (namelen != _hostname.length()) {
#ifdef LLMNR_DEBUG
Serial.println("QNAME len mismatch");
#endif
return;
}
char qname[64];
_conn_readS(qname, namelen);
_conn_read8();
qname[namelen] = '\0';
#ifdef LLMNR_DEBUG
Serial.print("QNAME ");
Serial.println(qname);
#endif
if (strcmp(_hostname.c_str(), qname)) {
#ifdef LLMNR_DEBUG
Serial.println("QNAME mismatch");
#endif
return;
}
uint16_t qtype = _conn_read16();
uint16_t qclass = _conn_read16();
#ifdef LLMNR_DEBUG
Serial.print("QTYPE ");
Serial.print(qtype);
Serial.print(" QCLASS ");
Serial.println(qclass);
#endif
bool have_rr =
(qtype == 1) && /* A */
(qclass == 1); /* IN */
_conn->flush();
#ifdef LLMNR_DEBUG
Serial.println("Match; responding");
if (!have_rr)
Serial.println("(no matching RRs)");
#endif
ip_addr_t remote_ip;
remote_ip.addr = _conn->getRemoteAddress();
struct ip_info ip_info;
bool match_ap = false;
if (wifi_get_opmode() & SOFTAP_MODE) {
wifi_get_ip_info(SOFTAP_IF, &ip_info);
if (ip_info.ip.addr && ip_addr_netcmp(&remote_ip, &ip_info.ip, &ip_info.netmask))
match_ap = true;
}
if (!match_ap)
wifi_get_ip_info(STATION_IF, &ip_info);
uint32_t ip = ip_info.ip.addr;
// Header
uint8_t header[] = {
id >> 8, id & 0xff, // ID
FLAGS_QR >> 8, 0, // FLAGS
0, 1, // QDCOUNT
0, !!have_rr, // ANCOUNT
0, 0, // NSCOUNT
0, 0, // ARCOUNT
};
_conn->append(reinterpret_cast<const char*>(header), sizeof(header));
// Question
_conn->append(reinterpret_cast<const char*>(&namelen), 1);
_conn->append(qname, namelen);
uint8_t q[] = {
0, // Name terminator
0, 1, // TYPE (A)
0, 1, // CLASS (IN)
};
_conn->append(reinterpret_cast<const char*>(q), sizeof(q));
// Answer, if we have one
if (have_rr) {
_conn->append(reinterpret_cast<const char*>(&namelen), 1);
_conn->append(qname, namelen);
uint8_t rr[] = {
0, // Name terminator
0, 1, // TYPE (A)
0, 1, // CLASS (IN)
0, 0, 0, 30, // TTL (30 seconds)
0, 4, // RDLENGTH
ip & 0xff, (ip >> 8) & 0xff, (ip >> 16) & 0xff, (ip >> 24) & 0xff, // RDATA
};
_conn->append(reinterpret_cast<const char*>(rr), sizeof(rr));
}
_conn->setMulticastInterface(remote_ip);
_conn->send(&remote_ip, _conn->getRemotePort());
}
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);
}