stream.cpp

三星2440原版bsp

//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// This source code is licensed under Microsoft Shared Source License
// Version 1.0 for Windows CE.
// For a copy of the license visit http://go.microsoft.com/fwlink/?LinkId=3223.
//
#include "pch.h"
#if ! (defined (UNDER_CE) || defined (WINCE_EMULATION))
#include "bt_100.h"
#endif

#if ! (defined (UNDER_CE) || defined (WINCE_EMULATION))
#pragma code_seg("PAGE", "CODE")
#endif

#define RtlUlonglongByteSwap do_not_use_RtlUlonglongByteSwap

#define WRITE_STREAM(_data, _size)          \
    {                                       \
        memcpy(Stream, (_data), (_size));   \
        Stream += (_size);                  \
    }
    
PUCHAR WriteVariableSizeToStream(UCHAR Type, ULONG DataSize, PUCHAR Stream)
{
    ULONG uint32;
    USHORT uint16;
    UCHAR byte;

    if (DataSize <= SIZE_8_BITS) {
        byte = FMT_TYPE(Type) | SIZE_INDEX_NEXT_8_BITS;
        WRITE_STREAM(&byte, sizeof(byte));

        byte = (UCHAR) DataSize;
        WRITE_STREAM(&byte, sizeof(byte));
    }
    else if (DataSize <= SIZE_16_BITS) {
        byte = FMT_TYPE(Type) | SIZE_INDEX_NEXT_16_BITS;
        WRITE_STREAM(&byte, sizeof(byte));

        uint16 = (USHORT) DataSize;
        uint16 = RtlUshortByteSwap(uint16);
        WRITE_STREAM(&uint16, sizeof(uint16));
    }
    else {
        byte = FMT_TYPE(Type) | SIZE_INDEX_NEXT_32_BITS;
        WRITE_STREAM(&byte, sizeof(byte));

        uint32 = RtlUlongByteSwap(DataSize);
        WRITE_STREAM(&uint32, sizeof(uint32));
    }

    return Stream;
}

PUCHAR WriteLeafToStream(PSDP_NODE Node, PUCHAR Stream)
{
    GUID uuid128;
    ULONGLONG uint64;
    ULONG uint32, uuid32;
    USHORT uint16;

    UCHAR byte;

    switch (Node->hdr.Type) {
    case SDP_TYPE_NIL:
        // write the header and that's it
        byte = 0x00;
        WRITE_STREAM(&byte, sizeof(byte));
        break;

    case SDP_TYPE_UINT:
    case SDP_TYPE_INT: 

        byte = FMT_TYPE(Node->hdr.Type) | FMT_SIZE_INDEX_FROM_ST(Node->hdr.SpecificType);
        WRITE_STREAM(&byte, sizeof(byte));

        switch (Node->hdr.SpecificType) {
        case SDP_ST_UINT8:
            WRITE_STREAM(&Node->u.uint8, Node->DataSize);
            break;

        case SDP_ST_INT8:  
            WRITE_STREAM(&Node->u.int8, Node->DataSize);
            break;

        case SDP_ST_UINT16: 
            uint16 = RtlUshortByteSwap(Node->u.uint16);
            WRITE_STREAM(&uint16, Node->DataSize);
            break;

        case SDP_ST_INT16: 
            uint16 = RtlUshortByteSwap((USHORT) Node->u.int16);
            WRITE_STREAM(&uint16, Node->DataSize);
            break;

        case SDP_ST_UINT32:  
            uint32 = RtlUlongByteSwap(Node->u.uint32);
            WRITE_STREAM(&uint32, Node->DataSize);
            break;

        case SDP_ST_INT32:
            uint32 = RtlUlongByteSwap((ULONG) Node->u.int32);
            WRITE_STREAM(&uint32, Node->DataSize);
            break;

        case SDP_ST_UINT64:
            uint64 = SdpUlonglongByteSwap(Node->u.uint64);
            WRITE_STREAM(&uint64, Node->DataSize);
            break;

        case SDP_ST_INT64:   
            uint64 = SdpUlonglongByteSwap((ULONGLONG) Node->u.int64);
            WRITE_STREAM(&uint64, Node->DataSize);
            break;

        case SDP_ST_UINT128: 
            uint64 = SdpUlonglongByteSwap(Node->u.uint128.HighPart);
            WRITE_STREAM(&uint64, sizeof(uint64));

            uint64 = SdpUlonglongByteSwap(Node->u.uint128.LowPart);
            WRITE_STREAM(&uint64, sizeof(uint64));
            break;

        case SDP_ST_INT128:  
            uint64 = SdpUlonglongByteSwap((LONGLONG) Node->u.int128.HighPart);
            WRITE_STREAM(&uint64, sizeof(uint64));

            uint64 = SdpUlonglongByteSwap(Node->u.int128.LowPart);
            WRITE_STREAM(&uint64, sizeof(uint64));
            break;
        }
        break;

    case SDP_TYPE_UUID:
        byte = FMT_TYPE(Node->hdr.Type) | FMT_SIZE_INDEX_FROM_ST(Node->hdr.SpecificType);
        WRITE_STREAM(&byte, sizeof(byte));

        switch (Node->hdr.SpecificType) {
        case SDP_ST_UUID16:
            uint16 = RtlUshortByteSwap(Node->u.uuid16);
            WRITE_STREAM(&uint16, Node->DataSize);
            break;

        case SDP_ST_UUID32:  
            uuid32 = RtlUlongByteSwap(Node->u.uuid32);
            WRITE_STREAM(&uuid32, Node->DataSize);
            break;

        case SDP_ST_UUID128: 
            SdpByteSwapUuid128(&Node->u.uuid128, &uuid128);
            WRITE_STREAM(&uuid128, Node->DataSize);
            break;
        }
        break;

    case SDP_TYPE_BOOLEAN:
        byte = FMT_TYPE(Node->hdr.Type) | SIZE_INDEX_ZERO;
        WRITE_STREAM(&byte, sizeof(byte));

        WRITE_STREAM(&Node->u.boolean, sizeof(Node->u.boolean));
        break;

    case SDP_TYPE_STRING:
    case SDP_TYPE_URL:
        Stream = WriteVariableSizeToStream((UCHAR) Node->hdr.Type, Node->DataSize, Stream);

        if (Node->hdr.Type == SDP_TYPE_STRING) {
            WRITE_STREAM(Node->u.string, Node->DataSize);
        }
        else if (Node->hdr.Type == SDP_TYPE_URL) {
            WRITE_STREAM(Node->u.url, Node->DataSize);
        }
		break;

    default:
        ASSERT(FALSE);
        break;
    }

    return Stream;
}

void
SdpRetrieveVariableSize(
    UCHAR *Stream,
    UCHAR SizeIndex,
    ULONG *ElementSize,
    ULONG *StorageSize
    )
{
    ULONG uint32;
    USHORT uint16;

    switch (SizeIndex) {
    case 5:
        *ElementSize = ((ULONG) *Stream);  
        *StorageSize = 1;
        break;

    case 6:
        RtlRetrieveUshort(&uint16, Stream);
        *ElementSize = (ULONG) RtlUshortByteSwap(uint16);
        *StorageSize = 2;
        break;

    case 7:
        RtlRetrieveUlong(&uint32, Stream);
        *ElementSize = RtlUlongByteSwap(uint32);
        *StorageSize = 4;
        break;
    }
}

void SdpRetrieveUuidFromStream(PUCHAR Stream, ULONG DataSize, GUID *pUuid, UCHAR bigEndian)
{
//  PUCHAR tmpStream;
    ULONG ulVal;
    USHORT usVal;

    switch (DataSize) {
    case 2:
        //
        // Retrieve the value and convert it to little endian
        //
        RtlRetrieveUshort(&usVal, Stream);
        usVal = RtlUshortByteSwap(usVal);

        //
        // Add the value
        //
        memcpy(pUuid, (PVOID) &Bluetooth_Base_UUID, sizeof(GUID));
        pUuid->Data1 += usVal;

        if (bigEndian) {
            //
            // convert the UUID to big endian
            //
            SdpByteSwapUuid128(pUuid, pUuid);
        }

        break;

    case 4:
        //
        // Retrieve the value and convert it to little endian
        //
        RtlRetrieveUlong(&ulVal, Stream);
        ulVal = RtlUlongByteSwap(ulVal);

        //
        // Add the value
        //
        memcpy(pUuid, (PVOID) &Bluetooth_Base_UUID, sizeof(GUID));
        pUuid->Data1 += ulVal;

        if (bigEndian) {
            //
            // convert the UUID to big endian
            //
            SdpByteSwapUuid128(pUuid, pUuid);
        }
        break;

    case 16:
        //
        // Just retrieve the UUID, already in big endian form
        //
        SdpRetrieveUuid128(Stream, pUuid);

        //
        // convert to little endian if the caller requires
        //
        if (!bigEndian) {
            SdpByteSwapUuid128(pUuid, pUuid);
        }
        break;
    }
}



PSDP_NODE
StreamToNode(
    PUCHAR Stream,
    ULONG Size,
    PSDP_NODE_HEADER Header,
    PSDP_NODE Nodes,
    PCHAR ExtraPool,
    UCHAR FullParse
    )
{
#define LINK_IN_NODE(_list, _node)                \
{                                                   \
    InsertTailList((_list), &(_node)->hdr.Link);      \
    (_node)++;                                     \
}

#define INC_STREAM(_inc)        \
{                               \
    Stream += (_inc);           \
    Size -= (_inc);             \
}

    SdpStack                stack;
    SD_STACK_ENTRY          *stackEntry;
	PLIST_ENTRY				head, newHead;
    PSDP_NODE               node;
    PUCHAR                  tmpStream;
    ULONG                   dataSize, storageSize, elementSize;
    NTSTATUS                status;

    UCHAR                   type, sizeIndex; // , byte

    SDP_LARGE_INTEGER_16    int128;
    SDP_ULARGE_INTEGER_16   uint128;
    GUID                    uuid128, uuid128Tmp;
    ULONGLONG               uint64;
    ULONG                   uint32;
    USHORT                  uint16;
//  UCHAR                   uint8;

    if (Header) {
        node = Nodes;
        head = &Header->Link;
    }
    else {
        SdpInitializeNodeTree(Nodes);
        head = &Nodes->hdr.Link;
        node = Nodes + 1;
    }

    while (1) {
        SdpRetrieveHeader(Stream, type, sizeIndex);
        INC_STREAM(1);

        switch (type) {
        case SDP_TYPE_NIL:
            SdpInitializeNodeNil(node);
            LINK_IN_NODE(head, node);
            break;

        case SDP_TYPE_BOOLEAN:
            //
            // Do we need to validate the value?
            //
            SdpInitializeNodeBoolean(node, *Stream);
            LINK_IN_NODE(head, node);
            INC_STREAM(1);
            break;

        case SDP_TYPE_UINT:
            //
            // valid size indecies are 0 - 4.
            //
            switch (sizeIndex) {
            case 0:  
                dataSize = 1; 
                SdpInitializeNodeUInt8(node, *Stream);
                break;

            case 1:  
                dataSize = 2; 
                RtlRetrieveUshort(&uint16, Stream);
                SdpInitializeNodeUInt16(node, RtlUshortByteSwap(uint16));
                break;

            case 2:  
                dataSize = 4; 
                RtlRetrieveUlong(&uint32, Stream);
                SdpInitializeNodeUInt32(node, RtlUlongByteSwap(uint32));
                break;

            case 3:  
                dataSize = 8; 
                SdpRetrieveUlonglong(&uint64, Stream);
                SdpInitializeNodeUInt64(node, SdpUlonglongByteSwap(uint64));
                break;

            case 4:  
                dataSize = 16; 

                // high quad word first, then the low part
                tmpStream = Stream;
                SdpRetrieveUlonglong(&uint128.HighPart, tmpStream);
                
                tmpStream = Stream + sizeof(uint128.HighPart);
                SdpRetrieveUlonglong(&uint128.LowPart, tmpStream);
                
                uint128.HighPart = SdpUlonglongByteSwap(uint128.HighPart);
                uint128.LowPart = SdpUlonglongByteSwap(uint128.LowPart);
                SdpInitializeNodeUInt128(node, &uint128);
                break;

            default:
                ASSERT(FALSE); 
                return NULL;
            }

            LINK_IN_NODE(head, node);
            INC_STREAM(dataSize);
            break;

        case SDP_TYPE_INT:
            //
            // valid size indecies are 0 - 4.
            //
            switch (sizeIndex) {
            case 0:  
                dataSize = 1;