int recvfrom_( int socket,char *buf,int size,int flags,int *_ip,int *_port){
struct sockaddr_in sa;
int sasize;
int count;
memset( &sa,0,sizeof(sa) );
sasize=sizeof(sa);
count=recvfrom(socket,buf,size,flags,(void*)&sa,&sasize);
*_ip=ntohl_(sa.sin_addr.s_addr);
*_port=ntohs_(sa.sin_port);
return count;
}
// return value:
// A DNSError_t (DNSSuccess on success, something else otherwise)
// in "int" mode: positive on success, negative on error
MDNSError_t EthernetBonjourClass::_processMDNSQuery()
{
MDNSError_t statusCode = MDNSSuccess;
DNSHeader_t dnsHeaderBuf;
DNSHeader_t* dnsHeader = &dnsHeaderBuf;
int i, j;
uint8_t* buf;
uint32_t xid;
uint16_t ptr, qCnt, aCnt, aaCnt, addCnt;
uint8_t recordsAskedFor[NumMDNSServiceRecords+2];
uint8_t recordsFound[2];
uint8_t wantsIPv6Addr = 0;
memset(recordsAskedFor, 0, sizeof(uint8_t)*(NumMDNSServiceRecords+2));
memset(recordsFound, 0, sizeof(uint8_t)*2);
if (0 == iUdp.parsePacket()) {
statusCode = MDNSTryLater;
goto errorReturn;
}
iUdp.read((unsigned char*)dnsHeader, sizeof(DNSHeader_t));
xid = ntohs_(dnsHeader->xid);
qCnt = ntohs_(dnsHeader->queryCount);
aCnt = ntohs_(dnsHeader->answerCount);
aaCnt = ntohs_(dnsHeader->authorityCount);
addCnt = ntohs_(dnsHeader->additionalCount);
if (0 == dnsHeader->queryResponse &&
DNSOpQuery == dnsHeader->opCode &&
MDNS_SERVER_PORT == iUdp.remotePort()) {
// process an MDNS query
uint8_t* buf = (uint8_t*)dnsHeader;
int rLen = 0, tLen = 0;
// read over the query section
for (i=0; i<qCnt; i++) {
// construct service name data structures for comparison
const uint8_t* servNames[NumMDNSServiceRecords+2];
int servLens[NumMDNSServiceRecords+2];
uint8_t servNamePos[NumMDNSServiceRecords+2];
uint8_t servMatches[NumMDNSServiceRecords+2];
// first entry is our own MDNS name, the rest are our services
servNames[0] = (const uint8_t*)this->_bonjourName;
servNamePos[0] = 0;
servLens[0] = strlen((char*)this->_bonjourName);
servMatches[0] = 1;
// second entry is our own the general DNS-SD service
servNames[1] = (const uint8_t*)DNS_SD_SERVICE;
servNamePos[1] = 0;
servLens[1] = strlen((char*)DNS_SD_SERVICE);
servMatches[1] = 1;
for (j=2; j<NumMDNSServiceRecords+2; j++)
if (NULL != this->_serviceRecords[j-2] && NULL != this->_serviceRecords[j-2]->servName) {
servNames[j] = this->_serviceRecords[j-2]->servName;
servLens[j] = strlen((char*)servNames[j]);
servMatches[j] = 1;
servNamePos[j] = 0;
} else {
servNames[j] = NULL;
servLens[j] = 0;
servMatches[j] = 0;
servNamePos[j] = 0;
}
tLen = 0;
do {
iUdp.read((unsigned char*)buf, 1);
rLen = buf[0];
tLen += 1;
if (rLen > 128) {// handle DNS name compression, kinda, sorta
iUdp.read((unsigned char*)buf, 1);
for (j=0; j<NumMDNSServiceRecords+2; j++) {
if (servNamePos[j] && servNamePos[j] != buf[0]) {
servMatches[j] = 0;
}
}
tLen += 1;
} else if (rLen > 0) {
int tr = rLen, ir;
while (tr > 0) {
ir = (tr > sizeof(DNSHeader_t)) ? sizeof(DNSHeader_t) : tr;
iUdp.read((unsigned char*)buf, ir);
tr -= ir;
for (j=0; j<NumMDNSServiceRecords+2; j++) {
if (!recordsAskedFor[j] && servMatches[j])
servMatches[j] &= this->_matchStringPart(&servNames[j], &servLens[j], buf,
ir);
}
}
tLen += rLen;
}
} while (rLen > 0 && rLen <= 128);
// if this matched a name of ours (and there are no characters left), then
// check whether this is an A record query (for our own name) or a PTR record query
// (for one of our services).
// if so, we'll note to send a record
iUdp.read((unsigned char*)buf, 4);
for (j=0; j<NumMDNSServiceRecords+2; j++) {
if (!recordsAskedFor[j] && servNames[j] && servMatches[j] && 0 == servLens[j]) {
if (0 == servNamePos[j])
servNamePos[j] = 4 - tLen;//TODO
if (buf[0] == 0 && buf[3] == 0x01 &&
(buf[2] == 0x00 || buf[2] == 0x80)) {
if ((0 == j && 0x01 == buf[1]) || (0 < j && (0x0c == buf[1] || 0x10 == buf[1] || 0x21 == buf[1])))
recordsAskedFor[j] = 1;
else if (0 == j && 0x1c == buf[1])
wantsIPv6Addr = 1;
}
}
}
}
}
#if (defined(HAS_SERVICE_REGISTRATION) && HAS_SERVICE_REGISTRATION) || (defined(HAS_NAME_BROWSING) && HAS_NAME_BROWSING)
else if (1 == dnsHeader->queryResponse &&
DNSOpQuery == dnsHeader->opCode &&
MDNS_SERVER_PORT == iUdp.remotePort() &&
(NULL != this->_resolveNames[0] || NULL != this->_resolveNames[1])) {
int offset = sizeof(DNSHeader_t);
uint8_t* buf = (uint8_t*)dnsHeader;
int rLen = 0, tLen = 0;
uint8_t* ptrNames[MDNS_MAX_SERVICES_PER_PACKET];
uint16_t ptrOffsets[MDNS_MAX_SERVICES_PER_PACKET];
uint16_t ptrPorts[MDNS_MAX_SERVICES_PER_PACKET];
uint8_t ptrIPs[MDNS_MAX_SERVICES_PER_PACKET];
uint8_t servIPs[MDNS_MAX_SERVICES_PER_PACKET][5];
uint8_t* servTxt[MDNS_MAX_SERVICES_PER_PACKET];
memset(servIPs, 0, sizeof(uint8_t)*MDNS_MAX_SERVICES_PER_PACKET*5);
memset(servTxt, 0, sizeof(uint8_t*)*MDNS_MAX_SERVICES_PER_PACKET);
const uint8_t* ptrNamesCmp[MDNS_MAX_SERVICES_PER_PACKET];
int ptrLensCmp[MDNS_MAX_SERVICES_PER_PACKET];
uint8_t ptrNamesMatches[MDNS_MAX_SERVICES_PER_PACKET];
uint8_t checkAARecords = 0;
memset(ptrNames, 0, sizeof(uint8_t*)*MDNS_MAX_SERVICES_PER_PACKET);
const uint8_t* servNames[2];
uint8_t servNamePos[2];
int servLens[2];
uint8_t servMatches[2];
uint8_t firstNamePtrByte = 0;
uint8_t partMatched[2];
uint8_t lastWasCompressed[2];
uint8_t servWasCompressed[2];
servNamePos[0] = servNamePos[1] = 0;
for (i=0; i<qCnt+aCnt+aaCnt+addCnt; i++) {
for (j=0; j<2; j++) {
if (NULL != this->_resolveNames[j]) {
servNames[j] = this->_resolveNames[j];
servLens[j] = strlen((const char*)this->_resolveNames[j]);
servMatches[j] = 1;
} else {
servNames[j] = NULL;
servLens[j] = servMatches[j] = 0;
}
}
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (NULL != ptrNames[j]) {
ptrNamesCmp[j] = ptrNames[j];
ptrLensCmp[j] = strlen((const char*)ptrNames[j]);
ptrNamesMatches[j] = 1;
}
}
partMatched[0] = partMatched[1] = 0;
lastWasCompressed[0] = lastWasCompressed[1] = 0;
servWasCompressed[0] = servWasCompressed[1] = 0;
firstNamePtrByte = 0;
tLen = 0;
do {
iUdp.read((unsigned char*)buf, 1);
rLen = buf[0];
tLen += 1;
if (rLen > 128) { // handle DNS name compression, kinda, sorta...
iUdp.read((unsigned char*)buf, 1);
for (j=0; j<2; j++) {
if (servNamePos[j] && servNamePos[j] != buf[0])
servMatches[j] = 0;
else
servWasCompressed[j] = 1;
lastWasCompressed[j] = 1;
}
tLen += 1;
if (0 == firstNamePtrByte)
firstNamePtrByte = buf[0];
} else if (rLen > 0) {
if (i < qCnt)
offset += rLen;
else {
int tr = rLen, ir;
if (0 == firstNamePtrByte)
firstNamePtrByte = offset-1; // -1, since we already read length (1 byte)
while (tr > 0) {
ir = (tr > sizeof(DNSHeader_t)) ? sizeof(DNSHeader_t) : tr;
iUdp.read((unsigned char*)buf, ir);
tr -= ir;
for (j=0; j<2; j++) {
if (!recordsFound[j] && servMatches[j] && servNames[j])
servMatches[j] &= this->_matchStringPart(&servNames[j], &servLens[j],
buf, ir);
if (!partMatched[j])
partMatched[j] = servMatches[j];
lastWasCompressed[j] = 0;
}
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (NULL != ptrNames[j] && ptrNamesMatches[j]) {
// only compare the part we have. this is incorrect, but good enough,
// since actual MDNS implementations won't go here anyways, as they
// should use name compression. This is just so that multiple Arduinos
// running this MDNSResponder code should be able to find each other's
// services.
if (ptrLensCmp[j] >= ir)
ptrNamesMatches[j] &= this->_matchStringPart(&ptrNamesCmp[j],
&ptrLensCmp[j], buf, ir);
}
}
}
tLen += rLen;
}
}
} while (rLen > 0 && rLen <= 128);
// if this matched a name of ours (and there are no characters left), then
// check whether this is an A record query (for our own name) or a PTR record query
// (for one of our services).
// if so, we'll note to send a record
if (i < qCnt)
offset += 4;
else if (i >= qCnt) {
if (i >= qCnt + aCnt && !checkAARecords)
break;
uint8_t packetHandled = 0;
iUdp.read((unsigned char*)buf, 4);
if (i < qCnt+aCnt) {
for (j=0; j<2; j++) {
if (0 == servNamePos[j])
servNamePos[j] = offset - 4 - tLen;
if (servNames[j] &&
((servMatches[j] && 0 == servLens[j]) ||
(partMatched[j] && lastWasCompressed[j]) ||
(servWasCompressed[j] && servMatches[j]))) { // somewhat handle compression by guessing
if (buf[0] == 0 && buf[1] == ((0 == j) ? 0x01 : 0x0c) &&
(buf[2] == 0x00 || buf[2] == 0x80) && buf[3] == 0x01) {
recordsFound[j] = 1;
// this is an A or PTR type response. Parse it as such.
iUdp.read((unsigned char*)buf, 6);
//uint32_t ttl = ntohl_(*(uint32_t*)buf);
uint16_t dataLen = ntohs_(*(uint16_t*)&buf[4]);
if (0 == j && 4 == dataLen) {
// ok, this is the IP address. report it via callback.
iUdp.read((unsigned char*)buf, 4);
this->_finishedResolvingName((char*)this->_resolveNames[0],
(const byte*)buf);
} else if (1 == j) {
uint8_t k;
for (k=0; k<MDNS_MAX_SERVICES_PER_PACKET; k++)
if (NULL == ptrNames[k])
break;
if (k < MDNS_MAX_SERVICES_PER_PACKET) {
int l = dataLen - 2; // -2: data compression of service postfix
uint8_t* ptrName = (uint8_t*)my_malloc(l);
if (ptrName) {
iUdp.read((unsigned char*)buf, 1);
iUdp.read((unsigned char*)ptrName, l-1);
if (buf[0] < l-1)
ptrName[buf[0]]; // this catches uncompressed names
else
ptrName[l-1] = '\0';
ptrNames[k] = ptrName;
ptrOffsets[k] = (uint16_t)(offset);
checkAARecords = 1;
}
}
}
//offset += dataLen;
packetHandled = 1;
}
}
}
} else if (i >= qCnt+aCnt+aaCnt) {
// check whether we find a service description
if (buf[1] == 0x21) {
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (ptrNames[j] &&
((firstNamePtrByte && firstNamePtrByte == ptrOffsets[j]) ||
(0 == ptrLensCmp[j] && ptrNamesMatches[j]))) {
// we have found the matching SRV location packet to a previous SRV domain
iUdp.read((unsigned char*)buf, 6);
//uint32_t ttl = ntohl_(*(uint32_t*)buf);
uint16_t dataLen = ntohs_(*(uint16_t*)&buf[4]);
if (dataLen >= 8) {
iUdp.read((unsigned char*)buf, 8);
ptrPorts[j] = ntohs_(*(uint16_t*)&buf[4]);
if (buf[6] > 128) { // target is a compressed name
ptrIPs[j] = buf[7];
} else { // target is uncompressed
ptrIPs[j] = offset+6;
}
}
offset += dataLen;
packetHandled = 1;
break;
}
}
} else if (buf[1] == 0x10) { // txt record
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (ptrNames[j] &&
((firstNamePtrByte && firstNamePtrByte == ptrOffsets[j]) ||
(0 == ptrLensCmp[j] && ptrNamesMatches[j]))) {
iUdp.read((unsigned char*)buf, 6);
//uint32_t ttl = ntohl_(*(uint32_t*)buf);
uint16_t dataLen = ntohs_(*(uint16_t*)&buf[4]);
// if there's a content to this txt record, save it for delivery
if (dataLen > 1 && NULL == servTxt[j]) {
servTxt[j] = (uint8_t*)my_malloc(dataLen+1);
if (NULL != servTxt[j]) {
iUdp.read((unsigned char*)servTxt[j], dataLen);
// zero-terminate
servTxt[j][dataLen] = '\0';
}
}
offset += dataLen;
packetHandled = 1;
break;
}
}
} else if (buf[1] == 0x01) { // A record (IPv4 address)
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (0 == servIPs[j][0]) {
servIPs[j][0] = firstNamePtrByte ? firstNamePtrByte : 255;
iUdp.read((unsigned char*)buf, 6);
uint16_t dataLen = ntohs_(*(uint16_t*)&buf[4]);
if (4 == dataLen) {
iUdp.read((unsigned char*)&servIPs[j][1], 4);
}
offset += dataLen;
packetHandled = 1;
break;
}
}
}
}
// eat the answer
if (!packetHandled) {
iUdp.read((unsigned char*)buf, 4); // ignore ttl
iUdp.read((unsigned char*)buf, 2); // length
iUdp.read((unsigned char*)buf, ntohs_(*(uint16_t*)buf)); // skip over content
}
}
}
// deliver the services discovered in this packet
if (NULL != this->_resolveNames[1]) {
char* typeName = (char*)this->_resolveNames[1];
char* p = (char*)this->_resolveNames[1];
while(*p && *p != '.')
p++;
*p = '\0';
for (i=0; i<MDNS_MAX_SERVICES_PER_PACKET; i++)
if (ptrNames[i]) {
const uint8_t* ipAddr = NULL;
const uint8_t* fallbackIpAddr = NULL;
for (j=0; j<MDNS_MAX_SERVICES_PER_PACKET; j++) {
if (servIPs[j][0] == ptrIPs[i] || servIPs[j][0] == 255) {
// the || part is such a hack, but it will work as long as there's only
// one A record per MDNS packet. f*****g DNS name compression.
ipAddr = &servIPs[j][1];
break;
} else if (NULL == fallbackIpAddr && 0 != servIPs[j][0])
fallbackIpAddr = &servIPs[j][1];
}
// if we can't find a matching IP, we try to use the first one we found.
if (NULL == ipAddr) ipAddr = fallbackIpAddr;
if (ipAddr && this->_serviceFoundCallback) {
this->_serviceFoundCallback(typeName,
this->_resolveServiceProto,
(const char*)ptrNames[i],
(const byte*)ipAddr,
(unsigned short)ptrPorts[i],
(const char*)servTxt[i]);
}
}
*p = '.';
}
uint8_t k;
for (k=0; k<MDNS_MAX_SERVICES_PER_PACKET; k++)
if (NULL != ptrNames[k]) {
my_free(ptrNames[k]);
if (NULL != servTxt[k])
my_free(servTxt[k]);
}
}
#endif // (defined(HAS_SERVICE_REGISTRATION) && HAS_SERVICE_REGISTRATION) || (defined(HAS_NAME_BROWSING) && HAS_NAME_BROWSING)
iUdp.flush();
errorReturn:
// now, handle the requests
for (j=0; j<NumMDNSServiceRecords+2; j++) {
if (recordsAskedFor[j]) {
if (0 == j)
(void)this->_sendMDNSMessage(iUdp.remoteIP(), xid, (int)MDNSPacketTypeMyIPAnswer, 0);
else if (1 == j) {
uint8_t k = 2;
for (k=0; k<NumMDNSServiceRecords; k++)
recordsAskedFor[k+2] = 1;
} else if (NULL != this->_serviceRecords[j-2])
(void)this->_sendMDNSMessage(iUdp.remoteIP(), xid, (int)MDNSPacketTypeServiceRecord, j-2);
}
}
// if we were asked for our IPv6 address, say that we don't have any
if (wantsIPv6Addr)
(void)this->_sendMDNSMessage(iUdp.remoteIP(), xid, (int)MDNSPacketTypeNoIPv6AddrAvailable, 0);
return statusCode;
}
