bgpd/pimd: fix zAPI parsing

Commit c99f3481a598 has changed the API. Now, the vrfid has been added in
the header, thus we must read it before parsing the rest of the message.

To ease code maintenance, let's add a new function to read a zAPI header.

Fixes: c99f3481a598 ("*: add VRF ID in the API message header")
Reported-by: Martin Winter <mwinter@opensourcerouting.org>
Signed-off-by: Nicolas Dichtel <nicolas.dichtel@6wind.com>
Acked-by: Donald Sharp <sharpd@cumulusnetworks.com>
Tested-by: Martin Winter <mwinter@opensourcerouting.org>
diff --git a/bgpd/bgp_nexthop.c b/bgpd/bgp_nexthop.c
index 7336793..c74beba 100644
--- a/bgpd/bgp_nexthop.c
+++ b/bgpd/bgp_nexthop.c
@@ -789,8 +789,9 @@
   uint16_t length;
   u_char marker;
   u_char version;
-  uint16_t command __attribute__((unused));
-  int nbytes __attribute__((unused));
+  uint16_t vrf_id;
+  uint16_t command;
+  int err;
   struct in_addr raddr __attribute__((unused));
   uint32_t metric;
   int i;
@@ -801,14 +802,13 @@
   s = zlookup->ibuf;
   stream_reset (s);
 
-  /* nbytes not being checked */
-  nbytes = stream_read (s, zlookup->sock, 2);
-  length = stream_getw (s);
-
-  nbytes = stream_read (s, zlookup->sock, length - 2);
-  marker = stream_getc (s);
-  version = stream_getc (s);
-  
+  err = zclient_read_header (s, zlookup->sock, &length, &marker, &version,
+                             &vrf_id, &command);
+  if (err < 0)
+    {
+      zlog_err("%s: zserv_read_header() failed", __func__);
+      return NULL;
+    }
   if (version != ZSERV_VERSION || marker != ZEBRA_HEADER_MARKER)
     {
       zlog_err("%s: socket %d version mismatch, marker %d, version %d",
@@ -816,9 +816,6 @@
       return NULL;
     }
   
-  /* XXX: not checking command */
-  command = stream_getw (s);
-  
   /* XXX: not doing anything with raddr */
   raddr.s_addr = stream_get_ipv4 (s);
   metric = stream_getl (s);
@@ -902,11 +899,11 @@
 zlookup_read_ipv6 (void)
 {
   struct stream *s;
-  uint16_t length;
+  uint16_t length, vrf_id, cmd;
   u_char version, marker;
   struct in6_addr raddr;
   uint32_t metric;
-  int i;
+  int i, err;
   u_char nexthop_num;
   struct nexthop *nexthop;
   struct bgp_nexthop_cache *bnc;
@@ -914,14 +911,13 @@
   s = zlookup->ibuf;
   stream_reset (s);
 
-  /* XXX: ignoring nbytes, see also zread_lookup */
-  stream_read (s, zlookup->sock, 2);
-  length = stream_getw (s);
-
-  stream_read (s, zlookup->sock, length - 2);
-  marker = stream_getc (s);
-  version = stream_getc (s);
-  
+  err = zclient_read_header (s, zlookup->sock, &length, &marker, &version,
+                             &vrf_id, &cmd);
+  if (err < 0)
+    {
+      zlog_err("%s: zserv_read_header() failed", __func__);
+      return NULL;
+    }
   if (version != ZSERV_VERSION || marker != ZEBRA_HEADER_MARKER)
     {
       zlog_err("%s: socket %d version mismatch, marker %d, version %d",
@@ -929,9 +925,6 @@
       return NULL;
     }
   
-  /* XXX: ignoring command */  
-  stream_getw (s);
-  
   /* XXX: not actually doing anything with raddr */
   stream_get (&raddr, s, 16);
 
@@ -1018,9 +1011,8 @@
 {
   struct stream *s;
   int ret;
-  u_int16_t length, command __attribute__((unused));
+  u_int16_t length, vrf_id, command;
   u_char version, marker;
-  int nbytes __attribute__((unused));
   struct in_addr addr __attribute__((unused));
   struct in_addr nexthop;
   u_int32_t metric = 0;
@@ -1066,16 +1058,13 @@
   /* Get result. */
   stream_reset (s);
 
-  /* Fetch length. */
-  /* XXX: not using nbytes */
-  nbytes = stream_read (s, zlookup->sock, 2);
-  length = stream_getw (s);
-
-  /* Fetch whole data. */
-  nbytes = stream_read (s, zlookup->sock, length - 2);
-  marker = stream_getc (s);
-  version = stream_getc (s);
-
+  ret = zclient_read_header (s, zlookup->sock, &length, &marker, &version,
+                             &vrf_id, &command);
+  if (ret < 0)
+    {
+      zlog_err("%s: zserv_read_header() failed", __func__);
+      return 0;
+    }
   if (version != ZSERV_VERSION || marker != ZEBRA_HEADER_MARKER)
     {
       zlog_err("%s: socket %d version mismatch, marker %d, version %d",
@@ -1083,9 +1072,6 @@
       return 0;
     }
   
-  /* XXX: not using command */
-  command = stream_getw (s);
-  
   /* XXX: not using addr */
   addr.s_addr = stream_get_ipv4 (s);
   metric = stream_getl (s);
diff --git a/lib/zclient.c b/lib/zclient.c
index 8e443e2..a095632 100644
--- a/lib/zclient.c
+++ b/lib/zclient.c
@@ -302,6 +302,25 @@
   stream_putw (s, command);
 }
 
+int
+zclient_read_header (struct stream *s, int sock, u_int16_t *size, u_char *marker,
+                     u_char *version, u_int16_t *vrf_id, u_int16_t *cmd)
+{
+  if (stream_read (s, sock, ZEBRA_HEADER_SIZE) != ZEBRA_HEADER_SIZE)
+    return -1;
+
+  *size = stream_getw (s) - ZEBRA_HEADER_SIZE;
+  *marker = stream_getc (s);
+  *version = stream_getc (s);
+  *vrf_id = stream_getw (s);
+  *cmd = stream_getw (s);
+
+  if (*size && stream_read (s, sock, *size) != *size)
+    return -1;
+
+  return 0;
+}
+
 /* Send simple Zebra message. */
 static int
 zebra_message_send (struct zclient *zclient, int command, vrf_id_t vrf_id)
diff --git a/lib/zclient.h b/lib/zclient.h
index 19b4f0e..3490b32 100644
--- a/lib/zclient.h
+++ b/lib/zclient.h
@@ -163,6 +163,9 @@
 
 /* create header for command, length to be filled in by user later */
 extern void zclient_create_header (struct stream *, uint16_t, vrf_id_t);
+extern int zclient_read_header (struct stream *s, int sock, u_int16_t *size,
+				u_char *marker, u_char *version,
+				u_int16_t *vrf_id, u_int16_t *cmd);
 
 extern struct interface *zebra_interface_add_read (struct stream *,
     vrf_id_t);
diff --git a/pimd/pim_zlookup.c b/pimd/pim_zlookup.c
index 6000367..fae8f81 100644
--- a/pimd/pim_zlookup.c
+++ b/pimd/pim_zlookup.c
@@ -149,17 +149,18 @@
 {
   int num_ifindex = 0;
   struct stream *s;
-  const uint16_t MIN_LEN = 14; /* getc=1 getc=1 getw=2 getipv4=4 getc=1 getl=4 getc=1 */
-  uint16_t length, len;
+  const uint16_t MIN_LEN = 10; /* getipv4=4 getc=1 getl=4 getc=1 */
+  uint16_t length;
   u_char marker;
   u_char version;
+  uint16_t vrf_id;
   uint16_t command;
   int nbytes;
   struct in_addr raddr;
   uint8_t distance;
   uint32_t metric;
   int nexthop_num;
-  int i;
+  int i, err;
 
   if (PIM_DEBUG_ZEBRA) {
     char addr_str[100];
@@ -172,41 +173,37 @@
   s = zlookup->ibuf;
   stream_reset(s);
 
-  nbytes = stream_read(s, zlookup->sock, 2);
-  if (nbytes < 2) {
-    zlog_err("%s %s: failure reading zclient lookup socket: nbytes=%d",
-	     __FILE__, __PRETTY_FUNCTION__, nbytes);
+  err = zclient_read_header (s, zlookup->sock, &length, &marker, &version,
+                             &vrf_id, &command);
+  if (err < 0) {
+    zlog_err("%s %s: zclient_read_header() failed",
+	     __FILE__, __PRETTY_FUNCTION__);
     zclient_lookup_failed(zlookup);
     return -1;
   }
-  length = stream_getw(s);
 
-  len = length - 2;
-
-  if (len < MIN_LEN) {
+  if (length < MIN_LEN) {
     zlog_err("%s %s: failure reading zclient lookup socket: len=%d < MIN_LEN=%d",
-	     __FILE__, __PRETTY_FUNCTION__, len, MIN_LEN);
+	     __FILE__, __PRETTY_FUNCTION__, length, MIN_LEN);
     zclient_lookup_failed(zlookup);
     return -2;
   }
-
-  nbytes = stream_read(s, zlookup->sock, len);
-  if (nbytes < (length - 2)) {
+  
+  nbytes = stream_read(s, zlookup->sock, length);
+  if (nbytes < length) {
     zlog_err("%s %s: failure reading zclient lookup socket: nbytes=%d < len=%d",
-	     __FILE__, __PRETTY_FUNCTION__, nbytes, len);
+	     __FILE__, __PRETTY_FUNCTION__, nbytes, length);
     zclient_lookup_failed(zlookup);
     return -3;
   }
-  marker = stream_getc(s);
-  version = stream_getc(s);
   
   if (version != ZSERV_VERSION || marker != ZEBRA_HEADER_MARKER) {
     zlog_err("%s: socket %d version mismatch, marker %d, version %d",
 	     __func__, zlookup->sock, marker, version);
+    zclient_lookup_failed(zlookup);
     return -4;
   }
     
-  command = stream_getw(s);
   if (command != ZEBRA_IPV4_NEXTHOP_LOOKUP_MRIB) {
     zlog_err("%s: socket %d command mismatch: %d",
             __func__, zlookup->sock, command);
@@ -236,19 +233,19 @@
     return -6;
   }
 
-  len -= MIN_LEN;
+  length -= MIN_LEN;
 
   for (i = 0; i < nexthop_num; ++i) {
     enum nexthop_types_t nexthop_type;
 
-    if (len < 1) {
+    if (length < 1) {
       zlog_err("%s: socket %d empty input expecting nexthop_type: len=%d",
-	       __func__, zlookup->sock, len);
+	       __func__, zlookup->sock, length);
       return -7;
     }
     
     nexthop_type = stream_getc(s);
-    --len;
+    --length;
 
     switch (nexthop_type) {
     case ZEBRA_NEXTHOP_IFINDEX:
@@ -263,13 +260,13 @@
 	return num_ifindex;
       }
       if (nexthop_type == ZEBRA_NEXTHOP_IPV4_IFINDEX) {
-	if (len < 4) {
+	if (length < 4) {
 	  zlog_err("%s: socket %d short input expecting nexthop IPv4-addr: len=%d",
-		   __func__, zlookup->sock, len);
+		   __func__, zlookup->sock, length);
 	  return -8;
 	}
 	nexthop_tab[num_ifindex].nexthop_addr.s_addr = stream_get_ipv4(s);
-	len -= 4;
+	length -= 4;
       }
       else {
 	nexthop_tab[num_ifindex].nexthop_addr.s_addr = PIM_NET_INADDR_ANY;
@@ -289,7 +286,7 @@
 	return num_ifindex;
       }
       nexthop_tab[num_ifindex].nexthop_addr.s_addr = stream_get_ipv4(s);
-      len -= 4;
+      length -= 4;
       nexthop_tab[num_ifindex].ifindex             = 0;
       nexthop_tab[num_ifindex].protocol_distance   = distance;
       nexthop_tab[num_ifindex].route_metric        = metric;