/*
=====================================
receiverespond.c
Author: Martin Olsson, molsson@csee.usf.edu and
Jakob Klamra, jklamra@csee.usf.edu

This program will sniff network traffic
filtering out UDP packets destined for
239.255.255.250:1900 which is the 
SSDP multicast address.
If a SSDP NOTIFY is received it is printed.
If a M-SEARCH is received it is answered
with a spoofed unicast HTTP-OK. The HTTP-OK is hardcoded
and is copied from the Intel UPnP Network light.
Please see http://www.intel.com/technology/upnp/
for download of the network light.

The program uses the library
netwib, which must be installed before
the program can be compiled or run.
Newib can be found here:
http://www.laurentconstantin.com/en/netw/netwib/
Netwib uses Winpcap which can be found here:
http://winpcap.polito.it/

This code should be compiled using
Visual C++ or Dev C++. Please refer
to netwib documentation for settings
of the compiler.

Parts of this code has been borrowed
from the testprograms for netwib.
Please download the whole developer package
from the netwib homepage to get acces
to documentation and testfiles.

This program was downloaded from
http://www.csee.usf.edu/~jklamra/upnp/
and is part of a master thesis about
power management in Universal Plug and Play.
=====================================
*/
#include <stdio.h>
#include <netwib.h>

netwib_err init_device(netwib_buf *pbuf, netwib_device_hwtype hwt);
netwib_err check_search(netwib_buf *checkbuf, netwib_bool *out);
netwib_err sniff(netwib_buf *pdevice, netwib_buf *pfilter, netwib_buf *ppacket);
netwib_err parse_packet(netwib_buf *in, netwib_device_dlttype dlt, 
                       netwib_linkhdr *plinkhdr, netwib_iphdr *piphdr,
                       netwib_udphdr *pudphdr, netwib_buf *rest);
netwib_err construct_msearch_answer(netwib_eth from_eth, netwib_eth to_eth,
						 netwib_ip from_ip, netwib_ip to_ip, netwib_port srcport, netwib_port dstport, 
						 netwib_buf *sendbuf);
netwib_err ip_checksum(netwib_iphdr *iphdr);
netwib_err spoof_send(netwib_buf *buf, netwib_buf *device, netwib_io *pio);

int main(void)
{
    netwib_err ret;
    netwib_buf device, filter, packet, rest, cmpbuf, msearchbuf, tempbuf, notifybuf, answer;
    netwib_bool event;
    netwib_io *pio;
    netwib_device_dlttype dlt;
    netwib_time t;
    netwib_linkhdr linkhdr;
    netwib_iphdr iphdr;
    netwib_udphdr udphdr;
    netwib_cmp cmp;
	netwib_conf_devices conf;
	netwib_ip my_ip;
	netwib_eth my_eth;
	

    netwib_er(netwib_fmt_display("starting\n"));
    netwib_init();
    netwib_er(netwib_buf_init_mallocdefault(&device));
    init_device(&device, NETWIB_DEVICE_HWTYPE_ETHER);
	netwib_eth_init_fields(0x00,0x01,0x02,0x03,0x04,0x05,&my_eth);//the ethernet address to spoof
	netwib_er(netwib_ip_init_ip4_fields(0,0,0,0,&my_ip)); //the IP to spoof
    
    netwib_er(netwib_buf_init_mallocdefault(&packet));
    //initiate the filter to listen for udp to SSDP-broadcast
    netwib_er(netwib_buf_init_mallocdefault(&filter));
    netwib_er(netwib_buf_append_text("udp and dst host 239.255.255.250 and dst port 1900", &filter));


    
    netwib_er(netwib_buf_init_mallocdefault(&msearchbuf));
	netwib_er(netwib_buf_init_mallocdefault(&notifybuf));
	netwib_buf_init_ext_text("4d2d534541524348", &tempbuf); //start of http when it is a m-search
	netwib_buf_decode(&tempbuf, NETWIB_DECODETYPE_HEXA, &msearchbuf);
	netwib_buf_init_ext_text("4e4f54494659", &tempbuf); // start of hht when it is a sspd-notify
	netwib_buf_decode(&tempbuf, NETWIB_DECODETYPE_HEXA, &notifybuf);
	netwib_buf_close(&tempbuf);
	
	
	netwib_er(netwib_time_init_time( NETWIB_TIME_INFINITE,&t)); // infinite time, 
    
    
    
    while(NETWIB_TRUE) {
		netwib_er(netwib_fmt_display("Receiving\n")); 
		netwib_er(netwib_buf_init_mallocdefault(&packet));
		netwib_io_init_sniff(&device, &filter, &pio); // initiate sniff session
		netwib_er(netwib_sniff_ctl_get_dlt(pio, &dlt)); // get dlt
                
        netwib_er(netwib_io_wait(pio, NETWIB_IO_WAYTYPE_READ, &t, &event)); // wait for event in io
    
        if (!event) {
             netwib_fmt_display("Nothing found \n");
        return(NETWIB_ERR_NOTFOUND);
        }
        ret = netwib_io_read(pio, &packet);
    
        if(ret == NETWIB_ERR_NOTCONVERTED) {
             netwib_er(netwib_fmt_display("Error while receiving: Data not converted"));
        }
        else if(ret == NETWIB_ERR_DATAMISSING) {
             netwib_er(netwib_fmt_display("Error while recieving: Datamissing"));
        }
	    else if (ret == NETWIB_ERR_DATANOTAVAIL) {
             netwib_er(netwib_fmt_display("Wait indicates event but nothing to read (this might be normal)\n"));
        }
        else if (ret == NETWIB_ERR_OK) {
             netwib_er(netwib_fmt_display("something received\n"));
             netwib_er(netwib_buf_init_mallocdefault(&rest));
             parse_packet(&packet, dlt, &linkhdr, &iphdr, &udphdr, &rest);
             
             netwib_er(netwib_buf_init_mallocdefault(&cmpbuf));
             netwib_buf_append_buf(&rest, &cmpbuf);
             cmpbuf.totalsize = 8;
             cmpbuf.endoffset = 8;
         
             netwib_buf_cmp(&cmpbuf, &msearchbuf, &cmp);
             if(cmp == NETWIB_CMP_EQ) {//received a M-Search, should check the service
				 //Answer the search
                    netwib_er(netwib_buf_shift(&rest, -82, 0));
					netwib_er(netwib_fmt_display("Packet is a M-search from ip %{ip}\n", &iphdr.src));
					netwib_er(netwib_buf_init_mallocdefault(&answer))
					construct_msearch_answer(my_eth, linkhdr.hdr.ether.src, my_ip, iphdr.src, udphdr.dst, 
												udphdr.src, &answer);					
					netwib_er(netwib_io_close(&pio));

					
					//init_device(&device, NETWIB_DEVICE_HWTYPE_ETHER);
					ret = spoof_send(&answer, &device, &pio);
					 if (ret == NETWIB_ERR_OK) {
					    netwib_er(netwib_fmt_display("Spoofing complete\n"));
					} else {
						netwib_er(netwib_fmt_display("Failed spoofing\n"));
					}  
					   netwib_er(netwib_buf_close(&answer));
                    
                    //netwib_fmt_display("Looking for %{buf}\n", &rest);
             }
             else {
                  cmpbuf.totalsize = 6;
                  cmpbuf.endoffset = 6;
                  netwib_buf_cmp(&cmpbuf, &notifybuf, &cmp);
                  if(cmp == NETWIB_CMP_EQ) {//received a SSDP-notify
					  //update cache
                         netwib_er(netwib_buf_shift(&rest, -78, 0));   
                         netwib_er(netwib_fmt_display("Packet is a SSDP-notify from ip %{ip}\n", &iphdr.src));
                         netwib_fmt_display("device: %{buf}\n", &rest);
						 netwib_er(netwib_io_close(&pio));
                  }
             }                                                 
        }
        
             
    }
	netwib_buf_close(&device);
	netwib_buf_close(&filter);
	netwib_buf_close(&rest);
    netwib_buf_close(&packet);
	netwib_er(netwib_io_close(&pio));
    netwib_close();
    
}


////////////////////////////////////////////////////////////////////////////
////               Answer an M-Search                                   ////
////////////////////////////////////////////////////////////////////////////
netwib_err construct_msearch_answer(netwib_eth from_eth, netwib_eth to_eth,
						 netwib_ip from_ip, netwib_ip to_ip, netwib_port srcport, netwib_port dstport, 
						 netwib_buf *sendbuf){
	netwib_iphdr iphdr;
    netwib_linkhdr linkhdr;
    netwib_udphdr udphdr;
	netwib_buf tempbuf, httpbuf, udpbuf, device;
    netwib_err ret;

	 //netwib_er(netwib_fmt_display("Init link header\n"));
    //initiate link hdr (ethernet)
    netwib_linkhdr_initdefault(NETWIB_DEVICE_DLTTYPE_EN10MB, &linkhdr);
    
    //initate eth header
    linkhdr.hdr.ether.dst = to_eth; //set dst mac address
    
    
    linkhdr.hdr.ether.src = from_eth; // set src mac address
    linkhdr.hdr.ether.type = NETWIB_ETHERHDRTYPE_IP4; //set linktype to ip4
    
    // initiate ip header
    //netwib_er(netwib_fmt_display("Init ip header\n"));
    netwib_er(netwib_iphdr_initdefault(NETWIB_IPTYPE_IP4, &iphdr));
    // sets id??
    iphdr.header.ip4.id = 123;
    
    // sets src ip
    iphdr.src = from_ip;
    
    //sets dst ip
    iphdr.dst = to_ip;
    
    //set total length
    iphdr.header.ip4.totlen = 278;
    
    // set protocol to UDP
    iphdr.protocol = NETWIB_IPPROTO_UDP;
    
    // initiate udp header
    netwib_er(netwib_udphdr_initdefault(&udphdr));
    // set src port
    udphdr.src = srcport;
    // set dst port
    udphdr.dst = dstport;
    // set length
    udphdr.len = 258;
       
    // hardcoded http (SSDP ALive)
    netwib_er(netwib_buf_init_mallocdefault(&tempbuf));
	netwib_er(netwib_buf_init_mallocdefault(&httpbuf));
	netwib_er(netwib_buf_init_ext_text("485454502f312e3120323030204f4b0d0a4c4f434154494f4e3a20687474703a2f2f3133312e3234372e322e31373a36313031332f0d0a4558543a200d0a55534e3a20757569643a37656139336539612d343764312d346339312d626130322d3335373066383638323036633a3a75706e703a726f6f746465766963650d0a5345525645523a2057696e646f7773204e542f352e302c2055506e502f312e302c20496e74656c20434c522053444b2f312e300d0a43414348452d434f4e54524f4c3a206d61782d6167653d3930300d0a53543a2075706e703a726f6f746465766963650d0a436f6e74656e742d4c656e6774683a20300d0a0d0a", &tempbuf));
    netwib_er(netwib_buf_decode(&tempbuf, NETWIB_DECODETYPE_HEXA, &httpbuf));
    netwib_er(netwib_buf_close(&tempbuf));
      
    
    ip_checksum(&iphdr); //calculate checksum for iphdr 

	// build the packet to send
    netwib_er(netwib_buf_init_mallocdefault(sendbuf)); // initiate the sendbuffer
    netwib_pkt_append_linkhdr(&linkhdr, sendbuf);
    netwib_pkt_append_iphdr(&iphdr, sendbuf);
    netwib_pkt_append_udphdr(&udphdr, sendbuf);
    netwib_buf_append_buf(&httpbuf, sendbuf);
    
    netwib_er(netwib_buf_close(&httpbuf));

    
}


////////////////////////////////////////////////////////////////////////////////
////                 Sending from buf on device                             ////
////////////////////////////////////////////////////////////////////////////////
netwib_err spoof_send(netwib_buf *buf, netwib_buf *device, netwib_io *pio) {

	netwib_device_dlttype dlt;
   netwib_err ret;

   ret = netwib_io_init_spoof_link(device, &pio); //initiate link spoof  
   if (ret == NETWIB_ERR_OK) {
	   //netwib_er(netwib_spoof_ctl_get_dlt(pio, &dlt));
	   //netwib_er(netwib_fmt_display(" spoof dlt=%{uint32}=%{buf}\n", dlt, &buf));
      ret = netwib_io_write(pio, buf);
      if (ret != NETWIB_ERR_OK) {
            netwib_er(netwib_fmt_display("sending failed\n"));
            netwib_err_display(ret, NETWIB_ERR_ENCODETYPE_FULL);
      }
      netwib_er(netwib_io_close(&pio));
   } else if (ret == NETWIB_ERR_LONOTSUPPORTED) {
     netwib_er(netwib_fmt_display("IP4 spoof is not supported\n"));
   } else {
     netwib_er(netwib_fmt_display("Failed initiating link spoof\n"));
     return(ret);
   }
    return(ret);
    

    
}

////////////////////////////////////////////////////////////////////////////////
////              Calculate ip checksum                                     ////
////////////////////////////////////////////////////////////////////////////////
netwib_err ip_checksum(netwib_iphdr *iphdr) {
    netwib_buf buf;
    netwib_err ret;
    
    ret = NETWIB_ERR_OK;
           
    netwib_er(netwib_buf_init_mallocdefault(&buf));
    netwib_pkt_append_iphdr(iphdr, &buf);
    netwib_checksum_buf(&buf, &iphdr->header.ip4.check);
    netwib_er(netwib_buf_close(&buf));
    return(ret);
}

////////////////////////////////////////////////////////////////////////////
////               Init device                                          ////
////////////////////////////////////////////////////////////////////////////
netwib_err init_device(netwib_buf *pbuf, netwib_device_hwtype hwt) {
    
	netwib_err ret;
    netwib_conf_devices_index *pconfindex;
	netwib_conf_devices conf;
    

    netwib_er(netwib_conf_devices_index_init(&conf, &pconfindex));
    ret = NETWIB_ERR_OK;
    while (NETWIB_TRUE) {
          ret = netwib_conf_devices_index_next(pconfindex);
          if (ret != NETWIB_ERR_OK) {
             if (ret == NETWIB_ERR_DATAEND) 
                ret = NETWIB_ERR_NOTFOUND;
                break;
             }        
          if (conf.hwtype == hwt) {
             netwib_er(netwib_buf_append_buf(&conf.device, pbuf));
             break;
          }
    }
    netwib_er(netwib_conf_devices_index_close(&pconfindex));
    if (ret == NETWIB_ERR_NOTFOUND) {
       netwib_er(netwib_fmt_display("No device of wanted type found\n"));
    }
    netwib_er(netwib_fmt_display("Device initiated\n"));
    
    return(ret);
}

////////////////////////////////////////////////////////////////////////////
////               Parse the received packet                            ////
////////////////////////////////////////////////////////////////////////////
netwib_err parse_packet(netwib_buf *in, netwib_device_dlttype dlt, 
                       netwib_linkhdr *plinkhdr, netwib_iphdr *piphdr,
                       netwib_udphdr *pudphdr, netwib_buf *rest) { //parse the different headers, works! should we add more error checks?
      netwib_err ret;
      netwib_uint32 skipsize;
      
      
       ret = NETWIB_ERR_OK;
	  netwib_buf_append_buf(in, rest);
      
      netwib_pkt_decode_linkhdr(dlt , rest, plinkhdr, &skipsize); // get the linkhdr
     // netwib_er(netwib_fmt_display("linkhdr %{linkhdr}\n", plinkhdr));
      netwib_er(netwib_buf_shift(rest, -skipsize, 0)); // shift away linkhdr
      netwib_pkt_decode_iphdr(rest, piphdr, &skipsize); //get the iphdr
      netwib_er(netwib_buf_shift(rest, -skipsize, 0)); // shift away iphdr
      netwib_pkt_decode_udphdr(rest, pudphdr, &skipsize); //get the udphdr
      netwib_er(netwib_buf_shift(rest, -skipsize, 0)); //shift away udphdr
     // netwib_er(netwib_fmt_display("rest %{buf}\n", rest));
      
      return(ret);                               
}