//
// Embedded 802.11 wireless network interface
// MAC layer management functions
// Copyright 2005 Digi International
//
#include "mac.h"
#include "mac_api.h"

wln_params macParams;   // Operating parameters
wln_status macStatus;   // Operating status
MacBuffer *bcnFrame;    // Beacon frame if IBSS
MacBuffer *respFrame;   // Probe response frame if IBSS
StaState staState;      // Station state: one of SS_xxx
uint16 bcnInt;          // Beacon interval (in TU)
uint16 atimWindow;      // ATIM window size, 0 if none
uint16 basicRates;      // Basic rate set
uint8 erpInfo;          // Extended rate PHY info
uint32 macRandom;       // Random value generated from network

// Scan parameters
static uint8 scan_ssid[WLN_SSID_SIZE];  // Service set ID
static int scan_ssid_len;           // SSID length, 0 for any
static uint32 scan_chan_mask;       // Channels to scan
static BOOL scan_requested;         // TRUE if requested by wln_scan()

static wln_scan_data *scan_data;    // Networks found by scan
static int scan_count;              // Number of networks found
static int scan_max;                // Number of networks allocated
static int scan_channel;            // Channel being scanned
static int scan_index;              // Next network to try
static StaState scan_state;         // Station state before scan
static uint32 scan_time;            // Time of last scan

static AuthState authState;         // Authentication state
static uint8 challenge[CHAL_SIZE];  // Authentication challenge

static uint32 mgmtTime;             // Last time management timer ran
static int retry;                   // Management retry counter

// Broadcast MAC address
static MacAddr bc_addr = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };

// WPA information element
static const uint8 wpaOUI[] = { 0x00, 0x50, 0xf2, 0x01 };

static void RecvScan (MacBuffer *buf);
static void RecvBeacon (MacBuffer *buf);
static void RecvAuthentication (MacBuffer *buf);
static void StartScan (uint8 *ssid, int ssid_len, int channel, BOOL requested);
static void ScanChannel (void);
static void SendMgmtFrame (void);
static void SendNextMgmtFrame (void);
static BOOL StartIBSS (void);
static BOOL MakeBeacon (int interval);
static void SetStatusLED (void);


//
// Initialize the MAC layer and enable the network interface
// Parameters must be set by wln_set_params() before calling
//
MacReturn MacInit (void)
{
    MacReturn ret;

    memset (&macStatus, 0, sizeof macStatus);
    bcnFrame = NULL;
    respFrame = NULL;
    scan_data = NULL;
    scan_max = 0;
    scan_time = 0;
    staState = scan_state = SS_WAIT;
    mgmtTime = GET_TIME();

    ret = InitBuffers();
    if (ret != ERR_OK)
        return ret;

    if (macParams.options & WLN_OPT_MULTI_DOMAIN)
    {
        macStatus.tx_power = -1;
        macParams.tx_power = WLN_TXPOWER_MIN;
    }

    ret = InitHW();
    if (ret != ERR_OK)
    {
        ReleaseBuffers();
        return ret;
    }

    InitCRC32();
    InitData();
    InitIR();

    // Seed the PRNG with the time and station MAC address
    // so that identical devices use a different sequence
    srand ((uint16) mgmtTime ^ macParams.sta_addr[3] ^ macParams.sta_addr[4] ^ macParams.sta_addr[5]);
    memcpy (&macRandom, macParams.sta_addr+2, sizeof macRandom);

    SetLED (FALSE);

    if (macParams.bss_type != WLN_BSS_NONE)
        StartScan (macParams.ssid, macParams.ssid_len, macParams.channel, FALSE);
    else
    {
         
        SetChannel (macParams.channel);
        // Set BSSID and SSID to receive all frames
        SetBSS (0, bc_addr, NULL, 0, 0, 0);
    }

    return ERR_OK;
}

//
// Shutdown the MAC layer and disable the network interface
//
void MacShutdown (void)
{
    macStatus.state = WLN_ST_STOPPED;
    SetLED (FALSE);
    ShutdownHW();
    ReleaseBuffers();
    free (scan_data);
}

//
// Process received frames and periodic tasks.
// Called from the network task.
// @return time interval in msecs to call again
//
uint32 MacTimer (void)
{
    MacBuffer *buf;
    uint32 now;

    // Process receive queue
    while (TRUE)
    {
        int level = DISABLE();
        RXQ_GET (buf);
        ENABLE (level);

        if (buf)
            RecvFrame (buf);
        else
            break;
    }

    GET_MUTEX();

    IRTimer();

    // Process channel scanning
    if (staState == SS_SCAN)
    {
        now = GET_TIME();
        if (now-scan_time >= SCAN_PERIOD)
        {
            scan_time = now;
            ScanChannel();
        }
    }

    if (macParams.bss_type != WLN_BSS_NONE)
    {
        // Process management tasks
        now = GET_TIME();
        if (now-mgmtTime >= MGMT_PERIOD)
        {
            mgmtTime = now;
            AgeRecvCache();
            SendMgmtFrame();
            SetStatusLED();
            UpdateRate();
        }
    }
    
    UpdateActivityLED (FALSE);

    PUT_MUTEX();

    if (staState == SS_SCAN)
        return SCAN_PERIOD;
    else
        return TIMER_PERIOD;
}

//
// Scan for wireless networks.
// Results are returned with a WLN_EVT_SCAN event.
// @param ssid      Service set ID
// @param ssid_len  SSID length in bytes, 0 to scan all
// @param channel   Channel: 1-WLN_CHAN_NUM, 0 to scan all
//
int wln_scan (uint8 *ssid, int ssid_len, int channel)
{
    GET_MUTEX();
    StartScan (ssid, ssid_len, channel, TRUE);
    PUT_MUTEX();

    return 0;
}

//
// Associate with a BSS
// @param ssid      Service set ID
// @param ssid_len  SSID length in bytes, 0 to scan all
// @param channel   Channel: 1-WLN_CHAN_NUM, 0 to scan all
// @param bss_addr  BSS to associate with
// @param bss_caps  BSS capabilities, one of more of @link WirelessCapValues
// @param wpa_ie WPA or RSN info element to send, NULL if none
//
int wln_associate (uint8 *ssid, int ssid_len, int channel, mac_addr bss_addr, uint16 bss_caps, uint8 *wpa_ie)
{
    if (wpa_ie && wpa_ie[1] + 2 > WLN_WPAIE_SIZE)
        return -1;

    GET_MUTEX();

    if (macStatus.state >= WLN_ST_ASSOC_ESS)
        wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);

    DPRINTF ("Join bssid=%02x:%02x:%02x:%02x:%02x:%02x ssid=%.*s chan=%d\n",
        bss_addr[0], bss_addr[1], bss_addr[2],
        bss_addr[3], bss_addr[4], bss_addr[5],
        ssid_len, ssid, channel);

    // Set BSS info
    memcpy (macStatus.ssid, ssid, ssid_len);
    macStatus.ssid_len = ssid_len;
    macStatus.channel = channel;
    macStatus.bss_caps = bss_caps;
    SET_ADDR (macStatus.bss_addr, bss_addr);
    memset (macStatus.wpa_info, 0, WLN_WPAIE_SIZE);
    memset (macStatus.rsn_info, 0, WLN_WPAIE_SIZE);
    
    if (wpa_ie)
    {
        if (wpa_ie[0] == ELEM_RSN)
            memcpy (macStatus.rsn_info, wpa_ie, wpa_ie[1] + 2);
        else
        memcpy (macStatus.wpa_info, wpa_ie, wpa_ie[1] + 2);
    }

    // Set BSS info in hardware
    SetChannel (channel);
    SetBSS (macStatus.bss_caps, macStatus.bss_addr,
            macStatus.ssid, macStatus.ssid_len, 0, 0);

    // Wait to receive a beacon
    staState = SS_BEACON;
	scan_state = SS_WAIT;
    mgmtTime = GET_TIME();

    PUT_MUTEX();
    return 0;
}

//
// Disassociate from a BSS
// @param bss_addr BSS to disassociate from
// @param reason reason code to send
//
int wln_disassociate (mac_addr bss_addr, int reason)
{
    MacBuffer *buf;

    GET_MUTEX();

    // Not associated with the BSS
    if (macStatus.state < WLN_ST_ASSOC_ESS ||
        !EQUAL_ADDR (bss_addr, macStatus.bss_addr))
    {
        PUT_MUTEX();
        return -1;
    }

    if (macStatus.bss_caps & CAP_ESS)
    {
    // Send disassociation
    if (staState == SS_ASSOC)
    {
        buf = MacGetBuffer();
        if (buf)
        {
            buf->macHdr.fc.type = TYPE_DISASSOC;
            buf->length = DATA_SIZE + 2;
            SET16 (buf->body, reason);
            SendFrames (buf, macStatus.bss_addr);
        }
    }

    // Send deauthentication
    if (staState == SS_AUTH || staState == SS_ASSOC)
    {
        buf = MacGetBuffer();
        if (buf)
        {
            buf->macHdr.fc.type = TYPE_DEAUTH;
            buf->length = DATA_SIZE + 2;
            SET16 (buf->body, reason);
            SendFrames (buf, macStatus.bss_addr);
        }
        }
    }

    if (macStatus.state >= WLN_ST_ASSOC_ESS)
        wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);

    // Clear BSSID so no more frames are received
    memset (macStatus.bss_addr, 0, WLN_ADDR_SIZE);
    macStatus.bss_caps = CAP_ESS;

    // Continue scanning. Timer will call NextBSS().
    staState = SS_BEACON;
	scan_state = SS_WAIT;
    mgmtTime = GET_TIME();

    PUT_MUTEX();
    return 0;
}

//
// Send a deauthentication or disassociation frame to the sender
// of a frame received when not associated
// @param rxBuf received frame
// @param reason reason code to send
// @param deauth deauth or disassociate type to send
//
int force_disassociate (MacBuffer *rxBuf, int reason, int deauth)
{
    MacBuffer *buf;
    int state;

    GET_MUTEX();

    buf = MacGetBuffer();
    if (buf)
    {
        if(!deauth)
            buf->macHdr.fc.type = TYPE_DISASSOC; // Send disassociation
        else
            buf->macHdr.fc.type = TYPE_DEAUTH;   // Send deauthentication

        buf->length = DATA_SIZE + 2;
        SET_ADDR (buf->macHdr.addr3, rxBuf->macHdr.addr3);
        SET16 (buf->body, reason);
    
        // Prevent SendFrames() from setting the addr3 field
        state = staState;
        staState = SS_BEACON;
        SendFrames (buf, rxBuf->macHdr.addr2);
        staState = state;
    }

    PUT_MUTEX();
    return 0;
}

//
// Process a received management frame
// @param buf Received frame
//
void RecvMgmtFrame (MacBuffer *buf)
{
    // Ignore frame if not:
    //   Beacon or probe response, or
    //   Joined ESS and from this BSS
    if (!(buf->macHdr.fc.type == TYPE_BEACON ||
          buf->macHdr.fc.type == TYPE_PROBE_RESP ||
          ((macStatus.bss_caps & CAP_ESS) && EQUAL_ADDR (buf->macHdr.addr2, macStatus.bss_addr))))
    {
        MacFreeBuffers (buf);
        return;
    }

    GET_MUTEX();

    switch (buf->macHdr.fc.type)
    {
        case TYPE_BEACON:
            // Accumulate timestamp and RSSI into random value
            macRandom ^= GET32 (buf->body) ^ buf->rx_signal;

            // Accept beacon as scan result if passively scanning for 802.11d.
            // Otherwise, don't accept beacon because it may not contain the SSID.
            // Probe responses are better because they do contain the SSID.
            if (staState == SS_SCAN &&
                (macParams.options & WLN_OPT_MULTI_DOMAIN) &&
                macStatus.tx_power < 0)
                RecvScan (buf);
            else if (staState >= SS_BEACON)
                RecvBeacon (buf);
            break;

        case TYPE_PROBE_RESP:
            if (staState == SS_SCAN)
                RecvScan (buf);
            break;
            
        case TYPE_DEAUTH:
            DPRINTF ("Deauthentication %d\n", GET16 (&buf->body[0]));
            if (staState == SS_ASSOC)
            {
                wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);

                if (GET16 (&buf->body[0]) == REAS_NOLONGERVALID ||
                    GET16 (&buf->body[0]) == REAS_INACTIVITY)
                {
                    // Send authentication request
                    staState = SS_JOIN;
                    authState = SA_INIT;
                    SendNextMgmtFrame();
                }
                else
                    NextBSS();
            }
            else if (staState == SS_AUTH)
                NextBSS();
            break;

        case TYPE_AUTH:
            DPRINTF ("Authentication %d\n", GET16 (&buf->body[4]));
            if (staState == SS_JOIN)
                RecvAuthentication (buf);
            break;

        case TYPE_DISASSOC:
            DPRINTF ("Disassociation %d\n", GET16 (&buf->body[0]));
            if (staState == SS_ASSOC)
            {
                wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);

                if (GET16 (&buf->body[0]) == REAS_INACTIVITY)
                {
                    // Send authentication request
                    staState = SS_JOIN;
                    authState = SA_INIT;
                    SendNextMgmtFrame();
                }
                else
                    NextBSS();
            }
            break;

        case TYPE_ASSOC_RESP:
            DPRINTF ("Association response %d\n", GET16 (&buf->body[2]));
            if (staState == SS_AUTH)
            {
                // Check association response
                if (((GET16 (&buf->body[0]) & (CAP_ESS|CAP_IBSS)) == CAP_ESS) &&
                    (GET16 (&buf->body[2]) == STAT_SUCCESS))
                {
                    staState = SS_ASSOC;
                    macStatus.state = WLN_ST_ASSOC_ESS;
                    wln_send_event (WLN_EVT_ASSOC, macStatus.bss_addr);
                }
                else
                    NextBSS();
            }
            break;
    }

    MacFreeBuffers (buf);
    PUT_MUTEX();
}

// parse country information elelment
// @param bp       country information data
// @param channel  current channel number

static void ParseMultiDomainInfo(uint8 *bp, int channel)
{
    int idx, len = (int)bp[1];

    if (len >= COUNTRY_INFO_MIN_LEN)
    {
        // save the country string
        memcpy(&macStatus.country_info, &bp[2], WLN_COUNTRY_STRLEN);

        idx = 5; /* first channel info */
        len -= 3;

        while ((len-3) >= 0)
        {
            // check the channel
            if ((channel >= bp[idx]) && (channel <= (bp[idx]+bp[idx+1])))
            {
                // save the maximum tx power
                if (bp[idx+2] > WLN_TXPOWER_MAX)
                    macStatus.tx_power = WLN_TXPOWER_MAX;
                else
                    macStatus.tx_power = (int)bp[idx+2];
                MacSetTxPower((unsigned char)macStatus.tx_power);
                break;
            }
            len -= 3;
            idx += 3;
        }
    }
}


//
// Parse a received beacon or probe response frame
// @param buf Received frame
// @param channel Expected channel
// @param sp BSS scan information returned
// @return TRUE if valid frame
//
static BOOL ParseBeacon (MacBuffer *buf, int channel, wln_scan_bss *sp)
{
    uint16 bssCaps = GET16 (&buf->body[10]);
    uint8 *bp;

    memset (sp, 0, sizeof(wln_scan_bss));

    // Save fixed fields
    SET_ADDR (sp->bss_addr, buf->macHdr.addr3);
    sp->channel = channel;
    sp->rx_signal = buf->rx_signal;
    sp->bss_caps = bssCaps;

    // Set default ERP info
    if (!(bssCaps & CAP_SHORTPRE))
        sp->erp_info |= ERP_BARKER;

    // Process each info element
    //for (bp = &buf->body[12]; bp+bp[1]+2 <= buf->body+buf->length; bp += bp[1]+2)
    for (bp = &buf->body[12]; bp+bp[1]+2 <= ((uint8*)&buf->macHdr)+(buf->length-FCS_SIZE); bp += bp[1]+2)
    {
        switch (bp[0])
        {
            case ELEM_SSID:
                // Check for valid SSID
                if (bp[1] > WLN_SSID_SIZE)
                    return FALSE;

                // Save SSID
                memcpy (sp->ssid, bp+2, bp[1]);
                sp->ssid_len = bp[1];
                break;

            case ELEM_SUPRATES:
            case ELEM_EXTSUPRATES:
                // Check supported rates
                if (!CheckRates (bp+2, bp[1], sp))
                    return FALSE;
                break;

            case ELEM_DSPARAM:
                // Check channel number
                if (!CHAN_5G (channel) &&
                    (bp[1] != 1 || bp[2] != channel))
                        return FALSE;
                break;

            case ELEM_IBSSPARAM:
                // Save ATIM window if IBSS
                if (bssCaps & CAP_IBSS)
                    sp->atim = GET16 (bp+2);
                break;

            case ELEM_ERPINFO:
                // Save extended rate PHY info
                sp->erp_info = bp[2];
                break;

            case ELEM_COUNTRY:
                if (macParams.options & WLN_OPT_MULTI_DOMAIN)
                    ParseMultiDomainInfo(bp, channel);
                break;

            case ELEM_VENDOR:
                // Save WPA info
                if ((bssCaps & (CAP_ESS|CAP_PRIVACY)) == (CAP_ESS|CAP_PRIVACY) &&
                    bp[1]+2 <= WLN_WPAIE_SIZE &&
                    memcmp (bp+2, wpaOUI, sizeof wpaOUI) == 0)
                        memcpy (sp->wpa_info, bp, bp[1]+2);
                    break;

            case ELEM_RSN:
                // Save RSN info
                if ((bssCaps & (CAP_ESS|CAP_PRIVACY)) == (CAP_ESS|CAP_PRIVACY) &&
                    bp[1]+2 <= WLN_WPAIE_SIZE)
                        memcpy (sp->rsn_info, bp, bp[1]+2);
                break;
        }
    }

    if (macParams.options & WLN_OPT_MULTI_DOMAIN)
    {
        if (macStatus.tx_power < 0)
            return FALSE;
    }

    return TRUE;
}

//
// Check if BSS info matches current parameters
// @param sp BSS scan information to check
// @return TRUE if match, FALSE if not
//
static BOOL CheckBSS (wln_scan_bss *sp)
{
    uint16 bssCaps;

    // Check capabilities
    bssCaps = sp->bss_caps;
    // want ESS, but not an ESS
    if ((macParams.bss_type == WLN_BSS_ESS && !(bssCaps & CAP_ESS)) ||
        // want IBSS, but not an IBSS
        ((macParams.bss_type == WLN_BSS_IBSS || macParams.bss_type == WLN_BSS_IBSS_JOIN)
            && !(bssCaps & CAP_IBSS)) ||
        // unsecured BSS not allowed
        (!(macParams.encrypt & WLN_ENCR_OPEN) && !(bssCaps & CAP_PRIVACY)) ||
        // secured BSS not allowed
        (!(macParams.encrypt & ~WLN_ENCR_OPEN) && (bssCaps & CAP_PRIVACY)))
            return FALSE;

    // Check SSID if looking for a specific one
    if (macParams.ssid_len > 0 &&
        !(sp->ssid_len == macParams.ssid_len &&
          memcmp (sp->ssid, macParams.ssid, macParams.ssid_len) == 0))
        return FALSE;

    // No rates supported
    if (sp->rates == 0)
        return FALSE;

    // If not WPA, check if WEP allowed
    if (sp->wpa_info_len == 0 &&
        sp->rsn_info_len == 0 &&
        !(macParams.encrypt & WLN_ENCR_WEP) &&
        (bssCaps & CAP_PRIVACY))
            return FALSE;

    return TRUE;
}

//
// Process a received beacon or probe response frame while scanning
// @param buf Received frame
//
static void RecvScan (MacBuffer *buf)
{
    wln_scan_bss *sp;
    int i;

    // Check if already have this BSS
    if (scan_data)
    for (i = scan_count-1; i >= 0; i--)
            if (scan_channel == scan_data->bss[i].channel &&
                EQUAL_ADDR (buf->macHdr.addr3, scan_data->bss[i].bss_addr))
                return;

    // Allocate more space for scan data if needed
    if (scan_count == scan_max)
    {
        wln_scan_data *sd = realloc (scan_data,
            sizeof(wln_scan_data) + (scan_count+SCAN_NUM)*sizeof(wln_scan_bss));
        if (sd == NULL)
            return;
        scan_data = sd;
        scan_max = scan_count+SCAN_NUM;
    }

    // Add new BSS to end of list
    sp = &scan_data->bss[scan_count];
    if (!ParseBeacon (buf, scan_channel, sp))
        return;
    scan_count++;
    
    DPRINTF ("Found bssid=%02x:%02x:%02x:%02x:%02x:%02x "
        "ssid=%.*s chan=%d rssi=%d caps=0x%04x rates=0x%04x\n",
        sp->bss_addr[0], sp->bss_addr[1], sp->bss_addr[2],
        sp->bss_addr[3], sp->bss_addr[4], sp->bss_addr[5],
        sp->ssid_len, sp->ssid, sp->channel,
        sp->rx_signal, sp->bss_caps, sp->rates);
}

//
// Process a received beacon or probe response frame
// while joined or trying to join a BSS
// @param buf Received frame
//
static void RecvBeacon (MacBuffer *buf)
{
    wln_scan_bss info;

    // Beacon is from another BSS
    if (!EQUAL_ADDR (macStatus.bss_addr, buf->macHdr.addr3))
    {
        // Found another IBSS that matches our SSID, join it
        if (staState == SS_ASSOC &&
            (macStatus.bss_caps & CAP_IBSS) &&
            ParseBeacon (buf, macStatus.channel, &info) &&
            (info.bss_caps & CAP_IBSS) &&
            info.ssid_len == macStatus.ssid_len &&
            memcmp (info.ssid, macStatus.ssid, info.ssid_len) == 0)
        {
            if (staState == SS_ASSOC)
                wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);

            staState = SS_BEACON;
            SET_ADDR (macStatus.bss_addr, info.bss_addr);
        }
        else
            return;
    }

    // Parse beacon from current BSS
    else if (!ParseBeacon (buf, macStatus.channel, &info))
        return;

    // Restore state after a scan
    if (scan_state >= SS_JOIN)
    {
        staState = scan_state;
        scan_state = SS_WAIT;
        bcnTime = 0;
    }

    // Waiting for first beacon from BSS
    if (staState == SS_BEACON)
    {
        // Set BSS info
        macStatus.authen = 0;
        macStatus.encrypt = 0;
		atimWindow = info.atim;
		basicRates = info.rates_basic;

        // Initialize rate info
        if (!InitRates (&info, TRUE))
            return;

        // Initialize non-WPA encryption
        SetDefaultKeys();

        // Initialize beacon timer
        bcnInt = GET16 (buf->body+8);
        bcnTime = 0;

        if (macStatus.bss_caps & CAP_ESS)
        {
            // Free any beacon or probe response frames
            MacFreeBuffers (bcnFrame);
            MacFreeBuffers (respFrame);
            bcnFrame = respFrame = NULL;
		}
        else
        {
            // Make IBSS beacon using current beacon interval
            if (!MakeBeacon (bcnInt))
                return;
		}

		// Update BSS info in hardware
		SetBSS (macStatus.bss_caps, macStatus.bss_addr,
				macStatus.ssid, macStatus.ssid_len, info.rates_basic, info.atim);

        if (macStatus.bss_caps & CAP_ESS)
        {
            // Send authentication frame if ESS
            staState = SS_JOIN;
            authState = SA_INIT;
            SendNextMgmtFrame();
        }
        else
        {
            staState = SS_ASSOC;
            macStatus.state = WLN_ST_JOIN_IBSS;
            wln_send_event (WLN_EVT_ASSOC, macStatus.bss_addr);
        }
    }

    // Update rate info if protection state changes
    else if (erpInfo != info.erp_info)
    {
        if (!InitRates (&info, FALSE))
            wln_disassociate (macStatus.bss_addr, REAS_DEAUTH_LEAVING);
    }

    // Update ERP info which can change every beacon
    erpInfo = info.erp_info;
}

//
// Process a received authentication frame
// @param buf Received frame
//
static void RecvAuthentication (MacBuffer *buf)
{
    // Check authentication response
    if (GET16 (&buf->body[4]) != STAT_SUCCESS)
    {
        // Try next method
        authState++;
        SendNextMgmtFrame();
        return;
    }

    switch (authState)
    {
        case SA_OPEN:
            if (GET16 (&buf->body[0]) == AUTH_OPEN &&
                GET16 (&buf->body[2]) == 2)
            {
                staState = SS_AUTH;
                macStatus.authen = WLN_AUTH_OPEN;
                SendNextMgmtFrame();
            }
            break;

        case SA_LEAP:
            if (GET16 (&buf->body[0]) == AUTH_LEAP &&
                GET16 (&buf->body[2]) == 2)
            {
                staState = SS_AUTH;
                macStatus.authen = WLN_AUTH_LEAP;
                SendNextMgmtFrame();
            }
            break;

        case SA_SHARED:
            // Shared key challenge (2nd frame)
            if (!buf->macHdr.fc.protected &&
                GET16 (&buf->body[0]) == AUTH_SHAREDKEY &&
                GET16 (&buf->body[2]) == 2 &&
                buf->body[6] == ELEM_CHALLENGE &&
                buf->body[7] == CHAL_SIZE)
            {
                memcpy (challenge, &buf->body[8], CHAL_SIZE);
                authState = SA_SHARED2;
                SendNextMgmtFrame();
            }
            break;

        case SA_SHARED2:
            // Shared key response (4th frame)
            if (GET16 (&buf->body[0]) == AUTH_SHAREDKEY &&
                GET16 (&buf->body[2]) == 4)
            {
                staState = SS_AUTH;
                macStatus.authen = WLN_AUTH_SHAREDKEY;
                SendNextMgmtFrame();
            }
            break;

        default:
            break;
    }

    // Ignore other received authentication frames
}

//
// Compare BSS information
// Return > 0 if b is better than a
//
static int CompareBSS (wln_scan_bss *a, wln_scan_bss *b)
{
#ifdef BLACKBOX_BRAND_ENABLED
    // Black Box brand does not have a preferred SSID
#else
    // If searching for any SSID, check for preferred SSID
    if (macParams.ssid_len == 0)
    {
        BOOL pref_a = (a->ssid_len == PREF_SSID_LEN &&
                       memcmp (a->ssid, PREF_SSID, PREF_SSID_LEN) == 0);
        BOOL pref_b = (b->ssid_len == PREF_SSID_LEN &&
                       memcmp (b->ssid, PREF_SSID, PREF_SSID_LEN) == 0);

        if (pref_b && !pref_a)
            return 1;
        if (pref_a && !pref_b)
            return -1;
    }
#endif

    // Use strongest signal
    return b->rx_signal - a->rx_signal;
}

//
// Start scanning for BSS info
//
static void StartScan (uint8 *ssid, int ssid_len, int channel, BOOL requested)
{
    // Save parameters
    memcpy (scan_ssid, ssid, ssid_len);
    scan_ssid_len = ssid_len;
    if (channel == WLN_CHAN_SCAN)
        scan_chan_mask = macParams.chan_mask;
    else
        scan_chan_mask = 1 << (channel-1);
    scan_requested = requested;

    // If not already scanning, save current state to restore after scan complete
    if (scan_state == SS_WAIT)
		scan_state = staState;

    // Switch to scan state
    staState = SS_SCAN;
    scan_channel = 0;
    scan_count = 0;
    scan_time = GET_TIME();
    retry = SCAN_LIMIT;

    if (!requested)
        macStatus.state = WLN_ST_SCANNING;

    ScanChannel();
}

//
// Scan a channel for BSS info
//
static void ScanChannel (void)
{
    retry++;

    // After SCAN_LIMIT (or first time), switch to next channel.
    // Receive frames on this channel until next timer tick.
    if (retry >= SCAN_LIMIT)
    {
        // Wait for transmit queue to empty
        if (txQueue.head != NULL || txState != ST_IDLE)
            return;

        // Find next active channel
        while (scan_channel <= WLN_CHAN_NUM)
        {
            scan_channel++;
            if (scan_chan_mask & (1 << (scan_channel-1)))
                break;
        }

        // Switch to next channel
        if (scan_channel <= WLN_CHAN_NUM)
        {
            retry = 0;
            SetChannel (scan_channel);

            // Set BSSID and SSID to receive all frames
            SetBSS (0, bc_addr, NULL, 0, 0, 0);
        }

        // Finished with application requested scan
        else if (scan_requested)
        {
            // Restore state before scan
            if (scan_state >= SS_JOIN)
            {
                // Set BSS info in hardware
                SetChannel (macStatus.channel);
                SetBSS (macStatus.bss_caps, macStatus.bss_addr,
                        macStatus.ssid, macStatus.ssid_len, basicRates, atimWindow);

                // Wait to receive a beacon
                staState = SS_BEACON;
                mgmtTime = GET_TIME();
            }
           else
		        staState = scan_state = SS_WAIT;

            // Send scan event with results to application
            scan_data->count = scan_count;
            wln_send_event (WLN_EVT_SCAN_RESULTS, scan_data);

            // Force new auto scan
            scan_data->count = 0;
            scan_time = 0;
        }

        // Finished with auto scan
        else
        {
            // Sort scan results by preference
            if (scan_count > 1)
                qsort (scan_data->bss, scan_count, sizeof (wln_scan_bss),
                       (int (*)(const void *, const void *)) CompareBSS);

            // Try to join first BSS
		    scan_state = SS_WAIT;
            scan_index = 0;
            NextBSS();
        }
    }

    // On other timer ticks, send probe request
    else
    {
        uint8 *bp;
        MacBuffer *buf;

        /* wait for power level from AP if 802.11d is enabled */
        if (macParams.options & WLN_OPT_MULTI_DOMAIN)
        {
            if (macStatus.tx_power < 0)
                return;
        }

        buf = MacGetBuffer();
        if (buf == NULL)
            return;
        bp = buf->body;

        // Set SSID
        bp[0] = ELEM_SSID;
        bp[1] = scan_ssid_len;
        memcpy (&buf->body[2], scan_ssid, scan_ssid_len);
        bp += bp[1] + 2;

        // Set supported rates
        bp = SetRates (bp, 0, scan_channel);

        buf->macHdr.fc.type = TYPE_PROBE_REQ;
        SET_ADDR (buf->macHdr.addr3, bc_addr);
        buf->length = DATA_SIZE + (bp - buf->body);

        if (CHAN_5G (scan_channel))
            rateMin = RATE_MIN_A;
        else
            rateMin = RATE_MIN_B;

        SendFrames (buf, bc_addr);
    }
}

//
// Try to associate with next BSS
//
void NextBSS (void)
{
    wln_scan_bss *sp;

    // Exit if not joining a BSS
    if (macParams.bss_type == WLN_BSS_NONE)
    {
        // Clear BSSID so no more frames are received
        memset (macStatus.bss_addr, 0, WLN_ADDR_SIZE);
        SetBSS (0, macStatus.bss_addr, NULL, 0, 0, 0);

        staState = SS_WAIT;
        macStatus.state = WLN_ST_STOPPED;

        return;
    }

    staState = SS_BEACON;
    macStatus.state = WLN_ST_SCANNING;

    // Scan data has expired, start a new scan
    if (scan_time == 0 || GET_TIME()-scan_time >= SCAN_MAX_TIME)
    {
        StartScan (macParams.ssid, macParams.ssid_len, macParams.channel, FALSE);
        return;
    }

    // Try next BSS in scan info
    while (scan_index < scan_count)
    {
        sp = &scan_data->bss[scan_index];
        scan_index++;

        // Check if BSS info matches current parameters
        if (!CheckBSS (sp))
            continue;

        // Set BSS info so event handler can call wln_get_status()
        memcpy (macStatus.ssid, sp->ssid, sp->ssid_len);
        macStatus.ssid_len = sp->ssid_len;
        macStatus.channel = sp->channel;
        macStatus.bss_caps = sp->bss_caps;
        SET_ADDR (macStatus.bss_addr, sp->bss_addr);
        memcpy (macStatus.wpa_info, sp->wpa_info, sp->wpa_info_len + 2);
        memcpy (macStatus.rsn_info, sp->rsn_info, sp->rsn_info_len + 2);
        macStatus.authen = 0;
        macStatus.encrypt = 0;
        macStatus.tx_rate = sp->tx_rate;
        macStatus.rx_signal = sp->rx_signal*RSSI_SCALE;

        // If have event callback function, notify application.
        // Application will call wln_associate() to associate with this BSS.
        if (!wln_send_event (WLN_EVT_JOIN, macStatus.bss_addr))
        {
            // Try to associate with BSS.
            wln_associate (macStatus.ssid, macStatus.ssid_len, macStatus.channel, 
                macStatus.bss_addr, macStatus.bss_caps, macStatus.wpa_info);
        }
        return;
    }

    // No more scan info, try to start an IBSS
    if (StartIBSS())
        return;

    // Restart scan after a delay
    staState = SS_WAIT;
    SendMgmtFrame();
}

//
// Send authentication request
//
static void SendAuthentication (void)
{
    MacBuffer *buf = MacGetBuffer();
    if (buf == NULL)
        return;

    // Set common frame contents
    buf->macHdr.fc.type = TYPE_AUTH;
    SET16 (&buf->body[2], 1);
    SET16 (&buf->body[4], 0);
    buf->length = DATA_SIZE + 6;

    retry++;
    if (retry >= MGMT_RETRY_LIMIT)
    {
        // Try next method
        authState++;
        retry = 0;
    }

    switch (authState)
    {
        case SA_LEAP:
            // LEAP allowed and not a WPA BSS
            if ((macParams.authen & WLN_AUTH_LEAP) && 
                macStatus.wpa_info_len == 0 &&
                macStatus.rsn_info_len == 0)
            {
                SET16 (&buf->body[0], AUTH_LEAP);
                authState = SA_LEAP;
                break;
            }
            // Fall through to next method

        case SA_SHARED:
            // Shared key allowed and not a WPA BSS
            if ((macParams.authen & WLN_AUTH_SHAREDKEY) &&
                macStatus.wpa_info_len == 0 &&
                macStatus.rsn_info_len == 0)
            {
                SET16 (&buf->body[0], AUTH_SHAREDKEY);
                authState = SA_SHARED;
                break;
            }
            // Fall through to next method

        case SA_OPEN:
        case SA_FAIL2:
            // Open system or WEP with 802.1X allowed, or a WPA BSS
            if ((macParams.authen & (WLN_AUTH_OPEN|WLN_AUTH_WEP_8021X)) ||
                macStatus.wpa_info_len > 0 ||
                macStatus.rsn_info_len > 0)
            {
                SET16 (&buf->body[0], AUTH_OPEN);
                authState = SA_OPEN;
                break;
            }
            // Fall through to next method

        case SA_FAIL:
            // All methods have failed
            MacFreeBuffers (buf);
            NextBSS();
            return;

        case SA_SHARED2:
            // Shared key response (3rd frame)
            SET16 (&buf->body[IV_SIZE], AUTH_SHAREDKEY);
            SET16 (&buf->body[IV_SIZE+2], 3);
            SET16 (&buf->body[IV_SIZE+4], 0);
            buf->body[IV_SIZE+6] = ELEM_CHALLENGE;
            buf->body[IV_SIZE+7] = CHAL_SIZE;
            memcpy (&buf->body[IV_SIZE+8], challenge, CHAL_SIZE);
            buf->length = DATA_SIZE + CHAL_SIZE + 8;

            // Encrypt challenge with WEP
            if (!wepCipher.Encrypt (buf))
            {
                // No WEP keys available
                MacFreeBuffers (buf);
                authState = SA_FAIL2;
                return;
            }

            authState = SA_SHARED2;
            break;
    }

    SendFrames (buf, macStatus.bss_addr);
}

//
// Send association request
//
static void SendAssociation (void)
{
    uint8 *bp;
    uint16 caps;

    MacBuffer *buf = MacGetBuffer();
    if (buf == NULL)
        return;
    bp = buf->body;

    // Set capabilities
    caps = CAP_ESS;
    if (macParams.options & WLN_OPT_SHORTPRE)
        caps |= CAP_SHORTPRE;
    // Always support short preamble and short slot time for 802.11g
    if (!(macParams.options & WLN_OPT_BONLY))
        caps |= CAP_SHORTPRE|CAP_SHORTSLOT;
    SET16 (bp, caps);
    SET16 (bp+2, 0);
    bp += 4;

    // Set SSID
    bp[0] = ELEM_SSID;
    bp[1] = macStatus.ssid_len;
    memcpy (bp+2, macStatus.ssid, macStatus.ssid_len);
    bp += bp[1] + 2;

    // Set supported rates
    bp = SetRates (bp, 0, macStatus.channel);

    // Set RSN or WPA info
    if (macStatus.rsn_info_len > 0)
    {
        memcpy (bp, macStatus.rsn_info, macStatus.rsn_info_len + 2);
        bp += bp[1] + 2;
    }
    else if (macStatus.wpa_info_len > 0)
    {
        memcpy (bp, macStatus.wpa_info, macStatus.wpa_info_len + 2);
        bp += bp[1] + 2;
    }

    buf->macHdr.fc.type = TYPE_ASSOC_REQ;
    buf->length = DATA_SIZE + (bp - buf->body);
    SendFrames (buf, macStatus.bss_addr);
}

//
// Clear retry count and send the next management frame
//
static void SendNextMgmtFrame (void)
{
    retry = 0;
    mgmtTime = GET_TIME();
    SendMgmtFrame();
}

//
// Check retry count and send the next management frame
//
extern int monitorMode;

static void SendMgmtFrame (void)
{
    switch (staState)
    {
        case SS_WAIT:
            // If joining a BSS, restart scan after SCAN_MIN_TIME
            if (macParams.bss_type != WLN_BSS_NONE && monitorMode == 0 &&
            	(scan_time == 0 || GET_TIME()-scan_time >= SCAN_MIN_TIME))
                StartScan (macParams.ssid, macParams.ssid_len, macParams.channel, FALSE);
            break;

        case SS_BEACON:
        	// No beacon received
			if (macStatus.bss_caps & CAP_ESS)
			{
            // Can't restore connection prior to a scan, notify application
            if (scan_state == SS_ASSOC)
                wln_send_event (WLN_EVT_DISASSOC, macStatus.bss_addr);
            scan_state = SS_WAIT;

				// Continue scanning
            NextBSS();
			}

			// If IBSS, there may not be another station sending beacons.
			// Just assume we are connected after a timeout.
			else
			{
				staState = SS_ASSOC;
				scan_state = SS_WAIT;
            }
            break;

        case SS_JOIN:
            SendAuthentication();
            break;

        case SS_AUTH:
            retry++;
            if (retry >= MGMT_RETRY_LIMIT)
                NextBSS();
            else
                SendAssociation();
            break;

        default:
            break;
    }
}

//
// Start a new IBSS
//
static BOOL StartIBSS (void)
{
    wln_scan_bss info;
    int i;

    // Must have specific type, channel, and SSID
    if (!(macParams.bss_type == WLN_BSS_IBSS &&
          macParams.channel != WLN_CHAN_SCAN &&
          macParams.ssid_len != 0))
        return FALSE;

    // Init BSS info
    memcpy (macStatus.ssid, macParams.ssid, macParams.ssid_len);
    macStatus.ssid_len = macParams.ssid_len;
    macStatus.channel = macParams.channel;
    macStatus.wpa_info_len = 0;
    macStatus.rsn_info_len = 0;
    macStatus.authen = 0;
    macStatus.encrypt = 0;
    atimWindow = 0;
    erpInfo = 0;

    // Set capabilities
    macStatus.bss_caps = CAP_IBSS;
    if (!(macParams.encrypt & WLN_ENCR_OPEN))
        macStatus.bss_caps |= CAP_PRIVACY;
    if (macParams.options & WLN_OPT_SHORTPRE)
        macStatus.bss_caps |= CAP_SHORTPRE;

    // Init rate info
    if (CHAN_5G (macStatus.channel))
        info.rates = RATE_MASK_A;
    else if (macParams.options & WLN_OPT_BONLY)
        info.rates = RATE_MASK_B;
    else
        info.rates = RATE_MASK_G;

    info.rates_basic = info.rates & RATE_MASK_BASIC;
	basicRates = info.rates_basic;
    info.erp_info = 0;
    InitRates (&info, TRUE);

    // Select a random BSSID
    for (i = 0; i < WLN_ADDR_SIZE; i++)
        macStatus.bss_addr[i] = (uint8) wln_rand();
    macStatus.bss_addr[0] &= ~MAC_GROUP;  // clear group bit
    macStatus.bss_addr[0] |=  MAC_LOCAL;  // set local bit

    if (!MakeBeacon (BEACON_INT))
        return FALSE;

    // Initialize non-WPA encryption
    SetDefaultKeys();

    // Set BSS info in hardware
    SetChannel (macStatus.channel);
    SetBSS (macStatus.bss_caps, macStatus.bss_addr,
            macStatus.ssid, macStatus.ssid_len, info.rates_basic, 0);

    DPRINTF ("Start bssid=%02x:%02x:%02x:%02x:%02x:%02x "
        "ssid=%.*s chan=%d caps=0x%04x\n",
        macStatus.bss_addr[0], macStatus.bss_addr[1], macStatus.bss_addr[2],
        macStatus.bss_addr[3], macStatus.bss_addr[4], macStatus.bss_addr[5],
        macStatus.ssid_len, macStatus.ssid, macStatus.channel, macStatus.bss_caps);

    staState = SS_ASSOC;
    macStatus.state = WLN_ST_START_IBSS;
    wln_send_event (WLN_EVT_ASSOC, macStatus.bss_addr);

    return TRUE;
}

//
// Create beacon and probe response frames to send in an IBSS
// @param interval Beacon interval in TU
// @return TRUE if success, FALSE if error
//
static BOOL MakeBeacon (int interval)
{
    uint8 *bp;
    uint16 caps;
    int dur;

    // Allocate or reuse current frames
    if (bcnFrame == NULL)
        bcnFrame = MacGetBuffer();
    if (respFrame == NULL)
        respFrame = MacGetBuffer();
    if (respFrame == NULL || bcnFrame == NULL)
        return FALSE;

    // Init beacon MAC header
    memset (&bcnFrame->macHdr, 0, sizeof (MacHeader));
    bcnFrame->macHdr.fc.type = TYPE_BEACON;
    memset (bcnFrame->macHdr.addr1, 0xff, WLN_ADDR_SIZE);
    SET_ADDR (bcnFrame->macHdr.addr2, macParams.sta_addr);
    SET_ADDR (bcnFrame->macHdr.addr3, macStatus.bss_addr);

    // Set fixed params
    // Timestamp is set by hardware
    bp = bcnFrame->body;
    SET16 (&bp[8], interval);

    // Set capabilities
    caps = macStatus.bss_caps & (CAP_ESS|CAP_IBSS|CAP_PRIVACY);
    if (macParams.options & WLN_OPT_SHORTPRE)
        caps |= CAP_SHORTPRE;
    SET16 (&bp[10], caps);
    bp += 12;

    // Set SSID
    bp[0] = ELEM_SSID;
    bp[1] = macStatus.ssid_len;
    memcpy (&bp[2], macStatus.ssid, macStatus.ssid_len);
    bp += bp[1] + 2;

    // Set supported rates
    bp = SetRates (bp, ELEM_SUPRATES, macStatus.channel);

    // Set channel number
    if (!CHAN_5G (macStatus.channel))
    {
        bp[0] = ELEM_DSPARAM;
        bp[1] = 1;
        bp[2] = macStatus.channel;
        bp += bp[1] + 2;
    }

    // Set IBSS ATIM window
    bp[0] = ELEM_IBSSPARAM;
    bp[1] = 2;
    SET16 (&bp[2], atimWindow);
    bp += bp[1] + 2;

    // Set ERP info.
    if (!CHAN_5G (macStatus.channel) && !(macParams.options & WLN_OPT_BONLY))
    {
        bp[0] = ELEM_ERPINFO;
        bp[1] = 1;
        bp[2] = 0;
        bp += bp[1] + 2;
    }

    // Set extended supported rates
    bp = SetRates (bp, ELEM_EXTSUPRATES, macStatus.channel);

    bcnFrame->length = DATA_SIZE + (bp - bcnFrame->body);
    SetPLCP (bcnFrame, rateMin);

    // Copy probe response from beacon
    memcpy (respFrame, bcnFrame,
        sizeof (MacBuffer) + bcnFrame->length - DATA_SIZE);

    // Set probe response type and duration
    respFrame->macHdr.fc.type = TYPE_PROBE_RESP;
    dur = GetAckDuration (respFrame->tx_rate);
    respFrame->macHdr.duration = HTON16(dur);

    return TRUE;
}

//
// Set link status LED based on management state
//
static void SetStatusLED (void)
{
    static int count = 0;
    count++;

    switch (macStatus.state)
    {
        case WLN_ST_JOIN_IBSS:
        case WLN_ST_START_IBSS:
            SetLED (count & 7); // slow blink, 7/8 on
            break;

        case WLN_ST_AUTH_ESS:
            SetLED (TRUE);      // solid on
            break;

        case WLN_ST_ASSOC_ESS:
            if (macStatus.wpa_info_len == 0 ||
                macStatus.rsn_info_len == 0)
            {
                SetLED (TRUE);  // solid on if not WPA
                break;
            }
            // Fall through

        default:
            SetLED (count & 1); // fast blink
            break;
    }
}

//
// Update activity LED
//
void UpdateActivityLED (BOOL on)
{
    static int count = 0;
    
    // Turn on if not aleady on, turn off after a delay
    if (count == 0)
        SetActivityLED (on);
        
    if (on)
        count = 5;
    else if (count > 0)
        count--;
}


