00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161 #include <netlink-local.h>
00162 #include <netlink/netlink.h>
00163 #include <netlink/utils.h>
00164 #include <netlink/cache.h>
00165 #include <netlink/attr.h>
00166 #include <linux/socket.h>
00167
00168 static size_t default_msg_size;
00169
00170 static void __init init_msg_size(void)
00171 {
00172 default_msg_size = getpagesize();
00173 }
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184 int nlmsg_msg_size(int payload)
00185 {
00186 return NLMSG_HDRLEN + payload;
00187 }
00188
00189
00190
00191
00192
00193 int nlmsg_total_size(int payload)
00194 {
00195 return NLMSG_ALIGN(nlmsg_msg_size(payload));
00196 }
00197
00198
00199
00200
00201
00202 int nlmsg_padlen(int payload)
00203 {
00204 return nlmsg_total_size(payload) - nlmsg_msg_size(payload);
00205 }
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218 void *nlmsg_data(const struct nlmsghdr *nlh)
00219 {
00220 return (unsigned char *) nlh + NLMSG_HDRLEN;
00221 }
00222
00223 void *nlmsg_tail(const struct nlmsghdr *nlh)
00224 {
00225 return (unsigned char *) nlh + NLMSG_ALIGN(nlh->nlmsg_len);
00226 }
00227
00228
00229
00230
00231
00232 int nlmsg_len(const struct nlmsghdr *nlh)
00233 {
00234 return nlh->nlmsg_len - NLMSG_HDRLEN;
00235 }
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249 struct nlattr *nlmsg_attrdata(const struct nlmsghdr *nlh, int hdrlen)
00250 {
00251 unsigned char *data = nlmsg_data(nlh);
00252 return (struct nlattr *) (data + NLMSG_ALIGN(hdrlen));
00253 }
00254
00255
00256
00257
00258
00259
00260 int nlmsg_attrlen(const struct nlmsghdr *nlh, int hdrlen)
00261 {
00262 return nlmsg_len(nlh) - NLMSG_ALIGN(hdrlen);
00263 }
00264
00265
00266
00267
00268
00269
00270
00271
00272 int nlmsg_valid_hdr(const struct nlmsghdr *nlh, int hdrlen)
00273 {
00274 if (nlh->nlmsg_len < nlmsg_msg_size(hdrlen))
00275 return 0;
00276
00277 return 1;
00278 }
00279
00280
00281
00282
00283
00284
00285 int nlmsg_ok(const struct nlmsghdr *nlh, int remaining)
00286 {
00287 return (remaining >= sizeof(struct nlmsghdr) &&
00288 nlh->nlmsg_len >= sizeof(struct nlmsghdr) &&
00289 nlh->nlmsg_len <= remaining);
00290 }
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 struct nlmsghdr *nlmsg_next(struct nlmsghdr *nlh, int *remaining)
00301 {
00302 int totlen = NLMSG_ALIGN(nlh->nlmsg_len);
00303
00304 *remaining -= totlen;
00305
00306 return (struct nlmsghdr *) ((unsigned char *) nlh + totlen);
00307 }
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319 int nlmsg_parse(struct nlmsghdr *nlh, int hdrlen, struct nlattr *tb[],
00320 int maxtype, struct nla_policy *policy)
00321 {
00322 if (!nlmsg_valid_hdr(nlh, hdrlen))
00323 return nl_errno(EINVAL);
00324
00325 return nla_parse(tb, maxtype, nlmsg_attrdata(nlh, hdrlen),
00326 nlmsg_attrlen(nlh, hdrlen), policy);
00327 }
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337 struct nlattr *nlmsg_find_attr(struct nlmsghdr *nlh, int hdrlen, int attrtype)
00338 {
00339 return nla_find(nlmsg_attrdata(nlh, hdrlen),
00340 nlmsg_attrlen(nlh, hdrlen), attrtype);
00341 }
00342
00343
00344
00345
00346
00347
00348
00349
00350 int nlmsg_validate(struct nlmsghdr *nlh, int hdrlen, int maxtype,
00351 struct nla_policy *policy)
00352 {
00353 if (!nlmsg_valid_hdr(nlh, hdrlen))
00354 return nl_errno(EINVAL);
00355
00356 return nla_validate(nlmsg_attrdata(nlh, hdrlen),
00357 nlmsg_attrlen(nlh, hdrlen), maxtype, policy);
00358 }
00359
00360
00361
00362
00363
00364
00365
00366
00367 static struct nl_msg *__nlmsg_alloc(size_t len)
00368 {
00369 struct nl_msg *nm;
00370
00371 nm = calloc(1, sizeof(*nm));
00372 if (!nm)
00373 goto errout;
00374
00375 nm->nm_nlh = calloc(1, len);
00376 if (!nm->nm_nlh)
00377 goto errout;
00378
00379 nm->nm_protocol = -1;
00380 nm->nm_size = len;
00381 nm->nm_nlh->nlmsg_len = nlmsg_total_size(0);
00382
00383 NL_DBG(2, "msg %p: Allocated new message, maxlen=%zu\n", nm, len);
00384
00385 return nm;
00386 errout:
00387 free(nm);
00388 nl_errno(ENOMEM);
00389 return NULL;
00390 }
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401 struct nl_msg *nlmsg_alloc(void)
00402 {
00403 return __nlmsg_alloc(default_msg_size);
00404 }
00405
00406
00407
00408
00409 struct nl_msg *nlmsg_alloc_size(size_t max)
00410 {
00411 return __nlmsg_alloc(max);
00412 }
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424 struct nl_msg *nlmsg_inherit(struct nlmsghdr *hdr)
00425 {
00426 struct nl_msg *nm;
00427
00428 nm = nlmsg_alloc();
00429 if (nm && hdr) {
00430 struct nlmsghdr *new = nm->nm_nlh;
00431
00432 new->nlmsg_type = hdr->nlmsg_type;
00433 new->nlmsg_flags = hdr->nlmsg_flags;
00434 new->nlmsg_seq = hdr->nlmsg_seq;
00435 new->nlmsg_pid = hdr->nlmsg_pid;
00436 }
00437
00438 return nm;
00439 }
00440
00441
00442
00443
00444
00445
00446
00447
00448 struct nl_msg *nlmsg_alloc_simple(int nlmsgtype, int flags)
00449 {
00450 struct nl_msg *msg;
00451 struct nlmsghdr nlh = {
00452 .nlmsg_type = nlmsgtype,
00453 .nlmsg_flags = flags,
00454 };
00455
00456 msg = nlmsg_inherit(&nlh);
00457 if (msg)
00458 NL_DBG(2, "msg %p: Allocated new simple message\n", msg);
00459
00460 return msg;
00461 }
00462
00463
00464
00465
00466
00467 void nlmsg_set_default_size(size_t max)
00468 {
00469 if (max < nlmsg_total_size(0))
00470 max = nlmsg_total_size(0);
00471
00472 default_msg_size = max;
00473 }
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484 struct nl_msg *nlmsg_convert(struct nlmsghdr *hdr)
00485 {
00486 struct nl_msg *nm;
00487
00488 nm = __nlmsg_alloc(NLMSG_ALIGN(hdr->nlmsg_len));
00489 if (!nm)
00490 goto errout;
00491
00492 memcpy(nm->nm_nlh, hdr, hdr->nlmsg_len);
00493
00494 return nm;
00495 errout:
00496 nlmsg_free(nm);
00497 return NULL;
00498 }
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512 void *nlmsg_reserve(struct nl_msg *n, size_t len, int pad)
00513 {
00514 void *buf = n->nm_nlh;
00515 size_t nlmsg_len = n->nm_nlh->nlmsg_len;
00516 size_t tlen;
00517
00518 tlen = pad ? ((len + (pad - 1)) & ~(pad - 1)) : len;
00519
00520 if ((tlen + nlmsg_len) > n->nm_size) {
00521 nl_errno(ENOBUFS);
00522 return NULL;
00523 }
00524
00525 buf += nlmsg_len;
00526 n->nm_nlh->nlmsg_len += tlen;
00527
00528 if (tlen > len)
00529 memset(buf + len, 0, tlen - len);
00530
00531 NL_DBG(2, "msg %p: Reserved %zu bytes, pad=%d, nlmsg_len=%d\n",
00532 n, len, pad, n->nm_nlh->nlmsg_len);
00533
00534 return buf;
00535 }
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549 int nlmsg_append(struct nl_msg *n, void *data, size_t len, int pad)
00550 {
00551 void *tmp;
00552
00553 tmp = nlmsg_reserve(n, len, pad);
00554 if (tmp == NULL)
00555 return nl_errno(ENOMEM);
00556
00557 memcpy(tmp, data, len);
00558 NL_DBG(2, "msg %p: Appended %zu bytes with padding %d\n", n, len, pad);
00559
00560 return 0;
00561 }
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577 int nlmsg_expand(struct nl_msg *n, size_t newlen)
00578 {
00579 void *tmp;
00580
00581 if (newlen <= n->nm_size)
00582 return nl_errno(EINVAL);
00583
00584 tmp = realloc(n->nm_nlh, newlen);
00585 if (tmp == NULL)
00586 return nl_errno(ENOMEM);
00587
00588 n->nm_nlh = tmp;
00589 n->nm_size = newlen;
00590
00591 return 0;
00592 }
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610 struct nlmsghdr *nlmsg_put(struct nl_msg *n, uint32_t pid, uint32_t seq,
00611 int type, int payload, int flags)
00612 {
00613 struct nlmsghdr *nlh;
00614
00615 if (n->nm_nlh->nlmsg_len < NLMSG_HDRLEN)
00616 BUG();
00617
00618 nlh = (struct nlmsghdr *) n->nm_nlh;
00619 nlh->nlmsg_type = type;
00620 nlh->nlmsg_flags = flags;
00621 nlh->nlmsg_pid = pid;
00622 nlh->nlmsg_seq = seq;
00623
00624 NL_DBG(2, "msg %p: Added netlink header type=%d, flags=%d, pid=%d, "
00625 "seq=%d\n", n, type, flags, pid, seq);
00626
00627 if (payload > 0 &&
00628 nlmsg_reserve(n, payload, NLMSG_ALIGNTO) == NULL)
00629 return NULL;
00630
00631 return nlh;
00632 }
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643 struct nlmsghdr *nlmsg_hdr(struct nl_msg *n)
00644 {
00645 return n->nm_nlh;
00646 }
00647
00648
00649
00650
00651
00652
00653
00654
00655
00656 void nlmsg_free(struct nl_msg *n)
00657 {
00658 if (!n)
00659 return;
00660
00661 free(n->nm_nlh);
00662 free(n);
00663 NL_DBG(2, "msg %p: Freed\n", n);
00664 }
00665
00666
00667
00668
00669
00670
00671
00672
00673 void nlmsg_set_proto(struct nl_msg *msg, int protocol)
00674 {
00675 msg->nm_protocol = protocol;
00676 }
00677
00678 int nlmsg_get_proto(struct nl_msg *msg)
00679 {
00680 return msg->nm_protocol;
00681 }
00682
00683 size_t nlmsg_get_max_size(struct nl_msg *msg)
00684 {
00685 return msg->nm_size;
00686 }
00687
00688 void nlmsg_set_src(struct nl_msg *msg, struct sockaddr_nl *addr)
00689 {
00690 memcpy(&msg->nm_src, addr, sizeof(*addr));
00691 }
00692
00693 struct sockaddr_nl *nlmsg_get_src(struct nl_msg *msg)
00694 {
00695 return &msg->nm_src;
00696 }
00697
00698 void nlmsg_set_dst(struct nl_msg *msg, struct sockaddr_nl *addr)
00699 {
00700 memcpy(&msg->nm_dst, addr, sizeof(*addr));
00701 }
00702
00703 struct sockaddr_nl *nlmsg_get_dst(struct nl_msg *msg)
00704 {
00705 return &msg->nm_dst;
00706 }
00707
00708 void nlmsg_set_creds(struct nl_msg *msg, struct ucred *creds)
00709 {
00710 memcpy(&msg->nm_creds, creds, sizeof(*creds));
00711 msg->nm_flags |= NL_MSG_CRED_PRESENT;
00712 }
00713
00714 struct ucred *nlmsg_get_creds(struct nl_msg *msg)
00715 {
00716 if (msg->nm_flags & NL_MSG_CRED_PRESENT)
00717 return &msg->nm_creds;
00718 return NULL;
00719 }
00720
00721
00722
00723
00724
00725
00726
00727
00728 static struct trans_tbl nl_msgtypes[] = {
00729 __ADD(NLMSG_NOOP,NOOP)
00730 __ADD(NLMSG_ERROR,ERROR)
00731 __ADD(NLMSG_DONE,DONE)
00732 __ADD(NLMSG_OVERRUN,OVERRUN)
00733 };
00734
00735 char *nl_nlmsgtype2str(int type, char *buf, size_t size)
00736 {
00737 return __type2str(type, buf, size, nl_msgtypes,
00738 ARRAY_SIZE(nl_msgtypes));
00739 }
00740
00741 int nl_str2nlmsgtype(const char *name)
00742 {
00743 return __str2type(name, nl_msgtypes, ARRAY_SIZE(nl_msgtypes));
00744 }
00745
00746
00747
00748
00749
00750
00751
00752
00753 char *nl_nlmsg_flags2str(int flags, char *buf, size_t len)
00754 {
00755 memset(buf, 0, len);
00756
00757 #define PRINT_FLAG(f) \
00758 if (flags & NLM_F_##f) { \
00759 flags &= ~NLM_F_##f; \
00760 strncat(buf, #f, len - strlen(buf) - 1); \
00761 if (flags) \
00762 strncat(buf, ",", len - strlen(buf) - 1); \
00763 }
00764
00765 PRINT_FLAG(REQUEST);
00766 PRINT_FLAG(MULTI);
00767 PRINT_FLAG(ACK);
00768 PRINT_FLAG(ECHO);
00769 PRINT_FLAG(ROOT);
00770 PRINT_FLAG(MATCH);
00771 PRINT_FLAG(ATOMIC);
00772 PRINT_FLAG(REPLACE);
00773 PRINT_FLAG(EXCL);
00774 PRINT_FLAG(CREATE);
00775 PRINT_FLAG(APPEND);
00776
00777 if (flags) {
00778 char s[32];
00779 snprintf(s, sizeof(s), "0x%x", flags);
00780 strncat(buf, s, len - strlen(buf) - 1);
00781 }
00782 #undef PRINT_FLAG
00783
00784 return buf;
00785 }
00786
00787
00788
00789
00790
00791
00792
00793
00794
00795 struct dp_xdata {
00796 void (*cb)(struct nl_object *, void *);
00797 void *arg;
00798 };
00799
00800
00801 static int parse_cb(struct nl_object *obj, struct nl_parser_param *p)
00802 {
00803 struct dp_xdata *x = p->pp_arg;
00804
00805 x->cb(obj, x->arg);
00806 return 0;
00807 }
00808
00809 int nl_msg_parse(struct nl_msg *msg, void (*cb)(struct nl_object *, void *),
00810 void *arg)
00811 {
00812 struct nl_cache_ops *ops;
00813 struct nl_parser_param p = {
00814 .pp_cb = parse_cb
00815 };
00816 struct dp_xdata x = {
00817 .cb = cb,
00818 .arg = arg,
00819 };
00820
00821 ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
00822 nlmsg_hdr(msg)->nlmsg_type);
00823 if (ops == NULL)
00824 return nl_error(ENOENT, "Unknown message type %d",
00825 nlmsg_hdr(msg)->nlmsg_type);
00826 p.pp_arg = &x;
00827
00828 return nl_cache_parse(ops, NULL, nlmsg_hdr(msg), &p);
00829 }
00830
00831
00832
00833
00834
00835
00836
00837
00838 static void prefix_line(FILE *ofd, int prefix)
00839 {
00840 int i;
00841
00842 for (i = 0; i < prefix; i++)
00843 fprintf(ofd, " ");
00844 }
00845
00846 static inline void dump_hex(FILE *ofd, char *start, int len, int prefix)
00847 {
00848 int i, a, c, limit;
00849 char ascii[21] = {0};
00850
00851 limit = 18 - (prefix * 2);
00852 prefix_line(ofd, prefix);
00853 fprintf(ofd, " ");
00854
00855 for (i = 0, a = 0, c = 0; i < len; i++) {
00856 int v = *(uint8_t *) (start + i);
00857
00858 fprintf(ofd, "%02x ", v);
00859 ascii[a++] = isprint(v) ? v : '.';
00860
00861 if (c == limit-1) {
00862 fprintf(ofd, "%s\n", ascii);
00863 if (i < (len - 1)) {
00864 prefix_line(ofd, prefix);
00865 fprintf(ofd, " ");
00866 }
00867 a = c = 0;
00868 memset(ascii, 0, sizeof(ascii));
00869 } else
00870 c++;
00871 }
00872
00873 if (c != 0) {
00874 for (i = 0; i < (limit - c); i++)
00875 fprintf(ofd, " ");
00876 fprintf(ofd, "%s\n", ascii);
00877 }
00878 }
00879
00880 static void print_hdr(FILE *ofd, struct nl_msg *msg)
00881 {
00882 struct nlmsghdr *nlh = nlmsg_hdr(msg);
00883 struct nl_cache_ops *ops;
00884 struct nl_msgtype *mt;
00885 char buf[128];
00886
00887 fprintf(ofd, " .nlmsg_len = %d\n", nlh->nlmsg_len);
00888
00889 ops = nl_cache_ops_associate(nlmsg_get_proto(msg), nlh->nlmsg_type);
00890 if (ops) {
00891 mt = nl_msgtype_lookup(ops, nlh->nlmsg_type);
00892 if (!mt)
00893 BUG();
00894
00895 snprintf(buf, sizeof(buf), "%s::%s", ops->co_name, mt->mt_name);
00896 } else
00897 nl_nlmsgtype2str(nlh->nlmsg_type, buf, sizeof(buf));
00898
00899 fprintf(ofd, " .nlmsg_type = %d <%s>\n", nlh->nlmsg_type, buf);
00900 fprintf(ofd, " .nlmsg_flags = %d <%s>\n", nlh->nlmsg_flags,
00901 nl_nlmsg_flags2str(nlh->nlmsg_flags, buf, sizeof(buf)));
00902 fprintf(ofd, " .nlmsg_seq = %d\n", nlh->nlmsg_seq);
00903 fprintf(ofd, " .nlmsg_pid = %d\n", nlh->nlmsg_pid);
00904
00905 }
00906
00907 static void dump_attrs(FILE *ofd, struct nlattr *attrs, int attrlen,
00908 int prefix)
00909 {
00910 int rem;
00911 struct nlattr *nla;
00912
00913 nla_for_each_attr(nla, attrs, attrlen, rem) {
00914 int padlen, alen = nla_len(nla);
00915
00916 prefix_line(ofd, prefix);
00917 fprintf(ofd, " [ATTR %02d%s] %d octets\n", nla_type(nla),
00918 nla->nla_type & NLA_F_NESTED ? " NESTED" : "",
00919 alen);
00920
00921 if (nla->nla_type & NLA_F_NESTED)
00922 dump_attrs(ofd, nla_data(nla), alen, prefix+1);
00923 else
00924 dump_hex(ofd, nla_data(nla), alen, prefix);
00925
00926 padlen = nla_padlen(alen);
00927 if (padlen > 0) {
00928 prefix_line(ofd, prefix);
00929 fprintf(ofd, " [PADDING] %d octets\n",
00930 padlen);
00931 dump_hex(ofd, nla_data(nla) + alen,
00932 padlen, prefix);
00933 }
00934 }
00935
00936 if (rem) {
00937 prefix_line(ofd, prefix);
00938 fprintf(ofd, " [LEFTOVER] %d octets\n", rem);
00939 }
00940 }
00941
00942
00943
00944
00945
00946
00947 void nl_msg_dump(struct nl_msg *msg, FILE *ofd)
00948 {
00949 struct nlmsghdr *hdr = nlmsg_hdr(msg);
00950
00951 fprintf(ofd,
00952 "-------------------------- BEGIN NETLINK MESSAGE "
00953 "---------------------------\n");
00954
00955 fprintf(ofd, " [HEADER] %Zu octets\n", sizeof(struct nlmsghdr));
00956 print_hdr(ofd, msg);
00957
00958 if (hdr->nlmsg_type == NLMSG_ERROR &&
00959 hdr->nlmsg_len >= nlmsg_msg_size(sizeof(struct nlmsgerr))) {
00960 struct nl_msg *errmsg;
00961 struct nlmsgerr *err = nlmsg_data(hdr);
00962
00963 fprintf(ofd, " [ERRORMSG] %Zu octets\n", sizeof(*err));
00964 fprintf(ofd, " .error = %d \"%s\"\n", err->error,
00965 strerror(-err->error));
00966 fprintf(ofd, " [ORIGINAL MESSAGE] %Zu octets\n", sizeof(*hdr));
00967
00968 errmsg = nlmsg_inherit(&err->msg);
00969 print_hdr(ofd, errmsg);
00970 nlmsg_free(errmsg);
00971 } else if (nlmsg_len(hdr) > 0) {
00972 struct nl_cache_ops *ops;
00973 int payloadlen = nlmsg_len(hdr);
00974 int attrlen = 0;
00975
00976 ops = nl_cache_ops_associate(nlmsg_get_proto(msg),
00977 hdr->nlmsg_type);
00978 if (ops) {
00979 attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
00980 payloadlen -= attrlen;
00981 }
00982
00983 fprintf(ofd, " [PAYLOAD] %d octets\n", payloadlen);
00984 dump_hex(ofd, nlmsg_data(hdr), payloadlen, 0);
00985
00986 if (attrlen) {
00987 struct nlattr *attrs;
00988 int attrlen;
00989
00990 attrs = nlmsg_attrdata(hdr, ops->co_hdrsize);
00991 attrlen = nlmsg_attrlen(hdr, ops->co_hdrsize);
00992 dump_attrs(ofd, attrs, attrlen, 0);
00993 }
00994 }
00995
00996 fprintf(ofd,
00997 "--------------------------- END NETLINK MESSAGE "
00998 "---------------------------\n");
00999 }
01000
01001
01002
01003