netlink: add basic message writing support to snl(3).

Differential Revision:	https://reviews.freebsd.org/D38947
MFC after:	2 weeks

2 files changed, 360 insertions, 0 deletions

diff --git a/sys/netlink/netlink_snl.h b/sys/netlink/netlink_snl.h
index 3c63e3f95419..19b3e2f985f3 100644
--- a/sys/netlink/netlink_snl.h
+++ b/sys/netlink/netlink_snl.h
@@ -124,6 +124,7 @@ struct snl_state {
 	struct linear_buffer *lb;
 };
 #define	SCRATCH_BUFFER_SIZE	1024
+#define	SNL_WRITER_BUFFER_SIZE	256
 
 typedef void snl_parse_field_f(struct snl_state *ss, void *hdr, void *target);
 struct snl_field_parser {
@@ -294,6 +295,30 @@ snl_read_message(struct snl_state *ss)
 	return (hdr);
 }
 
+static inline struct nlmsghdr *
+snl_read_reply(struct snl_state *ss, uint32_t nlmsg_seq)
+{
+	while (true) {
+		struct nlmsghdr *hdr = snl_read_message(ss);
+		if (hdr == NULL)
+			break;
+		if (hdr->nlmsg_seq == nlmsg_seq)
+			return (hdr);
+	}
+
+	return (NULL);
+}
+
+static inline struct nlmsghdr *
+snl_get_reply(struct snl_state *ss, struct nlmsghdr *hdr)
+{
+	uint32_t nlmsg_seq = hdr->nlmsg_seq;
+
+	if (snl_send(ss, hdr, hdr->nlmsg_len))
+		return (snl_read_reply(ss, nlmsg_seq));
+	return (NULL);
+}
+
 /*
  * Checks that attributes are sorted by attribute type.
  */
@@ -511,4 +536,303 @@ snl_field_get_uint32(struct snl_state *ss __unused, void *src, void *target)
 	*((uint32_t *)target) = *((uint32_t *)src);
 }
 
+struct snl_errmsg_data {
+	uint32_t	nlmsg_seq;
+	int		error;
+	char		*error_str;
+	uint32_t	error_offs;
+	struct nlattr	*cookie;
+};
+#define	_IN(_field)	offsetof(struct nlmsgerr, _field)
+#define	_OUT(_field)	offsetof(struct snl_errmsg_data, _field)
+static const struct snl_attr_parser nla_p_errmsg[] = {
+	{ .type = NLMSGERR_ATTR_MSG, .off = _OUT(error_str), .cb = snl_attr_get_string },
+	{ .type = NLMSGERR_ATTR_OFFS, .off = _OUT(error_offs), .cb = snl_attr_get_uint32 },
+	{ .type = NLMSGERR_ATTR_COOKIE, .off = _OUT(cookie), .cb = snl_attr_get_nla },
+};
+
+static const struct snl_field_parser nlf_p_errmsg[] = {
+	{ .off_in = _IN(error), .off_out = _OUT(error), .cb = snl_field_get_uint32 },
+	{ .off_in = _IN(msg.nlmsg_seq), .off_out = _OUT(nlmsg_seq), .cb = snl_field_get_uint32 },
+};
+#undef _IN
+#undef _OUT
+SNL_DECLARE_PARSER(snl_errmsg_parser, struct nlmsgerr, nlf_p_errmsg, nla_p_errmsg);
+
+static inline bool
+snl_check_return(struct snl_state *ss, struct nlmsghdr *hdr, struct snl_errmsg_data *e)
+{
+	if (hdr != NULL && hdr->nlmsg_type == NLMSG_ERROR)
+		return (snl_parse_nlmsg(ss, hdr, &snl_errmsg_parser, e));
+	return (false);
+}
+
+/* writer logic */
+struct snl_writer {
+	char			*base;
+	uint32_t		offset;
+	uint32_t		size;
+	struct nlmsghdr		*hdr;
+	struct snl_state	*ss;
+	bool			error;
+};
+
+static inline void
+snl_init_writer(struct snl_state *ss, struct snl_writer *nw)
+{
+	nw->size = SNL_WRITER_BUFFER_SIZE;
+	nw->base = snl_allocz(ss, nw->size);
+	if (nw->base == NULL) {
+		nw->error = true;
+		nw->size = 0;
+	}
+
+	nw->offset = 0;
+	nw->hdr = NULL;
+	nw->error = false;
+	nw->ss = ss;
+}
+
+static inline bool
+snl_realloc_msg_buffer(struct snl_writer *nw, size_t sz)
+{
+	uint32_t new_size = nw->size * 2;
+
+	while (new_size < nw->size + sz)
+		new_size *= 2;
+
+	if (nw->error)
+		return (false);
+
+	void *new_base = snl_allocz(nw->ss, new_size);
+	if (new_base == NULL) {
+		nw->error = true;
+		return (false);
+	}
+
+	memcpy(new_base, nw->base, nw->offset);
+	if (nw->hdr != NULL) {
+		int hdr_off = (char *)(nw->hdr) - nw->base;
+		nw->hdr = (struct nlmsghdr *)(void *)((char *)new_base + hdr_off);
+	}
+	nw->base = new_base;
+
+	return (true);
+}
+
+static inline void *
+snl_reserve_msg_data_raw(struct snl_writer *nw, size_t sz)
+{
+	sz = NETLINK_ALIGN(sz);
+
+        if (__predict_false(nw->offset + sz > nw->size)) {
+		if (!snl_realloc_msg_buffer(nw, sz))
+			return (NULL);
+        }
+
+        void *data_ptr = &nw->base[nw->offset];
+        nw->offset += sz;
+
+        return (data_ptr);
+}
+#define snl_reserve_msg_object(_ns, _t)	((_t *)snl_reserve_msg_data_raw(_ns, sizeof(_t)))
+#define snl_reserve_msg_data(_ns, _sz, _t)	((_t *)snl_reserve_msg_data_raw(_ns, _sz))
+
+static inline void *
+_snl_reserve_msg_attr(struct snl_writer *nw, uint16_t nla_type, uint16_t sz)
+{
+	sz += sizeof(struct nlattr);
+
+	struct nlattr *nla = snl_reserve_msg_data(nw, sz, struct nlattr);
+	if (__predict_false(nla == NULL))
+		return (NULL);
+	nla->nla_type = nla_type;
+	nla->nla_len = sz;
+
+	return ((void *)(nla + 1));
+}
+#define	snl_reserve_msg_attr(_ns, _at, _t)	((_t *)_snl_reserve_msg_attr(_ns, _at, sizeof(_t)))
+
+static inline bool
+snl_add_msg_attr(struct snl_writer *nw, int attr_type, int attr_len, const void *data)
+{
+	int required_len = NLA_ALIGN(attr_len + sizeof(struct nlattr));
+
+        if (__predict_false(nw->offset + required_len > nw->size)) {
+		if (!snl_realloc_msg_buffer(nw, required_len))
+			return (false);
+	}
+
+        struct nlattr *nla = (struct nlattr *)(&nw->base[nw->offset]);
+
+        nla->nla_len = attr_len + sizeof(struct nlattr);
+        nla->nla_type = attr_type;
+        if (attr_len > 0) {
+		if ((attr_len % 4) != 0) {
+			/* clear padding bytes */
+			bzero((char *)nla + required_len - 4, 4);
+		}
+                memcpy((nla + 1), data, attr_len);
+	}
+        nw->offset += required_len;
+        return (true);
+}
+
+static inline bool
+snl_add_msg_attr_raw(struct snl_writer *nw, const struct nlattr *nla_src)
+{
+	int attr_len = nla_src->nla_len - sizeof(struct nlattr);
+
+	assert(attr_len >= 0);
+
+	return (snl_add_msg_attr(nw, nla_src->nla_type, attr_len, (const void *)(nla_src + 1)));
+}
+
+static inline bool
+snl_add_msg_attr_u8(struct snl_writer *nw, int attrtype, uint8_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(uint8_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_u16(struct snl_writer *nw, int attrtype, uint16_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(uint16_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_u32(struct snl_writer *nw, int attrtype, uint32_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(uint32_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_u64(struct snl_writer *nw, int attrtype, uint64_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(uint64_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_s8(struct snl_writer *nw, int attrtype, int8_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(int8_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_s16(struct snl_writer *nw, int attrtype, int16_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(int16_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_s32(struct snl_writer *nw, int attrtype, int32_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(int32_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_s64(struct snl_writer *nw, int attrtype, int64_t value)
+{
+	return (snl_add_msg_attr(nw, attrtype, sizeof(int64_t), &value));
+}
+
+static inline bool
+snl_add_msg_attr_flag(struct snl_writer *nw, int attrtype)
+{
+	return (snl_add_msg_attr(nw, attrtype, 0, NULL));
+}
+
+static inline bool
+snl_add_msg_attr_string(struct snl_writer *nw, int attrtype, const char *str)
+{
+	return (snl_add_msg_attr(nw, attrtype, strlen(str) + 1, str));
+}
+
+
+static inline int
+snl_get_msg_offset(const struct snl_writer *nw)
+{
+        return (nw->offset - ((char *)nw->hdr - nw->base));
+}
+
+static inline void *
+_snl_restore_msg_offset(const struct snl_writer *nw, int off)
+{
+	return ((void *)((char *)nw->hdr + off));
+}
+#define	snl_restore_msg_offset(_ns, _off, _t)	((_t *)_snl_restore_msg_offset(_ns, _off))
+
+static inline int
+snl_add_msg_attr_nested(struct snl_writer *nw, int attrtype)
+{
+	int off = snl_get_msg_offset(nw);
+	struct nlattr *nla = snl_reserve_msg_data(nw, sizeof(struct nlattr), struct nlattr);
+	if (__predict_false(nla == NULL))
+		return (0);
+	nla->nla_type = attrtype;
+	return (off);
+}
+
+static inline void
+snl_end_attr_nested(const struct snl_writer *nw, int off)
+{
+	if (!nw->error) {
+		struct nlattr *nla = snl_restore_msg_offset(nw, off, struct nlattr);
+		nla->nla_len = NETLINK_ALIGN(snl_get_msg_offset(nw) - off);
+	}
+}
+
+static inline struct nlmsghdr *
+snl_create_msg_request(struct snl_writer *nw, int nlmsg_type)
+{
+	assert(nw->hdr == NULL);
+
+	struct nlmsghdr *hdr = snl_reserve_msg_object(nw, struct nlmsghdr);
+	hdr->nlmsg_type = nlmsg_type;
+	hdr->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+	nw->hdr = hdr;
+
+	return (hdr);
+}
+
+static void
+snl_abort_msg(struct snl_writer *nw)
+{
+	if (nw->hdr != NULL) {
+		int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
+
+		nw->offset -= offset;
+		nw->hdr = NULL;
+	}
+}
+
+static inline struct nlmsghdr *
+snl_finalize_msg(struct snl_writer *nw)
+{
+	if (nw->error)
+		snl_abort_msg(nw);
+	if (nw->hdr != NULL) {
+		struct nlmsghdr *hdr = nw->hdr;
+
+		int offset = (char *)(&nw->base[nw->offset]) - (char *)(nw->hdr);
+		hdr->nlmsg_len = offset;
+		hdr->nlmsg_seq = snl_get_seq(nw->ss);
+		nw->hdr = NULL;
+
+		return (hdr);
+	}
+	return (NULL);
+}
+
+static bool
+snl_send_msgs(struct snl_writer *nw)
+{
+	int offset = nw->offset;
+
+	assert(nw->hdr == NULL);
+	nw->offset = 0;
+
+	return (snl_send(nw->ss, nw->base, offset));
+}
+
 #endif
diff --git a/sys/netlink/netlink_snl_route.h b/sys/netlink/netlink_snl_route.h
index 4adb3d697ecd..281794a9d6f7 100644
--- a/sys/netlink/netlink_snl_route.h
+++ b/sys/netlink/netlink_snl_route.h
@@ -127,4 +127,40 @@ snl_attr_get_ipvia(struct snl_state *ss, struct nlattr *nla,
 	return (false);
 }
 
+static inline bool
+snl_add_msg_attr_ip(struct snl_writer *nw, int attrtype, const struct sockaddr *sa)
+{
+	const void *addr;
+
+	switch (sa->sa_family) {
+	case AF_INET:
+		addr = &((const struct sockaddr_in *)sa)->sin_addr;
+		return (snl_add_msg_attr(nw, attrtype, 4, addr));
+	case AF_INET6:
+		addr = &((const struct sockaddr_in6 *)sa)->sin6_addr;
+		return (snl_add_msg_attr(nw, attrtype, 16, addr));
+	}
+
+	return (false);
+}
+
+static inline bool
+snl_add_msg_attr_ipvia(struct snl_writer *nw, int attrtype, const struct sockaddr *sa)
+{
+	char buf[17];
+
+	buf[0] = sa->sa_family;
+
+	switch (sa->sa_family) {
+	case AF_INET:
+		memcpy(&buf[1], &((const struct sockaddr_in *)sa)->sin_addr, 4);
+		return (snl_add_msg_attr(nw, attrtype, 5, buf));
+	case AF_INET6:
+		memcpy(&buf[1], &((const struct sockaddr_in6 *)sa)->sin6_addr, 16);
+		return (snl_add_msg_attr(nw, attrtype, 17, buf));
+	}
+
+	return (false);
+}
+
 #endif