miniport.cxx

WinCE5.0部分核心源码

static void EthToBt (BD_ADDR *pbd_addr, unsigned char *pEth) {
    unsigned char *pbd = (unsigned char *)pbd_addr;
    for (int i = 0 ; i < 6 ; ++i) {
        pbd[i] = pEth[5-i];
    }
}

unsigned __int64 net48tohost64(unsigned char* pAddr) {
    return ((__int64)(pAddr[0]) << 40) | ((__int64)(pAddr[1]) << 32) | ((__int64)(pAddr[2]) << 24) | ((__int64)(pAddr[3]) << 16) | ((__int64)(pAddr[4]) << 8) | (__int64)(pAddr[5]);    
}

static BOOL VerifyPanUUID (GUID* pguid) {
    if ((*pguid == PANUServiceClass_UUID) || (*pguid == NAPServiceClass_UUID) || (*pguid == GNServiceClass_UUID))
        return TRUE;
    
    return FALSE;
}

static int CopyNdisToFlat
(
NDIS_BUFFER        *pNdisB,
unsigned char    *pFlatBuf,
int                cCount,
int                cStartOffset
) {
    int cOffset = 0;
    int cCopied = 0;
    while (pNdisB) {
        unsigned char *pVA = NULL;
        unsigned int uiLength = 0;

        NdisQueryBuffer (pNdisB, (PVOID *)&pVA, &uiLength);

        if (! pVA)
            break;

        if (cOffset + (int)uiLength > cStartOffset) {
            SVSUTIL_ASSERT (cOffset <= cStartOffset);
            int cBytesAvailable = cOffset + uiLength - cStartOffset;
            int cOffsetInBuffer = cStartOffset - cOffset;
            int cBytesToCopy = cBytesAvailable > cCount ? cCount : cBytesAvailable;
            memcpy (pFlatBuf, pVA + cOffsetInBuffer, cBytesToCopy);

            cCount -= cBytesToCopy;
            cStartOffset += cBytesToCopy;
            cCopied += cBytesToCopy;
            pFlatBuf += cBytesToCopy;

            SVSUTIL_ASSERT (cCount >= 0);

            if (cCount == 0)
                break;
        }

        cOffset += uiLength;
        NdisGetNextBuffer(pNdisB, &pNdisB);
    }

    return cCopied;
}

static int MpNdisStrCmp (UNICODE_STRING *pUStr, WCHAR *szStr) {
    int cStrLen  = wcslen (szStr);
    int cUStrLen = pUStr->Length/sizeof(WCHAR);

    if (cStrLen != cUStrLen)
        return cUStrLen - cStrLen;

    return wcsnicmp (pUStr->Buffer, szStr, cStrLen);
}

static BOOL HexStringToDword(WCHAR *&lpsz, DWORD &Value, int cDigits, WCHAR chDelim) {
    Value = 0;
    for (int Count = 0; Count < cDigits; Count++, lpsz++) {
        if (*lpsz >= '0' && *lpsz <= '9')
            Value = (Value << 4) + *lpsz - '0';
        else if (*lpsz >= 'A' && *lpsz <= 'F')
            Value = (Value << 4) + *lpsz - 'A' + 10;
        else if (*lpsz >= 'a' && *lpsz <= 'f')
            Value = (Value << 4) + *lpsz - 'a' + 10;
        else
            return(FALSE);
    }

    if (chDelim != 0)
        return *lpsz++ == chDelim;
    else
        return TRUE;
}

static int MpStrToGUID (WCHAR *szStr, GUID *pguid) {
    WCHAR *lpsz = szStr;
    DWORD dw;

    if (*lpsz++ != '{')
        return FALSE;

    if (!HexStringToDword(lpsz, pguid->Data1, sizeof(DWORD)*2, '-'))
        return FALSE;

    if (!HexStringToDword(lpsz, dw, sizeof(WORD)*2, '-'))
        return FALSE;

    pguid->Data2 = (WORD)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(WORD)*2, '-'))
        return FALSE;

    pguid->Data3 = (WORD)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[0] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, '-'))
        return FALSE;

    pguid->Data4[1] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[2] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[3] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[4] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[5] = (BYTE)dw;

    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[6] = (BYTE)dw;
    if (!HexStringToDword(lpsz, dw, sizeof(BYTE)*2, 0))
        return FALSE;

    pguid->Data4[7] = (BYTE)dw;

    if (*lpsz++ != '}')
        return FALSE;

    if (*lpsz++ != '\0')
        return FALSE;

    return TRUE;
}

static int MpNdisStrToGUID (UNICODE_STRING *pUStr, GUID *pguid) {
    if (pUStr->Length/sizeof(WCHAR) > 128)
        return FALSE;

    WCHAR szStr[_MAX_PATH];
    memcpy (szStr, pUStr->Buffer, pUStr->Length);
    szStr[pUStr->Length/sizeof(WCHAR)] = '\0';

    return MpStrToGUID (szStr, pguid);
}

static void QueuePendingPacket (BTHPAN_CONNECTION *pConn, BNEPPacket *pPendingPacket) {
    SVSUTIL_ASSERT (gpPAN->IsLocked ());
    
    if (! pConn->pPacketsPending) {
        pConn->pPacketsPending = pPendingPacket;            
    }
    else {
        BNEPPacket *pRunner = pConn->pPacketsPending;
        while (pRunner->pNext) {
            pRunner = pRunner->pNext;
        }
        pRunner->pNext = pPendingPacket;
    }
}

static void UUIDToWire (unsigned char *pBuff, GUID *pGuid) {
    *pBuff++ = (unsigned char)(pGuid->Data1 >> 24);
    *pBuff++ = (unsigned char)(pGuid->Data1 >> 16);
    *pBuff++ = (unsigned char)(pGuid->Data1 >> 8);
    *pBuff++ = (unsigned char)(pGuid->Data1);
    *pBuff++ = (unsigned char)(pGuid->Data2 >> 8);
    *pBuff++ = (unsigned char)(pGuid->Data2);
    *pBuff++ = (unsigned char)(pGuid->Data3 >> 8);
    *pBuff++ = (unsigned char)(pGuid->Data3);

    memcpy (pBuff, pGuid->Data4, sizeof(pGuid->Data4));
}

static BTHPAN_ADAPTER *AdapterFromCtx (void *pContext) {
    SVSUTIL_ASSERT (gpPAN->IsLocked());

    BTHPAN_ADAPTER *pAdapter = (BTHPAN_ADAPTER *)(((unsigned long)pContext) & ~0x3);   // Real pointer is 4-bytes aligned
    unsigned int uiRefCount = ((unsigned long)pContext) & 0x3;

    int iAdapter = pAdapter - gpPAN->aAdapters;
    if ((pAdapter == &gpPAN->aAdapters[iAdapter]) && (uiRefCount == (pAdapter->uiRef & 0x3)))
        return pAdapter;

    return NULL;
}

static void *AdapterToCtx (BTHPAN_ADAPTER *pAdapter) {
    SVSUTIL_ASSERT ((((unsigned long)pAdapter) & 0x3) == 0);
    return (void *)(((unsigned long)pAdapter) | (pAdapter->uiRef & 0x3));
}

static BTHPAN_ADAPTER *AdapterFromUUID (GUID *pguid) {
    for (int i = 0 ; i < SVSUTIL_ARRLEN (gpPAN->aAdapters) ; ++i) {
        if ((gpPAN->aAdapters[i].state == UP) && (gpPAN->aAdapters[i].service_id == *pguid))
            return &gpPAN->aAdapters[i];
    }

    return NULL;
}

static BTHPAN_ADAPTER *MpFindAndRefAdapter (void *pContext) {
    gpPAN->Lock ();

    BTHPAN_ADAPTER *pAdapter = AdapterFromCtx (pContext);

    if (pAdapter)
        pAdapter->AddRef ();
    else
        pAdapter = NULL;

    gpPAN->Unlock ();

    return pAdapter;
}

static void *ConnToCtx (BTHPAN_CONNECTION *pConn) {
    SVSUTIL_ASSERT ((((unsigned long)pConn) & 0x3) == 0);
    return (void *)(((unsigned long)pConn) | (pConn->uiRef & 0x3));
}

static BTHPAN_CONNECTION *ConnFromCtx (void *pCtx) {
    SVSUTIL_ASSERT (gpPAN->IsLocked ());

    BTHPAN_CONNECTION *pConn = (BTHPAN_CONNECTION *)(((unsigned long)pCtx) & ~0x3);
    unsigned int uiRefCount = ((unsigned long)pCtx) & 0x3;

    int iConn = pConn - gpPAN->aConn;
    if ((pConn == &gpPAN->aConn[iConn]) && ((pConn->uiRef & 0x3) == uiRefCount))
        return pConn;

    return NULL;
}

static BTHPAN_CONNECTION *ConnFromCid (unsigned short cid) {
    SVSUTIL_ASSERT (gpPAN->IsLocked ());
    for (int i = 0 ; i < SVSUTIL_ARRLEN(gpPAN->aConn) ; ++i) {
        if ((gpPAN->aConn[i].state != DOWN) && (gpPAN->aConn[i].cid == cid))
            return &gpPAN->aConn[i];
    }

    return NULL;
}

static int CountConn (BTHPAN_ADAPTER *pAdapter) {
    SVSUTIL_ASSERT (gpPAN->IsLocked ());

    int nAdapter = pAdapter - gpPAN->aAdapters;

    int iCount = 0;

    for (int i = 0 ; i < SVSUTIL_ARRLEN(gpPAN->aConn) ; ++i) {
        if (gpPAN->aConn[i].nAdapter == nAdapter)
            ++iCount;
    }

    return iCount;
}

static int IsBluetoothBasedGUID (GUID *pGUID, unsigned int *puiOffset) {
    if (memcmp (&pGUID->Data2, &Bluetooth_Base_UUID.Data2, sizeof (GUID) - offsetof (GUID, Data2)) != 0)
        return FALSE;

    if (pGUID->Data1 < Bluetooth_Base_UUID.Data1)
        return FALSE;

    *puiOffset = pGUID->Data1 - Bluetooth_Base_UUID.Data1;

    return TRUE;
}

//
// Bluetooth utilities
//
static void SetMediaParms (void) {
    HKEY hk;
    if (ERROR_SUCCESS == RegOpenKeyEx (HKEY_LOCAL_MACHINE, L"software\\microsoft\\bluetooth\\pan", 0, KEY_READ, &hk)) {
        DWORD dw;
        DWORD dwSize = sizeof(dw);
        DWORD dwType = 0;
        if ((ERROR_SUCCESS == RegQueryValueEx (hk, L"MediaDelay", NULL, &dwType, (LPBYTE)&dw, &dwSize)) && (dwType == REG_DWORD) &&
            (dwSize == sizeof(dw)) && ((dw == 0) || (dw >= BTHPAN_MEDIADELAY_MIN)))
            gpPAN->mediaDelay = dw;

        if (gpPAN->mediaDelay == 0) {
            gpPAN->mediaDelay = INFINITE;
        }

        dwSize = sizeof(dw);
        dwType = 0;
        if ((ERROR_SUCCESS == RegQueryValueEx (hk, L"InquiryLength", NULL, &dwType, (LPBYTE)&dw, &dwSize)) && (dwType == REG_DWORD) &&
            (dwSize == sizeof(dw)) && (dw > 0) && (dw < 256))
            gpPAN->ucInquiryLength = (unsigned char)dw;

        dwSize = sizeof(dw);
        dwType = 0;
        if ((ERROR_SUCCESS == RegQueryValueEx (hk, L"Authenticate", NULL, &dwType, (LPBYTE)&dw, &dwSize)) && (dwType == REG_DWORD) &&
            (dwSize == sizeof(dw)))
            gpPAN->fAuthenticate = (BOOL)dw;

        dwSize = sizeof(dw);
        dwType = 0;
        if ((ERROR_SUCCESS == RegQueryValueEx (hk, L"Encrypt", NULL, &dwType, (LPBYTE)&dw, &dwSize)) && (dwType == REG_DWORD) &&
            (dwSize == sizeof(dw)))
            gpPAN->fEncrypt = (BOOL)dw;

        RegCloseKey (hk);
    }
}

static void SetConnectionToDown (BTHPAN_CONNECTION *pConn) {
    pConn->state = DOWN;
    pConn->cid   = 0;
    pConn->fConfigState = 0;
    pConn->flags = 0;
    pConn->nAdapter = -1;

    memset (&pConn->dest_service_id, 0, sizeof(pConn->dest_service_id));
    memset (&pConn->filter, 0, sizeof(pConn->filter));

    ++pConn->uiRef;
}

static void SetConnectionToConnecting (BTHPAN_CONNECTION *pConn, int fPeerInit, unsigned short cid, BD_ADDR *pba, int nAdapter) {
    // Opening and closing stages are the only ones that need adapter locked. This MUST be called locked.

    pConn->state = CONNECTING;
    pConn->cid   = cid;
    pConn->fPeerInitiated = fPeerInit;
    pConn->nAdapter = nAdapter;

    if (pba) {
        pConn->ba       = *pba;
        BtToEth (pConn->eth, pba);
    }

    pConn->fConfigState = 0;
}

static void CloseConnection (BTHPAN_CONNECTION *pConn, BTHPAN_ADAPTER *pAdapter) {
    IFDBG(DebugOut (DEBUG_PAN_TRACE, L"CloseConnection 0x%08x (cid:0x%04x, ba %04x%08x)\n", pConn, pConn->cid, pConn->ba.NAP, pConn->ba.SAP));

    while (pConn->pPacketsPending) {
        BNEPPacket *pFree = pConn->pPacketsPending;
        pConn->pPacketsPending = pConn->pPacketsPending->pNext;
        delete pFree;
    }

    if (pConn->scTimeout) {
        btutil_UnScheduleEvent (pConn->scTimeout);
        pConn->scTimeout = 0;
    }

    if (! pAdapter)
        pAdapter = pConn->nAdapter == -1 ? NULL : &gpPAN->aAdapters[pConn->nAdapter];

    if (pAdapter)
        pAdapter->AddRef ();

    if (pConn->cid && (pConn->state != CLOSING)) {
        pConn->state = CLOSING;
        void *pContext = ConnToCtx (pConn);
        gpPAN->AddRef ();
        gpPAN->Unlock ();
        int iRes = gpPAN->l2cap_if.l2ca_Disconnect_In (gpPAN->hL2CAP, pContext, pConn->cid);
        gpPAN->Lock ();
        gpPAN->DelRef ();
        if (iRes != ERROR_SUCCESS) {
            IFDBG(DebugOut (DEBUG_ERROR, L"[PAN] CloseConnection : l2ca_Disconnect_In failed, %d\n", iRes));
            SetConnectionToDown (pConn);
        }
    } else if (pConn->state == CONNECTING)
        SetConnectionToDown (pConn);

    if (pConn->state == DOWN) {
        IFDBG(DebugOut (DEBUG_PAN_TRACE, L"CloseConnection : connection properly closed\n"));