#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/wireless.h>

#include "rt_tool.h"
#include "sha1.h"

/*------------------------------------------------------------------*/
/*
 * Open a socket.
 * Depending on the protocol present, open the right socket. The socket
 * will allow us to talk to the driver.
 */
int Open_Socket(void)
{
    static const int families[] = {
        AF_INET, AF_IPX, AF_AX25, AF_APPLETALK
    };
    unsigned int    i;
    int        sock;

    /*
    * Now pick any (exisiting) useful socket family for generic queries
    * Note : don't open all the socket, only returns when one matches,
    * all protocols might not be valid.
    * Workaround by Jim Kaba <jkaba@sarnoff.com>
    * Note : in 99% of the case, we will just open the inet_sock.
    * The remaining 1% case are not fully correct...
    */

    /* Try all families we support */
    for(i = 0; i < sizeof(families)/sizeof(int); ++i)
    {
        /* Try to open the socket, if success returns it */
        sock = socket(families[i], SOCK_DGRAM, 0);
        if(sock >= 0)
            return sock;
    }

    return -1;
}

int OidQueryInformation(USHORT OidQueryCode, int socket_id, char *DeviceName, void *ptr, ULONG PtrLength)
{
    struct iwreq wrq;

    strcpy(wrq.ifr_name, DeviceName);
    wrq.u.data.length = PtrLength;
    wrq.u.data.pointer = (caddr_t) ptr;
    wrq.u.data.flags = OidQueryCode;

    return (ioctl(socket_id, RT_PRIV_IOCTL, &wrq));
}

int OidSetInformation(USHORT OidQueryCode, int socket_id, char *DeviceName, void *ptr, ULONG PtrLength)
{
    struct iwreq wrq;

    strcpy(wrq.ifr_name, DeviceName);
    wrq.u.data.length = PtrLength;
    wrq.u.data.pointer = (caddr_t) ptr;
    wrq.u.data.flags = OidQueryCode | OID_GET_SET_TOGGLE;

    return (ioctl(socket_id, RT_PRIV_IOCTL, &wrq));
}

UINT ConvertRssiToSignalQuality(NDIS_802_11_RSSI RSSI)
{
	UINT signal_quality;
    if (RSSI >= -50)
        signal_quality = 100;
    else if (RSSI >= -80)    // between -50 ~ -80dbm
        signal_quality = (UINT)(24 + (RSSI + 80) * 2.6);   
    else if (RSSI >= -90)   // between -80 ~ -90dbm
        signal_quality = (UINT)((RSSI + 90) * 2.6);   
    else    // < -84 dbm
        signal_quality = 0;
 
    return signal_quality;
}

//
//  FUNCTION: BtoH(char *, char *, int)
//
//  PURPOSE:  Converts ascii byte to numeric
//
//  PARAMETERS:
//    ch - ascii byte to convert
//
//  RETURNS:
//    associated numeric value
//
//  COMMENTS:
//
//    Will convert any hex ascii digit to its numeric counterpart.
//    Puts in 0xff if not a valid hex digit.
//

unsigned char BtoH(char ch)
{
    if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
    if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
    if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
    return(255);
}

//
//  FUNCTION: AtoH(char *, UCHAR *, int)
//
//  PURPOSE:  Converts ascii string to network order hex
//
//  PARAMETERS:
//    src    - pointer to input ascii string
//    dest   - pointer to output hex
//    destlen - size of dest
//
//  COMMENTS:
//
//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
//    into upper nibble and 2nd ascii byte of pair into lower nibble.
//

void AtoH(char * src, UCHAR * dest, int destlen)
{
    char * srcptr;

    srcptr = src;
    PUCHAR destTemp = dest; 

    while(destlen--)
    {
        *destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
        *destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
        destTemp++;
    }
}

static void hmac_sha1(unsigned char *text, int text_len, unsigned char *key, int key_len, unsigned char *digest) 
{ 
    A_SHA_CTX context; 
    unsigned char k_ipad[65]; /* inner padding - key XORd with ipad */ 
    unsigned char k_opad[65]; /* outer padding - key XORd with opad */ 
    int i; 

    /* if key is longer than 64 bytes reset it to key=SHA1(key) */ 
    if (key_len > 64) 
    { 
        A_SHA_CTX tctx; 

        A_SHAInit(&tctx); 
        A_SHAUpdate(&tctx, key, key_len); 
        A_SHAFinal(&tctx, key); 

        key_len = 20; 
    } 

    /* 
    * the HMAC_SHA1 transform looks like: 
    * 
    * SHA1(K XOR opad, SHA1(K XOR ipad, text)) 
    * 
    * where K is an n byte key 
    * ipad is the byte 0x36 repeated 64 times 
    * opad is the byte 0x5c repeated 64 times 
    * and text is the data being protected 
    */ 

    /* start out by storing key in pads */ 
    memset(k_ipad, 0, sizeof k_ipad); 
    memset(k_opad, 0, sizeof k_opad); 
    memcpy(k_ipad, key, key_len); 
    memcpy(k_opad, key, key_len); 

    /* XOR key with ipad and opad values */ 
    for (i = 0; i < 64; i++) 
    { 
        k_ipad[i] ^= 0x36; 
        k_opad[i] ^= 0x5c; 
    } 

    /* perform inner SHA1*/ 
    A_SHAInit(&context); /* init context for 1st pass */ 
    A_SHAUpdate(&context, k_ipad, 64); /* start with inner pad */ 
    A_SHAUpdate(&context, text, text_len); /* then text of datagram */ 
    A_SHAFinal(&context, digest); /* finish up 1st pass */ 

    /* perform outer SHA1 */ 
    A_SHAInit(&context); /* init context for 2nd pass */ 
    A_SHAUpdate(&context, k_opad, 64); /* start with outer pad */ 
    A_SHAUpdate(&context, digest, 20); /* then results of 1st hash */ 
    A_SHAFinal(&context, digest); /* finish up 2nd pass */ 
} 

/*
* F(P, S, c, i) = U1 xor U2 xor ... Uc 
* U1 = PRF(P, S || Int(i)) 
* U2 = PRF(P, U1) 
* Uc = PRF(P, Uc-1) 
*/ 

static void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output) 
{ 
    unsigned char digest[36], digest1[A_SHA_DIGEST_LEN]; 
    int i, j; 

    /* U1 = PRF(P, S || int(i)) */ 
    memcpy(digest, ssid, ssidlength); 
    digest[ssidlength] = (unsigned char)((count>>24) & 0xff); 
    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff); 
    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff); 
    digest[ssidlength+3] = (unsigned char)(count & 0xff); 
    hmac_sha1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update

    /* output = U1 */ 
    memcpy(output, digest1, A_SHA_DIGEST_LEN); 

    for (i = 1; i < iterations; i++) 
    { 
        /* Un = PRF(P, Un-1) */ 
        hmac_sha1(digest1, A_SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
        memcpy(digest1, digest, A_SHA_DIGEST_LEN); 

        /* output = output xor Un */ 
        for (j = 0; j < A_SHA_DIGEST_LEN; j++) 
        { 
            output[j] ^= digest[j]; 
        } 
    } 
} 
/* 
* password - ascii string up to 63 characters in length 
* ssid - octet string up to 32 octets 
* ssidlength - length of ssid in octets 
* output must be 40 octets in length and outputs 256 bits of key 
*/ 
int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output) 
{ 
    if ((strlen(password) > 63) || (ssidlength > 32)) 
        return 0; 

    F(password, ssid, ssidlength, 4096, 1, output); 
    F(password, ssid, ssidlength, 4096, 2, &output[A_SHA_DIGEST_LEN]); 
    return 1; 
}

