l2tp: Take StopCCN's AVPs into account

Use the Assigned Tunnel ID and Result Code AVPs when handling StopCCN
messages.
The Result Code AVP is used to improve log messages, while the Assigned
Tunnel ID is used to set the tunnel's peer-tid field.

Setting peer-tid makes the ZLB to be sent to the right tunnel in case
no Assigned Tunnel ID had been received yet (this happens when the host
sends an SCCRQ but the peer directly rejects the connection with
StopCCN).

Signed-off-by: Guillaume Nault <g.nault@alphalink.fr>

2 files changed, 98 insertions, 0 deletions

diff --git a/accel-pppd/ctrl/l2tp/l2tp.c b/accel-pppd/ctrl/l2tp/l2tp.c
index 85b4b9b..0fff3a0 100644
--- a/accel-pppd/ctrl/l2tp/l2tp.c
+++ b/accel-pppd/ctrl/l2tp/l2tp.c
@@ -1722,10 +1722,107 @@ static int l2tp_recv_SCCCN(struct l2tp_conn_t *conn,
 	return 0;
 }
 
+static int rescode_get_data(const struct l2tp_attr_t *result_attr,
+			    uint16_t *res, uint16_t *err, char **err_msg)
+{
+	struct l2tp_avp_result_code *resavp = NULL;
+	int msglen;
+
+	if (result_attr->length != 2 && result_attr->length < sizeof(*resavp))
+		return -1;
+
+	if (result_attr->length == 2) {
+		/* No Error Code */
+		*res = ntohs(*(const uint16_t *)result_attr->val.octets);
+		return 1;
+	}
+
+	resavp = (struct l2tp_avp_result_code *)result_attr->val.octets;
+	*res = ntohs(resavp->result_code);
+	*err = ntohs(resavp->error_code);
+	msglen = result_attr->length - sizeof(*resavp);
+	if (msglen <= 0)
+		return 2;
+
+	*err_msg = _malloc(msglen + 1);
+	if (err_msg) {
+		memcpy(*err_msg, resavp->error_msg, msglen);
+		(*err_msg)[msglen] = '\0';
+	}
+
+	return 3;
+}
+
 static int l2tp_recv_StopCCN(struct l2tp_conn_t *conn,
 			     const struct l2tp_packet_t *pack)
 {
+	const struct l2tp_attr_t *assigned_tid = NULL;
+	const struct l2tp_attr_t *result_code = NULL;
+	const struct l2tp_attr_t *attr = NULL;
+	char *err_msg = NULL;
+	uint16_t res = 0;
+	uint16_t err = 0;
+
+	list_for_each_entry(attr, &pack->attrs, entry) {
+		switch(attr->attr->id) {
+		case Message_Type:
+			break;
+		case Assigned_Tunnel_ID:
+			assigned_tid = attr;
+			break;
+		case Result_Code:
+			result_code = attr;
+			break;
+		default:
+			if (attr->M) {
+				l2tp_conn_log(log_error, conn);
+				log_error("StopCCN: unknown attribute %i\n",
+					  attr->attr->id);
+			}
+			break;
+		}
+	}
+
+	if (assigned_tid) {
+		if (conn->peer_tid == 0)
+			conn->peer_tid = assigned_tid->val.uint16;
+		else if (conn->peer_tid != assigned_tid->val.uint16) {
+			l2tp_conn_log(log_error, conn);
+			log_error("StopCCN: invalid Assigned Tunnel ID %hu"
+				  " (tid: %hu, peer tid: %hu)\n",
+				  assigned_tid->val.uint16,
+				  conn->tid,  conn->peer_tid);
+		}
+	} else {
+		l2tp_conn_log(log_error, conn);
+		log_error("StopCCN: missing mandatory AVP:"
+			  " Assigned Tunnel ID\n");
+	}
+
+	if (result_code) {
+		if (rescode_get_data(result_code, &res, &err, &err_msg) < 0) {
+			l2tp_conn_log(log_error, conn);
+			log_error("StopCCN: invalid Result Code\n");
+		}
+	} else {
+		l2tp_conn_log(log_error, conn);
+		log_error("StopCCN: missing mandatory AVP:"
+			  " Result Code\n");
+	}
+
+	l2tp_conn_log(log_info2, conn);
+	log_info2("Tunnel %hu/%hu closed by peer"
+		  " (result: %hu, error: %hu%s%s%s\n)\n",
+		  conn->tid, conn->peer_tid, res, err,
+		  (err_msg) ? ", message: \"" : "",
+		  (err_msg) ? err_msg : "",
+		  (err_msg) ? "\"": "");
+
+	if (err_msg)
+		_free(err_msg);
+
 	l2tp_send_ZLB(conn);
+
 	return -1;
 }
 
diff --git a/accel-pppd/ctrl/l2tp/l2tp_prot.h b/accel-pppd/ctrl/l2tp/l2tp_prot.h
index 1c1d9da..559b29c 100644
--- a/accel-pppd/ctrl/l2tp/l2tp_prot.h
+++ b/accel-pppd/ctrl/l2tp/l2tp_prot.h
@@ -49,6 +49,7 @@ struct l2tp_avp_result_code
 {
 	uint16_t result_code;
 	uint16_t error_code;
+	char error_msg[0];
 } __attribute__((packed));
 
 #endif