ldpd: implement RFC 5561 (LDP Capabilities)

[mirror_frr.git] / ldpd / notification.c

diff --git a/ldpd/notification.c b/ldpd/notification.c
index d573925314b603d892e892e5754716a3e4c7085e..69d4ab8028d50683bc372313aba6fd21b2238f7f 100644 (file)
--- a/ldpd/notification.c
+++ b/ldpd/notification.c
@@ -24,6 +24,7 @@
 #include "ldpe.h"
 #include "ldp_debug.h"
 
+static int      gen_returned_tlvs(struct ibuf *, uint16_t, uint16_t, char *);
 static void     log_msg_notification(int, struct nbr *, struct notify_msg *);
 
 void
@@ -47,6 +48,8 @@ send_notification_full(struct tcp_conn *tcp, struct notify_msg *nm)
                        break;
                }
        }
+       if (nm->flags & F_NOTIF_RETURNED_TLVS)
+               size += TLV_HDR_SIZE * 2 + nm->rtlvs.length;
 
        if ((buf = ibuf_open(size)) == NULL)
                fatal(__func__);
@@ -60,6 +63,9 @@ send_notification_full(struct tcp_conn *tcp, struct notify_msg *nm)
                err |= gen_pw_status_tlv(buf, nm->pw_status);
        if (nm->flags & F_NOTIF_FEC)
                err |= gen_fec_tlv(buf, &nm->fec);
+       if (nm->flags & F_NOTIF_RETURNED_TLVS)
+               err |= gen_returned_tlvs(buf, nm->rtlvs.type, nm->rtlvs.length,
+                   nm->rtlvs.data);
        if (err) {
                ibuf_free(buf);
                return;
@@ -88,6 +94,27 @@ send_notification(struct tcp_conn *tcp, uint32_t status_code, uint32_t msg_id,
        send_notification_full(tcp, &nm);
 }
 
+void
+send_notification_rtlvs(struct nbr *nbr, uint32_t status_code, uint32_t msg_id,
+    uint16_t msg_type, uint16_t tlv_type, uint16_t tlv_len, char *tlv_data)
+{
+       struct notify_msg        nm;
+
+       memset(&nm, 0, sizeof(nm));
+       nm.status_code = status_code;
+       nm.msg_id = msg_id;
+       nm.msg_type = msg_type;
+       /* do not append the given TLV if it's too big (shouldn't happen) */
+       if (tlv_len < 1024) {
+               nm.rtlvs.type = tlv_type;
+               nm.rtlvs.length = tlv_len;
+               nm.rtlvs.data = tlv_data;
+               nm.flags |= F_NOTIF_RETURNED_TLVS;
+       }
+
+       send_notification_full(nbr->tcp, &nm);
+}
+
 int
 recv_notification(struct nbr *nbr, char *buf, uint16_t len)
 {
@@ -120,6 +147,7 @@ recv_notification(struct nbr *nbr, char *buf, uint16_t len)
        /* Optional Parameters */
        while (len > 0) {
                struct tlv      tlv;
+               uint16_t        tlv_type;
                uint16_t        tlv_len;
 
                if (len < sizeof(tlv)) {
@@ -128,6 +156,7 @@ recv_notification(struct nbr *nbr, char *buf, uint16_t len)
                }
 
                memcpy(&tlv, buf, TLV_HDR_SIZE);
+               tlv_type = ntohs(tlv.type);
                tlv_len = ntohs(tlv.length);
                if (tlv_len + TLV_HDR_SIZE > len) {
                        session_shutdown(nbr, S_BAD_TLV_LEN, msg.id, msg.type);
@@ -136,7 +165,7 @@ recv_notification(struct nbr *nbr, char *buf, uint16_t len)
                buf += TLV_HDR_SIZE;
                len -= TLV_HDR_SIZE;
 
-               switch (ntohs(tlv.type)) {
+               switch (tlv_type) {
                case TLV_TYPE_EXTSTATUS:
                case TLV_TYPE_RETURNEDPDU:
                case TLV_TYPE_RETURNEDMSG:
@@ -166,8 +195,8 @@ recv_notification(struct nbr *nbr, char *buf, uint16_t len)
                        break;
                default:
                        if (!(ntohs(tlv.type) & UNKNOWN_FLAG))
-                               send_notification(nbr->tcp, S_UNKNOWN_TLV,
-                                   msg.id, msg.type);
+                               send_notification_rtlvs(nbr, S_UNKNOWN_TLV,
+                                   msg.id, msg.type, tlv_type, tlv_len, buf);
                        /* ignore unknown tlv */
                        break;
                }
@@ -229,6 +258,26 @@ gen_status_tlv(struct ibuf *buf, uint32_t status_code, uint32_t msg_id,
        return (ibuf_add(buf, &st, STATUS_SIZE));
 }
 
+static int
+gen_returned_tlvs(struct ibuf *buf, uint16_t type, uint16_t length,
+    char *tlv_data)
+{
+       struct tlv       rtlvs;
+       struct tlv       tlv;
+       int              err;
+
+       rtlvs.type = htons(TLV_TYPE_RETURNED_TLVS);
+       rtlvs.length = htons(length + TLV_HDR_SIZE);
+       tlv.type = htons(type);
+       tlv.length = htons(length);
+
+       err = ibuf_add(buf, &rtlvs, sizeof(rtlvs));
+       err |= ibuf_add(buf, &tlv, sizeof(tlv));
+       err |= ibuf_add(buf, tlv_data, length);
+
+       return (err);
+}
+
 void
 log_msg_notification(int out, struct nbr *nbr, struct notify_msg *nm)
 {