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> 7 #include <net/if.h> 8 #include <sys/socket.h> 9 #include <sys/types.h> 10 #include <errno.h> 11 #include <resolv.h> 12 13 #include <libubox/uloop.h> 14 #include <libubox/avl-cmp.h> 15 16 #define FLAG_RESPONSE 0x8000 17 #define FLAG_OPCODE 0x7800 18 #define FLAG_AUTHORATIVE 0x0400 19 #define FLAG_RCODE 0x000f 20 21 #define TYPE_A 0x0001 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 28 29 #define IS_COMPRESSED(x) ((x & 0xc0) == 0xc0) 30 31 #define CLASS_FLUSH 0x8000 32 #define CLASS_UNICAST 0x8000 33 #define CLASS_IN 0x0001 34 35 #define MAX_NAME_LEN 256 36 #define MAX_DATA_LEN 8096 37 38 #include "qosify.h" 39 40 static struct uloop_fd ufd; 41 static struct uloop_timeout cname_gc_timer; 42 static AVL_TREE(cname_cache, avl_strcmp, false, NULL); 43 44 static uint64_t dns_cache_hits; 45 static uint64_t dns_cache_misses; 46 static uint32_t dns_cache_entry_count; 47 48 struct vlan_hdr { 49 uint16_t tci; 50 uint16_t proto; 51 }; 52 53 struct packet { 54 void *buffer; 55 unsigned int len; 56 }; 57 58 struct dns_header { 59 uint16_t id; 60 uint16_t flags; 61 uint16_t questions; 62 uint16_t answers; 63 uint16_t authority; 64 uint16_t additional; 65 } __packed; 66 67 struct dns_question { 68 uint16_t type; 69 uint16_t class; 70 } __packed; 71 72 struct dns_answer { 73 uint16_t type; 74 uint16_t class; 75 uint32_t ttl; 76 uint16_t rdlength; 77 } __packed; 78 79 struct cname_entry { 80 struct avl_node node; 81 uint32_t seq; 82 uint32_t pattern_id; 83 uint8_t dscp; 84 uint8_t age; 85 }; 86 87 static void *pkt_peek(struct packet *pkt, unsigned int len) 88 { 89 if (len > pkt->len) 90 return NULL; 91 92 return pkt->buffer; 93 } 94 95 96 static void *pkt_pull(struct packet *pkt, unsigned int len) 97 { 98 void *ret = pkt_peek(pkt, len); 99 100 if (!ret) 101 return NULL; 102 103 pkt->buffer += len; 104 pkt->len -= len; 105 106 return ret; 107 } 108 109 static int pkt_pull_name(struct packet *pkt, const void *hdr, char *dest) 110 { 111 int len; 112 113 if (dest) 114 len = dn_expand(hdr, pkt->buffer + pkt->len, pkt->buffer, 115 (void *)dest, MAX_NAME_LEN); 116 else 117 len = dn_skipname(pkt->buffer, pkt->buffer + pkt->len - 1); 118 119 if (len < 0 || !pkt_pull(pkt, len)) 120 return -1; 121 122 return 0; 123 } 124 125 static bool 126 proto_is_vlan(uint16_t proto) 127 { 128 return proto == ETH_P_8021Q || proto == ETH_P_8021AD; 129 } 130 131 static void 132 cname_cache_set(const char *name, uint8_t dscp, uint32_t seq, uint32_t pattern_id) 133 { 134 struct cname_entry *e; 135 136 e = avl_find_element(&cname_cache, name, e, node); 137 if (!e) { 138 char *name_buf; 139 140 e = calloc_a(sizeof(*e), &name_buf, strlen(name) + 1); 141 e->node.key = strcpy(name_buf, name); 142 avl_insert(&cname_cache, &e->node); 143 dns_cache_entry_count++; 144 } 145 146 e->age = 0; 147 e->dscp = dscp; 148 e->seq = seq; 149 e->pattern_id = pattern_id; 150 } 151 152 static int 153 cname_cache_get(const char *name, uint8_t *dscp, uint32_t *seq, uint32_t *pattern_id) 154 { 155 struct cname_entry *e; 156 157 e = avl_find_element(&cname_cache, name, e, node); 158 if (!e) { 159 dns_cache_misses++; 160 return -1; 161 } 162 163 dns_cache_hits++; 164 if (*dscp == 0xff || e->seq < *seq) { 165 *dscp = e->dscp; 166 *seq = e->seq; 167 if (pattern_id) 168 *pattern_id = e->pattern_id; 169 } 170 171 return 0; 172 } 173 174 static int 175 dns_parse_question(struct packet *pkt, const void *hdr, uint8_t *dscp, 176 uint32_t *seq, uint32_t *pattern_id) 177 { 178 char qname[MAX_NAME_LEN]; 179 180 if (pkt_pull_name(pkt, hdr, qname) || 181 !pkt_pull(pkt, sizeof(struct dns_question))) 182 return -1; 183 184 cname_cache_get(qname, dscp, seq, pattern_id); 185 qosify_map_lookup_dns_entry(qname, false, dscp, seq, pattern_id); 186 187 return 0; 188 } 189 190 static int 191 dns_parse_answer(struct packet *pkt, void *hdr, uint8_t *dscp, uint32_t *seq, 192 uint32_t *pattern_id) 193 { 194 struct qosify_map_data data = {}; 195 char cname[MAX_NAME_LEN]; 196 struct dns_answer *a; 197 int prev_timeout; 198 void *rdata; 199 int len; 200 201 if (pkt_pull_name(pkt, hdr, NULL)) 202 return -1; 203 204 a = pkt_pull(pkt, sizeof(*a)); 205 if (!a) 206 return -1; 207 208 len = be16_to_cpu(a->rdlength); 209 rdata = pkt_pull(pkt, len); 210 if (!rdata) 211 return -1; 212 213 switch (be16_to_cpu(a->type)) { 214 case TYPE_CNAME: 215 if (dn_expand(hdr, pkt->buffer + pkt->len, rdata, 216 cname, sizeof(cname)) < 0) 217 return -1; 218 219 qosify_map_lookup_dns_entry(cname, true, dscp, seq, pattern_id); 220 cname_cache_set(cname, *dscp, *seq, pattern_id ? *pattern_id : 0); 221 222 return 0; 223 case TYPE_A: 224 data.id = CL_MAP_IPV4_ADDR; 225 memcpy(&data.addr, rdata, 4); 226 break; 227 case TYPE_AAAA: 228 data.id = CL_MAP_IPV6_ADDR; 229 memcpy(&data.addr, rdata, 16); 230 break; 231 default: 232 return 0; 233 } 234 235 data.user = true; 236 data.dscp = *dscp; 237 data.pattern_id = pattern_id ? *pattern_id : 0; 238 239 prev_timeout = qosify_map_timeout; 240 qosify_map_timeout = be32_to_cpu(a->ttl); 241 __qosify_map_set_entry(&data); 242 qosify_map_timeout = prev_timeout; 243 244 return 0; 245 } 246 247 static void 248 qosify_dns_data_cb(struct packet *pkt) 249 { 250 struct dns_header *h; 251 uint32_t lookup_seq = 0; 252 uint32_t pattern_id = 0; 253 uint8_t dscp = 0xff; 254 int i; 255 256 h = pkt_pull(pkt, sizeof(*h)); 257 if (!h) 258 return; 259 260 if ((h->flags & cpu_to_be16(FLAG_RESPONSE | FLAG_OPCODE | FLAG_RCODE)) != 261 cpu_to_be16(FLAG_RESPONSE)) 262 return; 263 264 if (h->questions != cpu_to_be16(1)) 265 return; 266 267 if (dns_parse_question(pkt, h, &dscp, &lookup_seq, &pattern_id)) 268 return; 269 270 for (i = 0; i < be16_to_cpu(h->answers); i++) 271 if (dns_parse_answer(pkt, h, &dscp, &lookup_seq, &pattern_id)) 272 return; 273 } 274 275 static void 276 qosify_dns_packet_cb(struct packet *pkt) 277 { 278 struct ethhdr *eth; 279 struct ip6_hdr *ip6; 280 struct ip *ip; 281 uint16_t proto; 282 283 eth = pkt_pull(pkt, sizeof(*eth)); 284 if (!eth) 285 return; 286 287 proto = be16_to_cpu(eth->h_proto); 288 if (proto_is_vlan(proto)) { 289 struct vlan_hdr *vlan; 290 291 vlan = pkt_pull(pkt, sizeof(*vlan)); 292 if (!vlan) 293 return; 294 295 proto = be16_to_cpu(vlan->proto); 296 } 297 298 switch (proto) { 299 case ETH_P_IP: 300 ip = pkt_peek(pkt, sizeof(struct ip)); 301 if (!ip) 302 return; 303 304 if (!pkt_pull(pkt, ip->ip_hl * 4)) 305 return; 306 307 proto = ip->ip_p; 308 break; 309 case ETH_P_IPV6: 310 ip6 = pkt_pull(pkt, sizeof(*ip6)); 311 if (!ip6) 312 return; 313 314 proto = ip6->ip6_nxt; 315 break; 316 default: 317 return; 318 } 319 320 if (proto != IPPROTO_UDP) 321 return; 322 323 if (!pkt_pull(pkt, sizeof(struct udphdr))) 324 return; 325 326 qosify_dns_data_cb(pkt); 327 } 328 329 static void 330 qosify_dns_socket_cb(struct uloop_fd *fd, unsigned int events) 331 { 332 static uint8_t buf[8192]; 333 struct packet pkt = { 334 .buffer = buf, 335 }; 336 int len; 337 338 retry: 339 len = recvfrom(fd->fd, buf, sizeof(buf), MSG_DONTWAIT, NULL, NULL); 340 if (len < 0) { 341 if (errno == EINTR) 342 goto retry; 343 return; 344 } 345 346 if (!len) 347 return; 348 349 pkt.len = len; 350 qosify_dns_packet_cb(&pkt); 351 } 352 353 static void 354 qosify_cname_cache_gc(struct uloop_timeout *timeout) 355 { 356 struct cname_entry *e, *tmp; 357 358 avl_for_each_element_safe(&cname_cache, e, node, tmp) { 359 if (e->age++ < 5) 360 continue; 361 362 avl_delete(&cname_cache, &e->node); 363 dns_cache_entry_count--; 364 free(e); 365 } 366 367 uloop_timeout_set(timeout, 1000); 368 } 369 370 static int 371 qosify_open_dns_socket(void) 372 { 373 struct sockaddr_ll sll = { 374 .sll_family = AF_PACKET, 375 .sll_protocol = htons(ETH_P_ALL), 376 }; 377 int sock; 378 379 sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); 380 if (sock == -1) { 381 ULOG_ERR("failed to create raw socket: %s\n", strerror(errno)); 382 return -1; 383 } 384 385 sll.sll_ifindex = if_nametoindex(QOSIFY_DNS_IFNAME); 386 if (bind(sock, (struct sockaddr *)&sll, sizeof(sll))) { 387 ULOG_ERR("failed to bind socket to "QOSIFY_DNS_IFNAME": %s\n", 388 strerror(errno)); 389 goto error; 390 } 391 392 ufd.fd = sock; 393 ufd.cb = qosify_dns_socket_cb; 394 uloop_fd_add(&ufd, ULOOP_READ); 395 396 return 0; 397 398 error: 399 close(sock); 400 return -1; 401 } 402 403 static void 404 qosify_dns_del_ifb(void) 405 { 406 qosify_run_cmd("ip link del ifb-dns type ifb", true); 407 } 408 409 int qosify_dns_init(void) 410 { 411 cname_gc_timer.cb = qosify_cname_cache_gc; 412 qosify_cname_cache_gc(&cname_gc_timer); 413 414 qosify_dns_del_ifb(); 415 416 if (qosify_run_cmd("ip link add ifb-dns type ifb", false) || 417 qosify_run_cmd("ip link set dev ifb-dns up", false) || 418 qosify_open_dns_socket()) 419 return -1; 420 421 return 0; 422 } 423 424 void qosify_dns_stop(void) 425 { 426 struct cname_entry *e, *tmp; 427 428 if (ufd.registered) { 429 uloop_fd_delete(&ufd); 430 close(ufd.fd); 431 } 432 433 qosify_dns_del_ifb(); 434 435 avl_remove_all_elements(&cname_cache, e, node, tmp) 436 free(e); 437 438 dns_cache_entry_count = 0; 439 } 440 441 void qosify_dns_get_stats(struct blob_buf *b, bool reset) 442 { 443 blobmsg_add_u64(b, "hits", dns_cache_hits); 444 blobmsg_add_u64(b, "misses", dns_cache_misses); 445 blobmsg_add_u32(b, "size", dns_cache_entry_count); 446 447 if (reset) { 448 dns_cache_hits = 0; 449 dns_cache_misses = 0; 450 } 451 } 452 453
This page was automatically generated by LXR 0.3.1. • OpenWrt