#include <stdio.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <string.h>

#include "tsg_entry.h"
#include <MESA/MESA_handle_logger.h>
#include "tsg_protocol_common.h"

#define IPV4_TYPE               1     //ADDR_TYPE_IPV4 ==1 , 取的enum 0x0800
#define IPV6_TYPE               2     //ADDR_TYPE_IPV6 ==2 0x86dd   
#define TCP_TYPE                0x06
#define UDP_TYPE                0x11
#define ICMP_PROTOCOL_TYPE      0x01  //ipv4 icmp  proctocol
#define ICMPV6_PROTOCAL_TYPE    0x3a  //ipv6 icmpv6 protocl

#define ICMPV4_UNREACHABLE                  0x03
#define ICMPV4_PORT_UNREACHABLE             0x03
#define ICMPV4_ADMINISTRATIVELY_PROHIBITED  0x0d
#define ICMPV6_UNREACHABLE                  0x01
#define ICMPV6_PORT_UNREACHABLE             0X04
#define ICMPV6_ADMINISTRATIVELY_PROHIBITED  0x01


//icmp
#define ICMP_HEADR_BYTE_LEN   8
#define ICMP_MAX_LEN   2048
#define ETHERNET_BYTE_LEN 14

//icmpv4
#define IPV4_HEADR_BYTE_LEN 20
#define IPV4_IPADDR_BYTE_LEN 4
#define IPV4_TOTAL_LEN_INDEX  2
#define IPV4_TOTAL_BYTE_LEN  2
#define IPV4_SIP_INDEX 12
#define IPV4_DIP_INDEX 16
#define IPV4_PROTOCOL_INDEX 9
#define IPV4_CHECKSUM_INDEX 10
#define IPV4_CHECKSUM_BYTE_LEN 2
#define ICMPV4_SRCPACKET_MAX_LEN    548  // 548 == ipv4(max_len 576)-ip_len(20)-icmp_head_len(8)

//icmpv6
//#define ICMPV6_ICMP_INDEX 40
#define IPV6_HEADR_BYTE_LEN 40
#define IPV6_IPADDR_BYTE_LEN 16
#define IPV6_PAYLOAD_BYTE_LEN 2
#define IPV6_SIP_INDEX 8
#define IPV6_DIP_INDEX 24
#define IPV6_PAYLOAD_INDEX 4
#define IPV6_NEXT_HEADR_INDEX 6
#define IPV6_2IP_BYTE_LEN  32
#define IPV6_PESUDO_HEADR_BYTE_LEN 40
#define IPV6_PESUDO_UPPER_LAYER_PACKET_AND_ZERO_LEN 7
#define IPV6_PESUDO_HEAD_PAYLOAD_BYTE_LEN  4
#define ICMPV6_SRCPACKET_MTU 1232 // 1232 == ipv6(max_len 1280)-ipv6_headr_len(40)-icmp_head_len(8)
#define ICMPV6_MTU 1280               // ICMPV6_SRCPACKET_MTU + IPV6_HEADR_BYTE_LEN + ICMPV6_HEARD_LEN

typedef struct icmp_st_{
    char type;
    char code;
    short checksum;
    char un_user[4];
    char origin_data[ICMPV6_SRCPACKET_MTU];
}icmp_st;

static short in_checksum(void *pkg, int size)
{
    int             nleft   = size;
    int             sum     = 0;
    unsigned short  *w      = (unsigned short *)pkg;
    unsigned short  answer  = 0;

    while (nleft > 1)
    {
        sum += *w++;
        nleft -= 2;
    }

    if (nleft == 1)
    {
        * (unsigned char *) (&answer) = *(unsigned char *)w;
        sum += answer;
    }

    while (sum >> 16){
        sum = (sum & 0xFFFF) + (sum >> 16);
    }
    answer = ~sum &0xffff;

    return answer;
}


static void four_byte_alignment(unsigned short *icmp_len, unsigned short *icmp_checksum_len){
    short temp = 0;

    temp = *icmp_len % 4;
    if(temp >0){
        *icmp_len = *icmp_len+(4-temp);
        *icmp_checksum_len = *icmp_checksum_len+(4-temp);
    }

    return ;
}

/*
 *  ICMP 报文格式
 *  0                   1                   2                   3
 *  0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |     Type      |     Code      |          Checksum             |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                             unused                            |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |      Internet Header + 64 bits of Original Data Datagram      |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 *  ICMPV4 = [ipv4_headr]+[icmp]
 *  ICMPV4(MTU)  576
 */

static void format_icmpv4(const char *raw_pkt, char *ret_icmpv4, int *ret_len){
    icmp_st icmp = {0};
    char ipv4_headr[IPV4_HEADR_BYTE_LEN] = {0};
    unsigned short raw_ipv4_total_len = 0;
    unsigned short icmpv4_len = 0;
    unsigned short ipv4_total_len = 0;
    unsigned short ipv4_checksum = 0;
    unsigned short icmp_len = 0;
    unsigned short icmp_original_data_len = 0;
    unsigned short icmp_start_len = IPV4_HEADR_BYTE_LEN;

    memcpy(&raw_ipv4_total_len, &raw_pkt[IPV4_TOTAL_LEN_INDEX], IPV4_TOTAL_BYTE_LEN);
    raw_ipv4_total_len = htons(raw_ipv4_total_len);

    //src_packet_len = raw_ipv4_total_len + ETH_LEN;
    if(raw_ipv4_total_len >= ICMPV4_SRCPACKET_MAX_LEN){
        icmp_original_data_len = ICMPV4_SRCPACKET_MAX_LEN;
    }else{
        icmp_original_data_len = raw_ipv4_total_len;
    }

    icmp_len = icmp_original_data_len + ICMP_HEADR_BYTE_LEN;
    ipv4_total_len = icmp_len + IPV4_HEADR_BYTE_LEN;
    icmpv4_len = ipv4_total_len;

    //format ipv4 headr
    memcpy(ipv4_headr, raw_pkt, IPV4_HEADR_BYTE_LEN);                                        //copy source data
    memcpy(&ipv4_headr[IPV4_SIP_INDEX], &raw_pkt[IPV4_DIP_INDEX], IPV4_IPADDR_BYTE_LEN);     //format ipv4 sip
    memcpy(&ipv4_headr[IPV4_DIP_INDEX], &raw_pkt[IPV4_SIP_INDEX], IPV4_IPADDR_BYTE_LEN);     //format ipv4 dip
    ipv4_headr[IPV4_PROTOCOL_INDEX] = ICMP_PROTOCOL_TYPE;                                    //set ipv4 protocol type (0x01 == ICMP)
    ipv4_total_len = htons(ipv4_total_len);
    memcpy(&ipv4_headr[IPV4_TOTAL_LEN_INDEX], &ipv4_total_len, IPV4_TOTAL_BYTE_LEN);         //format ipv4 total len
    memset(&ipv4_headr[IPV4_CHECKSUM_INDEX], 0, IPV4_CHECKSUM_BYTE_LEN);                     //empty checksum data
    ipv4_checksum = in_checksum(ipv4_headr, IPV4_HEADR_BYTE_LEN);                            //calc ipv4 checksum
    memcpy(&ipv4_headr[IPV4_CHECKSUM_INDEX], &ipv4_checksum, IPV4_CHECKSUM_BYTE_LEN);        //format ipv4 checksum

    //format icmp
    memset(&icmp, 0, sizeof(icmp_st));
    memcpy(icmp.origin_data, raw_pkt, icmp_original_data_len);
    icmp.type = ICMPV4_UNREACHABLE;
    icmp.code = ICMPV4_ADMINISTRATIVELY_PROHIBITED;
    icmp.checksum = in_checksum((void*)&icmp, icmp_len);

    //format complete icmpv4 packet
    memcpy(ret_icmpv4, ipv4_headr, IPV4_HEADR_BYTE_LEN);
    memcpy(&ret_icmpv4[icmp_start_len], &icmp, icmp_len);
    *ret_len = icmpv4_len;

    return;
}

/*
 *  icmpv6数据包组�? *  [ICMPV6] = [ipv6_headr]+[icmp]
 *
 *  icmpv6的最大MTU应是ipv6的最小MTU(1280)
 *  <本端内容来自RFC443>
 *  Every ICMPv6 error message (type < 128) MUST include as much of
 *  the IPv6 offending (invoking) packet (the packet that caused the
 *  error) as possible without making the error message packet exceed
 *  the minimum IPv6 MTU [IPv6].
 *
 *  [ICMPV6] = [ipv6_headr]+[icmp]
 *
 *  计算ICMPV6的校验和�?(ICMP + 伪首�? 作为计算数据部分
 *  注意�?ICMPV6数据要补充成四字节对�? *  ipv6伪首部数�? 长度
 *  {
 *      Soucre Address :            16
 *      Destination Address:        16
 *      Upper-Layer Packet Length:  4
 *      zero:                       3
 *      Next Heard:                 1
 *  }
 *
 **  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                                                               |
 *  +                                                               +
 *  ~                                                               ~
 *  ~                         icmp                                  ~
 *  |                                                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +                         Source Address                        +
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +                      Destination Address                      +
 *  |                                                               |
 *  +                                                               +
 *  |                                                               |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                   Upper-Layer Packet Length                   |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *  |                      zero                     |  Next Header  |
 *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
 *
 *  [ICMPV6] = [ipv6_headr]+[icmp]
 *  ICMPV6(MTU) 1280
 */

static void format_icmpv6(const char *raw_pkt, char *ret_icmpv6, int *ret_len){
    icmp_st icmp = {0};
    char icmp_checksum_buf[ICMPV6_MTU] = {0};
    char ipv6_headr[IPV6_HEADR_BYTE_LEN] = {0};
    unsigned short ipv6_payload_len = 0;
    unsigned short raw_ipv6_payload_len = 0;
    unsigned short raw_pkt_len = 0;
    unsigned short icmpv6_len = 0;
    unsigned short icmp_len = 0;
    unsigned short icmp_original_data_len = 0;
    unsigned short icmp_checksum_len = 0;
    unsigned int upper_layer_packet_len = 0;

    memcpy(&raw_ipv6_payload_len, &raw_pkt[IPV6_PAYLOAD_INDEX], IPV6_PAYLOAD_BYTE_LEN);  //get ipv6_payload_len
    raw_pkt_len = ntohs(raw_ipv6_payload_len) + IPV6_HEADR_BYTE_LEN;

    if(raw_pkt_len >= ICMPV6_SRCPACKET_MTU){
        icmp_original_data_len = ICMPV6_SRCPACKET_MTU;
    }else{
        icmp_original_data_len = raw_pkt_len;
    }

    icmp_len = icmp_original_data_len + ICMP_HEADR_BYTE_LEN;
    icmpv6_len = IPV6_HEADR_BYTE_LEN + icmp_len;
    icmp_checksum_len = icmp_len + IPV6_PESUDO_HEADR_BYTE_LEN;
    ipv6_payload_len = htons(icmp_len);

    //format ipv6 headr
    memcpy(ipv6_headr, raw_pkt, IPV6_HEADR_BYTE_LEN);                                     //format source ipv6 data
    memcpy(&ipv6_headr[IPV6_SIP_INDEX], &raw_pkt[IPV6_DIP_INDEX], IPV6_IPADDR_BYTE_LEN);  //modify format sip
    memcpy(&ipv6_headr[IPV6_DIP_INDEX], &raw_pkt[IPV6_SIP_INDEX], IPV6_IPADDR_BYTE_LEN);  //modify format dip
    memcpy(&ipv6_headr[IPV6_PAYLOAD_INDEX], &ipv6_payload_len, IPV6_PAYLOAD_BYTE_LEN);    //modify format ipv6 payload
    ipv6_headr[IPV6_NEXT_HEADR_INDEX] = ICMPV6_PROTOCAL_TYPE;                             //modify format ipv6 protocol (icmpv6 == 0X3a);

    //format icmp
    memset(&icmp, 0, sizeof(icmp_st));
    icmp.type = ICMPV6_UNREACHABLE;
    icmp.code = ICMPV6_ADMINISTRATIVELY_PROHIBITED;
    memcpy(icmp.origin_data, raw_pkt, icmp_original_data_len);

    upper_layer_packet_len = htonl((int)icmp_len);
    four_byte_alignment(&icmp_len, &icmp_checksum_len); //icmp_len和icmp_checksum_len 补充�?字节对齐

    //calc icmp checksum
    memcpy(icmp_checksum_buf, &icmp, icmp_len);
    memcpy(&icmp_checksum_buf[icmp_len], &ipv6_headr[IPV6_SIP_INDEX], IPV6_2IP_BYTE_LEN);
    memcpy(&icmp_checksum_buf[icmp_len+IPV6_2IP_BYTE_LEN], &upper_layer_packet_len, IPV6_PESUDO_HEAD_PAYLOAD_BYTE_LEN);
    icmp_checksum_buf[icmp_len+IPV6_2IP_BYTE_LEN+IPV6_PESUDO_UPPER_LAYER_PACKET_AND_ZERO_LEN] = ICMPV6_PROTOCAL_TYPE;
    icmp.checksum = in_checksum(icmp_checksum_buf, icmp_checksum_len);

    //format complete icmpv6 packet
    memcpy(ret_icmpv6, ipv6_headr, IPV6_HEADR_BYTE_LEN);
    memcpy(&ret_icmpv6[IPV6_HEADR_BYTE_LEN], &icmp, icmp_len);
    *ret_len = icmpv6_len;

    return;
}

static void format_icmp(const char *raw_pkt, char *icmp_buf, int *icmp_len, int ip_type){
    if(IPV4_TYPE == ip_type) {
        format_icmpv4(raw_pkt, icmp_buf, icmp_len);
    } else{ //IPV6_TYPE
        format_icmpv6(raw_pkt, icmp_buf, icmp_len);
    }
    return;
}

/*   get option from raw packet.
 *   example:
 *   void *raw_pkt_data;
 *   ret = get_opt_from_rawpkt(voidpkt, RAW_PKT_GET_DATA, &raw_pkt_data);
 *   if(0 == ret){
 *           (struct mesa_ethernet_hdr *)raw_pkt_data;
 *   }else if(1 == ret){
 *           (raw_ipfrag_list_t *)raw_pkt_data;
 *   }else{
 *           error!
 *   }
 */

unsigned char send_icmp_unreachable(const struct streaminfo *a_stream)
{
    char icmp_buf[ICMP_MAX_LEN] = {0}; 
    char debug_buf[512] = {0};
    int  icmp_len = 0;
    int  get_rawpkt_ret = 0;
    void *raw_pkt = NULL;

    if(a_stream==NULL){
        MESA_handle_runtime_log(g_tsg_para.logger,
                RLOG_LV_DEBUG,
                __FUNCTION__,
                "a_stream is NULL");
        return STATE_DROPPKT;
    }                                                                                                                        

    get_rawpkt_ret = get_rawpkt_opt_from_streaminfo(a_stream, RAW_PKT_GET_DATA, &raw_pkt);
    MESA_handle_runtime_log(g_tsg_para.logger,
                RLOG_LV_DEBUG,
                __FUNCTION__,
                "get_raw_opt_from_streaminfo()  get_rawpkt_ret = %d", get_rawpkt_ret);
    if(0==get_rawpkt_ret && NULL!=raw_pkt){
        //(struct mesa_ethernet_hdr *)raw_pkt_data;
        format_icmp((char *)raw_pkt+ETHERNET_BYTE_LEN, icmp_buf, &icmp_len, a_stream->addr.addrtype);
    }else if(1==get_rawpkt_ret && NULL!=raw_pkt){
        //(raw_ipfrag_list_t *)raw_pkt_data;
        //format_icmp((char *)(((raw_ipfrag_list_t *)raw_pkt)->frag_packet)+ETHERNET_BYTE_LEN, icmp_buf, &icmp_len, a_stream->addr.addrtype);
        return STATE_DROPPKT;
    }else{
        //error
        return STATE_DROPPKT;
    }

    if(0 == tsg_send_inject_packet(a_stream, SIO_EXCLUDE_THIS_LAYER_HDR, icmp_buf, icmp_len, MESA_dir_reverse(a_stream->routedir))){
        sprintf(debug_buf, "Addr: %s, send sucess, get_rawpkt_ret %d, icmp_buf %p, icmp_len %d", PRINTADDR(a_stream, g_tsg_para.level), get_rawpkt_ret, icmp_buf, icmp_len);
    }else{
        sprintf(debug_buf, "Addr: %s, send failed, get_rawpkt_ret %d, icmp_buf %p, icmp_len %d", PRINTADDR(a_stream, g_tsg_para.level), get_rawpkt_ret, icmp_buf, icmp_len);
    }

    MESA_handle_runtime_log(g_tsg_para.logger,
            RLOG_LV_DEBUG,
            __FUNCTION__,
            debug_buf);

    return STATE_DROPME|STATE_DROPPKT;
}