1 #include <netinet/if_ether.h>
2 #include <netinet/in.h>
3 #include <netinet/ip.h>
4 #include <netinet/ip6.h>
5 #include <netinet/udp.h>
6 #include <netpacket/packet.h>
8 #include <sys/socket.h>
13 #include <libubox/uloop.h>
14 #include <libubox/avl-cmp.h>
16 #define FLAG_RESPONSE 0x8000
17 #define FLAG_OPCODE 0x7800
18 #define FLAG_AUTHORATIVE 0x0400
19 #define FLAG_RCODE 0x000f
22 #define TYPE_CNAME 0x0005
23 #define TYPE_PTR 0x000c
24 #define TYPE_TXT 0x0010
25 #define TYPE_AAAA 0x001c
26 #define TYPE_SRV 0x0021
27 #define TYPE_ANY 0x00ff
29 #define IS_COMPRESSED(x) ((x & 0xc0) == 0xc0)
31 #define CLASS_FLUSH 0x8000
32 #define CLASS_UNICAST 0x8000
33 #define CLASS_IN 0x0001
35 #define MAX_NAME_LEN 256
36 #define MAX_DATA_LEN 8096
40 static struct uloop_fd ufd
;
41 static struct uloop_timeout cname_gc_timer
;
42 static AVL_TREE(cname_cache
, avl_strcmp
, false, NULL
);
82 static void *pkt_peek(struct packet
*pkt
, unsigned int len
)
91 static void *pkt_pull(struct packet
*pkt
, unsigned int len
)
93 void *ret
= pkt_peek(pkt
, len
);
104 static int pkt_pull_name(struct packet
*pkt
, const void *hdr
, char *dest
)
109 len
= dn_expand(hdr
, pkt
->buffer
+ pkt
->len
, pkt
->buffer
,
110 (void *)dest
, MAX_NAME_LEN
);
112 len
= dn_skipname(pkt
->buffer
, pkt
->buffer
+ pkt
->len
- 1);
114 if (len
< 0 || !pkt_pull(pkt
, len
))
121 proto_is_vlan(uint16_t proto
)
123 return proto
== ETH_P_8021Q
|| proto
== ETH_P_8021AD
;
127 cname_cache_set(const char *name
, uint8_t dscp
, uint32_t seq
)
129 struct cname_entry
*e
;
131 e
= avl_find_element(&cname_cache
, name
, e
, node
);
135 e
= calloc_a(sizeof(*e
), &name_buf
, strlen(name
) + 1);
136 e
->node
.key
= strcpy(name_buf
, name
);
137 avl_insert(&cname_cache
, &e
->node
);
146 cname_cache_get(const char *name
, uint8_t *dscp
, uint32_t *seq
)
148 struct cname_entry
*e
;
150 e
= avl_find_element(&cname_cache
, name
, e
, node
);
154 if (*dscp
== 0xff || e
->seq
< *seq
) {
163 dns_parse_question(struct packet
*pkt
, const void *hdr
, uint8_t *dscp
, uint32_t *seq
)
165 char qname
[MAX_NAME_LEN
];
167 if (pkt_pull_name(pkt
, hdr
, qname
) ||
168 !pkt_pull(pkt
, sizeof(struct dns_question
)))
171 cname_cache_get(qname
, dscp
, seq
);
172 qosify_map_lookup_dns_entry(qname
, false, dscp
, seq
);
178 dns_parse_answer(struct packet
*pkt
, void *hdr
, uint8_t *dscp
, uint32_t *seq
)
180 struct qosify_map_data data
= {};
181 char cname
[MAX_NAME_LEN
];
182 struct dns_answer
*a
;
187 if (pkt_pull_name(pkt
, hdr
, NULL
))
190 a
= pkt_pull(pkt
, sizeof(*a
));
194 len
= be16_to_cpu(a
->rdlength
);
195 rdata
= pkt_pull(pkt
, len
);
199 switch (be16_to_cpu(a
->type
)) {
201 if (dn_expand(hdr
, pkt
->buffer
+ pkt
->len
, rdata
,
202 cname
, sizeof(cname
)) < 0)
205 qosify_map_lookup_dns_entry(cname
, true, dscp
, seq
);
206 cname_cache_set(cname
, *dscp
, *seq
);
210 data
.id
= CL_MAP_IPV4_ADDR
;
211 memcpy(&data
.addr
, rdata
, 4);
214 data
.id
= CL_MAP_IPV6_ADDR
;
215 memcpy(&data
.addr
, rdata
, 16);
224 prev_timeout
= qosify_map_timeout
;
225 qosify_map_timeout
= be32_to_cpu(a
->ttl
);
226 __qosify_map_set_entry(&data
);
227 qosify_map_timeout
= prev_timeout
;
233 qosify_dns_data_cb(struct packet
*pkt
)
235 struct dns_header
*h
;
236 uint32_t lookup_seq
= 0;
240 h
= pkt_pull(pkt
, sizeof(*h
));
244 if ((h
->flags
& cpu_to_be16(FLAG_RESPONSE
| FLAG_OPCODE
| FLAG_RCODE
)) !=
245 cpu_to_be16(FLAG_RESPONSE
))
248 if (h
->questions
!= cpu_to_be16(1))
251 if (dns_parse_question(pkt
, h
, &dscp
, &lookup_seq
))
254 for (i
= 0; i
< be16_to_cpu(h
->answers
); i
++)
255 if (dns_parse_answer(pkt
, h
, &dscp
, &lookup_seq
))
260 qosify_dns_packet_cb(struct packet
*pkt
)
267 eth
= pkt_pull(pkt
, sizeof(*eth
));
271 proto
= be16_to_cpu(eth
->h_proto
);
272 if (proto_is_vlan(proto
)) {
273 struct vlan_hdr
*vlan
;
275 vlan
= pkt_pull(pkt
, sizeof(*vlan
));
279 proto
= be16_to_cpu(vlan
->proto
);
284 ip
= pkt_peek(pkt
, sizeof(struct ip
));
288 if (!pkt_pull(pkt
, ip
->ip_hl
* 4))
294 ip6
= pkt_pull(pkt
, sizeof(*ip6
));
298 proto
= ip6
->ip6_nxt
;
304 if (proto
!= IPPROTO_UDP
)
307 if (!pkt_pull(pkt
, sizeof(struct udphdr
)))
310 qosify_dns_data_cb(pkt
);
314 qosify_dns_socket_cb(struct uloop_fd
*fd
, unsigned int events
)
316 static uint8_t buf
[8192];
317 struct packet pkt
= {
323 len
= recvfrom(fd
->fd
, buf
, sizeof(buf
), MSG_DONTWAIT
, NULL
, NULL
);
334 qosify_dns_packet_cb(&pkt
);
338 qosify_cname_cache_gc(struct uloop_timeout
*timeout
)
340 struct cname_entry
*e
, *tmp
;
342 avl_for_each_element_safe(&cname_cache
, e
, node
, tmp
) {
346 avl_delete(&cname_cache
, &e
->node
);
350 uloop_timeout_set(timeout
, 1000);
354 qosify_open_dns_socket(void)
356 struct sockaddr_ll sll
= {
357 .sll_family
= AF_PACKET
,
358 .sll_protocol
= htons(ETH_P_ALL
),
362 sock
= socket(PF_PACKET
, SOCK_RAW
, htons(ETH_P_ALL
));
364 ULOG_ERR("failed to create raw socket: %s\n", strerror(errno
));
368 sll
.sll_ifindex
= if_nametoindex(QOSIFY_DNS_IFNAME
);
369 if (bind(sock
, (struct sockaddr
*)&sll
, sizeof(sll
))) {
370 ULOG_ERR("failed to bind socket to "QOSIFY_DNS_IFNAME
": %s\n",
376 ufd
.cb
= qosify_dns_socket_cb
;
377 uloop_fd_add(&ufd
, ULOOP_READ
);
387 qosify_dns_del_ifb(void)
389 qosify_run_cmd("ip link del ifb-dns type ifb", true);
392 int qosify_dns_init(void)
394 cname_gc_timer
.cb
= qosify_cname_cache_gc
;
395 qosify_cname_cache_gc(&cname_gc_timer
);
397 qosify_dns_del_ifb();
399 if (qosify_run_cmd("ip link add ifb-dns type ifb", false) ||
400 qosify_run_cmd("ip link set dev ifb-dns up", false) ||
401 qosify_open_dns_socket())
407 void qosify_dns_stop(void)
409 struct cname_entry
*e
, *tmp
;
411 if (ufd
.registered
) {
412 uloop_fd_delete(&ufd
);
416 qosify_dns_del_ifb();
418 avl_remove_all_elements(&cname_cache
, e
, node
, tmp
)