Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2011 The Android Open Source Project |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | */ |
| 16 | |
| 17 | /* NOTICE: This is a clean room re-implementation of libnl */ |
| 18 | |
| 19 | #include <errno.h> |
| 20 | #include "netlink/netlink.h" |
| 21 | #include "netlink/msg.h" |
| 22 | #include "netlink/attr.h" |
| 23 | #include "netlink-types.h" |
| 24 | |
| 25 | /* Return payload of string attribute. */ |
| 26 | char *nla_get_string(struct nlattr *nla) |
| 27 | { |
| 28 | return (char *) nla_data(nla); |
| 29 | } |
| 30 | |
| 31 | /* Return payload of 16 bit integer attribute. */ |
| 32 | uint16_t nla_get_u16(struct nlattr *nla) |
| 33 | { |
| 34 | return *((uint16_t *) nla_data(nla)); |
| 35 | } |
| 36 | |
| 37 | /* Return payload of 32 bit integer attribute. */ |
| 38 | uint32_t nla_get_u32(struct nlattr *nla) |
| 39 | { |
| 40 | return *((uint32_t *) nla_data(nla)); |
| 41 | } |
| 42 | |
| 43 | /* Return value of 8 bit integer attribute. */ |
| 44 | uint8_t nla_get_u8(struct nlattr *nla) |
| 45 | { |
| 46 | return *((uint8_t *) nla_data(nla)); |
| 47 | } |
| 48 | |
| 49 | /* Return payload of uint64_t attribute. */ |
| 50 | uint64_t nla_get_u64(struct nlattr *nla) |
| 51 | { |
| 52 | uint64_t tmp; |
| 53 | nla_memcpy(&tmp, nla, sizeof(tmp)); |
| 54 | return tmp; |
| 55 | } |
| 56 | |
| 57 | /* Head of payload */ |
| 58 | void *nla_data(const struct nlattr *nla) |
| 59 | { |
| 60 | return (void *) ((char *) nla + NLA_HDRLEN); |
| 61 | } |
| 62 | |
| 63 | /* Return length of the payload . */ |
| 64 | int nla_len(const struct nlattr *nla) |
| 65 | { |
| 66 | return nla->nla_len - NLA_HDRLEN; |
| 67 | } |
| 68 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 69 | int nla_padlen(int payload) |
| 70 | { |
| 71 | return NLA_ALIGN(payload) - payload; |
| 72 | } |
| 73 | |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 74 | /* Start a new level of nested attributes. */ |
| 75 | struct nlattr *nla_nest_start(struct nl_msg *msg, int attrtype) |
| 76 | { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 77 | struct nlattr *start = (struct nlattr *)nlmsg_tail(msg->nm_nlh); |
| 78 | int rc; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 79 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 80 | rc = nla_put(msg, attrtype, 0, NULL); |
| 81 | if (rc < 0) |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 82 | return NULL; |
| 83 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 84 | return start; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 85 | } |
| 86 | |
| 87 | /* Finalize nesting of attributes. */ |
| 88 | int nla_nest_end(struct nl_msg *msg, struct nlattr *start) |
| 89 | { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 90 | /* Set attribute size */ |
| 91 | start->nla_len = (unsigned char *)nlmsg_tail(nlmsg_hdr(msg)) - |
| 92 | (unsigned char *)start; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 93 | return 0; |
| 94 | } |
| 95 | |
| 96 | /* Return next attribute in a stream of attributes. */ |
| 97 | struct nlattr *nla_next(const struct nlattr *nla, int *remaining) |
| 98 | { |
| 99 | struct nlattr *next_nla = NULL; |
| 100 | if (nla->nla_len >= sizeof(struct nlattr) && |
| 101 | nla->nla_len <= *remaining){ |
| 102 | next_nla = (struct nlattr *) \ |
| 103 | ((char *) nla + NLA_ALIGN(nla->nla_len)); |
| 104 | *remaining = *remaining - NLA_ALIGN(nla->nla_len); |
| 105 | } |
| 106 | |
| 107 | return next_nla; |
| 108 | |
| 109 | } |
| 110 | |
| 111 | /* Check if the attribute header and payload can be accessed safely. */ |
| 112 | int nla_ok(const struct nlattr *nla, int remaining) |
| 113 | { |
| 114 | return remaining > 0 && |
| 115 | nla->nla_len >= sizeof(struct nlattr) && |
| 116 | sizeof(struct nlattr) <= (unsigned int) remaining && |
| 117 | nla->nla_len <= remaining; |
| 118 | } |
| 119 | |
| 120 | /* Create attribute index based on a stream of attributes. */ |
| 121 | /* NOTE: Policy not used ! */ |
| 122 | int nla_parse(struct nlattr *tb[], int maxtype, struct nlattr *head, |
| 123 | int len, struct nla_policy *policy) |
| 124 | { |
| 125 | struct nlattr *pos; |
| 126 | int rem; |
| 127 | |
| 128 | /* First clear table */ |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 129 | memset(tb, 0, (maxtype + 1) * sizeof(struct nlattr *)); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 130 | |
| 131 | nla_for_each_attr(pos, head, len, rem) { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 132 | int type = nla_type(pos); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 133 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 134 | if ((type <= maxtype) && (type != 0)) |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 135 | tb[type] = pos; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 136 | } |
| 137 | |
| 138 | return 0; |
| 139 | } |
| 140 | |
| 141 | |
| 142 | /* Create attribute index based on nested attribute. */ |
| 143 | int nla_parse_nested(struct nlattr *tb[], int maxtype, |
| 144 | struct nlattr *nla, struct nla_policy *policy) |
| 145 | { |
| 146 | return nla_parse(tb, maxtype, nla_data(nla), nla_len(nla), policy); |
| 147 | } |
| 148 | |
| 149 | |
| 150 | /* Add a unspecific attribute to netlink message. */ |
| 151 | int nla_put(struct nl_msg *msg, int attrtype, int datalen, const void *data) |
| 152 | { |
| 153 | struct nlattr *nla; |
| 154 | |
| 155 | /* Reserve space and init nla header */ |
| 156 | nla = nla_reserve(msg, attrtype, datalen); |
Frank Maker | 1b53483 | 2011-06-30 17:00:49 -0700 | [diff] [blame] | 157 | if (nla) { |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 158 | memcpy(nla_data(nla), data, datalen); |
Frank Maker | 1b53483 | 2011-06-30 17:00:49 -0700 | [diff] [blame] | 159 | return 0; |
| 160 | } |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 161 | |
Frank Maker | 1b53483 | 2011-06-30 17:00:49 -0700 | [diff] [blame] | 162 | return -EINVAL; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 163 | |
| 164 | } |
| 165 | |
| 166 | |
| 167 | /* Add nested attributes to netlink message. */ |
| 168 | /* Takes the attributes found in the nested message and appends them |
| 169 | * to the message msg nested in a container of the type attrtype. The |
| 170 | * nested message may not have a family specific header */ |
| 171 | int nla_put_nested(struct nl_msg *msg, int attrtype, struct nl_msg *nested) |
| 172 | { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 173 | int rc; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 174 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 175 | rc = nla_put(msg, attrtype, nlmsg_attrlen(nlmsg_hdr(nested), 0), |
| 176 | nlmsg_attrdata(nlmsg_hdr(nested), 0)); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 177 | return rc; |
| 178 | |
| 179 | } |
| 180 | |
| 181 | /* Return type of the attribute. */ |
| 182 | int nla_type(const struct nlattr *nla) |
| 183 | { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 184 | return (int)nla->nla_type & NLA_TYPE_MASK; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 185 | } |
| 186 | |
| 187 | /* Reserves room for an attribute in specified netlink message and fills |
| 188 | * in the attribute header (type,length). Return NULL if insufficient space */ |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 189 | struct nlattr *nla_reserve(struct nl_msg *msg, int attrtype, int data_len) |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 190 | { |
| 191 | |
| 192 | struct nlattr *nla; |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 193 | const unsigned int NEW_SIZE = NLMSG_ALIGN(msg->nm_nlh->nlmsg_len) + |
| 194 | NLA_ALIGN(NLA_HDRLEN + data_len); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 195 | |
| 196 | /* Check enough space for attribute */ |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 197 | if (NEW_SIZE > msg->nm_size) |
| 198 | return NULL; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 199 | |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 200 | nla = (struct nlattr *)nlmsg_tail(msg->nm_nlh); |
| 201 | nla->nla_type = attrtype; |
| 202 | nla->nla_len = NLA_HDRLEN + data_len; |
| 203 | memset((unsigned char *)nla + nla->nla_len, 0, nla_padlen(data_len)); |
| 204 | msg->nm_nlh->nlmsg_len = NEW_SIZE; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 205 | return nla; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 206 | } |
| 207 | |
| 208 | /* Copy attribute payload to another memory area. */ |
| 209 | int nla_memcpy(void *dest, struct nlattr *src, int count) |
| 210 | { |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 211 | if (!src || !dest) |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 212 | return 0; |
Dmitry Shmidt | 64f17e8 | 2011-08-10 15:25:51 -0700 | [diff] [blame] | 213 | if (count > nla_len(src)) |
| 214 | count = nla_len(src); |
| 215 | memcpy(dest, nla_data(src), count); |
| 216 | return count; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 217 | } |