diff --git a/mdns.c b/mdns.c index 3553f04..6a1853d 100644 --- a/mdns.c +++ b/mdns.c @@ -601,14 +601,13 @@ uint32_t mdns_read_u32(const uint8_t *ptr) { ((ptr[3] & 0xFF) << 0); } -// initialize the packet for reply // clears the packet of list structures but not its list items -void mdns_init_reply(struct mdns_pkt *pkt, uint16_t id) { +void mdns_init_pkt(struct mdns_pkt *pkt, uint16_t id) { // copy transaction ID pkt->id = id; - // response flags - pkt->flags = MDNS_FLAG_RESP | MDNS_FLAG_AA; + // question flags + pkt->flags = 0; rr_list_destroy(pkt->rr_qn, 0); rr_list_destroy(pkt->rr_ans, 0); @@ -626,6 +625,14 @@ void mdns_init_reply(struct mdns_pkt *pkt, uint16_t id) { pkt->num_add_rr = 0; } +// initialize the packet for reply +void mdns_init_reply(struct mdns_pkt *pkt, uint16_t id) { + mdns_init_pkt(pkt, id); + + // response flags + pkt->flags = MDNS_FLAG_RESP | MDNS_FLAG_AA; +} + // destroys an mdns_pkt struct, including its contents void mdns_pkt_destroy(struct mdns_pkt *p) { rr_list_destroy(p->rr_qn, 1); @@ -640,13 +647,11 @@ void mdns_pkt_destroy(struct mdns_pkt *p) { // parse the MDNS questions section // stores the parsed data in the given mdns_pkt struct static size_t mdns_parse_qn(uint8_t *pkt_buf, size_t pkt_len, size_t off, - struct mdns_pkt *pkt) { + struct rr_list **list) { const uint8_t *p = pkt_buf + off; struct rr_entry *rr; uint8_t *name; - assert(pkt != NULL); - rr = malloc(sizeof(struct rr_entry)); memset(rr, 0, sizeof(struct rr_entry)); @@ -661,7 +666,7 @@ static size_t mdns_parse_qn(uint8_t *pkt_buf, size_t pkt_len, size_t off, rr->rr_class = mdns_read_u16(p) & ~0x80; p += sizeof(uint16_t); - rr_list_append(&pkt->rr_qn, rr); + rr_list_append(list, rr); return p - (pkt_buf + off); } @@ -669,7 +674,7 @@ static size_t mdns_parse_qn(uint8_t *pkt_buf, size_t pkt_len, size_t off, // parse the MDNS RR section // stores the parsed data in the given mdns_pkt struct static size_t mdns_parse_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, - struct mdns_pkt *pkt) { + struct rr_list **list) { const uint8_t *p = pkt_buf + off; const uint8_t *e = pkt_buf + pkt_len; struct rr_entry *rr; @@ -678,10 +683,10 @@ static size_t mdns_parse_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, struct rr_data_txt *txt_rec; int parse_error = 0; - assert(pkt != NULL); - - if (off > pkt_len) + if (off > pkt_len) { + DEBUG_PRINTF("error length %ld %ld\n", off, pkt_len); return 0; + } rr = malloc(sizeof(struct rr_entry)); memset(rr, 0, sizeof(struct rr_entry)); @@ -704,6 +709,13 @@ static size_t mdns_parse_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, rr_data_len = mdns_read_u16(p); p += sizeof(uint16_t); + if (rr_data_len == 0) + { + DEBUG_PRINTF("no data %ld\n", p - (pkt_buf + off)); + rr_list_append(list, rr); + return p - (pkt_buf + off); + } + if (p + rr_data_len > e) { DEBUG_PRINTF("rr_data_len goes beyond packet buffer: %zu > %zu\n", rr_data_len, e - p); rr_entry_destroy(rr); @@ -778,6 +790,18 @@ static size_t mdns_parse_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, } break; + case RR_SRV: + srv_rec = &rr->data.SRV; + srv_rec->priority = mdns_read_u16(p); + p += sizeof(uint16_t); + srv_rec->weight = mdns_read_u16(p); + p += sizeof(uint16_t); + srv_rec->port = mdns_read_u16(p); + p += sizeof(uint16_t); + srv_rec->target = uncompress_nlabel(pkt_buf, pkt_len, p - pkt_buf); + p += label_len(pkt_buf, pkt_len, p - pkt_buf); + break; + default: // skip to end of RR data p = e; @@ -789,7 +813,7 @@ static size_t mdns_parse_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, return 0; } - rr_list_append(&pkt->rr_ans, rr); + rr_list_append(list, rr); return p - (pkt_buf + off); } @@ -818,7 +842,7 @@ struct mdns_pkt *mdns_parse_pkt(uint8_t *pkt_buf, size_t pkt_len) { // parse questions for (i = 0; i < pkt->num_qn; i++) { - size_t l = mdns_parse_qn(pkt_buf, pkt_len, off, pkt); + size_t l = mdns_parse_qn(pkt_buf, pkt_len, off, &pkt->rr_qn); if (! l) { DEBUG_PRINTF("error parsing question #%d\n", i); mdns_pkt_destroy(pkt); @@ -830,7 +854,7 @@ struct mdns_pkt *mdns_parse_pkt(uint8_t *pkt_buf, size_t pkt_len) { // parse answer RRs for (i = 0; i < pkt->num_ans_rr; i++) { - size_t l = mdns_parse_rr(pkt_buf, pkt_len, off, pkt); + size_t l = mdns_parse_rr(pkt_buf, pkt_len, off, &pkt->rr_ans); if (! l) { DEBUG_PRINTF("error parsing answer #%d\n", i); mdns_pkt_destroy(pkt); @@ -840,7 +864,29 @@ struct mdns_pkt *mdns_parse_pkt(uint8_t *pkt_buf, size_t pkt_len) { off += l; } - // TODO: parse the authority and additional RR sections + // parse authority RRs + for (i = 0; i < pkt->num_auth_rr; i++) { + size_t l = mdns_parse_rr(pkt_buf, pkt_len, off, &pkt->rr_auth); + if (! l) { + DEBUG_PRINTF("error parsing authority #%d\n", i); + mdns_pkt_destroy(pkt); + return NULL; + } + + off += l; + } + + // parse additional RRs + for (i = 0; i < pkt->num_add_rr; i++) { + size_t l = mdns_parse_rr(pkt_buf, pkt_len, off, &pkt->rr_add); + if (! l) { + DEBUG_PRINTF("error parsing additional #%d\n", i); + mdns_pkt_destroy(pkt); + return NULL; + } + + off += l; + } return pkt; } @@ -936,10 +982,12 @@ static size_t mdns_encode_rr(uint8_t *pkt_buf, size_t pkt_len, size_t off, break; case RR_PTR: - label = rr->data.PTR.name ? - rr->data.PTR.name : - rr->data.PTR.entry->name; - p += mdns_encode_name(pkt_buf, pkt_len, p - pkt_buf, label, comp); + if (rr->data.PTR.name != NULL || rr->data.PTR.entry != NULL) { + label = rr->data.PTR.name ? + rr->data.PTR.name : + rr->data.PTR.entry->name; + p += mdns_encode_name(pkt_buf, pkt_len, p - pkt_buf, label, comp); + } break; case RR_TXT: @@ -996,7 +1044,7 @@ size_t mdns_encode_pkt(struct mdns_pkt *answer, uint8_t *pkt_buf, size_t pkt_len //uint8_t *e = pkt_buf + pkt_len; size_t off; int i; - struct rr_list *rr_set[3]; + struct rr_list *rr_set[4]; assert(answer != NULL); assert(pkt_len >= 12); @@ -1004,9 +1052,6 @@ size_t mdns_encode_pkt(struct mdns_pkt *answer, uint8_t *pkt_buf, size_t pkt_len if (p == NULL) return -1; - // this is an Answer - number of qns should be zero - assert(answer->num_qn == 0); - p = mdns_write_u16(p, answer->id); p = mdns_write_u16(p, answer->flags); p = mdns_write_u16(p, answer->num_qn); @@ -1027,9 +1072,10 @@ size_t mdns_encode_pkt(struct mdns_pkt *answer, uint8_t *pkt_buf, size_t pkt_len comp->pos = 0; // skip encoding of qn - rr_set[0] = answer->rr_ans; - rr_set[1] = answer->rr_auth; - rr_set[2] = answer->rr_add; + rr_set[0] = answer->rr_qn; + rr_set[1] = answer->rr_ans; + rr_set[2] = answer->rr_auth; + rr_set[3] = answer->rr_add; // encode answer, authority and additional RRs for (i = 0; i < sizeof(rr_set) / sizeof(rr_set[0]); i++) { diff --git a/mdns.h b/mdns.h index 42a8423..f8504c9 100644 --- a/mdns.h +++ b/mdns.h @@ -169,6 +169,7 @@ struct mdns_pkt { struct mdns_pkt *mdns_parse_pkt(uint8_t *pkt_buf, size_t pkt_len); void mdns_init_reply(struct mdns_pkt *pkt, uint16_t id); +void mdns_init_pkt(struct mdns_pkt *pkt, uint16_t id); size_t mdns_encode_pkt(struct mdns_pkt *answer, uint8_t *pkt_buf, size_t pkt_len); void mdns_pkt_destroy(struct mdns_pkt *p); diff --git a/mdnsd.c b/mdnsd.c index 7eb486e..6466e8b 100644 --- a/mdnsd.c +++ b/mdnsd.c @@ -90,6 +90,8 @@ struct mdns_service { struct rr_list *entries; }; +static int display_mdns_segment(struct mdnsd *svr, struct rr_list *segm, const char *name); + ///////////////////////////////// @@ -277,6 +279,93 @@ static void announce_srv(struct mdnsd *svr, struct mdns_pkt *reply, uint8_t *nam // additional records for additional records add_related_rr(svr, reply->rr_add, reply); + + display_mdns_segment(svr, reply->rr_ans, "announce ans"); + display_mdns_segment(svr, reply->rr_add, "announce add"); +} + +static void query_svr(struct mdnsd *svr, struct mdns_pkt *query) { + mdns_init_pkt(query, 0); + + struct rr_list *n; + n = svr->queries; + for (; n; n = n->next) { + rr_list_append(&query->rr_qn, n->e); + query->num_qn++; + } +} + +static int display_rr_ptr(struct mdnsd *svr, struct rr_entry *ptr) { + int ret = -1; + char *namestr = nlabel_to_str(ptr->name); + DEBUG_PRINTF("type %s (%02x) %s - ", rr_get_type_name(ptr->type), ptr->type, namestr); + switch (ptr->type) { + case RR_PTR: + if (ptr->data.PTR.name != NULL) { + char *namestr = nlabel_to_str(ptr->data.PTR.name); + DEBUG_PRINTF("name: %s ", namestr); + free(namestr); + } else if (ptr->name != NULL) { + char *namestr = nlabel_to_str(ptr->name); + DEBUG_PRINTF("name: %s ", namestr); + free(namestr); + } + if (ptr->data.PTR.entry != NULL) { + DEBUG_PRINTF("ptr: "); + display_rr_ptr(svr, ptr->data.PTR.entry); + } + break; + case RR_SRV: + DEBUG_PRINTF("port: %d - %d - %d", ptr->data.SRV.port, ptr->data.SRV.weight, ptr->data.SRV.priority); + if (ptr->data.SRV.target != NULL) { + char *namestr = nlabel_to_str(ptr->data.SRV.target); + DEBUG_PRINTF(" target: %s", namestr); + free(namestr); + } + break; + case RR_TXT: { + struct rr_data_txt *txte; + DEBUG_PRINTF("txt"); + for (txte = &ptr->data.TXT; txte; txte = txte->next) { + char *namestr = nlabel_to_str(txte->txt); + DEBUG_PRINTF(" %s -", namestr); + free(namestr); + } + } + break; + case RR_A: { + struct in_addr in; + in.s_addr = ptr->data.A.addr; + DEBUG_PRINTF("address: %s", inet_ntoa(in)); + } + default: + break; + } + struct rr_entry *e = rr_entry_match(svr->queries, ptr); + if (e != NULL) { + DEBUG_PRINTF("receive answer for query %s\n", namestr); + if (ret == -1) + ret = 0; + } + free(namestr); + return ret; +} + +static int display_mdns_segment(struct mdnsd *svr, struct rr_list *segm, const char *name) { + int i; + struct rr_list *ans; + int ret = -1; + + ans = NULL; i = 0; + for (ans = segm; ans; ) { + struct rr_list *next = ans->next; + DEBUG_PRINTF("%s #%d: ", name, i); + ret = display_rr_ptr(svr, ans->e); + DEBUG_PRINTF("\n"); + ans = next; + i++; + } + return ret; } // processes the incoming MDNS packet @@ -285,20 +374,22 @@ static int process_mdns_pkt(struct mdnsd *svr, struct mdns_pkt *pkt, struct mdns int i; struct rr_list *qnl; struct rr_list *ans, *prev_ans; + int ret = 0; assert(pkt != NULL); + DEBUG_PRINTF("id = %04x, flags = %04x, qn = %d, ans = %d, add = %d %p\n", + pkt->id, + pkt->flags, + pkt->num_qn, + pkt->num_ans_rr, + pkt->num_add_rr, pkt->rr_add); + // is it standard query? if ((pkt->flags & MDNS_FLAG_RESP) == 0 && MDNS_FLAG_GET_OPCODE(pkt->flags) == 0) { mdns_init_reply(reply, pkt->id); - DEBUG_PRINTF("flags = %04x, qn = %d, ans = %d, add = %d\n", - pkt->flags, - pkt->num_qn, - pkt->num_ans_rr, - pkt->num_add_rr); - // loop through questions qnl = pkt->rr_qn; for (i = 0; i < pkt->num_qn; i++, qnl = qnl->next) { @@ -357,12 +448,15 @@ static int process_mdns_pkt(struct mdnsd *svr, struct mdns_pkt *pkt, struct mdns // additional records for additional records add_related_rr(svr, reply->rr_add, reply); - DEBUG_PRINTF("\n"); - - return reply->num_ans_rr; + ret = reply->num_ans_rr; } - return 0; + display_mdns_segment(svr, pkt->rr_ans, "ans"); + display_mdns_segment(svr, pkt->rr_auth, "auth"); + display_mdns_segment(svr, pkt->rr_add, "add"); + DEBUG_PRINTF("\n"); + + return ret; } int create_pipe(int handles[2]) {