mDNS: move domain related functions and output related funtions

mDNS.c needed to become cleaner. Domain related functions are
moved to the mdns_domain.c util module. The output related
functions are split off (clear separation between defining the
packet and generating the packet). The output functions can
now be found in mdns_out.c.

mDNS move probe question packet generation to mdns_send_outpacket

The probe any questions were added to the pbuf in the send_probe
routine. It is better if we move all pbuf generation to the output
function so later on packets can be delayed etc. keep it all in
one place.

mDNS: move legacy question generation to mdns_send_outpacket

It's better to do the pbuf generation in one place.
Especially important for message delaying etc.

mDNS take out domain related functionality and put in other file.

The mDNS file is getting very big and a lot still needs to be added.
For clarity reasons it's better to split these domain functions
from the main mDNS file.

mDNS split off output related functionality and put in other file

A lot of functions are only needed for the generation of the pbuf,
by separating them into another file we clean up the mdns file.
We only need the mdns_send_outpacket function as interface.
Packet definition is now completely separated from packet
generation.

src/apps/mdns/mdns_domain.c

@@ -0,0 +1,546 @@
+/**
+ * @file
+ * MDNS responder implementation - domain related functionalities
+ */
+
+/*
+ * Copyright (c) 2015 Verisure Innovation AB
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Erik Ekman <erik@kryo.se>
+ * Author: Jasper Verschueren <jasper.verschueren@apart-audio.com>
+ *
+ */
+
+#include "lwip/apps/mdns.h"
+#include "lwip/apps/mdns_domain.h"
+#include "lwip/apps/mdns_priv.h"
+#include "lwip/prot/dns.h"
+
+#if LWIP_IPV6
+#include "lwip/prot/ip6.h"
+#endif
+
+#if LWIP_MDNS_RESPONDER
+
+/* Stored offsets to beginning of domain names
+ * Used for compression.
+ */
+#define DOMAIN_JUMP_SIZE 2
+#define DOMAIN_JUMP 0xc000
+
+#define TOPDOMAIN_LOCAL "local"
+
+#define REVERSE_PTR_TOPDOMAIN "arpa"
+#define REVERSE_PTR_V4_DOMAIN "in-addr"
+#define REVERSE_PTR_V6_DOMAIN "ip6"
+
+static const char *dnssd_protos[] = {
+  "_udp", /* DNSSD_PROTO_UDP */
+  "_tcp", /* DNSSD_PROTO_TCP */
+};
+
+/* forward declarations (function prototypes)*/
+static err_t mdns_domain_add_label_base(struct mdns_domain *domain, u8_t len);
+static err_t mdns_domain_add_label_pbuf(struct mdns_domain *domain,
+                                        const struct pbuf *p, u16_t offset,
+                                        u8_t len);
+static u16_t mdns_readname_loop(struct pbuf *p, u16_t offset,
+                                struct mdns_domain *domain, unsigned depth);
+static err_t mdns_add_dotlocal(struct mdns_domain *domain);
+
+
+static err_t
+mdns_domain_add_label_base(struct mdns_domain *domain, u8_t len)
+{
+  if (len > MDNS_LABEL_MAXLEN) {
+    return ERR_VAL;
+  }
+  if (len > 0 && (1 + len + domain->length >= MDNS_DOMAIN_MAXLEN)) {
+    return ERR_VAL;
+  }
+  /* Allow only zero marker on last byte */
+  if (len == 0 && (1 + domain->length > MDNS_DOMAIN_MAXLEN)) {
+    return ERR_VAL;
+  }
+  domain->name[domain->length] = len;
+  domain->length++;
+  return ERR_OK;
+}
+
+/**
+ * Add a label part to a domain
+ * @param domain The domain to add a label to
+ * @param label The label to add, like &lt;hostname&gt;, 'local', 'com' or ''
+ * @param len The length of the label
+ * @return ERR_OK on success, an err_t otherwise if label too long
+ */
+err_t
+mdns_domain_add_label(struct mdns_domain *domain, const char *label, u8_t len)
+{
+  err_t err = mdns_domain_add_label_base(domain, len);
+  if (err != ERR_OK) {
+    return err;
+  }
+  if (len) {
+    MEMCPY(&domain->name[domain->length], label, len);
+    domain->length += len;
+  }
+  return ERR_OK;
+}
+
+/**
+ * Add a label part to a domain (@see mdns_domain_add_label but copy directly from pbuf)
+ */
+static err_t
+mdns_domain_add_label_pbuf(struct mdns_domain *domain, const struct pbuf *p, u16_t offset, u8_t len)
+{
+  err_t err = mdns_domain_add_label_base(domain, len);
+  if (err != ERR_OK) {
+    return err;
+  }
+  if (len) {
+    if (pbuf_copy_partial(p, &domain->name[domain->length], len, offset) != len) {
+      /* take back the ++ done before */
+      domain->length--;
+      return ERR_ARG;
+    }
+    domain->length += len;
+  }
+  return ERR_OK;
+}
+
+/**
+ * Internal readname function with max 6 levels of recursion following jumps
+ * while decompressing name
+ */
+static u16_t
+mdns_readname_loop(struct pbuf *p, u16_t offset, struct mdns_domain *domain, unsigned depth)
+{
+  u8_t c;
+
+  do {
+    if (depth > 5) {
+      /* Too many jumps */
+      return MDNS_READNAME_ERROR;
+    }
+
+    c = pbuf_get_at(p, offset);
+    offset++;
+
+    /* is this a compressed label? */
+    if ((c & 0xc0) == 0xc0) {
+      u16_t jumpaddr;
+      if (offset >= p->tot_len) {
+        /* Make sure both jump bytes fit in the packet */
+        return MDNS_READNAME_ERROR;
+      }
+      jumpaddr = (((c & 0x3f) << 8) | (pbuf_get_at(p, offset) & 0xff));
+      offset++;
+      if (jumpaddr >= SIZEOF_DNS_HDR && jumpaddr < p->tot_len) {
+        u16_t res;
+        /* Recursive call, maximum depth will be checked */
+        res = mdns_readname_loop(p, jumpaddr, domain, depth + 1);
+        /* Dont return offset since new bytes were not read (jumped to somewhere in packet) */
+        if (res == MDNS_READNAME_ERROR) {
+          return res;
+        }
+      } else {
+        return MDNS_READNAME_ERROR;
+      }
+      break;
+    }
+
+    /* normal label */
+    if (c <= MDNS_LABEL_MAXLEN) {
+      err_t res;
+
+      if (c + domain->length >= MDNS_DOMAIN_MAXLEN) {
+        return MDNS_READNAME_ERROR;
+      }
+      res = mdns_domain_add_label_pbuf(domain, p, offset, c);
+      if (res != ERR_OK) {
+        return MDNS_READNAME_ERROR;
+      }
+      offset += c;
+    } else {
+      /* bad length byte */
+      return MDNS_READNAME_ERROR;
+    }
+  } while (c != 0);
+
+  return offset;
+}
+
+/**
+ * Read possibly compressed domain name from packet buffer
+ * @param p The packet
+ * @param offset start position of domain name in packet
+ * @param domain The domain name destination
+ * @return The new offset after the domain, or MDNS_READNAME_ERROR
+ *         if reading failed
+ */
+u16_t
+mdns_readname(struct pbuf *p, u16_t offset, struct mdns_domain *domain)
+{
+  memset(domain, 0, sizeof(struct mdns_domain));
+  return mdns_readname_loop(p, offset, domain, 0);
+}
+
+/**
+ * Print domain name to debug output
+ * @param domain The domain name
+ */
+void
+mdns_domain_debug_print(struct mdns_domain *domain)
+{
+  u8_t *src = domain->name;
+  u8_t i;
+
+  while (*src) {
+    u8_t label_len = *src;
+    src++;
+    for (i = 0; i < label_len; i++) {
+      LWIP_DEBUGF(MDNS_DEBUG, ("%c", src[i]));
+    }
+    src += label_len;
+    LWIP_DEBUGF(MDNS_DEBUG, ("."));
+  }
+}
+
+/**
+ * Return 1 if contents of domains match (case-insensitive)
+ * @param a Domain name to compare 1
+ * @param b Domain name to compare 2
+ * @return 1 if domains are equal ignoring case, 0 otherwise
+ */
+int
+mdns_domain_eq(struct mdns_domain *a, struct mdns_domain *b)
+{
+  u8_t *ptra, *ptrb;
+  u8_t len;
+  int res;
+
+  if (a->length != b->length) {
+    return 0;
+  }
+
+  ptra = a->name;
+  ptrb = b->name;
+  while (*ptra && *ptrb && ptra < &a->name[a->length]) {
+    if (*ptra != *ptrb) {
+      return 0;
+    }
+    len = *ptra;
+    ptra++;
+    ptrb++;
+    res = lwip_strnicmp((char *) ptra, (char *) ptrb, len);
+    if (res != 0) {
+      return 0;
+    }
+    ptra += len;
+    ptrb += len;
+  }
+  if (*ptra != *ptrb && ptra < &a->name[a->length]) {
+    return 0;
+  }
+  return 1;
+}
+
+#if LWIP_IPV4
+/**
+ * Build domain for reverse lookup of IPv4 address
+ * like 12.0.168.192.in-addr.arpa. for 192.168.0.12
+ * @param domain Where to write the domain name
+ * @param addr Pointer to an IPv4 address to encode
+ * @return ERR_OK if domain was written, an err_t otherwise
+ */
+err_t
+mdns_build_reverse_v4_domain(struct mdns_domain *domain, const ip4_addr_t *addr)
+{
+  int i;
+  err_t res;
+  const u8_t *ptr;
+
+  LWIP_UNUSED_ARG(res);
+  if (!domain || !addr) {
+    return ERR_ARG;
+  }
+  memset(domain, 0, sizeof(struct mdns_domain));
+  ptr = (const u8_t *) addr;
+  for (i = sizeof(ip4_addr_t) - 1; i >= 0; i--) {
+    char buf[4];
+    u8_t val = ptr[i];
+
+    lwip_itoa(buf, sizeof(buf), val);
+    res = mdns_domain_add_label(domain, buf, (u8_t)strlen(buf));
+    LWIP_ERROR("mdns_build_reverse_v4_domain: Failed to add label", (res == ERR_OK), return res);
+  }
+  res = mdns_domain_add_label(domain, REVERSE_PTR_V4_DOMAIN, (u8_t)(sizeof(REVERSE_PTR_V4_DOMAIN) - 1));
+  LWIP_ERROR("mdns_build_reverse_v4_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, REVERSE_PTR_TOPDOMAIN, (u8_t)(sizeof(REVERSE_PTR_TOPDOMAIN) - 1));
+  LWIP_ERROR("mdns_build_reverse_v4_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, NULL, 0);
+  LWIP_ERROR("mdns_build_reverse_v4_domain: Failed to add label", (res == ERR_OK), return res);
+
+  return ERR_OK;
+}
+#endif
+
+#if LWIP_IPV6
+/**
+ * Build domain for reverse lookup of IP address
+ * like b.a.9.8.7.6.5.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.8.b.d.0.1.0.0.2.ip6.arpa. for 2001:db8::567:89ab
+ * @param domain Where to write the domain name
+ * @param addr Pointer to an IPv6 address to encode
+ * @return ERR_OK if domain was written, an err_t otherwise
+ */
+err_t
+mdns_build_reverse_v6_domain(struct mdns_domain *domain, const ip6_addr_t *addr)
+{
+  int i;
+  err_t res;
+  const u8_t *ptr;
+  LWIP_UNUSED_ARG(res);
+  if (!domain || !addr) {
+    return ERR_ARG;
+  }
+  memset(domain, 0, sizeof(struct mdns_domain));
+  ptr = (const u8_t *) addr;
+  for (i = sizeof(ip6_addr_p_t) - 1; i >= 0; i--) {
+    char buf;
+    u8_t byte = ptr[i];
+    int j;
+    for (j = 0; j < 2; j++) {
+      if ((byte & 0x0F) < 0xA) {
+        buf = '0' + (byte & 0x0F);
+      } else {
+        buf = 'a' + (byte & 0x0F) - 0xA;
+      }
+      res = mdns_domain_add_label(domain, &buf, sizeof(buf));
+      LWIP_ERROR("mdns_build_reverse_v6_domain: Failed to add label", (res == ERR_OK), return res);
+      byte >>= 4;
+    }
+  }
+  res = mdns_domain_add_label(domain, REVERSE_PTR_V6_DOMAIN, (u8_t)(sizeof(REVERSE_PTR_V6_DOMAIN) - 1));
+  LWIP_ERROR("mdns_build_reverse_v6_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, REVERSE_PTR_TOPDOMAIN, (u8_t)(sizeof(REVERSE_PTR_TOPDOMAIN) - 1));
+  LWIP_ERROR("mdns_build_reverse_v6_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, NULL, 0);
+  LWIP_ERROR("mdns_build_reverse_v6_domain: Failed to add label", (res == ERR_OK), return res);
+
+  return ERR_OK;
+}
+#endif
+
+/* Add .local. to domain */
+static err_t
+mdns_add_dotlocal(struct mdns_domain *domain)
+{
+  err_t res = mdns_domain_add_label(domain, TOPDOMAIN_LOCAL, (u8_t)(sizeof(TOPDOMAIN_LOCAL) - 1));
+  LWIP_UNUSED_ARG(res);
+  LWIP_ERROR("mdns_add_dotlocal: Failed to add label", (res == ERR_OK), return res);
+  return mdns_domain_add_label(domain, NULL, 0);
+}
+
+/**
+ * Build the <hostname>.local. domain name
+ * @param domain Where to write the domain name
+ * @param mdns TMDNS netif descriptor.
+ * @return ERR_OK if domain <hostname>.local. was written, an err_t otherwise
+ */
+err_t
+mdns_build_host_domain(struct mdns_domain *domain, struct mdns_host *mdns)
+{
+  err_t res;
+  LWIP_UNUSED_ARG(res);
+  memset(domain, 0, sizeof(struct mdns_domain));
+  LWIP_ERROR("mdns_build_host_domain: mdns != NULL", (mdns != NULL), return ERR_VAL);
+  res = mdns_domain_add_label(domain, mdns->name, (u8_t)strlen(mdns->name));
+  LWIP_ERROR("mdns_build_host_domain: Failed to add label", (res == ERR_OK), return res);
+  return mdns_add_dotlocal(domain);
+}
+
+/**
+ * Build the lookup-all-services special DNS-SD domain name
+ * @param domain Where to write the domain name
+ * @return ERR_OK if domain _services._dns-sd._udp.local. was written, an err_t otherwise
+ */
+err_t
+mdns_build_dnssd_domain(struct mdns_domain *domain)
+{
+  err_t res;
+  LWIP_UNUSED_ARG(res);
+  memset(domain, 0, sizeof(struct mdns_domain));
+  res = mdns_domain_add_label(domain, "_services", (u8_t)(sizeof("_services") - 1));
+  LWIP_ERROR("mdns_build_dnssd_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, "_dns-sd", (u8_t)(sizeof("_dns-sd") - 1));
+  LWIP_ERROR("mdns_build_dnssd_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, dnssd_protos[DNSSD_PROTO_UDP], (u8_t)strlen(dnssd_protos[DNSSD_PROTO_UDP]));
+  LWIP_ERROR("mdns_build_dnssd_domain: Failed to add label", (res == ERR_OK), return res);
+  return mdns_add_dotlocal(domain);
+}
+
+/**
+ * Build domain name for a service
+ * @param domain Where to write the domain name
+ * @param service The service struct, containing service name, type and protocol
+ * @param include_name Whether to include the service name in the domain
+ * @return ERR_OK if domain was written. If service name is included,
+ *         <name>.<type>.<proto>.local. will be written, otherwise <type>.<proto>.local.
+ *         An err_t is returned on error.
+ */
+err_t
+mdns_build_service_domain(struct mdns_domain *domain, struct mdns_service *service, int include_name)
+{
+  err_t res;
+  LWIP_UNUSED_ARG(res);
+  memset(domain, 0, sizeof(struct mdns_domain));
+  if (include_name) {
+    res = mdns_domain_add_label(domain, service->name, (u8_t)strlen(service->name));
+    LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  }
+  res = mdns_domain_add_label(domain, service->service, (u8_t)strlen(service->service));
+  LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, dnssd_protos[service->proto], (u8_t)strlen(dnssd_protos[service->proto]));
+  LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  return mdns_add_dotlocal(domain);
+}
+
+/**
+ * Return bytes needed to write before jump for best result of compressing supplied domain
+ * against domain in outpacket starting at specified offset.
+ * If a match is found, offset is updated to where to jump to
+ * @param pbuf Pointer to pbuf with the partially constructed DNS packet
+ * @param offset Start position of a domain written earlier. If this location is suitable
+ *               for compression, the pointer is updated to where in the domain to jump to.
+ * @param domain The domain to write
+ * @return Number of bytes to write of the new domain before writing a jump to the offset.
+ *         If compression can not be done against this previous domain name, the full new
+ *         domain length is returned.
+ */
+u16_t
+mdns_compress_domain(struct pbuf *pbuf, u16_t *offset, struct mdns_domain *domain)
+{
+  struct mdns_domain target;
+  u16_t target_end;
+  u8_t target_len;
+  u8_t writelen = 0;
+  u8_t *ptr;
+  if (pbuf == NULL) {
+    return domain->length;
+  }
+  target_end = mdns_readname(pbuf, *offset, &target);
+  if (target_end == MDNS_READNAME_ERROR) {
+    return domain->length;
+  }
+  target_len = (u8_t)(target_end - *offset);
+  ptr = domain->name;
+  while (writelen < domain->length) {
+    u8_t domainlen = (u8_t)(domain->length - writelen);
+    u8_t labellen;
+    if (domainlen <= target.length && domainlen > DOMAIN_JUMP_SIZE) {
+      /* Compare domains if target is long enough, and we have enough left of the domain */
+      u8_t targetpos = (u8_t)(target.length - domainlen);
+      if ((targetpos + DOMAIN_JUMP_SIZE) >= target_len) {
+        /* We are checking at or beyond a jump in the original, stop looking */
+        break;
+      }
+      if (target.length >= domainlen &&
+          memcmp(&domain->name[writelen], &target.name[targetpos], domainlen) == 0) {
+        *offset += targetpos;
+        return writelen;
+      }
+    }
+    /* Skip to next label in domain */
+    labellen = *ptr;
+    writelen += 1 + labellen;
+    ptr += 1 + labellen;
+  }
+  /* Nothing found */
+  return domain->length;
+}
+
+/**
+ * Write domain to outpacket. Compression will be attempted,
+ * unless domain->skip_compression is set.
+ * @param outpkt The outpacket to write to
+ * @param domain The domain name to write
+ * @return ERR_OK on success, an err_t otherwise
+ */
+err_t
+mdns_write_domain(struct mdns_outpacket *outpkt, struct mdns_domain *domain)
+{
+  int i;
+  err_t res;
+  u16_t writelen = domain->length;
+  u16_t jump_offset = 0;
+  u16_t jump;
+
+  if (!domain->skip_compression) {
+    for (i = 0; i < NUM_DOMAIN_OFFSETS; i++) {
+      u16_t offset = outpkt->domain_offsets[i];
+      if (offset) {
+        u16_t len = mdns_compress_domain(outpkt->pbuf, &offset, domain);
+        if (len < writelen) {
+          writelen = len;
+          jump_offset = offset;
+        }
+      }
+    }
+  }
+
+  if (writelen) {
+    /* Write uncompressed part of name */
+    res = pbuf_take_at(outpkt->pbuf, domain->name, writelen, outpkt->write_offset);
+    if (res != ERR_OK) {
+      return res;
+    }
+
+    /* Store offset of this new domain */
+    for (i = 0; i < NUM_DOMAIN_OFFSETS; i++) {
+      if (outpkt->domain_offsets[i] == 0) {
+        outpkt->domain_offsets[i] = outpkt->write_offset;
+        break;
+      }
+    }
+
+    outpkt->write_offset += writelen;
+  }
+  if (jump_offset) {
+    /* Write jump */
+    jump = lwip_htons(DOMAIN_JUMP | jump_offset);
+    res = pbuf_take_at(outpkt->pbuf, &jump, DOMAIN_JUMP_SIZE, outpkt->write_offset);
+    if (res != ERR_OK) {
+      return res;
+    }
+    outpkt->write_offset += DOMAIN_JUMP_SIZE;
+  }
+  return ERR_OK;
+}
+
+#endif /* LWIP_MDNS_RESPONDER */

src/apps/mdns/mdns_out.c

@@ -0,0 +1,706 @@
+/**
+ * @file
+ * MDNS responder implementation - output related functionalities
+ */
+
+/*
+ * Copyright (c) 2015 Verisure Innovation AB
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Erik Ekman <erik@kryo.se>
+ * Author: Jasper Verschueren <jasper.verschueren@apart-audio.com>
+ *
+ */
+
+#include "lwip/apps/mdns_out.h"
+#include "lwip/apps/mdns_priv.h"
+#include "lwip/apps/mdns_domain.h"
+#include "lwip/prot/dns.h"
+#include "lwip/prot/iana.h"
+#include "lwip/udp.h"
+
+
+#if LWIP_IPV6
+#include "lwip/prot/ip6.h"
+#endif
+
+
+#if LWIP_MDNS_RESPONDER
+
+/* Payload size allocated for each outgoing UDP packet */
+#define OUTPACKET_SIZE 500
+
+/**
+ * Call user supplied function to setup TXT data
+ * @param service The service to build TXT record for
+ */
+void
+mdns_prepare_txtdata(struct mdns_service *service)
+{
+  memset(&service->txtdata, 0, sizeof(struct mdns_domain));
+  if (service->txt_fn) {
+    service->txt_fn(service, service->txt_userdata);
+  }
+}
+
+/**
+ * Write a question to an outpacket
+ * A question contains domain, type and class. Since an answer also starts with these fields this function is also
+ * called from mdns_add_answer().
+ * @param outpkt The outpacket to write to
+ * @param domain The domain name the answer is for
+ * @param type The DNS type of the answer (like 'AAAA', 'SRV')
+ * @param klass The DNS type of the answer (like 'IN')
+ * @param unicast If highest bit in class should be set, to instruct the responder to
+ *                reply with a unicast packet
+ * @return ERR_OK on success, an err_t otherwise
+ */
+static err_t
+mdns_add_question(struct mdns_outpacket *outpkt, struct mdns_domain *domain,
+                  u16_t type, u16_t klass, u16_t unicast)
+{
+  u16_t question_len;
+  u16_t field16;
+  err_t res;
+
+  if (!outpkt->pbuf) {
+    /* If no pbuf is active, allocate one */
+    outpkt->pbuf = pbuf_alloc(PBUF_TRANSPORT, OUTPACKET_SIZE, PBUF_RAM);
+    if (!outpkt->pbuf) {
+      return ERR_MEM;
+    }
+    outpkt->write_offset = SIZEOF_DNS_HDR;
+  }
+
+  /* Worst case calculation. Domain string might be compressed */
+  question_len = domain->length + sizeof(type) + sizeof(klass);
+  if (outpkt->write_offset + question_len > outpkt->pbuf->tot_len) {
+    /* No space */
+    return ERR_MEM;
+  }
+
+  /* Write name */
+  res = mdns_write_domain(outpkt, domain);
+  if (res != ERR_OK) {
+    return res;
+  }
+
+  /* Write type */
+  field16 = lwip_htons(type);
+  res = pbuf_take_at(outpkt->pbuf, &field16, sizeof(field16), outpkt->write_offset);
+  if (res != ERR_OK) {
+    return res;
+  }
+  outpkt->write_offset += sizeof(field16);
+
+  /* Write class */
+  if (unicast) {
+    klass |= 0x8000;
+  }
+  field16 = lwip_htons(klass);
+  res = pbuf_take_at(outpkt->pbuf, &field16, sizeof(field16), outpkt->write_offset);
+  if (res != ERR_OK) {
+    return res;
+  }
+  outpkt->write_offset += sizeof(field16);
+
+  return ERR_OK;
+}
+
+/**
+ * Write answer to reply packet.
+ * buf or answer_domain can be null. The rd_length written will be buf_length +
+ * size of (compressed) domain. Most uses will need either buf or answer_domain,
+ * special case is SRV that starts with 3 u16 and then a domain name.
+ * @param reply The outpacket to write to
+ * @param domain The domain name the answer is for
+ * @param type The DNS type of the answer (like 'AAAA', 'SRV')
+ * @param klass The DNS type of the answer (like 'IN')
+ * @param cache_flush If highest bit in class should be set, to instruct receiver that
+ *                    this reply replaces any earlier answer for this domain/type/class
+ * @param ttl Validity time in seconds to send out for IP address data in DNS replies
+ * @param buf Pointer to buffer of answer data
+ * @param buf_length Length of variable data
+ * @param answer_domain A domain to write after any buffer data as answer
+ * @return ERR_OK on success, an err_t otherwise
+ */
+static err_t
+mdns_add_answer(struct mdns_outpacket *reply, struct mdns_domain *domain,
+                u16_t type, u16_t klass, u16_t cache_flush, u32_t ttl,
+                const u8_t *buf, size_t buf_length, struct mdns_domain *answer_domain)
+{
+  u16_t answer_len;
+  u16_t field16;
+  u16_t rdlen_offset;
+  u16_t answer_offset;
+  u32_t field32;
+  err_t res;
+
+  if (!reply->pbuf) {
+    /* If no pbuf is active, allocate one */
+    reply->pbuf = pbuf_alloc(PBUF_TRANSPORT, OUTPACKET_SIZE, PBUF_RAM);
+    if (!reply->pbuf) {
+      return ERR_MEM;
+    }
+    reply->write_offset = SIZEOF_DNS_HDR;
+  }
+
+  /* Worst case calculation. Domain strings might be compressed */
+  answer_len = domain->length + sizeof(type) + sizeof(klass) + sizeof(ttl) + sizeof(field16)/*rd_length*/;
+  if (buf) {
+    answer_len += (u16_t)buf_length;
+  }
+  if (answer_domain) {
+    answer_len += answer_domain->length;
+  }
+  if (reply->write_offset + answer_len > reply->pbuf->tot_len) {
+    /* No space */
+    return ERR_MEM;
+  }
+
+  /* Answer starts with same data as question, then more fields */
+  mdns_add_question(reply, domain, type, klass, cache_flush);
+
+  /* Write TTL */
+  field32 = lwip_htonl(ttl);
+  res = pbuf_take_at(reply->pbuf, &field32, sizeof(field32), reply->write_offset);
+  if (res != ERR_OK) {
+    return res;
+  }
+  reply->write_offset += sizeof(field32);
+
+  /* Store offsets and skip forward to the data */
+  rdlen_offset = reply->write_offset;
+  reply->write_offset += sizeof(field16);
+  answer_offset = reply->write_offset;
+
+  if (buf) {
+    /* Write static data */
+    res = pbuf_take_at(reply->pbuf, buf, (u16_t)buf_length, reply->write_offset);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->write_offset += (u16_t)buf_length;
+  }
+
+  if (answer_domain) {
+    /* Write name answer (compressed if possible) */
+    res = mdns_write_domain(reply, answer_domain);
+    if (res != ERR_OK) {
+      return res;
+    }
+  }
+
+  /* Write rd_length after when we know the answer size */
+  field16 = lwip_htons(reply->write_offset - answer_offset);
+  res = pbuf_take_at(reply->pbuf, &field16, sizeof(field16), rdlen_offset);
+
+  return res;
+}
+
+/** Write an ANY host question to outpacket */
+static err_t
+mdns_add_any_host_question(struct mdns_outpacket *outpkt,
+                           struct mdns_outmsg *msg,
+                           u16_t request_unicast_reply)
+{
+  struct mdns_domain host;
+  mdns_build_host_domain(&host, netif_mdns_data(msg->netif));
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Adding host question for ANY type\n"));
+  return mdns_add_question(outpkt, &host, DNS_RRTYPE_ANY, DNS_RRCLASS_IN,
+                           request_unicast_reply);
+}
+
+/** Write an ANY service instance question to outpacket */
+static err_t
+mdns_add_any_service_question(struct mdns_outpacket *outpkt,
+                              struct mdns_service *service,
+                              u16_t request_unicast_reply)
+{
+  struct mdns_domain domain;
+  mdns_build_service_domain(&domain, service, 1);
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Adding service instance question for ANY type\n"));
+  return mdns_add_question(outpkt, &domain, DNS_RRTYPE_ANY, DNS_RRCLASS_IN,
+                           request_unicast_reply);
+}
+
+#if LWIP_IPV4
+/** Write an IPv4 address (A) RR to outpacket */
+static err_t
+mdns_add_a_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg)
+{
+  err_t res;
+  struct mdns_domain host;
+  mdns_build_host_domain(&host, netif_mdns_data(msg->netif));
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &host, DNS_RRTYPE_A, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with A record\n"));
+  return mdns_add_answer(reply, &host, DNS_RRTYPE_A, DNS_RRCLASS_IN, msg->cache_flush,
+                         (netif_mdns_data(msg->netif))->dns_ttl,
+                         (const u8_t *) netif_ip4_addr(msg->netif), sizeof(ip4_addr_t), NULL);
+}
+
+/** Write a 4.3.2.1.in-addr.arpa -> hostname.local PTR RR to outpacket */
+static err_t
+mdns_add_hostv4_ptr_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg)
+{
+  err_t res;
+  struct mdns_domain host, revhost;
+  mdns_build_host_domain(&host, netif_mdns_data(msg->netif));
+  mdns_build_reverse_v4_domain(&revhost, netif_ip4_addr(msg->netif));
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &revhost, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with v4 PTR record\n"));
+  return mdns_add_answer(reply, &revhost, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, msg->cache_flush,
+                         (netif_mdns_data(msg->netif))->dns_ttl, NULL, 0, &host);
+}
+#endif
+
+#if LWIP_IPV6
+/** Write an IPv6 address (AAAA) RR to outpacket */
+static err_t
+mdns_add_aaaa_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg, int addrindex)
+{
+  err_t res;
+  struct mdns_domain host;
+  mdns_build_host_domain(&host, netif_mdns_data(msg->netif));
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &host, DNS_RRTYPE_AAAA, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with AAAA record\n"));
+  return mdns_add_answer(reply, &host, DNS_RRTYPE_AAAA, DNS_RRCLASS_IN, msg->cache_flush,
+                         (netif_mdns_data(msg->netif))->dns_ttl,
+                         (const u8_t *) netif_ip6_addr(msg->netif, addrindex), sizeof(ip6_addr_p_t), NULL);
+}
+
+/** Write a x.y.z.ip6.arpa -> hostname.local PTR RR to outpacket */
+static err_t
+mdns_add_hostv6_ptr_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg, int addrindex)
+{
+  err_t res;
+  struct mdns_domain host, revhost;
+  mdns_build_host_domain(&host, netif_mdns_data(msg->netif));
+  mdns_build_reverse_v6_domain(&revhost, netif_ip6_addr(msg->netif, addrindex));
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &revhost, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with v6 PTR record\n"));
+  return mdns_add_answer(reply, &revhost, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, msg->cache_flush,
+                         (netif_mdns_data(msg->netif))->dns_ttl, NULL, 0, &host);
+}
+#endif
+
+/** Write an all-services -> servicetype PTR RR to outpacket */
+static err_t
+mdns_add_servicetype_ptr_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg,
+                                struct mdns_service *service)
+{
+  err_t res;
+  struct mdns_domain service_type, service_dnssd;
+  mdns_build_service_domain(&service_type, service, 0);
+  mdns_build_dnssd_domain(&service_dnssd);
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &service_dnssd, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with service type PTR record\n"));
+  return mdns_add_answer(reply, &service_dnssd, DNS_RRTYPE_PTR, DNS_RRCLASS_IN,
+                         0, service->dns_ttl, NULL, 0, &service_type);
+}
+
+/** Write a servicetype -> servicename PTR RR to outpacket */
+static err_t
+mdns_add_servicename_ptr_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg,
+                                struct mdns_service *service)
+{
+  err_t res;
+  struct mdns_domain service_type, service_instance;
+  mdns_build_service_domain(&service_type, service, 0);
+  mdns_build_service_domain(&service_instance, service, 1);
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &service_type, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with service name PTR record\n"));
+  return mdns_add_answer(reply, &service_type, DNS_RRTYPE_PTR, DNS_RRCLASS_IN,
+                         0, service->dns_ttl, NULL, 0, &service_instance);
+}
+
+/** Write a SRV RR to outpacket */
+static err_t
+mdns_add_srv_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg,
+                    struct mdns_host *mdns, struct mdns_service *service)
+{
+  err_t res;
+  struct mdns_domain service_instance, srvhost;
+  u16_t srvdata[3];
+  mdns_build_service_domain(&service_instance, service, 1);
+  mdns_build_host_domain(&srvhost, mdns);
+  if (msg->legacy_query) {
+    /* RFC 6762 section 18.14:
+     * In legacy unicast responses generated to answer legacy queries,
+     * name compression MUST NOT be performed on SRV records.
+     */
+    srvhost.skip_compression = 1;
+    /* When answering to a legacy querier, we need to repeat the question.
+     * But this only needs to be done for the question asked (max one question),
+     * not for the additional records. */
+    if(reply->questions < 1) {
+      LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+      res = mdns_add_question(reply, &service_instance, DNS_RRTYPE_SRV, DNS_RRCLASS_IN, 0);
+      if (res != ERR_OK) {
+        return res;
+      }
+      reply->questions = 1;
+    }
+  }
+  srvdata[0] = lwip_htons(SRV_PRIORITY);
+  srvdata[1] = lwip_htons(SRV_WEIGHT);
+  srvdata[2] = lwip_htons(service->port);
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with SRV record\n"));
+  return mdns_add_answer(reply, &service_instance, DNS_RRTYPE_SRV, DNS_RRCLASS_IN,
+                         msg->cache_flush, service->dns_ttl,
+                         (const u8_t *) &srvdata, sizeof(srvdata), &srvhost);
+}
+
+/** Write a TXT RR to outpacket */
+static err_t
+mdns_add_txt_answer(struct mdns_outpacket *reply, struct mdns_outmsg *msg,
+                    struct mdns_service *service)
+{
+  err_t res;
+  struct mdns_domain service_instance;
+  mdns_build_service_domain(&service_instance, service, 1);
+  mdns_prepare_txtdata(service);
+  /* When answering to a legacy querier, we need to repeat the question.
+   * But this only needs to be done for the question asked (max one question),
+   * not for the additional records. */
+  if(msg->legacy_query && reply->questions < 1) {
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Add question for legacy query\n"));
+    res = mdns_add_question(reply, &service_instance, DNS_RRTYPE_TXT, DNS_RRCLASS_IN, 0);
+    if (res != ERR_OK) {
+      return res;
+    }
+    reply->questions = 1;
+  }
+  LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Responding with TXT record\n"));
+  return mdns_add_answer(reply, &service_instance, DNS_RRTYPE_TXT, DNS_RRCLASS_IN,
+                         msg->cache_flush, service->dns_ttl,
+                         (u8_t *) &service->txtdata.name, service->txtdata.length, NULL);
+}
+
+
+static err_t
+mdns_add_probe_questions_to_outpacket(struct mdns_outpacket *outpkt, struct mdns_outmsg *msg)
+{
+  err_t res;
+  int i;
+  struct mdns_host *mdns = netif_mdns_data(msg->netif);
+
+  /* Write host questions (probing or legacy query) */
+  if(msg->host_questions & QUESTION_PROBE_HOST_ANY) {
+    res = mdns_add_any_host_question(outpkt, msg, 1);
+    if (res != ERR_OK) {
+      return res;
+    }
+    outpkt->questions++;
+  }
+  /* Write service questions (probing or legacy query) */
+  for (i = 0; i < MDNS_MAX_SERVICES; i++) {
+    struct mdns_service* service = mdns->services[i];
+    if (!service) {
+      continue;
+    }
+    if(msg->serv_questions[i] & QUESTION_PROBE_SERVICE_NAME_ANY) {
+      res = mdns_add_any_service_question(outpkt, service, 1);
+      if (res != ERR_OK) {
+        return res;
+      }
+      outpkt->questions++;
+    }
+  }
+  return ERR_OK;
+}
+
+/**
+ * Send chosen answers as a reply
+ *
+ * Add all selected answers (first write will allocate pbuf)
+ * Add additional answers based on the selected answers
+ * Send the packet
+ */
+err_t
+mdns_send_outpacket(struct mdns_outmsg *msg)
+{
+  struct mdns_service *service;
+  struct mdns_outpacket outpkt;
+  err_t res = ERR_ARG;
+  int i;
+  struct mdns_host *mdns = netif_mdns_data(msg->netif);
+  u16_t answers = 0;
+
+  memset(&outpkt, 0, sizeof(outpkt));
+
+  res = mdns_add_probe_questions_to_outpacket(&outpkt, msg);
+  if (res != ERR_OK) {
+    goto cleanup;
+  }
+
+  /* Write answers to host questions */
+#if LWIP_IPV4
+  if (msg->host_replies & REPLY_HOST_A) {
+    res = mdns_add_a_answer(&outpkt, msg);
+    if (res != ERR_OK) {
+      goto cleanup;
+    }
+    answers++;
+  }
+  if (msg->host_replies & REPLY_HOST_PTR_V4) {
+    res = mdns_add_hostv4_ptr_answer(&outpkt, msg);
+    if (res != ERR_OK) {
+      goto cleanup;
+    }
+    answers++;
+  }
+#endif
+#if LWIP_IPV6
+  if (msg->host_replies & REPLY_HOST_AAAA) {
+    int addrindex;
+    for (addrindex = 0; addrindex < LWIP_IPV6_NUM_ADDRESSES; addrindex++) {
+      if (ip6_addr_isvalid(netif_ip6_addr_state(msg->netif, addrindex))) {
+        res = mdns_add_aaaa_answer(&outpkt, msg, addrindex);
+        if (res != ERR_OK) {
+          goto cleanup;
+        }
+        answers++;
+      }
+    }
+  }
+  if (msg->host_replies & REPLY_HOST_PTR_V6) {
+    u8_t rev_addrs = msg->host_reverse_v6_replies;
+    int addrindex = 0;
+    while (rev_addrs) {
+      if (rev_addrs & 1) {
+        res = mdns_add_hostv6_ptr_answer(&outpkt, msg, addrindex);
+        if (res != ERR_OK) {
+          goto cleanup;
+        }
+        answers++;
+      }
+      addrindex++;
+      rev_addrs >>= 1;
+    }
+  }
+#endif
+
+  /* Write answers to service questions */
+  for (i = 0; i < MDNS_MAX_SERVICES; i++) {
+    service = mdns->services[i];
+    if (!service) {
+      continue;
+    }
+
+    if (msg->serv_replies[i] & REPLY_SERVICE_TYPE_PTR) {
+      res = mdns_add_servicetype_ptr_answer(&outpkt, msg, service);
+      if (res != ERR_OK) {
+        goto cleanup;
+      }
+      answers++;
+    }
+
+    if (msg->serv_replies[i] & REPLY_SERVICE_NAME_PTR) {
+      res = mdns_add_servicename_ptr_answer(&outpkt, msg, service);
+      if (res != ERR_OK) {
+        goto cleanup;
+      }
+      answers++;
+    }
+
+    if (msg->serv_replies[i] & REPLY_SERVICE_SRV) {
+      res = mdns_add_srv_answer(&outpkt, msg, mdns, service);
+      if (res != ERR_OK) {
+        goto cleanup;
+      }
+      answers++;
+    }
+
+    if (msg->serv_replies[i] & REPLY_SERVICE_TXT) {
+      res = mdns_add_txt_answer(&outpkt, msg, service);
+      if (res != ERR_OK) {
+        goto cleanup;
+      }
+      answers++;
+    }
+  }
+
+  /* if this is a response, the data above is anwers, else this is a probe and
+   * the answers above goes into auth section */
+  if (msg->flags & DNS_FLAG1_RESPONSE) {
+    outpkt.answers += answers;
+  } else {
+    outpkt.authoritative += answers;
+  }
+
+  /* All answers written, add additional RRs */
+  for (i = 0; i < MDNS_MAX_SERVICES; i++) {
+    service = mdns->services[i];
+    if (!service) {
+      continue;
+    }
+
+    if (msg->serv_replies[i] & REPLY_SERVICE_NAME_PTR) {
+      /* Our service instance requested, include SRV & TXT
+       * if they are already not requested. */
+      if (!(msg->serv_replies[i] & REPLY_SERVICE_SRV)) {
+        res = mdns_add_srv_answer(&outpkt, msg, mdns, service);
+        if (res != ERR_OK) {
+          goto cleanup;
+        }
+        outpkt.additional++;
+      }
+
+      if (!(msg->serv_replies[i] & REPLY_SERVICE_TXT)) {
+        res = mdns_add_txt_answer(&outpkt, msg, service);
+        if (res != ERR_OK) {
+          goto cleanup;
+        }
+        outpkt.additional++;
+      }
+    }
+
+    /* If service instance, SRV, record or an IP address is requested,
+     * supply all addresses for the host
+     */
+    if ((msg->serv_replies[i] & (REPLY_SERVICE_NAME_PTR | REPLY_SERVICE_SRV)) ||
+        (msg->host_replies & (REPLY_HOST_A | REPLY_HOST_AAAA))) {
+#if LWIP_IPV6
+      if (!(msg->host_replies & REPLY_HOST_AAAA)) {
+        int addrindex;
+        for (addrindex = 0; addrindex < LWIP_IPV6_NUM_ADDRESSES; addrindex++) {
+          if (ip6_addr_isvalid(netif_ip6_addr_state(msg->netif, addrindex))) {
+            res = mdns_add_aaaa_answer(&outpkt, msg, addrindex);
+            if (res != ERR_OK) {
+              goto cleanup;
+            }
+            outpkt.additional++;
+          }
+        }
+      }
+#endif
+#if LWIP_IPV4
+      if (!(msg->host_replies & REPLY_HOST_A) &&
+          !ip4_addr_isany_val(*netif_ip4_addr(msg->netif))) {
+        res = mdns_add_a_answer(&outpkt, msg);
+        if (res != ERR_OK) {
+          goto cleanup;
+        }
+        outpkt.additional++;
+      }
+#endif
+    }
+  }
+
+  if (outpkt.pbuf) {
+    struct dns_hdr hdr;
+
+    /* Write header */
+    memset(&hdr, 0, sizeof(hdr));
+    hdr.flags1 = msg->flags;
+    hdr.numquestions = lwip_htons(outpkt.questions);
+    hdr.numanswers = lwip_htons(outpkt.answers);
+    hdr.numauthrr = lwip_htons(outpkt.authoritative);
+    hdr.numextrarr = lwip_htons(outpkt.additional);
+    hdr.id = lwip_htons(msg->tx_id);
+    pbuf_take(outpkt.pbuf, &hdr, sizeof(hdr));
+
+    /* Shrink packet */
+    pbuf_realloc(outpkt.pbuf, outpkt.write_offset);
+
+    /* Send created packet */
+    LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Sending packet, len=%d, unicast=%d\n",
+                outpkt.write_offset, msg->unicast_reply));
+
+    res = udp_sendto_if(get_mdns_pcb(), outpkt.pbuf, &msg->dest_addr, msg->dest_port, msg->netif);
+  }
+
+cleanup:
+  if (outpkt.pbuf) {
+    pbuf_free(outpkt.pbuf);
+    outpkt.pbuf = NULL;
+  }
+  return res;
+}
+
+#endif /* LWIP_MDNS_RESPONDER */

src/include/lwip/apps/mdns.h

@@ -96,6 +96,9 @@ void mdns_resp_announce(struct netif *netif);
  */
 #define mdns_resp_netif_settings_changed(netif) mdns_resp_announce(netif)
 
+struct mdns_host* netif_mdns_data(struct netif *netif);
+struct udp_pcb* get_mdns_pcb(void);
+
 #endif /* LWIP_MDNS_RESPONDER */
 
 #ifdef __cplusplus

src/include/lwip/apps/mdns_domain.h

@@ -0,0 +1,71 @@
+/**
+ * @file
+ * MDNS responder - domain related functionalities 
+ */
+
+ /*
+ * Copyright (c) 2015 Verisure Innovation AB
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Erik Ekman <erik@kryo.se>
+ * Author: Jasper Verschueren <jasper.verschueren@apart-audio.com>
+ *
+ */
+
+#ifndef LWIP_HDR_APPS_MDNS_DOMAIN_H
+#define LWIP_HDR_APPS_MDNS_DOMAIN_H
+
+#include "lwip/apps/mdns_opts.h"
+#include "lwip/apps/mdns_priv.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if LWIP_MDNS_RESPONDER
+
+/* Domain methods - also visible for unit tests */
+
+err_t mdns_domain_add_label(struct mdns_domain *domain, const char *label, u8_t len);
+u16_t mdns_readname(struct pbuf *p, u16_t offset, struct mdns_domain *domain);
+void mdns_domain_debug_print(struct mdns_domain *domain);
+int mdns_domain_eq(struct mdns_domain *a, struct mdns_domain *b);
+err_t mdns_build_reverse_v4_domain(struct mdns_domain *domain, const ip4_addr_t *addr);
+err_t mdns_build_reverse_v6_domain(struct mdns_domain *domain, const ip6_addr_t *addr);
+err_t mdns_build_host_domain(struct mdns_domain *domain, struct mdns_host *mdns);
+err_t mdns_build_dnssd_domain(struct mdns_domain *domain);
+err_t mdns_build_service_domain(struct mdns_domain *domain, struct mdns_service *service, int include_name);
+u16_t mdns_compress_domain(struct pbuf *pbuf, u16_t *offset, struct mdns_domain *domain);
+err_t mdns_write_domain(struct mdns_outpacket *outpkt, struct mdns_domain *domain);
+
+#endif /* LWIP_MDNS_RESPONDER */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* LWIP_HDR_APPS_MDNS_DOMAIN_H */

src/include/lwip/apps/mdns_out.h

@@ -0,0 +1,85 @@
+/**
+ * @file
+ * MDNS responder - output related functionalities
+ */
+
+ /*
+ * Copyright (c) 2015 Verisure Innovation AB
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ *    this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
+ * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+ * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
+ * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ *
+ * This file is part of the lwIP TCP/IP stack.
+ *
+ * Author: Erik Ekman <erik@kryo.se>
+ * Author: Jasper Verschueren <jasper.verschueren@apart-audio.com>
+ *
+ */
+
+#ifndef LWIP_HDR_APPS_MDNS_OUT_H
+#define LWIP_HDR_APPS_MDNS_OUT_H
+
+#include "lwip/apps/mdns_opts.h"
+#include "lwip/apps/mdns_priv.h"
+#include "lwip/netif.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if LWIP_MDNS_RESPONDER
+
+/** Bitmasks outmsg generation */
+/* Probe for ALL types with hostname */
+#define QUESTION_PROBE_HOST_ANY          0x10
+/* Probe for ALL types with service instance name */
+#define QUESTION_PROBE_SERVICE_NAME_ANY  0x10
+
+/* Lookup from hostname -> IPv4 */
+#define REPLY_HOST_A            0x01
+/* Lookup from IPv4/v6 -> hostname */
+#define REPLY_HOST_PTR_V4       0x02
+/* Lookup from hostname -> IPv6 */
+#define REPLY_HOST_AAAA         0x04
+/* Lookup from hostname -> IPv6 */
+#define REPLY_HOST_PTR_V6       0x08
+
+/* Lookup for service types */
+#define REPLY_SERVICE_TYPE_PTR  0x10
+/* Lookup for instances of service */
+#define REPLY_SERVICE_NAME_PTR  0x20
+/* Lookup for location of service instance */
+#define REPLY_SERVICE_SRV       0x40
+/* Lookup for text info on service instance */
+#define REPLY_SERVICE_TXT       0x80
+
+err_t mdns_send_outpacket(struct mdns_outmsg *msg);
+void mdns_prepare_txtdata(struct mdns_service *service);
+
+#endif /* LWIP_MDNS_RESPONDER */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* LWIP_HDR_APPS_MDNS_OUT_H */

src/include/lwip/apps/mdns_priv.h

@@ -37,6 +37,7 @@
 #ifndef LWIP_HDR_MDNS_PRIV_H
 #define LWIP_HDR_MDNS_PRIV_H
 
+#include "lwip/apps/mdns.h"
 #include "lwip/apps/mdns_opts.h"
 #include "lwip/pbuf.h"
 
@@ -46,10 +47,14 @@ extern "C" {
 
 #if LWIP_MDNS_RESPONDER
 
-/* Domain struct and methods - visible for unit tests */
-
 #define MDNS_DOMAIN_MAXLEN 256
 #define MDNS_READNAME_ERROR 0xFFFF
+#define NUM_DOMAIN_OFFSETS 10
+
+#define SRV_PRIORITY 0
+#define SRV_WEIGHT   0
+
+/* Domain structs - also visible for unit tests */
 
 struct mdns_domain {
   /* Encoded domain name */
@@ -60,10 +65,88 @@ struct mdns_domain {
   u8_t skip_compression;
 };
 
-err_t mdns_domain_add_label(struct mdns_domain *domain, const char *label, u8_t len);
-u16_t mdns_readname(struct pbuf *p, u16_t offset, struct mdns_domain *domain);
-int mdns_domain_eq(struct mdns_domain *a, struct mdns_domain *b);
-u16_t mdns_compress_domain(struct pbuf *pbuf, u16_t *offset, struct mdns_domain *domain);
+/** Description of a service */
+struct mdns_service {
+  /** TXT record to answer with */
+  struct mdns_domain txtdata;
+  /** Name of service, like 'myweb' */
+  char name[MDNS_LABEL_MAXLEN + 1];
+  /** Type of service, like '_http' */
+  char service[MDNS_LABEL_MAXLEN + 1];
+  /** Callback function and userdata
+   * to update txtdata buffer */
+  service_get_txt_fn_t txt_fn;
+  void *txt_userdata;
+  /** TTL in seconds of SRV/TXT replies */
+  u32_t dns_ttl;
+  /** Protocol, TCP or UDP */
+  u16_t proto;
+  /** Port of the service */
+  u16_t port;
+};
+
+/** Description of a host/netif */
+struct mdns_host {
+  /** Hostname */
+  char name[MDNS_LABEL_MAXLEN + 1];
+  /** Pointer to services */
+  struct mdns_service *services[MDNS_MAX_SERVICES];
+  /** TTL in seconds of A/AAAA/PTR replies */
+  u32_t dns_ttl;
+  /** Number of probes sent for the current name */
+  u8_t probes_sent;
+  /** State in probing sequence */
+  u8_t probing_state;
+};
+
+/** mDNS output packet */
+struct mdns_outpacket {
+  /** Packet data */
+  struct pbuf *pbuf;
+  /** Current write offset in packet */
+  u16_t write_offset;
+  /** Number of questions written */
+  u16_t questions;
+  /** Number of normal answers written */
+  u16_t answers;
+  /** Number of authoritative answers written */
+  u16_t authoritative;
+  /** Number of additional answers written */
+  u16_t additional;
+  /** Offsets for written domain names in packet.
+   *  Used for compression */
+  u16_t domain_offsets[NUM_DOMAIN_OFFSETS];
+};
+
+/** mDNS output message */
+struct mdns_outmsg {
+  /** Netif to send the packet on */
+  struct netif *netif;
+  /** Identifier. Used in legacy queries */
+  u16_t tx_id;
+  /** dns flags */
+  u8_t flags;
+  /** Destination IP/port if sent unicast */
+  ip_addr_t dest_addr;
+  u16_t dest_port;
+  /** If all answers in packet should set cache_flush bit */
+  u8_t cache_flush;
+  /** If reply should be sent unicast */
+  u8_t unicast_reply;
+  /** If legacy query. (tx_id needed, and write
+   *  question again in reply before answer) */
+  u8_t legacy_query;
+  /* Question bitmask for host information */
+  u8_t host_questions;
+  /* Questions bitmask per service */
+  u8_t serv_questions[MDNS_MAX_SERVICES];
+  /* Reply bitmask for host information */
+  u8_t host_replies;
+  /* Bitmask for which reverse IPv6 hosts to answer */
+  u8_t host_reverse_v6_replies;
+  /* Reply bitmask per service */
+  u8_t serv_replies[MDNS_MAX_SERVICES];
+};
 
 #endif /* LWIP_MDNS_RESPONDER */
 

test/unit/mdns/test_mdns.c

@@ -34,6 +34,7 @@
 
 #include "lwip/pbuf.h"
 #include "lwip/apps/mdns.h"
+#include "lwip/apps/mdns_domain.h"
 #include "lwip/apps/mdns_priv.h"
 
 START_TEST(readname_basic)