From db634d69107cab091c79268cb6f6bce0c1fc4aeb Mon Sep 17 00:00:00 2001
From: David Girault <dgirault@sevenhugs.com>
Date: Fri, 21 Apr 2017 12:05:47 +0200
Subject: [PATCH] mdns: added support for searching services

Two new API:
err_t mdns_search_service(const char *name, const char *service, enum mdns_sd_proto proto,
                          struct netif *netif, search_result_fn_t result_fn, void *arg,
                          s8_t *request_id);
void mdns_search_stop(s8_t request_id);

One compilation flags:
LWIP_MDNS_SEARCH

One options flags:
MDNS_MAX_REQUESTS

Some structure declarations moved to allow use by callback result function.
---
 src/apps/mdns/mdns.c              | 253 ++++++++++++++++++++++++++++++++++----
 src/include/lwip/apps/mdns.h      |  47 +++++++
 src/include/lwip/apps/mdns_opts.h |  12 ++
 src/include/lwip/apps/mdns_priv.h |  28 +++--
 4 files changed, 302 insertions(+), 38 deletions(-)

diff --git a/src/apps/mdns/mdns.c b/src/apps/mdns/mdns.c
index 824ec9aa..34d3f1e6 100644
--- a/src/apps/mdns/mdns.c
+++ b/src/apps/mdns/mdns.c
@@ -89,6 +89,10 @@ static const ip_addr_t v4group = DNS_MQUERY_IPV4_GROUP_INIT;
 static const ip_addr_t v6group = DNS_MQUERY_IPV6_GROUP_INIT;
 #endif
 
+#if LWIP_MDNS_SEARCH
+static struct mdns_request mdns_requests[MDNS_MAX_REQUESTS];
+#endif
+
 #define MDNS_PORT 5353
 #define MDNS_TTL  255
 
@@ -235,32 +239,12 @@ struct mdns_outpacket {
   u8_t serv_replies[MDNS_MAX_SERVICES];
 };
 
-/** Domain, type and class.
- *  Shared between questions and answers */
-struct mdns_rr_info {
-  struct mdns_domain domain;
-  u16_t type;
-  u16_t klass;
-};
-
 struct mdns_question {
   struct mdns_rr_info info;
   /** unicast reply requested */
   u16_t unicast;
 };
 
-struct mdns_answer {
-  struct mdns_rr_info info;
-  /** cache flush command bit */
-  u16_t cache_flush;
-  /* Validity time in seconds */
-  u32_t ttl;
-  /** Length of variable answer */
-  u16_t rd_length;
-  /** Offset of start of variable answer in packet */
-  u16_t rd_offset;
-};
-
 static err_t
 mdns_domain_add_label_base(struct mdns_domain *domain, u8_t len)
 {
@@ -624,6 +608,33 @@ mdns_build_service_domain(struct mdns_domain *domain, struct mdns_service *servi
   return mdns_add_dotlocal(domain);
 }
 
+#if LWIP_MDNS_SEARCH
+/**
+ * Build domain name for a request
+ * @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.
+ */
+static err_t
+mdns_build_request_domain(struct mdns_domain *domain, struct mdns_request *request, int include_name)
+{
+  err_t res;
+  memset(domain, 0, sizeof(struct mdns_domain));
+  if (include_name) {
+    res = mdns_domain_add_label(domain, request->name, (u8_t)strlen(request->name));
+    LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  }
+  res = mdns_domain_add_label(domain, request->service, (u8_t)strlen(request->service));
+  LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  res = mdns_domain_add_label(domain, dnssd_protos[request->proto], (u8_t)strlen(dnssd_protos[request->proto]));
+  LWIP_ERROR("mdns_build_service_domain: Failed to add label", (res == ERR_OK), return res);
+  return mdns_add_dotlocal(domain);
+}
+#endif
+
 /**
  * Check which replies we should send for a host/netif based on question
  * @param netif The network interface that received the question
@@ -739,6 +750,45 @@ check_service(struct mdns_service *service, struct mdns_rr_info *rr)
   return replies;
 }
 
+#if LWIP_MDNS_SEARCH
+/**
+ * Check if question belong to a specified request
+ * @param request A ongoing MDNS request
+ * @param rr Domain/type/class from an answer
+ * @return Bitmask of which matching replies
+ */
+static int
+check_request(struct mdns_request *request, struct mdns_rr_info *rr)
+{
+  err_t res;
+  int replies = 0;
+  struct mdns_domain mydomain;
+
+  if (rr->klass != DNS_RRCLASS_IN && rr->klass != DNS_RRCLASS_ANY) {
+    /* Invalid class */
+    return 0;
+  }
+
+  res = mdns_build_request_domain(&mydomain, request, 0);
+  if (res == ERR_OK && mdns_domain_eq(&rr->domain, &mydomain) &&
+      (rr->type == DNS_RRTYPE_PTR || rr->type == DNS_RRTYPE_ANY)) {
+    /* Request for the instance of my service */
+    replies |= REPLY_SERVICE_TYPE_PTR;
+  }
+  res = mdns_build_request_domain(&mydomain, request, 1);
+  if (res == ERR_OK && mdns_domain_eq(&rr->domain, &mydomain)) {
+    /* Request for info about my service */
+    if (rr->type == DNS_RRTYPE_SRV || rr->type == DNS_RRTYPE_ANY) {
+      replies |= REPLY_SERVICE_SRV;
+    }
+    if (rr->type == DNS_RRTYPE_TXT || rr->type == DNS_RRTYPE_ANY) {
+      replies |= REPLY_SERVICE_TXT;
+    }
+  }
+  return replies;
+}
+#endif
+
 /**
  * Return bytes needed to write before jump for best result of compressing supplied domain
  * against domain in outpacket starting at specified offset.
@@ -1273,7 +1323,7 @@ mdns_init_outpacket(struct mdns_outpacket *out, struct mdns_packet *in)
  * Send the packet
  */
 static void
-mdns_send_outpacket(struct mdns_outpacket *outpkt)
+mdns_send_outpacket(struct mdns_outpacket *outpkt, u8_t flags)
 {
   struct mdns_service *service;
   err_t res;
@@ -1431,7 +1481,7 @@ mdns_send_outpacket(struct mdns_outpacket *outpkt)
 
     /* Write header */
     memset(&hdr, 0, sizeof(hdr));
-    hdr.flags1 = DNS_FLAG1_RESPONSE | DNS_FLAG1_AUTHORATIVE;
+    hdr.flags1 = flags;
     hdr.numanswers = lwip_htons(outpkt->answers);
     hdr.numextrarr = lwip_htons(outpkt->additional);
     if (outpkt->legacy_query) {
@@ -1506,9 +1556,46 @@ mdns_announce(struct netif *netif, const ip_addr_t *destination)
 
   announce.dest_port = MDNS_PORT;
   SMEMCPY(&announce.dest_addr, destination, sizeof(announce.dest_addr));
-  mdns_send_outpacket(&announce);
+  mdns_send_outpacket(&announce, DNS_FLAG1_RESPONSE | DNS_FLAG1_AUTHORATIVE);
 }
 
+#if LWIP_MDNS_SEARCH
+/**
+ * Send search request containing all our known data
+ * @param netif The network interface to send on
+ * @param destination The target address to send to (usually multicast address)
+ */
+static void
+mdns_search(struct mdns_request *req, struct netif *netif, const ip_addr_t *destination)
+{
+  struct mdns_outpacket request;
+  struct mdns_domain dom;
+  err_t res;
+
+  memset(&request, 0, sizeof(request));
+  request.netif = netif;
+  request.cache_flush = 0;
+  
+  /* Add question (legacy packet only has 1 question) */
+  mdns_build_request_domain(&dom, req, req->name[0]);
+  res = mdns_add_question(&request, &dom, DNS_RRTYPE_PTR, DNS_RRCLASS_IN, 0);
+  if (res != ERR_OK) {
+      goto cleanup;
+  }
+
+  request.legacy_query = 1;
+  request.dest_port = MDNS_PORT;
+  SMEMCPY(&request.dest_addr, destination, sizeof(request.dest_addr));
+  mdns_send_outpacket(&request, 0);
+cleanup:
+  if (request.pbuf) {
+    /* This should only happen if we fail to alloc/write question for legacy query */
+    pbuf_free(request.pbuf);
+    request.pbuf = NULL;
+  }
+}
+#endif
+
 /**
  * Handle question MDNS packet
  * 1. Parse all questions and set bits what answers to send
@@ -1715,7 +1802,7 @@ mdns_handle_question(struct mdns_packet *pkt)
     }
   }
 
-  mdns_send_outpacket(&reply);
+  mdns_send_outpacket(&reply, DNS_FLAG1_RESPONSE | DNS_FLAG1_AUTHORATIVE);
 
 cleanup:
   if (reply.pbuf) {
@@ -1725,6 +1812,26 @@ cleanup:
   }
 }
 
+
+/**
+ * Loockup matching request for response MDNS packet
+ */
+#if LWIP_MDNS_SEARCH
+static struct mdns_request *
+mdns_loockup_request(struct mdns_rr_info *rr)
+{
+  int i;
+  /* search originating request */
+  for (i = 0; i < MDNS_MAX_REQUESTS; i++) {
+    if ((mdns_requests[i].result_fn != NULL) &&
+        (check_request(&mdns_requests[i], rr) != 0)) {
+        return &mdns_requests[i];
+    }
+  }
+  return NULL;
+}
+#endif
+
 /**
  * Handle response MDNS packet
  * Only prints debug for now. Will need more code to do conflict resolution.
@@ -1732,16 +1839,24 @@ cleanup:
 static void
 mdns_handle_response(struct mdns_packet *pkt)
 {
+#if LWIP_MDNS_SEARCH
+  struct mdns_request *req = NULL;
+  s8_t first = 1;
+#endif
   /* Ignore all questions */
   while (pkt->questions_left) {
     struct mdns_question q;
     err_t res;
-
     res = mdns_read_question(pkt, &q);
     if (res != ERR_OK) {
       LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Failed to parse question, skipping response packet\n"));
       return;
     }
+#if LWIP_MDNS_SEARCH
+    else {
+        req = mdns_loockup_request(&q.info);
+    }
+#endif
   }
 
   while (pkt->answers_left) {
@@ -1757,6 +1872,19 @@ mdns_handle_response(struct mdns_packet *pkt)
     LWIP_DEBUGF(MDNS_DEBUG, ("MDNS: Answer for domain "));
     mdns_domain_debug_print(&ans.info.domain);
     LWIP_DEBUGF(MDNS_DEBUG, (" type %d class %d\n", ans.info.type, ans.info.klass));
+#if LWIP_MDNS_SEARCH
+    if (!req) { /* Try hard to search matching request */
+      req = mdns_loockup_request(&ans.info);
+    }
+    if (req && req->result_fn) {
+      int flags = (first ? MDNS_SEARCH_RESULT_FIRST : 0) |
+          (!pkt->answers_left ? MDNS_SEARCH_RESULT_LAST : 0);
+      u16_t offset;
+      struct pbuf *p = pbuf_skip(pkt->pbuf, ans.rd_offset, &offset);
+      req->result_fn(&ans, (const char *)p->payload + offset, ans.rd_length, flags, req->arg);
+      first = 0;
+    }
+#endif
   }
 }
 
@@ -2084,6 +2212,77 @@ mdns_resp_add_service_txtitem(struct mdns_service *service, const char *txt, u8_
   return mdns_domain_add_label(&service->txtdata, txt, txt_len);
 }
 
+#if LWIP_MDNS_SEARCH
+/**
+ * @ingroup mdns
+ * Stop a search request.
+ * @param req The search request to stop
+ */
+void
+mdns_search_stop(s8_t request_id)
+{
+  LWIP_ASSERT("mdns_search_stop: bad request_id", (request_id >= 0) && (request_id < MDNS_MAX_REQUESTS));
+  struct mdns_request *req = &mdns_requests[request_id];
+  if (req && req->result_fn)
+    req->result_fn = NULL;
+}
+
+/**
+ * @ingroup mdns
+ * Search a specific service on the network.
+ * @param name The name of the service
+ * @param service The service type, like "_http"
+ * @param proto The service protocol, DNSSD_PROTO_TCP for TCP ("_tcp") and DNSSD_PROTO_UDP
+ *              for others ("_udp")
+ * @param netif The network interface where to send search request
+ * @param result_fn Callback to send answer received. Will be called for each answer of a
+ *                  responce frame matching request sent
+ * @param arg Userdata pointer for result_fn
+ * @param request_id Returned request identifier to allow stop it.
+ * @return ERR_OK if the search request was created and sent, an err_t otherwise
+ */
+err_t
+mdns_search_service(const char *name, const char *service, enum mdns_sd_proto proto,
+                    struct netif *netif, search_result_fn_t result_fn, void *arg,
+                    s8_t *request_id)
+{
+  int i;
+  s8_t slot = -1;
+  struct mdns_request *req;
+  if (name)
+    LWIP_ERROR("mdns_search_service: Name too long", (strlen(name) <= MDNS_LABEL_MAXLEN), return ERR_VAL);
+  LWIP_ERROR("mdns_search_service: Service too long", (strlen(service) <= MDNS_LABEL_MAXLEN), return ERR_VAL);
+  LWIP_ERROR("mdns_search_service: Bad reqid pointer", request_id, return ERR_VAL);
+  LWIP_ERROR("mdns_search_service: Bad proto (need TCP or UDP)", (proto == DNSSD_PROTO_TCP || proto == DNSSD_PROTO_UDP), return ERR_VAL);
+  for (i = 0; i < MDNS_MAX_REQUESTS; i++) {
+    if (mdns_requests[i].result_fn == NULL) {
+      slot = i;
+      break;
+    }
+  }
+  LWIP_ERROR("mdns_search_service: Too many requests (increase MDNS_MAX_REQUESTS)", (slot >= 0), return ERR_MEM);
+
+  req = &mdns_requests[slot];
+  memset(req, 0, sizeof(struct mdns_request));
+  req->result_fn = result_fn;
+  req->arg = arg;
+  req->proto = (u16_t)proto;
+  MEMCPY(&req->service, service, LWIP_MIN(MDNS_LABEL_MAXLEN, strlen(service)));
+  if (name)
+    MEMCPY(&req->name, name, LWIP_MIN(MDNS_LABEL_MAXLEN, strlen(name)));
+  /* save request id (slot) in pointer provided by caller */
+  *request_id = slot;
+  /* now prepare a MDNS request and send it (on specified interface) */
+#if LWIP_IPV6
+  mdns_search(req, netif, IP6_ADDR_ANY);
+#endif
+#if LWIP_IPV4
+  mdns_search(req, netif, IP4_ADDR_ANY);
+#endif
+  return ERR_OK;
+}
+#endif
+
 /**
  * @ingroup mdns
  * Initiate MDNS responder. Will open UDP sockets on port 5353
@@ -2092,7 +2291,9 @@ void
 mdns_resp_init(void)
 {
   err_t res;
-
+#if LWIP_MDNS_SEARCH
+  memset(mdns_requests, 0, sizeof(mdns_requests));
+#endif
   mdns_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);
   LWIP_ASSERT("Failed to allocate pcb", mdns_pcb != NULL);
 #if LWIP_MULTICAST_TX_OPTIONS
diff --git a/src/include/lwip/apps/mdns.h b/src/include/lwip/apps/mdns.h
index f5c6ed82..59b5ed48 100644
--- a/src/include/lwip/apps/mdns.h
+++ b/src/include/lwip/apps/mdns.h
@@ -53,6 +53,40 @@ enum mdns_sd_proto {
 struct mdns_host;
 struct mdns_service;
 
+/* Domain struct and methods */
+
+#define MDNS_DOMAIN_MAXLEN 256
+
+struct mdns_domain {
+  /* Encoded domain name */
+  u8_t name[MDNS_DOMAIN_MAXLEN];
+  /* Total length of domain name, including zero */
+  u16_t length;
+  /* Set if compression of this domain is not allowed */
+  u8_t skip_compression;
+};
+
+/** Domain, type and class.
+ *  Shared between questions and answers */
+struct mdns_rr_info {
+  struct mdns_domain domain;
+  u16_t type;
+  u16_t klass;
+};
+
+struct mdns_answer {
+  struct mdns_rr_info info;
+  /** cache flush command bit */
+  u16_t cache_flush;
+  /* Validity time in seconds */
+  u32_t ttl;
+  /** Length of variable answer */
+  u16_t rd_length;
+  /** Offset of start of variable answer in packet */
+  u16_t rd_offset;
+};
+
+
 /** Callback function to add text to a reply, called when generating the reply */
 typedef void (*service_get_txt_fn_t)(struct mdns_service *service, void *txt_userdata);
 
@@ -68,6 +102,19 @@ err_t mdns_resp_add_service_txtitem(struct mdns_service *service, const char *tx
 
 void mdns_resp_netif_settings_changed(struct netif *netif);
 
+#if LWIP_MDNS_SEARCH
+typedef void (*search_result_fn_t)(struct mdns_answer *answer, const char *varpart, int varlen, int flags, void *arg);
+/* flags bits, both can be set! */
+#define MDNS_SEARCH_RESULT_FIRST    1 /* First answer in received frame. */
+#define MDNS_SEARCH_RESULT_LAST     2 /* Last answer. */
+
+err_t mdns_search_service(const char *name, const char *service, enum mdns_sd_proto proto,
+                          struct netif *netif, search_result_fn_t result_fn, void *arg,
+                          s8_t *request_id);
+void mdns_search_stop(s8_t request_id);
+
+#endif /* LWIP_MDNS_SEARCH */
+
 #endif /* LWIP_MDNS_RESPONDER */
 
 #endif /* LWIP_HDR_APPS_MDNS_H */
diff --git a/src/include/lwip/apps/mdns_opts.h b/src/include/lwip/apps/mdns_opts.h
index bf186bcc..b8297451 100644
--- a/src/include/lwip/apps/mdns_opts.h
+++ b/src/include/lwip/apps/mdns_opts.h
@@ -60,6 +60,18 @@
 #endif
 
 /**
+ * LWIP_MDNS_SEARCH==1: Turn on search over multicast DNS module.
+ */
+#ifndef LWIP_MDNS_SEARCH
+#define LWIP_MDNS_SEARCH                1
+#endif
+
+/** The maximum number of running requests */
+#ifndef MDNS_MAX_REQUESTS
+#define MDNS_MAX_REQUESTS               2
+#endif
+
+/**
  * MDNS_DEBUG: Enable debugging for multicast DNS.
  */
 #ifndef MDNS_DEBUG
diff --git a/src/include/lwip/apps/mdns_priv.h b/src/include/lwip/apps/mdns_priv.h
index 8ee6db86..d52e01c3 100644
--- a/src/include/lwip/apps/mdns_priv.h
+++ b/src/include/lwip/apps/mdns_priv.h
@@ -42,25 +42,29 @@
 
 #if LWIP_MDNS_RESPONDER
 
-/* Domain struct and methods - visible for unit tests */
-
-#define MDNS_DOMAIN_MAXLEN 256
+/* Domain methods - visible for unit tests */
 #define MDNS_READNAME_ERROR 0xFFFF
 
-struct mdns_domain {
-  /* Encoded domain name */
-  u8_t name[MDNS_DOMAIN_MAXLEN];
-  /* Total length of domain name, including zero */
-  u16_t length;
-  /* Set if compression of this domain is not allowed */
-  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);
 
+
+#if LWIP_MDNS_SEARCH
+struct mdns_request {
+  /** Name of service, like 'myweb' */
+  char name[MDNS_LABEL_MAXLEN + 1];
+  /** Type of service, like '_http' */
+  char service[MDNS_LABEL_MAXLEN + 1];
+  /** Callback function called for each response */
+  search_result_fn_t result_fn;
+  void *arg;
+  /** Protocol, TCP or UDP */
+  u16_t proto;
+};
+#endif
+
 #endif /* LWIP_MDNS_RESPONDER */
 
 #endif /* LWIP_HDR_MDNS_PRIV_H */
-- 
2.11.0

