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