usbkeyemu.c

USB HASP key emulator, based on USB bus driver

            LogMessage ("KEY_FN_READ_3WORDS\n");
            // Decode memory offset
            Chiper(&request->param1, 2, pKeyData);
            // Do read
            if (pKeyData->isKeyOpened && request->param1>=0 && (request->param1*2)<GetMemorySize(pKeyData)) {
                keyResponse.status=KEY_OPERATION_STATUS_OK;
                RtlCopyMemory(keyResponse.data, &pKeyData->memory[request->param1*2], sizeof(USHORT)*3);
                outDataLen=sizeof(USHORT)*3;
                encodeOutData=1;
            }
            break;

        case KEY_FN_WRITE_WORD:
            LogMessage ("KEY_FN_WRITE_WORD\n");
            // Decode memory offset & value
            Chiper(&request->param1, 4, pKeyData);
            LogMessage ("offset=%X data=%X\n", request->param1, request->param2);
            // Do write
            if (pKeyData->isKeyOpened && request->param1>=0 && (request->param1*2)<GetMemorySize(pKeyData)) {
                keyResponse.status=KEY_OPERATION_STATUS_OK;
                RtlCopyMemory(&pKeyData->memory[request->param1*2], &request->param2, sizeof(USHORT));
                outDataLen=0;
                encodeOutData=0;
            }
            break;

        case KEY_FN_READ_ST:
            LogMessage ("KEY_FN_READ_ST\n");
            // Do read ST
            if (pKeyData->isKeyOpened) {
                LONG i;
                keyResponse.status=KEY_OPERATION_STATUS_OK;
                for (i=7; i>=0; i--) 
                    keyResponse.data[7-i]=pKeyData->secTable[i];
                outDataLen=8;
                encodeOutData=1;
            }
            break;

        case KEY_FN_HASH_DWORD:
            LogMessage ("KEY_FN_HASH_DWORD\n");
            // Decode dword
            Chiper(&request->param1, 4, pKeyData);
            // Do hash dword
            if (pKeyData->isKeyOpened) {
                keyResponse.status=KEY_OPERATION_STATUS_OK;
                RtlCopyMemory(keyResponse.data, &request->param1, 4);
                HashDWORD((DWORD *)keyResponse.data, pKeyData->edStruct);
                outDataLen=sizeof(ULONG);
                encodeOutData=1;
            }
            break;
    }

    //
    // Return results
    //

    // Create encodedStatus
    LogMessage ("Create encodedStatus\n");
    // Randomize encodedStatus
    pKeyData->encodedStatus^=(UCHAR)time.LowPart;
    // If status in range KEY_OPERATION_STATUS_OK...KEY_OPERATION_STATUS_LAST
    if (keyResponse.status>=KEY_OPERATION_STATUS_OK && keyResponse.status<=KEY_OPERATION_STATUS_LAST)
        // Then create encoded status
        do {
            keyResponse.encodedStatus=++pKeyData->encodedStatus;
        } while (CheckEncodedStatus(request->majorFnCode & 0x7F, 0x02, &keyResponse.status)==0);

    // Store encoded status
    status=keyResponse.status;
    encodedStatus=keyResponse.encodedStatus;
    LogMessage ("Encoded status: %02X\n", encodedStatus);

    // Crypt status & encoded status 
    Chiper(&keyResponse.status, 2, pKeyData);

    // Crypt data
    if (encodeOutData)
        Chiper(&keyResponse.data, outDataLen, pKeyData);

    // Shuffle encoding keys
    if (status==0) {
        pKeyData->chiperKey2=(pKeyData->chiperKey2 & 0xFF) | (encodedStatus << 8);
        LogMessage ("Shuffle keys: chiperKey1=%08X, chiperKey2=%08X,\n",
            pKeyData->chiperKey1, pKeyData->chiperKey2);
    }

    // Set out data size
    *outBufLen=min(sizeof(USHORT)+outDataLen, *outBufLen);
    LogMessage ("Out data size: %X\n", *outBufLen);

    // Copy data into out buffer
    RtlCopyMemory(outBuf, &keyResponse, *outBufLen);  
}

#ifdef DEBUG_FULL
//
// USB function codes to description string conversion list
//
static WCHAR *fnCodeList[] = {
        L"URB_FUNCTION_SELECT_CONFIGURATION",
        L"URB_FUNCTION_SELECT_INTERFACE",
        L"URB_FUNCTION_ABORT_PIPE",
        L"URB_FUNCTION_TAKE_FRAME_LENGTH_CONTROL",
        L"URB_FUNCTION_RELEASE_FRAME_LENGTH_CONTROL",
        L"URB_FUNCTION_GET_FRAME_LENGTH",
        L"URB_FUNCTION_SET_FRAME_LENGTH",
        L"URB_FUNCTION_GET_CURRENT_FRAME_NUMBER",
        L"URB_FUNCTION_CONTROL_TRANSFER",
        L"URB_FUNCTION_BULK_OR_INTERRUPT_TRANSFER",
        L"URB_FUNCTION_ISOCH_TRANSFER",
        L"URB_FUNCTION_GET_DESCRIPTOR_FROM_DEVICE",
        L"URB_FUNCTION_SET_DESCRIPTOR_TO_DEVICE",
        L"URB_FUNCTION_SET_FEATURE_TO_DEVICE",
        L"URB_FUNCTION_SET_FEATURE_TO_INTERFACE",
        L"URB_FUNCTION_SET_FEATURE_TO_ENDPOINT",
        L"URB_FUNCTION_CLEAR_FEATURE_TO_DEVICE",
        L"URB_FUNCTION_CLEAR_FEATURE_TO_INTERFACE",
        L"URB_FUNCTION_CLEAR_FEATURE_TO_ENDPOINT",
        L"URB_FUNCTION_GET_STATUS_FROM_DEVICE",
        L"URB_FUNCTION_GET_STATUS_FROM_INTERFACE",
        L"URB_FUNCTION_GET_STATUS_FROM_ENDPOINT",
        L"URB_FUNCTION_RESERVED_0X0016",
        L"URB_FUNCTION_VENDOR_DEVICE",
        L"URB_FUNCTION_VENDOR_INTERFACE",
        L"URB_FUNCTION_VENDOR_ENDPOINT",
        L"URB_FUNCTION_CLASS_DEVICE",
        L"URB_FUNCTION_CLASS_INTERFACE",
        L"URB_FUNCTION_CLASS_ENDPOINT",
        L"URB_FUNCTION_RESERVE_0X001D",
        L"URB_FUNCTION_SYNC_RESET_PIPE_AND_CLEAR_STALL",
        L"URB_FUNCTION_CLASS_OTHER",
        L"URB_FUNCTION_VENDOR_OTHER",
        L"URB_FUNCTION_GET_STATUS_FROM_OTHER",
        L"URB_FUNCTION_CLEAR_FEATURE_TO_OTHER",
        L"URB_FUNCTION_SET_FEATURE_TO_OTHER",
        L"URB_FUNCTION_GET_DESCRIPTOR_FROM_ENDPOINT",
        L"URB_FUNCTION_SET_DESCRIPTOR_TO_ENDPOINT",
        L"URB_FUNCTION_GET_CONFIGURATION",
        L"URB_FUNCTION_GET_INTERFACE",
        L"URB_FUNCTION_GET_DESCRIPTOR_FROM_INTERFACE",
        L"URB_FUNCTION_SET_DESCRIPTOR_TO_INTERFACE",
        L"URB_FUNCTION_GET_MS_FEATURE_DESCRIPTOR",
        L"URB_FUNCTION_RESERVE_0X002B",
        L"URB_FUNCTION_RESERVE_0X002C",
        L"URB_FUNCTION_RESERVE_0X002D",
        L"URB_FUNCTION_RESERVE_0X002E",
        L"URB_FUNCTION_RESERVE_0X002F",
        L"URB_FUNCTION_SYNC_RESET_PIPE",
        L"URB_FUNCTION_SYNC_CLEAR_STALL",
};
#endif

NTSTATUS
Bus_HandleUSBIoCtl (
    IN  PDEVICE_OBJECT  DeviceObject,
    IN  PIRP            Irp
    )
/*++
Routine Description:

    Handle user mode PlugIn, UnPlug and device Eject requests.

Arguments:

   DeviceObject - pointer to a device object.

   Irp - pointer to an I/O Request Packet.

Return Value:

   NT status code

--*/
{
    PIO_STACK_LOCATION      irpStack;
    NTSTATUS                status;
    ULONG                   inlen, outlen, i;
    PVOID                   buffer;
    PWCHAR                  str1, str2;
    PURB                    urb;
    PPDO_DEVICE_DATA        pdoData;

    NTSTATUS                status1;
    HANDLE                  FileHandle;
    IO_STATUS_BLOCK         ioStatusBlock;
    
    PAGED_CODE ();
    
    pdoData = (PPDO_DEVICE_DATA) DeviceObject->DeviceExtension;
      
    Bus_KdPrint(pdoData, BUS_DBG_IOCTL_TRACE, ("Recive IRP_MJ_INTERNAL_DEVICE_CONTROL\n"));

    //
    // We only take Device Control requests for the devices.
    //

    if (pdoData->IsFDO) {
    
        //
        // These commands are only allowed to go to the devices.
        //   
        status = STATUS_INVALID_DEVICE_REQUEST;
        Irp->IoStatus.Status = status;
        IoCompleteRequest (Irp, IO_NO_INCREMENT);
        return status;

    }

    //
    // Check to see whether the bus is removed
    //
    
    if (pdoData->DevicePnPState == Deleted) {
        Irp->IoStatus.Status = status = STATUS_DELETE_PENDING;
        IoCompleteRequest (Irp, IO_NO_INCREMENT);
        return status;
    }

    //
    // Get IRP packet info
    //
    irpStack = IoGetCurrentIrpStackLocation (Irp);

    buffer = Irp->AssociatedIrp.SystemBuffer;  
    inlen  = irpStack->Parameters.DeviceIoControl.InputBufferLength;
    outlen = irpStack->Parameters.DeviceIoControl.OutputBufferLength;

    //
    // Get URB
    // 
    urb = irpStack->Parameters.Others.Argument1;

    //
    // And set status to 'unhandled device request'
    //
    status = STATUS_INVALID_DEVICE_REQUEST;
    
    //
    // Analyse requested IoControlCode
    //
    switch (irpStack->Parameters.DeviceIoControl.IoControlCode) {
        //
        // Request for USB bus, handle it
        //
        case IOCTL_INTERNAL_USB_SUBMIT_URB:
            Bus_KdPrint(pdoData, BUS_DBG_IOCTL_TRACE, ("Recive IOCTL_INTERNAL_USB_SUBMIT_URB\n"));
            if (urb) {
                #ifdef DEBUG_FULL
                //
                // Print request info
                //
                str1 = ExAllocatePoolWithTag (PagedPool, 512, VUSB_POOL_TAG);
                if (!str1) {
                   status = STATUS_INSUFFICIENT_RESOURCES;
                   break;
                }
                str2 = ExAllocatePoolWithTag (PagedPool, 512, VUSB_POOL_TAG);
                if (!str2) {
                   ExFreePool(str1);
                   status = STATUS_INSUFFICIENT_RESOURCES;
                   break;
                }

                if (urb->UrbHeader.Function==URB_FUNCTION_GET_DESCRIPTOR_FROM_DEVICE || urb->UrbHeader.Function==URB_FUNCTION_VENDOR_DEVICE) {
                    PrintBufferContent(str1,
                            urb->UrbControlVendorClassRequest.TransferBuffer,
                            urb->UrbControlVendorClassRequest.TransferBufferLength);
                    PrintBufferContent(str2,
                            &urb->UrbControlVendorClassRequest.Request,
                            1+2+2+2);
                    LogMessage ("Bus_HandleUSBIoCtl(): in\n"
                             "\tFunction:  %ws (%X)\n"
                             "\tLength:    %X\n"
                             "\tTransfer buffer length:     %X\n"
                             "\tTransfer buffer contents:   %ws\n"
                             "\tRequest buffer:             %ws\n"
                             "\tRequest:        %X\n"
                             "\tValue:          %X\n"
                             "\tIndex:          %X\n"
                             "\tTransferFlags:  %X\n"
                             "\tDescriptorType: %X\n"
                             "\tLanguageId:     %X\n",
                            (urb->UrbHeader.Function>=0 && urb->UrbHeader.Function<=0x31)?fnCodeList[urb->UrbHeader.Function]:L"UNKNOWN\0",
                            urb->UrbHeader.Function,
                            urb->UrbHeader.Length,
                            urb->UrbControlVendorClassRequest.TransferBufferLength,
                            str1,
                            str2,
                            urb->UrbControlVendorClassRequest.Request,
                            urb->UrbControlVendorClassRequest.Value,
                            urb->UrbControlVendorClassRequest.Index,
                            urb->UrbControlVendorClassRequest.TransferFlags,
                            urb->UrbControlDescriptorRequest.DescriptorType,
                            urb->UrbControlDescriptorRequest.LanguageId
                    );
                } else
                    LogMessage ("Bus_HandleUSBIoCtl(): in\n"
                            "\tFunction:  %ws (%X)\n"
                            "\tLength:    %X\n",
                            (urb->UrbHeader.Function>=0 && urb->UrbHeader.Function<=0x31)?fnCodeList[urb->UrbHeader.Function]:L"UNKNOWN\0",
                            urb->UrbHeader.Function,
                            urb->UrbHeader.Length
                    );
                #endif

                // Analyse requested URB function code
                switch (urb->UrbHeader.Function) {
                    //
                    // Get info about device fn
                    //
                    case URB_FUNCTION_GET_DESCRIPTOR_FROM_DEVICE:
                        switch (urb->UrbControlDescriptorRequest.DescriptorType) {
                            //
                            // Info about hardware of USB device
                            //
                            case USB_DEVICE_DESCRIPTOR_TYPE: {
                                    USB_DEVICE_DESCRIPTOR deviceDesc;
                                    deviceDesc.bLength=sizeof(deviceDesc);
                                    deviceDesc.bDescriptorType=USB_DEVICE_DESCRIPTOR_TYPE;
                                    deviceDesc.bcdUSB=0x100;
                                    deviceDesc.bDeviceClass=USB_DEVICE_CLASS_VENDOR_SPECIFIC;
                                    deviceDesc.bDeviceSubClass=0;
                                    deviceDesc.bDeviceProtocol=0;
                                    deviceDesc.bMaxPacketSize0=8;
                                    deviceDesc.idVendor=0x529;
                                    deviceDesc.idProduct=1;
                                    deviceDesc.bcdDevice=0x100;
                                    deviceDesc.iManufacturer=1;
                                    deviceDesc.iProduct=2;
                                    deviceDesc.iSerialNumber=0;
                                    deviceDesc.bNumConfigurations=1;

                                    urb->UrbControlVendorClassRequest.TransferBufferLength=
                                            min(urb->UrbControlVendorClassRequest.TransferBufferLength, sizeof(deviceDesc));

                                    RtlCopyMemory(urb->UrbControlVendorClassRequest.TransferBuffer,
                                        &deviceDesc,
                                        urb->UrbControlVendorClassRequest.TransferBufferLength
                                    );

                                    status = STATUS_SUCCESS;
                                    URB_STATUS(urb) = USBD_STATUS_SUCCESS;
                                }
                                break;

                            //
                            // Info about possible configurations of USB device
                            //
                            case USB_CONFIGURATION_DESCRIPTOR_TYPE: {
                                    struct  {
                                        USB_CONFIGURATION_DESCRIPTOR configDesc;
                                        USB_INTERFACE_DESCRIPTOR interfaceDesc;
                                    } configInfo;
                                    configInfo.configDesc.bLength=sizeof(configInfo.configDesc);
                                    configInfo.configDesc.bDescriptorType=USB_CONFIGURATION_DESCRIPTOR_TYPE;
                                    configInfo.configDesc.wTotalLength=sizeof(configInfo.configDesc)+sizeof(configInfo.interfaceDesc);
                                    configInfo.configDesc.bNumInterfaces=1;
                                    configInfo.configDesc.bConfigurationValue=1;
                                    configInfo.configDesc.iConfiguration=0;
                                    configInfo.configDesc.bmAttributes=USB_CONFIG_BUS_POWERED;
                                    configInfo.configDesc.MaxPower=54/2;

                                    configInfo.interfaceDesc.bLength=sizeof(configInfo.interfaceDesc);
                                    configInfo.interfaceDesc.bDescriptorType=USB_INTERFACE_DESCRIPTOR_TYPE;
                                    configInfo.interfaceDesc.bInterfaceNumber=0;