00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <stdio.h>
00024 #include <stdlib.h>
00025 #include <unistd.h>
00026 #include <string.h>
00027 #include <ctype.h>
00028 #include <time.h>
00029 #include <errno.h>
00030 #include <netinet/in.h>
00031 #include <sys/socket.h>
00032
00033 #include <libnfnetlink/libnfnetlink.h>
00034 #include <libnetfilter_queue/libnetfilter_queue.h>
00035
00091 struct nfq_handle
00092 {
00093 struct nfnl_handle *nfnlh;
00094 struct nfnl_subsys_handle *nfnlssh;
00095 struct nfq_q_handle *qh_list;
00096 };
00097
00098 struct nfq_q_handle
00099 {
00100 struct nfq_q_handle *next;
00101 struct nfq_handle *h;
00102 u_int16_t id;
00103
00104 nfq_callback *cb;
00105 void *data;
00106 };
00107
00108 struct nfq_data {
00109 struct nfattr **data;
00110 };
00111
00112 int nfq_errno;
00113
00114
00115
00116
00117
00118 static void del_qh(struct nfq_q_handle *qh)
00119 {
00120 struct nfq_q_handle *cur_qh, *prev_qh = NULL;
00121
00122 for (cur_qh = qh->h->qh_list; cur_qh; cur_qh = cur_qh->next) {
00123 if (cur_qh == qh) {
00124 if (prev_qh)
00125 prev_qh->next = qh->next;
00126 else
00127 qh->h->qh_list = qh->next;
00128 return;
00129 }
00130 prev_qh = cur_qh;
00131 }
00132 }
00133
00134 static void add_qh(struct nfq_q_handle *qh)
00135 {
00136 qh->next = qh->h->qh_list;
00137 qh->h->qh_list = qh;
00138 }
00139
00140 static struct nfq_q_handle *find_qh(struct nfq_handle *h, u_int16_t id)
00141 {
00142 struct nfq_q_handle *qh;
00143
00144 for (qh = h->qh_list; qh; qh = qh->next) {
00145 if (qh->id == id)
00146 return qh;
00147 }
00148 return NULL;
00149 }
00150
00151
00152 static int
00153 __build_send_cfg_msg(struct nfq_handle *h, u_int8_t command,
00154 u_int16_t queuenum, u_int16_t pf)
00155 {
00156 union {
00157 char buf[NFNL_HEADER_LEN
00158 +NFA_LENGTH(sizeof(struct nfqnl_msg_config_cmd))];
00159 struct nlmsghdr nmh;
00160 } u;
00161 struct nfqnl_msg_config_cmd cmd;
00162
00163 nfnl_fill_hdr(h->nfnlssh, &u.nmh, 0, AF_UNSPEC, queuenum,
00164 NFQNL_MSG_CONFIG, NLM_F_REQUEST|NLM_F_ACK);
00165
00166 cmd.command = command;
00167 cmd.pf = htons(pf);
00168 nfnl_addattr_l(&u.nmh, sizeof(u), NFQA_CFG_CMD, &cmd, sizeof(cmd));
00169
00170 return nfnl_query(h->nfnlh, &u.nmh);
00171 }
00172
00173 static int __nfq_rcv_pkt(struct nlmsghdr *nlh, struct nfattr *nfa[],
00174 void *data)
00175 {
00176 struct nfgenmsg *nfmsg = NLMSG_DATA(nlh);
00177 struct nfq_handle *h = data;
00178 u_int16_t queue_num = ntohs(nfmsg->res_id);
00179 struct nfq_q_handle *qh = find_qh(h, queue_num);
00180 struct nfq_data nfqa;
00181
00182 if (!qh)
00183 return -ENODEV;
00184
00185 if (!qh->cb)
00186 return -ENODEV;
00187
00188 nfqa.data = nfa;
00189
00190 return qh->cb(qh, nfmsg, &nfqa, qh->data);
00191 }
00192
00193 static struct nfnl_callback pkt_cb = {
00194 .call = &__nfq_rcv_pkt,
00195 .attr_count = NFQA_MAX,
00196 };
00197
00198
00199
00200 struct nfnl_handle *nfq_nfnlh(struct nfq_handle *h)
00201 {
00202 return h->nfnlh;
00203 }
00204
00270 int nfq_fd(struct nfq_handle *h)
00271 {
00272 return nfnl_fd(nfq_nfnlh(h));
00273 }
00274
00325 struct nfq_handle *nfq_open(void)
00326 {
00327 struct nfnl_handle *nfnlh = nfnl_open();
00328 struct nfq_handle *qh;
00329
00330 if (!nfnlh)
00331 return NULL;
00332
00333
00334 nfnl_unset_sequence_tracking(nfnlh);
00335
00336 qh = nfq_open_nfnl(nfnlh);
00337 if (!qh)
00338 nfnl_close(nfnlh);
00339
00340 return qh;
00341 }
00342
00357 struct nfq_handle *nfq_open_nfnl(struct nfnl_handle *nfnlh)
00358 {
00359 struct nfq_handle *h;
00360 int err;
00361
00362 h = malloc(sizeof(*h));
00363 if (!h)
00364 return NULL;
00365
00366 memset(h, 0, sizeof(*h));
00367 h->nfnlh = nfnlh;
00368
00369 h->nfnlssh = nfnl_subsys_open(h->nfnlh, NFNL_SUBSYS_QUEUE,
00370 NFQNL_MSG_MAX, 0);
00371 if (!h->nfnlssh) {
00372
00373 goto out_free;
00374 }
00375
00376 pkt_cb.data = h;
00377 err = nfnl_callback_register(h->nfnlssh, NFQNL_MSG_PACKET, &pkt_cb);
00378 if (err < 0) {
00379 nfq_errno = err;
00380 goto out_close;
00381 }
00382
00383 return h;
00384 out_close:
00385 nfnl_subsys_close(h->nfnlssh);
00386 out_free:
00387 free(h);
00388 return NULL;
00389 }
00390
00408 int nfq_close(struct nfq_handle *h)
00409 {
00410 int ret;
00411
00412 ret = nfnl_close(h->nfnlh);
00413 if (ret == 0)
00414 free(h);
00415 return ret;
00416 }
00417
00428 int nfq_bind_pf(struct nfq_handle *h, u_int16_t pf)
00429 {
00430 return __build_send_cfg_msg(h, NFQNL_CFG_CMD_PF_BIND, 0, pf);
00431 }
00432
00441 int nfq_unbind_pf(struct nfq_handle *h, u_int16_t pf)
00442 {
00443 return __build_send_cfg_msg(h, NFQNL_CFG_CMD_PF_UNBIND, 0, pf);
00444 }
00445
00446
00447
00489 struct nfq_q_handle *nfq_create_queue(struct nfq_handle *h,
00490 u_int16_t num,
00491 nfq_callback *cb,
00492 void *data)
00493 {
00494 int ret;
00495 struct nfq_q_handle *qh;
00496
00497 if (find_qh(h, num))
00498 return NULL;
00499
00500 qh = malloc(sizeof(*qh));
00501
00502 memset(qh, 0, sizeof(*qh));
00503 qh->h = h;
00504 qh->id = num;
00505 qh->cb = cb;
00506 qh->data = data;
00507
00508 ret = __build_send_cfg_msg(h, NFQNL_CFG_CMD_BIND, num, 0);
00509 if (ret < 0) {
00510 nfq_errno = ret;
00511 free(qh);
00512 return NULL;
00513 }
00514
00515 add_qh(qh);
00516 return qh;
00517 }
00518
00535 int nfq_destroy_queue(struct nfq_q_handle *qh)
00536 {
00537 int ret = __build_send_cfg_msg(qh->h, NFQNL_CFG_CMD_UNBIND, qh->id, 0);
00538 if (ret == 0) {
00539 del_qh(qh);
00540 free(qh);
00541 }
00542
00543 return ret;
00544 }
00545
00558 int nfq_handle_packet(struct nfq_handle *h, char *buf, int len)
00559 {
00560 return nfnl_handle_packet(h->nfnlh, buf, len);
00561 }
00562
00578 int nfq_set_mode(struct nfq_q_handle *qh,
00579 u_int8_t mode, u_int32_t range)
00580 {
00581 union {
00582 char buf[NFNL_HEADER_LEN
00583 +NFA_LENGTH(sizeof(struct nfqnl_msg_config_params))];
00584 struct nlmsghdr nmh;
00585 } u;
00586 struct nfqnl_msg_config_params params;
00587
00588 nfnl_fill_hdr(qh->h->nfnlssh, &u.nmh, 0, AF_UNSPEC, qh->id,
00589 NFQNL_MSG_CONFIG, NLM_F_REQUEST|NLM_F_ACK);
00590
00591 params.copy_range = htonl(range);
00592 params.copy_mode = mode;
00593 nfnl_addattr_l(&u.nmh, sizeof(u), NFQA_CFG_PARAMS, ¶ms,
00594 sizeof(params));
00595
00596 return nfnl_query(qh->h->nfnlh, &u.nmh);
00597 }
00598
00610 int nfq_set_queue_maxlen(struct nfq_q_handle *qh,
00611 u_int32_t queuelen)
00612 {
00613 union {
00614 char buf[NFNL_HEADER_LEN
00615 +NFA_LENGTH(sizeof(struct nfqnl_msg_config_params))];
00616 struct nlmsghdr nmh;
00617 } u;
00618 u_int32_t queue_maxlen = htonl(queuelen);
00619
00620 nfnl_fill_hdr(qh->h->nfnlssh, &u.nmh, 0, AF_UNSPEC, qh->id,
00621 NFQNL_MSG_CONFIG, NLM_F_REQUEST|NLM_F_ACK);
00622
00623 nfnl_addattr_l(&u.nmh, sizeof(u), NFQA_CFG_QUEUE_MAXLEN, &queue_maxlen,
00624 sizeof(queue_maxlen));
00625
00626 return nfnl_query(qh->h->nfnlh, &u.nmh);
00627 }
00628
00633 static int __set_verdict(struct nfq_q_handle *qh, u_int32_t id,
00634 u_int32_t verdict, u_int32_t mark, int set_mark,
00635 u_int32_t data_len, const unsigned char *data)
00636 {
00637 struct nfqnl_msg_verdict_hdr vh;
00638 union {
00639 char buf[NFNL_HEADER_LEN
00640 +NFA_LENGTH(sizeof(mark))
00641 +NFA_LENGTH(sizeof(vh))];
00642 struct nlmsghdr nmh;
00643 } u;
00644
00645 struct iovec iov[3];
00646 int nvecs;
00647
00648
00649
00650 struct nfattr data_attr;
00651
00652 memset(iov, 0, sizeof(iov));
00653
00654 vh.verdict = htonl(verdict);
00655 vh.id = htonl(id);
00656
00657 nfnl_fill_hdr(qh->h->nfnlssh, &u.nmh, 0, AF_UNSPEC, qh->id,
00658 NFQNL_MSG_VERDICT, NLM_F_REQUEST);
00659
00660
00661 nfnl_addattr_l(&u.nmh, sizeof(u), NFQA_VERDICT_HDR, &vh, sizeof(vh));
00662
00663 if (set_mark)
00664 nfnl_addattr32(&u.nmh, sizeof(u), NFQA_MARK, mark);
00665
00666 iov[0].iov_base = &u.nmh;
00667 iov[0].iov_len = NLMSG_TAIL(&u.nmh) - (void *)&u.nmh;
00668 nvecs = 1;
00669
00670 if (data_len) {
00671
00672 nfnl_build_nfa_iovec(&iov[1], &data_attr, NFQA_PAYLOAD,
00673 data_len, (unsigned char *) data);
00674 nvecs += 2;
00675
00676
00677
00678
00679 u.nmh.nlmsg_len += data_attr.nfa_len;
00680 }
00681
00682 return nfnl_sendiov(qh->h->nfnlh, iov, nvecs, 0);
00683 }
00684
00712 int nfq_set_verdict(struct nfq_q_handle *qh, u_int32_t id,
00713 u_int32_t verdict, u_int32_t data_len,
00714 const unsigned char *buf)
00715 {
00716 return __set_verdict(qh, id, verdict, 0, 0, data_len, buf);
00717 }
00718
00728 int nfq_set_verdict2(struct nfq_q_handle *qh, u_int32_t id,
00729 u_int32_t verdict, u_int32_t mark,
00730 u_int32_t data_len, const unsigned char *buf)
00731 {
00732 return __set_verdict(qh, id, verdict, htonl(mark), 1, data_len, buf);
00733 }
00734
00749 int nfq_set_verdict_mark(struct nfq_q_handle *qh, u_int32_t id,
00750 u_int32_t verdict, u_int32_t mark,
00751 u_int32_t data_len, const unsigned char *buf)
00752 {
00753 return __set_verdict(qh, id, verdict, mark, 1, data_len, buf);
00754 }
00755
00762
00763
00764
00765
00789 struct nfqnl_msg_packet_hdr *nfq_get_msg_packet_hdr(struct nfq_data *nfad)
00790 {
00791 return nfnl_get_pointer_to_data(nfad->data, NFQA_PACKET_HDR,
00792 struct nfqnl_msg_packet_hdr);
00793 }
00794
00801 uint32_t nfq_get_nfmark(struct nfq_data *nfad)
00802 {
00803 return ntohl(nfnl_get_data(nfad->data, NFQA_MARK, u_int32_t));
00804 }
00805
00815 int nfq_get_timestamp(struct nfq_data *nfad, struct timeval *tv)
00816 {
00817 struct nfqnl_msg_packet_timestamp *qpt;
00818 qpt = nfnl_get_pointer_to_data(nfad->data, NFQA_TIMESTAMP,
00819 struct nfqnl_msg_packet_timestamp);
00820 if (!qpt)
00821 return -1;
00822
00823 tv->tv_sec = __be64_to_cpu(qpt->sec);
00824 tv->tv_usec = __be64_to_cpu(qpt->usec);
00825
00826 return 0;
00827 }
00828
00840 u_int32_t nfq_get_indev(struct nfq_data *nfad)
00841 {
00842 return ntohl(nfnl_get_data(nfad->data, NFQA_IFINDEX_INDEV, u_int32_t));
00843 }
00844
00853 u_int32_t nfq_get_physindev(struct nfq_data *nfad)
00854 {
00855 return ntohl(nfnl_get_data(nfad->data, NFQA_IFINDEX_PHYSINDEV, u_int32_t));
00856 }
00857
00866 u_int32_t nfq_get_outdev(struct nfq_data *nfad)
00867 {
00868 return ntohl(nfnl_get_data(nfad->data, NFQA_IFINDEX_OUTDEV, u_int32_t));
00869 }
00870
00881 u_int32_t nfq_get_physoutdev(struct nfq_data *nfad)
00882 {
00883 return ntohl(nfnl_get_data(nfad->data, NFQA_IFINDEX_PHYSOUTDEV, u_int32_t));
00884 }
00885
00924 int nfq_get_indev_name(struct nlif_handle *nlif_handle,
00925 struct nfq_data *nfad, char *name)
00926 {
00927 u_int32_t ifindex = nfq_get_indev(nfad);
00928 return nlif_index2name(nlif_handle, ifindex, name);
00929 }
00930
00943 int nfq_get_physindev_name(struct nlif_handle *nlif_handle,
00944 struct nfq_data *nfad, char *name)
00945 {
00946 u_int32_t ifindex = nfq_get_physindev(nfad);
00947 return nlif_index2name(nlif_handle, ifindex, name);
00948 }
00949
00962 int nfq_get_outdev_name(struct nlif_handle *nlif_handle,
00963 struct nfq_data *nfad, char *name)
00964 {
00965 u_int32_t ifindex = nfq_get_outdev(nfad);
00966 return nlif_index2name(nlif_handle, ifindex, name);
00967 }
00968
00982 int nfq_get_physoutdev_name(struct nlif_handle *nlif_handle,
00983 struct nfq_data *nfad, char *name)
00984 {
00985 u_int32_t ifindex = nfq_get_physoutdev(nfad);
00986 return nlif_index2name(nlif_handle, ifindex, name);
00987 }
00988
01011 struct nfqnl_msg_packet_hw *nfq_get_packet_hw(struct nfq_data *nfad)
01012 {
01013 return nfnl_get_pointer_to_data(nfad->data, NFQA_HWADDR,
01014 struct nfqnl_msg_packet_hw);
01015 }
01016
01028 int nfq_get_payload(struct nfq_data *nfad, unsigned char **data)
01029 {
01030 *data = nfnl_get_pointer_to_data(nfad->data, NFQA_PAYLOAD, char);
01031 if (*data)
01032 return NFA_PAYLOAD(nfad->data[NFQA_PAYLOAD-1]);
01033
01034 return -1;
01035 }
01036
01041 #define SNPRINTF_FAILURE(ret, rem, offset, len) \
01042 do { \
01043 if (ret < 0) \
01044 return ret; \
01045 len += ret; \
01046 if (ret > rem) \
01047 ret = rem; \
01048 offset += ret; \
01049 rem -= ret; \
01050 } while (0)
01051
01080 int nfq_snprintf_xml(char *buf, size_t rem, struct nfq_data *tb, int flags)
01081 {
01082 struct nfqnl_msg_packet_hdr *ph;
01083 struct nfqnl_msg_packet_hw *hwph;
01084 u_int32_t mark, ifi;
01085 int size, offset = 0, len = 0, ret;
01086 unsigned char *data;
01087
01088 size = snprintf(buf + offset, rem, "<pkt>");
01089 SNPRINTF_FAILURE(size, rem, offset, len);
01090
01091 if (flags & NFQ_XML_TIME) {
01092 time_t t;
01093 struct tm tm;
01094
01095 t = time(NULL);
01096 if (localtime_r(&t, &tm) == NULL)
01097 return -1;
01098
01099 size = snprintf(buf + offset, rem, "<when>");
01100 SNPRINTF_FAILURE(size, rem, offset, len);
01101
01102 size = snprintf(buf + offset, rem,
01103 "<hour>%d</hour>", tm.tm_hour);
01104 SNPRINTF_FAILURE(size, rem, offset, len);
01105
01106 size = snprintf(buf + offset,
01107 rem, "<min>%02d</min>", tm.tm_min);
01108 SNPRINTF_FAILURE(size, rem, offset, len);
01109
01110 size = snprintf(buf + offset,
01111 rem, "<sec>%02d</sec>", tm.tm_sec);
01112 SNPRINTF_FAILURE(size, rem, offset, len);
01113
01114 size = snprintf(buf + offset, rem, "<wday>%d</wday>",
01115 tm.tm_wday + 1);
01116 SNPRINTF_FAILURE(size, rem, offset, len);
01117
01118 size = snprintf(buf + offset, rem, "<day>%d</day>", tm.tm_mday);
01119 SNPRINTF_FAILURE(size, rem, offset, len);
01120
01121 size = snprintf(buf + offset, rem, "<month>%d</month>",
01122 tm.tm_mon + 1);
01123 SNPRINTF_FAILURE(size, rem, offset, len);
01124
01125 size = snprintf(buf + offset, rem, "<year>%d</year>",
01126 1900 + tm.tm_year);
01127 SNPRINTF_FAILURE(size, rem, offset, len);
01128
01129 size = snprintf(buf + offset, rem, "</when>");
01130 SNPRINTF_FAILURE(size, rem, offset, len);
01131 }
01132
01133 ph = nfq_get_msg_packet_hdr(tb);
01134 if (ph) {
01135 size = snprintf(buf + offset, rem,
01136 "<hook>%u</hook><id>%u</id>",
01137 ph->hook, ntohl(ph->packet_id));
01138 SNPRINTF_FAILURE(size, rem, offset, len);
01139
01140 hwph = nfq_get_packet_hw(tb);
01141 if (hwph && (flags & NFQ_XML_HW)) {
01142 int i, hlen = ntohs(hwph->hw_addrlen);
01143
01144 size = snprintf(buf + offset, rem, "<hw><proto>%04x"
01145 "</proto>",
01146 ntohs(ph->hw_protocol));
01147 SNPRINTF_FAILURE(size, rem, offset, len);
01148
01149 size = snprintf(buf + offset, rem, "<src>");
01150 SNPRINTF_FAILURE(size, rem, offset, len);
01151
01152 for (i=0; i<hlen; i++) {
01153 size = snprintf(buf + offset, rem, "%02x",
01154 hwph->hw_addr[i]);
01155 SNPRINTF_FAILURE(size, rem, offset, len);
01156 }
01157
01158 size = snprintf(buf + offset, rem, "</src></hw>");
01159 SNPRINTF_FAILURE(size, rem, offset, len);
01160 } else if (flags & NFQ_XML_HW) {
01161 size = snprintf(buf + offset, rem, "<hw><proto>%04x"
01162 "</proto></hw>",
01163 ntohs(ph->hw_protocol));
01164 SNPRINTF_FAILURE(size, rem, offset, len);
01165 }
01166 }
01167
01168 mark = nfq_get_nfmark(tb);
01169 if (mark && (flags & NFQ_XML_MARK)) {
01170 size = snprintf(buf + offset, rem, "<mark>%u</mark>", mark);
01171 SNPRINTF_FAILURE(size, rem, offset, len);
01172 }
01173
01174 ifi = nfq_get_indev(tb);
01175 if (ifi && (flags & NFQ_XML_DEV)) {
01176 size = snprintf(buf + offset, rem, "<indev>%u</indev>", ifi);
01177 SNPRINTF_FAILURE(size, rem, offset, len);
01178 }
01179
01180 ifi = nfq_get_outdev(tb);
01181 if (ifi && (flags & NFQ_XML_DEV)) {
01182 size = snprintf(buf + offset, rem, "<outdev>%u</outdev>", ifi);
01183 SNPRINTF_FAILURE(size, rem, offset, len);
01184 }
01185
01186 ifi = nfq_get_physindev(tb);
01187 if (ifi && (flags & NFQ_XML_PHYSDEV)) {
01188 size = snprintf(buf + offset, rem,
01189 "<physindev>%u</physindev>", ifi);
01190 SNPRINTF_FAILURE(size, rem, offset, len);
01191 }
01192
01193 ifi = nfq_get_physoutdev(tb);
01194 if (ifi && (flags & NFQ_XML_PHYSDEV)) {
01195 size = snprintf(buf + offset, rem,
01196 "<physoutdev>%u</physoutdev>", ifi);
01197 SNPRINTF_FAILURE(size, rem, offset, len);
01198 }
01199
01200 ret = nfq_get_payload(tb, &data);
01201 if (ret >= 0 && (flags & NFQ_XML_PAYLOAD)) {
01202 int i;
01203
01204 size = snprintf(buf + offset, rem, "<payload>");
01205 SNPRINTF_FAILURE(size, rem, offset, len);
01206
01207 for (i=0; i<ret; i++) {
01208 size = snprintf(buf + offset, rem, "%02x",
01209 data[i] & 0xff);
01210 SNPRINTF_FAILURE(size, rem, offset, len);
01211 }
01212
01213 size = snprintf(buf + offset, rem, "</payload>");
01214 SNPRINTF_FAILURE(size, rem, offset, len);
01215 }
01216
01217 size = snprintf(buf + offset, rem, "</pkt>");
01218 SNPRINTF_FAILURE(size, rem, offset, len);
01219
01220 return len;
01221 }
01222