static void
data_header_print(u_int16_t fc, const u_char *p, const u_int8_t **srcp,
const u_int8_t **dstp)
{
u_int subtype = FC_SUBTYPE(fc);
if (DATA_FRAME_IS_CF_ACK(subtype) || DATA_FRAME_IS_CF_POLL(subtype) ||
DATA_FRAME_IS_QOS(subtype)) {
printf("CF ");
if (DATA_FRAME_IS_CF_ACK(subtype)) {
if (DATA_FRAME_IS_CF_POLL(subtype))
printf("Ack/Poll");
else
printf("Ack");
} else {
if (DATA_FRAME_IS_CF_POLL(subtype))
printf("Poll");
}
if (DATA_FRAME_IS_QOS(subtype))
printf("+QoS");
printf(" ");
}
#define ADDR1 (p + 4)
#define ADDR2 (p + 10)
#define ADDR3 (p + 16)
#define ADDR4 (p + 24)
if (!FC_TO_DS(fc) && !FC_FROM_DS(fc)) {
if (srcp != NULL)
*srcp = ADDR2;
if (dstp != NULL)
*dstp = ADDR1;
if (!eflag)
return;
printf("DA:%s SA:%s BSSID:%s ",
etheraddr_string(ADDR1), etheraddr_string(ADDR2),
etheraddr_string(ADDR3));
} else if (!FC_TO_DS(fc) && FC_FROM_DS(fc)) {
if (srcp != NULL)
*srcp = ADDR3;
if (dstp != NULL)
*dstp = ADDR1;
if (!eflag)
return;
printf("DA:%s BSSID:%s SA:%s ",
etheraddr_string(ADDR1), etheraddr_string(ADDR2),
etheraddr_string(ADDR3));
} else if (FC_TO_DS(fc) && !FC_FROM_DS(fc)) {
if (srcp != NULL)
*srcp = ADDR2;
if (dstp != NULL)
*dstp = ADDR3;
if (!eflag)
return;
printf("BSSID:%s SA:%s DA:%s ",
etheraddr_string(ADDR1), etheraddr_string(ADDR2),
etheraddr_string(ADDR3));
} else if (FC_TO_DS(fc) && FC_FROM_DS(fc)) {
if (srcp != NULL)
*srcp = ADDR4;
if (dstp != NULL)
*dstp = ADDR3;
if (!eflag)
return;
printf("RA:%s TA:%s DA:%s SA:%s ",
etheraddr_string(ADDR1), etheraddr_string(ADDR2),
etheraddr_string(ADDR3), etheraddr_string(ADDR4));
}
#undef ADDR1
#undef ADDR2
#undef ADDR3
#undef ADDR4
}
int mac_header_parser(unsigned char * p,
int pkt_len,
int cap_len,
int path_type,
int radiotap_len)
{
unsigned char * p_start ;
p_start = p;
if (debug_mode) {
if (path_type ==1 ) {
p += radiotap_len ; // HOMESAW_TX_FRAME_HEADER;
if (*(p+1)==0x0 && *(p) ==0x50)
return 0; //mgmt frames transmitted by router
if (*(p+1)==0x0 && *(p) ==0x84)
return 0; //control frames transmitted by router
if (*(p+1)==0x42 && *(p) ==0x8)
return 0; //control frames transmitted by router
if(*(p)==0xc0 && *(p+1)==0x0 )
return 0 ; //mgmt type but is 0c (deauth)
//printf("\nin mp: mac header: %02x %02x %d \n", *p, *(p+1),pkt_len );
}
else{
p += radiotap_len ; //HOMESAW_RX_FRAME_HEADER;
}
}
p += radiotap_len ; //HOMESAW_RX/TX_FRAME_HEADER;
memset(&clh,0,sizeof(struct control_layer_header));
memset(&mlh,0,sizeof(struct mgmt_beacon_layer_header));
memset(&mlh_t,0,sizeof(struct mgmt_layer_err_header));
memset(&dlh,0,sizeof(struct data_layer_header));
u_int16_t fc = EXTRACT_LE_16BITS(p);
struct mgmt_header_t *mgmt_h =NULL;
struct ctrl_ps_poll_t * c_poll = NULL ;
struct ctrl_bar_t * c_bar =NULL;
struct ctrl_rts_t * rts = NULL;
struct ctrl_cts_t *cts= NULL;
switch (FC_TYPE(fc)) {
case MGT_FRAME:
mgmt_h = (struct mgmt_header_t *) p;
switch(FC_SUBTYPE(fc)){
case ST_BEACON:
memcpy(mlh.src_mac,mgmt_h->sa,6);
mlh.pkt_len=pkt_len;
mlh.frame_control = fc ;
mlh.seq_ctrl = pletohs(&(mgmt_h->seq_ctrl));
parse_beacon(p+MGT_FRAME_HDR_LEN, (unsigned int)cap_len, &mlh );
mgmt_beacon_count++;
if(mgmt_beacon_count<512){
address_mgmt_beacon_table_update(&mgmt_beacon_address_table, p_start, &mlh);
break;
}
default :
memcpy(mlh_t.src_mac,mgmt_h->sa,6);
mlh_t.pkt_len=pkt_len;
mlh_t.frame_control = fc ;
mlh_t.seq_ctrl = pletohs(&(mgmt_h->seq_ctrl)); //EXTRACT_LE_16BITS(mgmt_h->seq_ctrl);
address_mgmt_common_table_update(&mgmt_common_address_table, p_start, &mlh_t);
break ;
}
break ;
case CONTROL_FRAME:
clh.pkt_len= pkt_len;
clh.frame_control =fc ;
switch(FC_SUBTYPE(fc)){
case CTRL_BAR:
c_bar = (struct ctrl_bar_t *)p;
memcpy(clh.src_mac,c_bar->ra,6);
address_control_table_update(&control_address_table , p_start, &clh);
break ;
case CTRL_PS_POLL :
c_poll = (struct ctrl_ps_poll_t *)p;
memcpy(clh.src_mac,c_poll->bssid,6);
address_control_table_update(&control_address_table , p_start, &clh);
break ;
case CTRL_RTS :
rts = (struct ctrl_rts_t *) p;
memcpy(clh.src_mac,rts->ra,6);
address_control_table_update(&control_address_table , p_start, &clh);
if (debug_mode) {
print_mac(rts->ta,"rts ta ");
}
break;
/* Not used as all kinds of packets are not useful and then we were
case CTRL_ACK :
cts= (struct ctrl_cts_t * ) p;
print_mac(cts->ra, "ack frames\n ");
break ;
case CTRL_END_ACK:
cts= (struct ctrl_cts_t * ) p;
print_mac(cts->ra, "end ack\n ");
break ;
case CTRL_CF_END:
cts= (struct ctrl_cts_t *) p;
print_mac(cts->ra, "cf end ack\n ");
*/
default : // Use the common structure of rest
cts= (struct ctrl_cts_t * ) p;
memcpy(clh.src_mac,cts->ra,6);
address_control_table_update(&control_address_table, p_start, &clh);
if (debug_mode) {
print_mac(cts->ra, "default control err\n ");
}
break;
}
break ;
case DATA_FRAME : {
// printf("data frame \n");
struct ieee80211_hdr * sc = (struct ieee80211_hdr *)p;
// printf("control sequence = %u \n", pletohs(&(sc->seq_ctrl)) );
dlh.pkt_len=pkt_len;
dlh.frame_control =fc ;
if (debug_mode) {
if( DATA_FRAME_IS_NULL(FC_SUBTYPE(fc))){
printf("null type\n");
}
else if (DATA_FRAME_IS_CF_ACK(FC_SUBTYPE(fc))){
printf("cf-type\n");
}
else if (DATA_FRAME_IS_CF_POLL(FC_SUBTYPE(fc))){
printf("poll type\n");
}
else if (DATA_FRAME_IS_QOS(FC_SUBTYPE(fc))){
printf("qos type\n");
}
if(IS_DATA_DATA(FC_SUBTYPE(fc))){
printf(" data-data\n");
}else if(IS_DATA_DATA_CF_ACK(FC_SUBTYPE(fc))){
printf("cf-ack\n");
}else if(IS_DATA_DATA_CF_POLL(FC_SUBTYPE(fc))){
printf("cf-poll\n");
}else if(IS_DATA_DATA_CF_ACK_POLL(FC_SUBTYPE(fc))){
printf("cf-poll-ack\n");
}else if(IS_DATA_NODATA(FC_SUBTYPE(fc))){
printf("no data\n");
}else if(IS_DATA_NODATA_CF_ACK(FC_SUBTYPE(fc))){
printf("nodata cf ack\n");
}else if(IS_DATA_NODATA_CF_POLL (FC_SUBTYPE(fc))){
printf("nodata cf poll\n");
}else if(IS_DATA_NODATA_CF_ACK_POLL (FC_SUBTYPE(fc))){
printf(" cf ack poll\n");
}
printf("subtype:%02x \n",FC_SUBTYPE(fc));
printf("seq ctrl : %d \n", dlh.seq_ctrl );
}
dlh.seq_ctrl = pletohs(&(sc->seq_ctrl));
int hdrlen = (FC_TO_DS(fc) && FC_FROM_DS(fc)) ? 30 : 24;
if (DATA_FRAME_IS_QOS(FC_SUBTYPE(fc)))
hdrlen += 2;
// but there is 8 bytes offset after mac header of 26 bytes, thats for qos data packet
#define ADDR1 (p + 4)
#define ADDR2 (p + 10)
#define ADDR3 (p + 16)
if (!FC_TO_DS(fc) && !FC_FROM_DS(fc)) {
memcpy(dlh.src_mac,ADDR2,6);
memcpy(dlh.dest_mac,ADDR1,6);
#ifdef DEBUG
print_mac(ADDR2,"1 addr2");
print_mac(ADDR1,"1 addr1");
#endif
} else if (!FC_TO_DS(fc) && FC_FROM_DS(fc)) {
if (radiotap_len ==HOMESAW_TX_FRAME_HEADER )
mac_address_map(&devices,ADDR1);
memcpy(dlh.src_mac,ADDR3,6);
memcpy(dlh.dest_mac,ADDR1,6);
// printf("f in anon 2 \n");
if (debug_mode) {
print_mac(ADDR3,"2 src");
print_mac(ADDR1,"2 dest");
}
} else if (FC_TO_DS(fc) && !FC_FROM_DS(fc)) {
memcpy(dlh.src_mac,ADDR2,6);
memcpy(dlh.dest_mac,ADDR3,6);
if (debug_mode) {
print_mac(ADDR2,"3 src");
print_mac(ADDR3,"3 dest");
}
} else if (FC_TO_DS(fc) && FC_FROM_DS(fc)) {
#define ADDR4 (p + 24)
memcpy(dlh.src_mac,ADDR4,6);
memcpy(dlh.dest_mac,ADDR3,6);
if (debug_mode) {
print_mac(ADDR4,"4 src");
print_mac(ADDR3,"4 dest");
}
#undef ADDR4
}
#undef ADDR1
#undef ADDR2
#undef ADDR3
address_data_table_update(&data_address_table ,p_start, &dlh,path_type, 0); // is_more flag: write tcp/udp headers to file
#ifdef TRANSPORT_LAYER_CAPTURE
transport_header_parser(fc,p_start,p+hdrlen,pkt_len,path_type,&dlh);
#endif
break ;
}
break;
default :
printf("imposs pkt ! pkt len is %d\n ",pkt_len);
exit(EXIT_FAILURE);
}
return 0 ;
}
