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 <string.h> |
| 21 | #include <unistd.h> |
| 22 | #include <fcntl.h> |
| 23 | #include <sys/socket.h> |
| 24 | #include "netlink-types.h" |
| 25 | |
| 26 | #define NL_BUFFER_SZ (32768U) |
| 27 | |
| 28 | /* Checks message for completeness and sends it out */ |
| 29 | int nl_send_auto_complete(struct nl_sock *sk, struct nl_msg *msg) |
| 30 | { |
| 31 | struct nlmsghdr *nlh = msg->nm_nlh; |
| 32 | struct timeval tv; |
| 33 | |
| 34 | if (!nlh) { |
| 35 | int errsv = errno; |
| 36 | fprintf(stderr, "Netlink message header is NULL!\n"); |
| 37 | return -errsv; |
| 38 | } |
| 39 | |
| 40 | /* Complete the nl_msg header */ |
| 41 | if (gettimeofday(&tv, NULL)) |
| 42 | nlh->nlmsg_seq = 1; |
| 43 | else |
| 44 | nlh->nlmsg_seq = (int) tv.tv_sec; |
| 45 | nlh->nlmsg_pid = sk->s_local.nl_pid; |
| 46 | nlh->nlmsg_flags |= NLM_F_REQUEST | NLM_F_ACK; |
| 47 | |
| 48 | return nl_send(sk, msg); |
| 49 | } |
| 50 | |
| 51 | /* Receives a netlink message, allocates a buffer in *buf and stores |
| 52 | * the message content. The peer's netlink address is stored in |
| 53 | * *nla. The caller is responsible for freeing the buffer allocated in |
| 54 | * *buf if a positive value is returned. Interrupted system calls are |
| 55 | * handled by repeating the read. The input buffer size is determined |
| 56 | * by peeking before the actual read is done */ |
| 57 | int nl_recv(struct nl_sock *sk, struct sockaddr_nl *nla, \ |
| 58 | unsigned char **buf, struct ucred **creds) |
| 59 | { |
| 60 | int rc = -1; |
| 61 | int sk_flags; |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 62 | int RECV_BUF_SIZE = getpagesize(); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 63 | int errsv; |
| 64 | struct iovec recvmsg_iov; |
| 65 | struct msghdr msg; |
| 66 | |
| 67 | /* Allocate buffer */ |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 68 | *buf = (unsigned char *) malloc(RECV_BUF_SIZE); |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 69 | if (!(*buf)) { |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 70 | rc = -ENOMEM; |
| 71 | goto fail; |
| 72 | } |
| 73 | |
| 74 | /* Prepare to receive message */ |
| 75 | recvmsg_iov.iov_base = *buf; |
| 76 | recvmsg_iov.iov_len = RECV_BUF_SIZE; |
| 77 | |
| 78 | msg.msg_name = (void *) &sk->s_peer; |
| 79 | msg.msg_namelen = sizeof(sk->s_peer); |
| 80 | msg.msg_iov = &recvmsg_iov; |
| 81 | msg.msg_iovlen = 1; |
| 82 | msg.msg_control = NULL; |
| 83 | msg.msg_controllen = 0; |
| 84 | msg.msg_flags = 0; |
| 85 | |
| 86 | /* Make non blocking and then restore previous setting */ |
| 87 | sk_flags = fcntl(sk->s_fd, F_GETFL, 0); |
| 88 | fcntl(sk->s_fd, F_SETFL, O_NONBLOCK); |
| 89 | rc = recvmsg(sk->s_fd, &msg, 0); |
| 90 | errsv = errno; |
| 91 | fcntl(sk->s_fd, F_SETFL, sk_flags); |
| 92 | |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 93 | if (rc < 0) { |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 94 | rc = -errsv; |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 95 | free(*buf); |
| 96 | *buf = NULL; |
| 97 | } |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 98 | |
| 99 | fail: |
| 100 | return rc; |
| 101 | } |
| 102 | |
| 103 | /* Receive a set of messages from a netlink socket */ |
| 104 | /* NOTE: Does not currently support callback replacements!!! */ |
| 105 | int nl_recvmsgs(struct nl_sock *sk, struct nl_cb *cb) |
| 106 | { |
| 107 | struct sockaddr_nl nla; |
| 108 | struct ucred *creds; |
| 109 | |
| 110 | int rc, cb_rc = NL_OK, done = 0; |
| 111 | |
| 112 | do { |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 113 | unsigned char *buf; |
| 114 | int i, rem, flags; |
| 115 | struct nlmsghdr *nlh; |
| 116 | struct nlmsgerr *nlme; |
| 117 | struct nl_msg *msg; |
| 118 | |
| 119 | done = 0; |
| 120 | rc = nl_recv(sk, &nla, &buf, &creds); |
| 121 | if (rc < 0) |
| 122 | break; |
| 123 | |
| 124 | nlmsg_for_each_msg(nlh, (struct nlmsghdr *) buf, rc, rem) { |
| 125 | |
| 126 | if (rc <= 0 || cb_rc == NL_STOP) |
| 127 | break; |
| 128 | |
| 129 | /* Check for callbacks */ |
| 130 | |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 131 | msg = (struct nl_msg *) malloc(sizeof(struct nl_msg)); |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 132 | memset(msg, 0, sizeof(*msg)); |
| 133 | msg->nm_nlh = nlh; |
| 134 | |
| 135 | /* Check netlink message type */ |
| 136 | |
| 137 | switch (msg->nm_nlh->nlmsg_type) { |
| 138 | case NLMSG_ERROR: /* Used for ACK too */ |
| 139 | /* Certainly we should be doing some |
| 140 | * checking here to make sure this |
| 141 | * message is intended for us */ |
| 142 | nlme = nlmsg_data(msg->nm_nlh); |
| 143 | if (nlme->error == 0) |
| 144 | msg->nm_nlh->nlmsg_flags |= NLM_F_ACK; |
| 145 | |
| 146 | rc = nlme->error; |
| 147 | cb_rc = cb->cb_err(&nla, nlme, cb->cb_err_arg); |
| 148 | nlme = NULL; |
| 149 | break; |
| 150 | |
| 151 | case NLMSG_DONE: |
| 152 | done = 1; |
| 153 | |
| 154 | case NLMSG_OVERRUN: |
| 155 | case NLMSG_NOOP: |
| 156 | default: |
| 157 | break; |
| 158 | }; |
| 159 | |
| 160 | for (i = 0; i <= NL_CB_TYPE_MAX; i++) { |
| 161 | |
| 162 | if (cb->cb_set[i]) { |
| 163 | switch (i) { |
| 164 | case NL_CB_VALID: |
| 165 | if (rc > 0) |
| 166 | cb_rc = cb->cb_set[i](msg, cb->cb_args[i]); |
| 167 | break; |
| 168 | |
| 169 | case NL_CB_FINISH: |
| 170 | if ((msg->nm_nlh->nlmsg_flags & NLM_F_MULTI) && |
| 171 | (msg->nm_nlh->nlmsg_type & NLMSG_DONE)) |
| 172 | cb_rc = cb->cb_set[i](msg, cb->cb_args[i]); |
| 173 | |
| 174 | break; |
| 175 | |
| 176 | case NL_CB_ACK: |
| 177 | if (msg->nm_nlh->nlmsg_flags & NLM_F_ACK) |
| 178 | cb_rc = cb->cb_set[i](msg, cb->cb_args[i]); |
| 179 | |
| 180 | break; |
| 181 | default: |
| 182 | break; |
| 183 | } |
| 184 | } |
| 185 | } |
| 186 | |
| 187 | free(msg); |
| 188 | if (done) |
| 189 | break; |
| 190 | } |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 191 | free(buf); |
| 192 | buf = NULL; |
| 193 | |
| 194 | if (done) |
| 195 | break; |
| 196 | } while (rc > 0 && cb_rc != NL_STOP); |
| 197 | |
| 198 | success: |
| 199 | fail: |
Dmitry Shmidt | d99fe5e | 2011-11-01 15:24:02 -0700 | [diff] [blame] | 200 | return rc; |
Frank Maker | ed6b39c | 2011-05-23 21:14:58 -0700 | [diff] [blame] | 201 | } |
| 202 | |
| 203 | /* Send raw data over netlink socket */ |
| 204 | int nl_send(struct nl_sock *sk, struct nl_msg *msg) |
| 205 | { |
| 206 | struct nlmsghdr *nlh = nlmsg_hdr(msg); |
| 207 | struct iovec msg_iov; |
| 208 | |
| 209 | /* Create IO vector with Netlink message */ |
| 210 | msg_iov.iov_base = nlh; |
| 211 | msg_iov.iov_len = nlh->nlmsg_len; |
| 212 | |
| 213 | return nl_send_iovec(sk, msg, &msg_iov, 1); |
| 214 | } |
| 215 | |
| 216 | /* Send netlink message */ |
| 217 | int nl_send_iovec(struct nl_sock *sk, struct nl_msg *msg, |
| 218 | struct iovec *iov, unsigned iovlen) |
| 219 | { |
| 220 | int rc; |
| 221 | |
| 222 | /* Socket message */ |
| 223 | struct msghdr mh = { |
| 224 | .msg_name = (void *) &sk->s_peer, |
| 225 | .msg_namelen = sizeof(sk->s_peer), |
| 226 | .msg_iov = iov, |
| 227 | .msg_iovlen = iovlen, |
| 228 | .msg_control = NULL, |
| 229 | .msg_controllen = 0, |
| 230 | .msg_flags = 0 |
| 231 | }; |
| 232 | |
| 233 | /* Send message and verify sent */ |
| 234 | rc = nl_sendmsg(sk, (struct nl_msg *) &mh, 0); |
| 235 | if (rc < 0) |
| 236 | fprintf(stderr, "Error sending netlink message: %d\n", errno); |
| 237 | return rc; |
| 238 | |
| 239 | } |
| 240 | |
| 241 | /* Send netlink message with control over sendmsg() message header */ |
| 242 | int nl_sendmsg(struct nl_sock *sk, struct nl_msg *msg, struct msghdr *hdr) |
| 243 | { |
| 244 | return sendmsg(sk->s_fd, (struct msghdr *) msg, (int) hdr); |
| 245 | } |
| 246 | |
| 247 | /* Create and connect netlink socket */ |
| 248 | int nl_connect(struct nl_sock *sk, int protocol) |
| 249 | { |
| 250 | struct sockaddr addr; |
| 251 | socklen_t addrlen; |
| 252 | int rc; |
| 253 | |
| 254 | /* Create RX socket */ |
| 255 | sk->s_fd = socket(PF_NETLINK, SOCK_RAW, protocol); |
| 256 | if (sk->s_fd < 0) |
| 257 | return -errno; |
| 258 | |
| 259 | /* Set size of RX and TX buffers */ |
| 260 | if (nl_socket_set_buffer_size(sk, NL_BUFFER_SZ, NL_BUFFER_SZ) < 0) |
| 261 | return -errno; |
| 262 | |
| 263 | /* Bind RX socket */ |
| 264 | rc = bind(sk->s_fd, (struct sockaddr *)&sk->s_local, \ |
| 265 | sizeof(sk->s_local)); |
| 266 | if (rc < 0) |
| 267 | return -errno; |
| 268 | addrlen = sizeof(addr); |
| 269 | getsockname(sk->s_fd, &addr, &addrlen); |
| 270 | |
| 271 | return 0; |
| 272 | |
| 273 | } |