netlink: add basic message writing support to snl(3).
Differential Revision: https://reviews.freebsd.org/D38947 MFC after: 2 weeks
This commit is contained in:
Alexander V. Chernikov
parent
ecad3f5c4d
commit
76f6d39150
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user