Exemplo n.º 1
0
VOID
KeStartAllProcessors (
    VOID
    )

/*++

Routine Description:

    This function is called during phase 1 initialization on the master boot
    processor to start all of the other registered processors.

Arguments:

    None.

Return Value:

    None.

--*/

{

#if !defined(NT_UP)

    ULONG AllocationSize;
    PUCHAR Base;
    PKPCR CurrentPcr = KeGetPcr();
    PVOID DataBlock;
    PVOID DpcStack;
    PKGDTENTRY64 GdtBase;
    ULONG GdtOffset;
    ULONG IdtOffset;
    UCHAR Index;
    PVOID KernelStack;
    ULONG LogicalProcessors;
    ULONG MaximumProcessors;
    PKNODE Node;
    UCHAR NodeNumber = 0;
    UCHAR Number;
    KIRQL OldIrql;
    PKNODE OldNode;
    PKNODE ParentNode;
    PKPCR PcrBase;
    PKPRCB Prcb;
    USHORT ProcessorId;
    KPROCESSOR_STATE ProcessorState;
    PKTSS64 SysTssBase;
    PKGDTENTRY64 TebBase;
    PETHREAD Thread;

    //
    // Ensure that prefetch instructions in the IPI path are patched out
    // if necessary before starting other processors.
    //

    OldIrql = KeRaiseIrqlToSynchLevel();
    KiIpiSendRequest(1, 0, 0, IPI_FLUSH_SINGLE);
    KeLowerIrql(OldIrql);

    //
    // Do not start additional processors if the relocate physical loader
    // switch has been specified.
    // 

    if (KeLoaderBlock->LoadOptions != NULL) {
        if (strstr(KeLoaderBlock->LoadOptions, "RELOCATEPHYSICAL") != NULL) {
            return;
        }
    }

    //
    // If this a multinode system and processor zero is not on node zero,
    // then move it to the appropriate node.
    //

    if (KeNumberNodes > 1) {
        if (NT_SUCCESS(KiQueryProcessorNode(0, &ProcessorId, &NodeNumber))) {
            if (NodeNumber != 0) {
                KiNode0.ProcessorMask = 0;
                KiNodeInit[0] = KiNode0;
                KeNodeBlock[0] = &KiNodeInit[0];
                KiNode0 = *KeNodeBlock[NodeNumber];
                KeNodeBlock[NodeNumber] = &KiNode0;
                KiNode0.ProcessorMask = 1;
            }

        } else {
            goto StartFailure;
        }
    }

    //
    // Calculate the size of the per processor data structures.
    //
    // This includes:
    //
    //   PCR (including the PRCB)
    //   System TSS
    //   Idle Thread Object
    //   Double Fault Stack
    //   Machine Check Stack
    //   NMI Stack
    //   Multinode structure
    //   GDT
    //   IDT
    //
    // A DPC and Idle stack are also allocated, but they are done separately.
    //

    AllocationSize = ROUNDUP64(sizeof(KPCR)) +
                     ROUNDUP64(sizeof(KTSS64)) +
                     ROUNDUP64(sizeof(ETHREAD)) +
                     ROUNDUP64(DOUBLE_FAULT_STACK_SIZE) +
                     ROUNDUP64(KERNEL_MCA_EXCEPTION_STACK_SIZE) +
                     ROUNDUP64(NMI_STACK_SIZE) +
                     ROUNDUP64(sizeof(KNODE));

    //
    // Save the offset of the GDT in the allocation structure and add in
    // the size of the GDT.
    //

    GdtOffset = AllocationSize;
    AllocationSize +=
            CurrentPcr->Prcb.ProcessorState.SpecialRegisters.Gdtr.Limit + 1;

    //
    // Save the offset of the IDT in the allocation structure and add in
    // the size of the IDT.
    //

    IdtOffset = AllocationSize;
    AllocationSize +=
            CurrentPcr->Prcb.ProcessorState.SpecialRegisters.Idtr.Limit + 1;

    //
    // If the registered number of processors is greater than the maximum
    // number of processors supported, then only allow the maximum number
    // of supported processors.
    //

    if (KeRegisteredProcessors > MAXIMUM_PROCESSORS) {
        KeRegisteredProcessors = MAXIMUM_PROCESSORS;
    }

    //
    // Set barrier that will prevent any other processor from entering the
    // idle loop until all processors have been started.
    //

    KiBarrierWait = 1;

    //
    // Initialize the fixed part of the processor state that will be used to
    // start processors. Each processor starts in the system initialization
    // code with address of the loader parameter block as an argument.
    //

    RtlZeroMemory(&ProcessorState, sizeof(KPROCESSOR_STATE));
    ProcessorState.ContextFrame.Rcx = (ULONG64)KeLoaderBlock;
    ProcessorState.ContextFrame.Rip = (ULONG64)KiSystemStartup;
    ProcessorState.ContextFrame.SegCs = KGDT64_R0_CODE;
    ProcessorState.ContextFrame.SegDs = KGDT64_R3_DATA | RPL_MASK;
    ProcessorState.ContextFrame.SegEs = KGDT64_R3_DATA | RPL_MASK;
    ProcessorState.ContextFrame.SegFs = KGDT64_R3_CMTEB | RPL_MASK;
    ProcessorState.ContextFrame.SegGs = KGDT64_R3_DATA | RPL_MASK;
    ProcessorState.ContextFrame.SegSs = KGDT64_R0_DATA;

    //
    // Check to determine if hyper-threading is really enabled. Intel chips
    // claim to be hyper-threaded with the number of logical processors
    // greater than one even when hyper-threading is disabled in the BIOS.
    //

    LogicalProcessors = KiLogicalProcessors;
    if (HalIsHyperThreadingEnabled() == FALSE) {
        LogicalProcessors = 1;
    }

    //
    // If the total number of logical processors has not been set with
    // the /NUMPROC loader option, then set the maximum number of logical
    // processors to the number of registered processors times the number
    // of logical processors per registered processor.
    //
    // N.B. The number of logical processors is never allowed to exceed
    //      the number of registered processors times the number of logical
    //      processors per physical processor.
    //

    MaximumProcessors = KeNumprocSpecified;
    if (MaximumProcessors == 0) {
        MaximumProcessors = KeRegisteredProcessors * LogicalProcessors;
    }

    //
    // Loop trying to start a new processors until a new processor can't be
    // started or an allocation failure occurs.
    //
    // N.B. The below processor start code relies on the fact a physical
    //      processor is started followed by all its logical processors.
    //      The HAL guarantees this by sorting the ACPI processor table
    //      by APIC id.
    //

    Index = 0;
    Number = 0;
    while ((Index < (MAXIMUM_PROCESSORS - 1)) &&
           ((ULONG)KeNumberProcessors < MaximumProcessors) &&
           ((ULONG)KeNumberProcessors / LogicalProcessors) < KeRegisteredProcessors) {

        //
        // If this is a multinode system and current processor does not
        // exist on any node, then skip it.
        //

        Index += 1;
        if (KeNumberNodes > 1) {
            if (!NT_SUCCESS(KiQueryProcessorNode(Index, &ProcessorId, &NodeNumber))) {
                continue;
            }
        }

        //
        // Increment the processor number.
        //

        Number += 1;

        //
        // Allocate memory for the new processor specific data. If the
        // allocation fails, then stop starting processors.
        //

        DataBlock = MmAllocateIndependentPages(AllocationSize, NodeNumber);
        if (DataBlock == NULL) {
            goto StartFailure;
        }

        //
        // Allocate a pool tag table for the new processor.
        //

        if (ExCreatePoolTagTable(Number, NodeNumber) == NULL) {
            goto StartFailure;
        }

        //
        // Zero the allocated memory.
        //

        Base = (PUCHAR)DataBlock;
        RtlZeroMemory(DataBlock, AllocationSize);

        //
        // Copy and initialize the GDT for the next processor.
        //

        KiCopyDescriptorMemory(&CurrentPcr->Prcb.ProcessorState.SpecialRegisters.Gdtr,
                               &ProcessorState.SpecialRegisters.Gdtr,
                               Base + GdtOffset);

        GdtBase = (PKGDTENTRY64)ProcessorState.SpecialRegisters.Gdtr.Base;

        //
        // Encode the processor number in the upper 6 bits of the compatibility
        // mode TEB descriptor.
        //

        TebBase = (PKGDTENTRY64)((PCHAR)GdtBase + KGDT64_R3_CMTEB);
        TebBase->Bits.LimitHigh = Number >> 2;
        TebBase->LimitLow = ((Number & 0x3) << 14) | (TebBase->LimitLow & 0x3fff);

        //
        // Copy and initialize the IDT for the next processor.
        //

        KiCopyDescriptorMemory(&CurrentPcr->Prcb.ProcessorState.SpecialRegisters.Idtr,
                               &ProcessorState.SpecialRegisters.Idtr,
                               Base + IdtOffset);

        //
        // Set the PCR base address for the next processor, set the processor
        // number, and set the processor speed.
        //
        // N.B. The PCR address is passed to the next processor by computing
        //      the containing address with respect to the PRCB.
        //

        PcrBase = (PKPCR)Base;
        PcrBase->ObsoleteNumber = Number;
        PcrBase->Prcb.Number = Number;
        PcrBase->Prcb.MHz = KeGetCurrentPrcb()->MHz;
        Base += ROUNDUP64(sizeof(KPCR));

        //
        // Set the system TSS descriptor base for the next processor.
        //

        SysTssBase = (PKTSS64)Base;
        KiSetDescriptorBase(KGDT64_SYS_TSS / 16, GdtBase, SysTssBase);
        Base += ROUNDUP64(sizeof(KTSS64));

        //
        // Initialize the panic stack address for double fault and NMI.
        //

        Base += DOUBLE_FAULT_STACK_SIZE;
        SysTssBase->Ist[TSS_IST_PANIC] = (ULONG64)Base;

        //
        // Initialize the machine check stack address.
        //

        Base += KERNEL_MCA_EXCEPTION_STACK_SIZE;
        SysTssBase->Ist[TSS_IST_MCA] = (ULONG64)Base;

        //
        // Initialize the NMI stack address.
        //

        Base += NMI_STACK_SIZE;
        SysTssBase->Ist[TSS_IST_NMI] = (ULONG64)Base;

        //
        // Idle Thread thread object.
        //

        Thread = (PETHREAD)Base;
        Base += ROUNDUP64(sizeof(ETHREAD));

        //
        // Set other special registers in the processor state.
        //

        ProcessorState.SpecialRegisters.Cr0 = ReadCR0();
        ProcessorState.SpecialRegisters.Cr3 = ReadCR3();
        ProcessorState.ContextFrame.EFlags = 0;
        ProcessorState.SpecialRegisters.Tr  = KGDT64_SYS_TSS;
        GdtBase[KGDT64_SYS_TSS / 16].Bytes.Flags1 = 0x89;
        ProcessorState.SpecialRegisters.Cr4 = ReadCR4();

        //
        // Allocate a kernel stack and a DPC stack for the next processor.
        //

        KernelStack = MmCreateKernelStack(FALSE, NodeNumber);
        if (KernelStack == NULL) {
            goto StartFailure;
        }

        DpcStack = MmCreateKernelStack(FALSE, NodeNumber);
        if (DpcStack == NULL) {
            goto StartFailure;
        }

        //
        // Initialize the kernel stack for the system TSS.
        //
        // N.B. System startup must be called with a stack pointer that is
        //      8 mod 16.
        //

        SysTssBase->Rsp0 = (ULONG64)KernelStack - sizeof(PVOID) * 4;
        ProcessorState.ContextFrame.Rsp = (ULONG64)KernelStack - 8;

        //
        // If this is the first processor on this node, then use the space
        // already allocated for the node. Otherwise, the space allocated
        // is not used.
        //

        Node = KeNodeBlock[NodeNumber];
        OldNode = Node;
        if (Node == &KiNodeInit[NodeNumber]) {
            Node = (PKNODE)Base;
            *Node = KiNodeInit[NodeNumber];
            KeNodeBlock[NodeNumber] = Node;
        }

        Base += ROUNDUP64(sizeof(KNODE));

        //
        // Set the parent node address.
        //

        PcrBase->Prcb.ParentNode = Node;

        //
        // Adjust the loader block so it has the next processor state. Ensure
        // that the kernel stack has space for home registers for up to four
        // parameters.
        //

        KeLoaderBlock->KernelStack = (ULONG64)DpcStack - (sizeof(PVOID) * 4);
        KeLoaderBlock->Thread = (ULONG64)Thread;
        KeLoaderBlock->Prcb = (ULONG64)(&PcrBase->Prcb);

        //
        // Attempt to start the next processor. If a processor cannot be
        // started, then deallocate memory and stop starting processors.
        //

        if (HalStartNextProcessor(KeLoaderBlock, &ProcessorState) == 0) {

            //
            // Restore the old node address in the node address array before
            // freeing the allocated data block (the node structure lies
            // within the data block).
            //

            *OldNode = *Node;
            KeNodeBlock[NodeNumber] = OldNode;
            ExDeletePoolTagTable(Number);
            MmFreeIndependentPages(DataBlock, AllocationSize);
            MmDeleteKernelStack(KernelStack, FALSE);
            MmDeleteKernelStack(DpcStack, FALSE);
            break;
        }

        Node->ProcessorMask |= AFFINITY_MASK(Number);

        //
        // Wait for processor to initialize.
        //

        while (*((volatile ULONG64 *)&KeLoaderBlock->Prcb) != 0) {
            KeYieldProcessor();
        }
    }

    //
    // All processors have been started. If this is a multinode system, then
    // allocate any missing node structures.
    //

    if (KeNumberNodes > 1) {
        for (Index = 0; Index < KeNumberNodes; Index += 1) {
            if (KeNodeBlock[Index] == &KiNodeInit[Index]) {
                Node = ExAllocatePoolWithTag(NonPagedPool, sizeof(KNODE), '  eK');
                if (Node != NULL) {
                    *Node = KiNodeInit[Index];
                    KeNodeBlock[Index] = Node;

                } else {
                    goto StartFailure;
                }
            }
        }

    } else if (KiNode0.ProcessorMask != KeActiveProcessors) {
        goto StartFailure;
    }

    //
    // Clear node structure address for nonexistent nodes.
    //

    for (Index = KeNumberNodes; Index < MAXIMUM_CCNUMA_NODES; Index += 1) {
        KeNodeBlock[Index] = NULL;
    }

    //
    // Copy the node color and shifted color to the PRCB of each processor.
    //

    for (Index = 0; Index < (ULONG)KeNumberProcessors; Index += 1) {
        Prcb = KiProcessorBlock[Index];
        ParentNode = Prcb->ParentNode;
        Prcb->NodeColor = ParentNode->Color;
        Prcb->NodeShiftedColor = ParentNode->MmShiftedColor;
        Prcb->SecondaryColorMask = MmSecondaryColorMask;
    }

    //
    // Reset the initialization bit in prefetch retry.
    //

    KiPrefetchRetry &= ~0x80;

    //
    // Reset and synchronize the performance counters of all processors, by
    // applying a null adjustment to the interrupt time.
    //

    KeAdjustInterruptTime(0);

    //
    // Allow all processors that were started to enter the idle loop and
    // begin execution.
    //

    KiBarrierWait = 0;

#endif //

    return;

    //
    // The failure to allocate memory or a unsuccessful status was returned
    // during the attempt to start processors. This is considered fatal since
    // something is very wrong.
    //

#if !defined(NT_UP)

StartFailure:
    KeBugCheckEx(PHASE1_INITIALIZATION_FAILED, 0, 0, 20, 0);

#endif

}
Exemplo n.º 2
0
NTSTATUS
DriverDeviceControl(
    IN PDEVICE_OBJECT DeviceObject,
    IN PIRP Irp
    )
{

    NTSTATUS           Status = STATUS_UNSUCCESSFUL;
    PIO_STACK_LOCATION IrpSp;
    ULONG              IOControlCode = 0;
    ULONG              dwBytesWritten = 0;
    PCHAR              pInBuf = NULL, pOutBuf = NULL;
    unsigned int       _cpu_thread_id = 0;
    unsigned int       new_cpu_thread_id = 0;
    ULONG              _num_active_cpus = 0;
    KAFFINITY          _kaffinity = 0;
    UINT32             core_id = 0;

    //
    // Get the current IRP stack location of this request
    //
    IrpSp = IoGetCurrentIrpStackLocation (Irp);
    IOControlCode = IrpSp->Parameters.DeviceIoControl.IoControlCode;

    DbgPrint( "[chipsec] >>>>>>>>>> IOCTL >>>>>>>>>>\n" );
    DbgPrint( "[chipsec] DeviceObject = 0x%x IOCTL = 0x%x\n", DeviceObject, IOControlCode );
    DbgPrint( "[chipsec] InputBufferLength = 0x%x, OutputBufferLength = 0x%x\n", IrpSp->Parameters.DeviceIoControl.InputBufferLength, IrpSp->Parameters.DeviceIoControl.OutputBufferLength );

    //
    // CPU thread ID
    // 
    _num_active_cpus = KeQueryActiveProcessorCount( NULL );
    _kaffinity       = KeQueryActiveProcessors();
    _cpu_thread_id   = KeGetCurrentProcessorNumber();
    DbgPrint( "[chipsec] Active CPU threads         : %d (KeNumberProcessors = %d)\n", _num_active_cpus, KeNumberProcessors );
    DbgPrint( "[chipsec] Active CPU mask (KAFFINITY): 0x%08X\n", _kaffinity );
    DbgPrint( "[chipsec] Current CPU thread         : %d\n", _cpu_thread_id );

    //
    // Switch on the IOCTL code that is being requested by the user.  If the
    // operation is a valid one for this device do the needful.
    //
    Irp -> IoStatus.Information = 0;
    switch( IOControlCode )
      {
        case READ_PCI_CFG_REGISTER:
          {
            DWORD val;
            BYTE size = 0;
            WORD bdf[4];
            BYTE bus = 0, dev = 0, fun = 0, off = 0;
            DbgPrint( "[chipsec] > READ_PCI_CFG_REGISTER\n" );

            RtlCopyBytes( bdf,Irp->AssociatedIrp.SystemBuffer, 4*sizeof(WORD) );
            RtlCopyBytes( &size, (BYTE*)Irp->AssociatedIrp.SystemBuffer + 4*sizeof(WORD), sizeof(BYTE) );
            bus = (UINT8)bdf[0];
            dev = (UINT8)bdf[1];
            fun = (UINT8)bdf[2];
            off = (UINT8)bdf[3];

            if( 1 != size && 2 != size && 4 != size)
              {
              DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
              Status = STATUS_INVALID_PARAMETER;
              break;
              }
            val = ReadPCICfg( bus, dev, fun, off, size );             

            IrpSp->Parameters.Read.Length = size;
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (VOID*)&val, size );
            DbgPrint( "[chipsec][READ_PCI_CFG_REGISTER] B/D/F: %#04x/%#04x/%#04x, OFFSET: %#04x, value = %#010x (size = 0x%x)\n", bus, dev, fun, off, val, size );

            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }
        case WRITE_PCI_CFG_REGISTER:
          {
            DWORD val = 0;
            WORD bdf[6];
            BYTE bus = 0, dev = 0, fun = 0, off = 0;
            BYTE size = 0;
            DbgPrint( "[chipsec] > WRITE_PCI_CFG_REGISTER\n" );

            RtlCopyBytes( bdf, Irp->AssociatedIrp.SystemBuffer, 6 * sizeof(WORD) );
            bus = (UINT8)bdf[0];
            dev = (UINT8)bdf[1];
            fun = (UINT8)bdf[2];
            off = (UINT8)bdf[3];
            RtlCopyBytes( &size, (BYTE*)Irp->AssociatedIrp.SystemBuffer + 6*sizeof(WORD), sizeof(BYTE) );

            val = ((DWORD)bdf[5] << 16) | bdf[4];
            DbgPrint( "[chipsec][WRITE_PCI_CFG_REGISTER] B/D/F: %#02x/%#02x/%#02x, OFFSET: %#02x, value = %#010x (size = %#02x)\n", bus, dev, fun, off, val, size );
            WritePCICfg( bus, dev, fun, off, size, val );

            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_READ_PHYSMEM:
          {
            UINT32 len = 0;
            PVOID virt_addr;
            PHYSICAL_ADDRESS phys_addr = { 0x0, 0x0 };

            DbgPrint( "[chipsec] > IOCTL_READ_PHYSMEM\n" );
            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength < 3*sizeof(UINT32))
              {
                DbgPrint( "[chipsec][IOCTL_READ_PHYSMEM] ERROR: STATUS_INVALID_PARAMETER\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }

            pInBuf = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;

            phys_addr.HighPart = ((UINT32*)pInBuf)[0];
            phys_addr.LowPart  = ((UINT32*)pInBuf)[1];
            len                = ((UINT32*)pInBuf)[2];
            if( !len ) len = 4;

            if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength < len )
              {
                DbgPrint( "[chipsec][IOCTL_READ_PHYSMEM] ERROR: STATUS_BUFFER_TOO_SMALL\n" );
                Status = STATUS_BUFFER_TOO_SMALL;
                break;
              }

            __try
              {
                Status = _read_phys_mem( phys_addr, len, pOutBuf );
              }
            __except (EXCEPTION_EXECUTE_HANDLER)
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][IOCTL_READ_PHYSMEM] ERROR: exception code 0x%X\n", Status );
                break;
              }

            if( NT_SUCCESS(Status) )
              {
                DbgPrint( "[chipsec][IOCTL_READ_PHYSMEM] Contents:\n" );
                _dump_buffer( (unsigned char *)pOutBuf, min(len,0x100) );
                dwBytesWritten = len;
              }
            break;
          }
        case IOCTL_WRITE_PHYSMEM:
          {
            UINT32 len = 0;
            PVOID virt_addr = 0;
            PHYSICAL_ADDRESS phys_addr = { 0x0, 0x0 };

            DbgPrint( "[chipsec] > IOCTL_WRITE_PHYSMEM\n" );
            if( Irp->AssociatedIrp.SystemBuffer )
              {
                pInBuf = Irp->AssociatedIrp.SystemBuffer;
                pOutBuf = Irp->AssociatedIrp.SystemBuffer;

                if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < 3*sizeof(UINT32) )
                  {
                    DbgPrint( "[chipsec][IOCTL_WRITE_PHYSMEM] ERROR: STATUS_INVALID_PARAMETER\n" );
                    Status = STATUS_INVALID_PARAMETER;
                    break;
                  }

                phys_addr.HighPart = ((UINT32*)pInBuf)[0];
                phys_addr.LowPart  = ((UINT32*)pInBuf)[1];
                len                = ((UINT32*)pInBuf)[2];
                ((UINT32*)pInBuf) += 3;

                if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < len + 3*sizeof(UINT32) )
                  {
                    DbgPrint( "[chipsec][IOCTL_WRITE_PHYSMEM] ERROR: STATUS_INVALID_PARAMETER\n" );
                    Status = STATUS_INVALID_PARAMETER;
                    break;
                  }

                DbgPrint( "[chipsec][IOCTL_WRITE_PHYSMEM] Writing contents:\n" );
                _dump_buffer( (unsigned char *)pInBuf, min(len,0x100) );

                __try
                  {
                    Status = _write_phys_mem( phys_addr, len, pInBuf );
                  }
                __except (EXCEPTION_EXECUTE_HANDLER)
                  {
                    Status = GetExceptionCode();
                    DbgPrint( "[chipsec][IOCTL_WRITE_PHYSMEM] ERROR: exception code 0x%X\n", Status );
                    break;
                  }

                break;
              }
          }
        case IOCTL_ALLOC_PHYSMEM:
          {
            SIZE_T NumberOfBytes = 0;
            PVOID va = 0;
            PHYSICAL_ADDRESS HighestAcceptableAddress = { 0xFFFFFFFF, 0xFFFFFFFF };

            DbgPrint( "[chipsec] > IOCTL_ALLOC_PHYSMEM\n" );
            pInBuf  = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;
            if( !pInBuf || IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(UINT64) + sizeof(UINT32))
              {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            RtlCopyBytes( &HighestAcceptableAddress.QuadPart, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(UINT64) );
            RtlCopyBytes( &NumberOfBytes, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(UINT64), sizeof(UINT32) );
            DbgPrint( "[chipsec] Allocating: NumberOfBytes = 0x%X, PhysAddr = 0x%I64x", NumberOfBytes, HighestAcceptableAddress.QuadPart );
            va = MmAllocateContiguousMemory( NumberOfBytes, HighestAcceptableAddress );
            if( !va )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_UNSUCCESSFUL - could not allocate memory\n" );
                Status = STATUS_UNSUCCESSFUL;
              }
            else if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength < 2*sizeof(UINT64) )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_BUFFER_TOO_SMALL - should be at least 2*UINT64\n" );
                Status = STATUS_BUFFER_TOO_SMALL;
              }
            else
              {
                PHYSICAL_ADDRESS pa   = MmGetPhysicalAddress( va );
                DbgPrint( "[chipsec] Allocated Buffer: VirtAddr = 0x%I64x, PhysAddr = 0x%I64x\n", (UINT64)va, pa.QuadPart );
                ((UINT64*)pOutBuf)[0] = (UINT64)va;
                ((UINT64*)pOutBuf)[1] = pa.QuadPart;

                IrpSp->Parameters.Read.Length = 2*sizeof(UINT64);
                dwBytesWritten = IrpSp->Parameters.Read.Length;
                Status = STATUS_SUCCESS;
              }

            break;
          }

        case IOCTL_FREE_PHYSMEM:
          {
            UINT64 va = 0x0;
            pInBuf  = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;

            DbgPrint( "[chipsec] > IOCTL_FREE_PHYSMEM\n" );
            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength != sizeof(UINT64))
            {
               DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
               Status = STATUS_INVALID_PARAMETER;
               break;
            }

            RtlCopyBytes( &va, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(UINT64) );
            DbgPrint( "[chipsec][IOCTL_FREE_PHYSMEM] Virtual address of the memory being freed: 0x%I64X\n", va );
            MmFreeContiguousMemory( (PVOID)va );

            IrpSp->Parameters.Read.Length = 0;
            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }

        case IOCTL_GET_PHYSADDR:
          {
            UINT64 va = 0x0;
            PHYSICAL_ADDRESS pa = { 0x0, 0x0 };

            pInBuf  = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;

            DbgPrint( "[chipsec] > IOCTL_GET_PHYSADDR\n" );
            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength != sizeof(UINT64))
            {
               DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
               Status = STATUS_INVALID_PARAMETER;
               break;
            }

            if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength < sizeof(UINT64))
            {
               DbgPrint( "[chipsec] ERROR: STATUS_BUFFER_TOO_SMALL\n" );
               Status = STATUS_BUFFER_TOO_SMALL;
               break;
            }

            RtlCopyBytes( &va, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(UINT64) );
            pa = MmGetPhysicalAddress( (PVOID)va );

            DbgPrint( "[chipsec][IOCTL_GET_PHYSADDR] Traslated virtual address 0x%I64X to physical: 0x%I64X\n", va, pa.QuadPart, pa.LowPart);
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (void*)&pa, sizeof(UINT64) );
            IrpSp->Parameters.Read.Length = sizeof(UINT64);
            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }

        case IOCTL_MAP_IO_SPACE:
          {
            PVOID va  = 0x0;
            PHYSICAL_ADDRESS pa = { 0x0, 0x0 };
            unsigned int len = 0;
            unsigned int cache_type = 0;

            pInBuf  = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;

            DbgPrint( "[chipsec] > IOCTL_MAP_IO_SPACE\n" );
            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength != 3*8)
            {
               DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
               Status = STATUS_INVALID_PARAMETER;
               break;
            }

            if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength < sizeof(UINT64))
            {
               DbgPrint( "[chipsec] ERROR: STATUS_BUFFER_TOO_SMALL\n" );
               Status = STATUS_BUFFER_TOO_SMALL;
               break;
            }

            RtlCopyBytes( &pa,         (BYTE*)Irp->AssociatedIrp.SystemBuffer + 0x00, 0x8 );
            RtlCopyBytes( &len,        (BYTE*)Irp->AssociatedIrp.SystemBuffer + 0x08, 0x4 );
            RtlCopyBytes( &cache_type, (BYTE*)Irp->AssociatedIrp.SystemBuffer + 0x10, 0x4 );

            va = MmMapIoSpace(pa, len, cache_type);

            DbgPrint( "[chipsec][IOCTL_MAP_IO_SPACE] Mapping physical address 0x%016llX to virtual 0x%016llX\n", pa, va);
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (void*)&va, sizeof(va) );
            IrpSp->Parameters.Read.Length = sizeof(va);
            dwBytesWritten = sizeof(va);
            Status = STATUS_SUCCESS;
            break;
          }

        case IOCTL_LOAD_UCODE_PATCH:
          {
            PVOID ucode_buf = NULL;
            UINT64 ucode_start = 0;
            UINT16 ucode_size = 0;
            UINT32 _eax = 0, _edx = 0;
            int CPUInfo[4] = {-1};

            DbgPrint("[chipsec] > IOCTL_LOAD_UCODE_UPDATE\n" );

            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(BYTE) + sizeof(UINT16) )
            {
               DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER (input buffer size < 3)\n" );
               Status = STATUS_INVALID_PARAMETER;
               break;
            }

            RtlCopyBytes( &new_cpu_thread_id, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(BYTE) );
            if( new_cpu_thread_id >= _num_active_cpus ) new_cpu_thread_id = 0;
            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] Changed CPU thread to %d\n", KeGetCurrentProcessorNumber() );

            RtlCopyBytes( &ucode_size, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(BYTE), sizeof(UINT16) );
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] Ucode update size = 0x%X\n", ucode_size );

            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < ucode_size + sizeof(BYTE) + sizeof(UINT16) )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER (input buffer size < ucode_size + 3)\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }

            ucode_buf = ExAllocatePoolWithTag( NonPagedPool, ucode_size, 0x3184 );
            if( !ucode_buf )
              {
                DbgPrint( "[chipsec] ERROR: couldn't allocate pool for ucode binary\n" );
                break;
              }           
            RtlCopyBytes( ucode_buf, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(BYTE) + sizeof(UINT16), ucode_size );
            ucode_start = (UINT64)ucode_buf;
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] ucode update address = 0x%p (eax = 0x%08X, edx = 0x%08X)\n", ucode_start, (UINT32)(ucode_start & 0xFFFFFFFF), (UINT32)((ucode_start >> 32) & 0xFFFFFFFF) );
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] ucode update contents:\n" );
            _dump_buffer( (unsigned char *)ucode_buf, min(ucode_size,0x100) );

            // --
            // -- trigger CPU ucode patch update
            // -- pInBuf points to the beginning of ucode update binary
            // --
            _wrmsr( MSR_IA32_BIOS_UPDT_TRIG, (UINT32)((ucode_start >> 32) & 0xFFFFFFFF), (UINT32)(ucode_start & 0xFFFFFFFF) );

            ExFreePoolWithTag( ucode_buf, 0x3184 );

            // --
            // -- check if patch was loaded
            // --
            // -- need to clear IA32_BIOS_SIGN_ID MSR first
            // -- CPUID will deposit an update ID value in 64-bit MSR at address MSR_IA32_BIOS_SIGN_ID
            // -- read IA32_BIOS_SIGN_ID MSR to check patch ID != 0
            // --
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] checking ucode update was loaded..\n" );
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] clear IA32_BIOS_SIGN_ID, CPUID EAX=1, read back IA32_BIOS_SIGN_ID\n" );
            _wrmsr( MSR_IA32_BIOS_SIGN_ID, 0, 0 );
            __cpuid(CPUInfo, 1);
            _rdmsr( MSR_IA32_BIOS_SIGN_ID, &_eax, &_edx );
            DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] RDMSR( IA32_BIOS_SIGN_ID=0x8b ) = 0x%08x%08x\n", _edx, _eax );
            if( 0 != _edx ) DbgPrint( "[chipsec][IOCTL_LOAD_UCODE_UPDATE] Microcode update loaded (ID != 0)\n" );
            else            DbgPrint( "[chipsec] ERROR: Microcode update failed\n" );

            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_WRMSR:
          {

            UINT32 msrData[3];
            UINT32 _eax = 0, _edx = 0;
            unsigned int _msr_addr;

            DbgPrint("[chipsec] > IOCTL_WRMSR\n");

            pInBuf = Irp->AssociatedIrp.SystemBuffer;
            if( !pInBuf )
              {
	        DbgPrint( "[chipsec][IOCTL_WRMSR] ERROR: NO data provided\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(BYTE) + 3*sizeof(UINT32) )
              {
                DbgPrint( "[chipsec][IOCTL_WRMSR] ERROR: STATUS_INVALID_PARAMETER (input buffer size < sizeof(BYTE) + 3*sizeof(UINT32))\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }

            RtlCopyBytes( &new_cpu_thread_id, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(BYTE) );
            if( new_cpu_thread_id >= _num_active_cpus ) new_cpu_thread_id = 0;
            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );
            DbgPrint( "[chipsec][IOCTL_WRMSR] Changed CPU thread to %d\n", KeGetCurrentProcessorNumber() );

            RtlCopyBytes( msrData, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(BYTE), 3 * sizeof(UINT32) );
            _msr_addr = msrData[0];
            _eax      = msrData[1];
            _edx      = msrData[2];
            DbgPrint( "[chipsec][IOCTL_WRMSR] WRMSR( 0x%x ) <-- 0x%08x%08x\n", _msr_addr, _edx, _eax );

            // --
            // -- write MSR
            // --
            __try
              {
                _wrmsr( _msr_addr, _edx, _eax );
              }
            __except (EXCEPTION_EXECUTE_HANDLER)
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][IOCTL_WRMSR] ERROR: exception code 0x%X\n", Status );
                break;
              }

            // --
            // -- read MSR to check if it was written
            // --
//            _rdmsr( _msr_addr, &_eax, &_edx );
//            DbgPrint( "[chipsec][IOCTL_WRMSR] RDMSR( 0x%x ) --> 0x%08x%08x\n", _msr_addr, _edx, _eax );

            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_RDMSR:
          {
            UINT32 msrData[1];
            UINT32 _eax = 0;
            UINT32 _edx = 0;
            UINT32 _msr_addr = 0;

            DbgPrint("[chipsec] > IOCTL_RDMSR\n");

            pInBuf  = Irp->AssociatedIrp.SystemBuffer;
            pOutBuf = Irp->AssociatedIrp.SystemBuffer;
            if( !pInBuf )
              {
                DbgPrint( "[chipsec] ERROR: No input provided\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(BYTE) + sizeof(UINT32) )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER - input buffer size < sizeof(BYTE) + sizeof(UINT32)\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }

            RtlCopyBytes( &new_cpu_thread_id, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(BYTE) );
            if( new_cpu_thread_id >= _num_active_cpus ) new_cpu_thread_id = 0;
            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );
            DbgPrint( "[chipsec][IOCTL_RDMSR] Changed CPU thread to %d\n", KeGetCurrentProcessorNumber() );

            RtlCopyBytes( msrData, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(BYTE), sizeof(UINT32) );
            _msr_addr = msrData[0];

            __try
              {
                _rdmsr( _msr_addr, &_eax, &_edx );
              }
            __except( EXCEPTION_EXECUTE_HANDLER )
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][IOCTL_RDMSR] ERROR: exception code 0x%X\n", Status );
                break;
              }
            DbgPrint( "[chipsec][IOCTL_RDMSR] RDMSR( 0x%x ) --> 0x%08x%08x\n", _msr_addr, _edx, _eax );

            if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength >= 2*sizeof(UINT32) )
              {
                IrpSp->Parameters.Read.Length = 2*sizeof(UINT32);
                RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (VOID*)&_eax, sizeof(UINT32) );
                RtlCopyBytes( ((UINT8*)Irp->AssociatedIrp.SystemBuffer) + sizeof(UINT32), (VOID*)&_edx, sizeof(UINT32) );

                dwBytesWritten = 2*sizeof(UINT32);
                Status = STATUS_SUCCESS;
              }
            else
              {
                DbgPrint( "[chipsec] ERROR: STATUS_BUFFER_TOO_SMALL - should be at least 2 UINT32\n" );
                Status = STATUS_BUFFER_TOO_SMALL;
              }

            break;
          }
        case READ_IO_PORT:
          {
            DWORD value;
            BYTE size = 0;
            WORD io_port;
            DbgPrint( "[chipsec] > READ_IO_PORT\n" );

            RtlCopyBytes( &io_port, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(WORD) );
            RtlCopyBytes( &size, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(WORD), sizeof(BYTE) );
            if( 1 != size && 2 != size && 4 != size)
              {
              DbgPrint( "[chipsec][READ_IO_PORT] ERROR: STATUS_INVALID_PARAMETER\n" );
              Status = STATUS_INVALID_PARAMETER;
              break;
              }

            __try
              {
                value = ReadIOPort( io_port, size );             
              }
            __except( EXCEPTION_EXECUTE_HANDLER )
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][READ_IO_PORT] ERROR: exception code 0x%X\n", Status );
                break;
              }

            IrpSp->Parameters.Read.Length = size;
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (VOID*)&value, size );
            DbgPrint( "[chipsec][READ_IO_PORT] I/O Port %#04x, value = %#010x (size = %#02x)\n", io_port, value, size );

            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }
        case WRITE_IO_PORT:
          {
            DWORD value = 0;
            WORD io_port = 0;
            BYTE size = 0;
            DbgPrint( "[chipsec] > WRITE_IO_PORT\n" );

            RtlCopyBytes( &io_port, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(WORD) );
            RtlCopyBytes( &value, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(WORD), sizeof(DWORD) );
            RtlCopyBytes( &size, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(WORD) + sizeof(DWORD), sizeof(BYTE) );
            DbgPrint( "[chipsec][WRITE_IO_PORT] I/O Port %#04x, value = %#010x (size = %#02x)\n", io_port, value, size );

            __try
              {
                WriteIOPort( value, io_port, size );
              }
            __except( EXCEPTION_EXECUTE_HANDLER )
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][WRITE_IO_PORT] ERROR: exception code 0x%X\n", Status );
                break;
              }

            Status = STATUS_SUCCESS;
            break;
          }
        case GET_CPU_DESCRIPTOR_TABLE:
          {
            BYTE dt_code = 0;
            DESCRIPTOR_TABLE_RECORD dtr;
            PDESCRIPTOR_TABLE_RECORD pdtr = &dtr;
            PHYSICAL_ADDRESS dt_pa;

            DbgPrint( "[chipsec] > GET_CPU_DESCRIPTOR_TABLE\n" );

            RtlCopyBytes( &new_cpu_thread_id, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(BYTE) );
            if( new_cpu_thread_id >= _num_active_cpus ) new_cpu_thread_id = 0;
            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );
            DbgPrint( "[chipsec][GET_CPU_DESCRIPTOR_TABLE] Changed CPU thread to %d\n", KeGetCurrentProcessorNumber() );
            RtlCopyBytes( &dt_code, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(BYTE), sizeof(BYTE) );
            DbgPrint( "[chipsec][GET_CPU_DESCRIPTOR_TABLE] Descriptor table: %x\n", dt_code );

            switch( dt_code )
              {
                case CPU_DT_CODE_GDTR:  { _store_gdtr( (void*)pdtr ); break; }
                case CPU_DT_CODE_LDTR:  { _store_ldtr( (void*)pdtr ); break; }
                case CPU_DT_CODE_IDTR:
                default:                { _store_idtr( (void*)pdtr ); break; }
              }

            DbgPrint( "[chipsec][GET_CPU_DESCRIPTOR_TABLE] Descriptor table register contents:\n" );
            _dump_buffer( (unsigned char *)pdtr, sizeof(DESCRIPTOR_TABLE_RECORD) );
            DbgPrint( "[chipsec][GET_CPU_DESCRIPTOR_TABLE] IDTR: Limit = 0x%04x, Base = 0x%I64x\n", dtr.limit, dtr.base );

            dt_pa = MmGetPhysicalAddress( (PVOID)dtr.base );
            DbgPrint( "[chipsec][GET_CPU_DESCRIPTOR_TABLE] Descriptor table PA: 0x%I64X (0x%08X_%08X)\n", dt_pa.QuadPart, dt_pa.HighPart, dt_pa.LowPart );

            IrpSp->Parameters.Read.Length = sizeof(DESCRIPTOR_TABLE_RECORD) + sizeof(dt_pa.QuadPart);
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (void*)pdtr, sizeof(DESCRIPTOR_TABLE_RECORD) );    
            RtlCopyBytes( (UINT8*)Irp->AssociatedIrp.SystemBuffer + sizeof(DESCRIPTOR_TABLE_RECORD), (VOID*)&dt_pa.QuadPart, sizeof(dt_pa.QuadPart) );    

            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_SWSMI:
          {
            CPU_REG_TYPE gprs[6] = {0};
            CPU_REG_TYPE _rax = 0, _rbx = 0, _rcx = 0, _rdx = 0, _rsi = 0, _rdi = 0;
            unsigned int _smi_code_data = 0;

            DbgPrint("[chipsec] > IOCTL_SWSMI\n");
            pInBuf = Irp->AssociatedIrp.SystemBuffer;
            if( !pInBuf )
              {
	        DbgPrint( "[chipsec] ERROR: NO data provided\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(UINT16) + sizeof(gprs) )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER (input buffer size < sizeof(UINT16) + sizeof(gprs))\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            RtlCopyBytes( &_smi_code_data, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(UINT16) );
            RtlCopyBytes( gprs, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(UINT16), sizeof(gprs) );
            _rax = gprs[ 0 ];
            _rbx = gprs[ 1 ];
            _rcx = gprs[ 2 ];
            _rdx = gprs[ 3 ];
            _rsi = gprs[ 4 ];
            _rdi = gprs[ 5 ];
            DbgPrint( "[chipsec][IOCTL_SWSMI] SW SMI to ports 0x%X-0x%X <- 0x%04X\n", 0xB2, 0xB3, _smi_code_data );
            DbgPrint( "                       RAX = 0x%I64x\n", _rax );
            DbgPrint( "                       RBX = 0x%I64x\n", _rbx );
            DbgPrint( "                       RCX = 0x%I64x\n", _rcx );
            DbgPrint( "                       RDX = 0x%I64x\n", _rdx );
            DbgPrint( "                       RSI = 0x%I64x\n", _rsi );
            DbgPrint( "                       RDI = 0x%I64x\n", _rdi );
            // --
            // -- send SMI using port 0xB2
            // --
            __try
              {
                _swsmi( _smi_code_data, _rax, _rbx, _rcx, _rdx, _rsi, _rdi );
              }
            __except( EXCEPTION_EXECUTE_HANDLER )
              {
                Status = GetExceptionCode();
                break;
              }
            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_CPUID:
          {
            DWORD CPUInfo[4] = {-1};
            DWORD gprs[2] = {0};
            DWORD _rax = 0, _rcx = 0;
            //CPU_REG_TYPE gprs[6];
            //CPU_REG_TYPE _rax = 0, _rbx = 0, _rcx = 0, _rdx = 0, _rsi = 0, _rdi = 0;

            DbgPrint("[chipsec] > IOCTL_CPUID\n");
            pInBuf = Irp->AssociatedIrp.SystemBuffer;
            if( !pInBuf )
              {
	        DbgPrint( "[chipsec] ERROR: NO data provided\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < sizeof(gprs) )
              {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER (input buffer size < %d)\n", sizeof(gprs) );
                Status = STATUS_INVALID_PARAMETER;
                break;
              }
            RtlCopyBytes( gprs, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(gprs) );
            _rax = gprs[ 0 ];
            _rcx = gprs[ 1 ];
            DbgPrint( "[chipsec][IOCTL_CPUID] CPUID:\n" );
            DbgPrint( "                       EAX = 0x%08X\n", _rax );
            DbgPrint( "                       ECX = 0x%08X\n", _rcx );

            __cpuidex( CPUInfo, _rax, _rcx );

            DbgPrint( "[chipsec][IOCTL_CPUID] CPUID returned:\n" );
            DbgPrint( "                       EAX = 0x%08X\n", CPUInfo[0] );
            DbgPrint( "                       EBX = 0x%08X\n", CPUInfo[1] );
            DbgPrint( "                       ECX = 0x%08X\n", CPUInfo[2] );
            DbgPrint( "                       EDX = 0x%08X\n", CPUInfo[3] );

            IrpSp->Parameters.Read.Length = sizeof(CPUInfo);
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (void*)CPUInfo, sizeof(CPUInfo) );    

            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }

        case IOCTL_WRCR:
          {
            UINT64 val64 = 0;
            CPU_REG_TYPE value = 0;
            WORD cr_reg = 0;
            DbgPrint( "[chipsec] > WRITE_CR\n" );

            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < (sizeof(cr_reg) + sizeof(val64) + sizeof(BYTE)))
            {
                 Status = STATUS_INVALID_PARAMETER;
                 break;
            }

            RtlCopyBytes( &cr_reg, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(cr_reg) );
            RtlCopyBytes( &val64, (BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(cr_reg), sizeof(val64) );
            new_cpu_thread_id = *((BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(cr_reg) + sizeof(val64));
            if( new_cpu_thread_id >= _num_active_cpus )
            {
            //    new_cpu_thread_id = 0;
                 Status = STATUS_INVALID_PARAMETER;
                 break;
            }

            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );
            value = (CPU_REG_TYPE)val64;
            DbgPrint( "[chipsec][WRITE_CR] CR Reg %#04x, value = %#010x \n", cr_reg, value );

            switch (cr_reg) {
            case 0: WriteCR0(value);
                Status = STATUS_SUCCESS;
                break;
            case 2: WriteCR2(value);
                Status = STATUS_SUCCESS;
                break;
            case 3: WriteCR3(value);
                Status = STATUS_SUCCESS;
                break;
            case 4: WriteCR4(value);
                Status = STATUS_SUCCESS;
                break;
            case 8:
#if defined(_M_AMD64)
                WriteCR8(value);
                Status = STATUS_SUCCESS;
                break;
#endif
            default:
                Status = STATUS_INVALID_PARAMETER;
                break;
            }

            if( !NT_SUCCESS(Status) ) {
                break;
            }

            dwBytesWritten = 0;
            Status = STATUS_SUCCESS;
            break;
          }
        case IOCTL_RDCR:
          {
            UINT64 val64 = 0;
            CPU_REG_TYPE value = 0;
            WORD cr_reg = 0;
            DbgPrint( "[chipsec] > READ_CR\n" );

            if( IrpSp->Parameters.DeviceIoControl.InputBufferLength < (sizeof(cr_reg)+sizeof(BYTE))
             || IrpSp->Parameters.DeviceIoControl.OutputBufferLength < (sizeof(val64))
              )
            {
                 Status = STATUS_INVALID_PARAMETER;
                 break;
            }

            RtlCopyBytes( &cr_reg, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(cr_reg) );
            new_cpu_thread_id = *((BYTE*)Irp->AssociatedIrp.SystemBuffer + sizeof(cr_reg));
            if( new_cpu_thread_id >= _num_active_cpus )
            {
            //    new_cpu_thread_id = 0;
                 Status = STATUS_INVALID_PARAMETER;
                 break;
            }

            KeSetSystemAffinityThread( (KAFFINITY)(1 << new_cpu_thread_id) );

            switch (cr_reg) {
            case 0: value = ReadCR0();
                Status = STATUS_SUCCESS;
                break;
            case 2: value = ReadCR2();
                Status = STATUS_SUCCESS;
                break;
            case 3: value = ReadCR3();
                Status = STATUS_SUCCESS;
                break;
            case 4: value = ReadCR4();
                Status = STATUS_SUCCESS;
                break;
            case 8:
#if defined(_M_AMD64)
                value = ReadCR8();
                Status = STATUS_SUCCESS;
                break;
#endif
            default:
                Status = STATUS_INVALID_PARAMETER;
                break;
            }
            
            if( !NT_SUCCESS(Status) ) {
                break;
            }

            val64 = value;
            RtlCopyBytes( (BYTE*)Irp->AssociatedIrp.SystemBuffer, &val64, sizeof(val64) );
            dwBytesWritten = sizeof(val64);

            DbgPrint( "[chipsec][READ_CR] CR Reg %#04x, value = %#010x \n", cr_reg, value );

            Status = STATUS_SUCCESS;
            break;

          }
        case IOCTL_HYPERCALL:
          {
            CPU_REG_TYPE regs[11] = {0};
            CPU_REG_TYPE result = 0;

            DbgPrint("[chipsec] > IOCTL_HYPERCALL\n");
            pInBuf = Irp->AssociatedIrp.SystemBuffer;

            if( !Irp->AssociatedIrp.SystemBuffer ||
                IrpSp->Parameters.DeviceIoControl.InputBufferLength != sizeof(regs))
            {
                DbgPrint( "[chipsec] ERROR: STATUS_INVALID_PARAMETER\n" );
                Status = STATUS_INVALID_PARAMETER;
                break;
            }

            if( IrpSp->Parameters.DeviceIoControl.OutputBufferLength < sizeof(result))
            {
               DbgPrint( "[chipsec] ERROR: STATUS_BUFFER_TOO_SMALL\n" );
               Status = STATUS_BUFFER_TOO_SMALL;
               break;
            }

            RtlCopyBytes( regs, (BYTE*)Irp->AssociatedIrp.SystemBuffer, sizeof(regs) );
            DbgPrint( "[chipsec][IOCTL_HYPERCALL] HYPERCALL:\n" );
            #if defined(_M_AMD64)
            DbgPrint( "    RCX = 0x%016llX  RDX = 0x%016llX\n", regs[0], regs[1] );
            DbgPrint( "    R8  = 0x%016llX  R9  = 0x%016llX\n", regs[2], regs[3] );
            DbgPrint( "    R10 = 0x%016llX  R11 = 0x%016llX\n", regs[4], regs[5] );
            DbgPrint( "    RAX = 0x%016llX  RBX = 0x%016llX\n", regs[6], regs[7] );
            DbgPrint( "    RDI = 0x%016llX  RSI = 0x%016llX\n", regs[8], regs[9] );
            #endif
            #if defined(_M_IX86)
            DbgPrint( "    EAX = 0x%08X  EBX = 0x%08X  ECX = 0x%08X\n", regs[6], regs[7], regs[0] );
            DbgPrint( "    EDX = 0x%08X  ESI = 0x%08X  EDI = 0x%08X\n", regs[1], regs[8], regs[9] );
            #endif
            DbgPrint( "    XMM0-XMM5 buffer VA = 0x%016llX\n", regs[9] );

            __try
              {
                result = hypercall(regs[0], regs[1], regs[2], regs[3], regs[4], regs[5], regs[6], regs[7], regs[8], regs[9], regs[10], &hypercall_page);
              }
            __except( EXCEPTION_EXECUTE_HANDLER )
              {
                Status = GetExceptionCode();
                DbgPrint( "[chipsec][IOCTL_HYPERCALL] ERROR: exception code 0x%X\n", Status );
                break;
              }

            DbgPrint( "[chipsec][IOCTL_HYPERCALL] returned: 0x%016llX\n", result);

            IrpSp->Parameters.Read.Length = sizeof(result);
            RtlCopyBytes( Irp->AssociatedIrp.SystemBuffer, (void*)&result, sizeof(result) );

            dwBytesWritten = IrpSp->Parameters.Read.Length;
            Status = STATUS_SUCCESS;
            break;
          }

        default:
            DbgPrint( "[chipsec] ERROR: invalid IOCTL\n");
            Status = STATUS_NOT_IMPLEMENTED;
            break;

      } // -- switch
Exemplo n.º 3
0
/* Handles ioctl's from userspace.
   See ioctl codes in chipsec-common/chipsec_ioctl.h
 */
static kern_return_t pmem_ioctl(dev_t dev, u_long cmd, caddr_t data, int flag,
                                struct proc *p) {
    //TODO (dynamically allocate these)
    pci_msg_t kpci;
    mmio_msg_t kmmio;
    cr_msg_t kcr;
    io_msg_t kio;
    msr_msg_t kmsr;
    cpuid_msg_t kcpuid;
    swsmi_msg_t kswsmi;
    hypercall_msg_t khypercall;
    msgbus_msg_t kmsgbus;
    cpudes_msg_t kcpudes;
    alloc_pmem_msg_t kalloc_pmem;

    pmem_log("cmd = %x", cmd);

    switch (cmd) {

        case CHIPSEC_IOC_RDPCI:
            pmem_log("RDPCI");
            log_addr((uint64_t) data, 64, "data");
            log_addr((uint64_t) &kpci, 64, "&krdpci");
            bcopy(data, &kpci, sizeof(pci_msg_t));
            pmem_log("ReadPCICfg(%lx, %lx, %lx, %lx, %lx)",
                     kpci.bus, kpci.device, kpci.function,
                     kpci.offset, kpci.length);

            kpci.value = ReadPCICfg(kpci.bus, kpci.device, kpci.function,
                             kpci.offset, kpci.length);
            pmem_log("kpci.value = %08x", kpci.value);

            bcopy(&kpci, data, sizeof(pci_msg_t));
            break;

        case CHIPSEC_IOC_WRPCI:
            pmem_log("WRPCI");
            bcopy(data, &kpci, sizeof(pci_msg_t));
            pmem_log("WritePCICfg(%lx, %lx, %lx, %lx, %lx, %lx)",
                     kpci.bus, kpci.device, kpci.function,
                     kpci.offset, kpci.length, kpci.value);

            WritePCICfg(kpci.bus, kpci.device, kpci.function,
                        kpci.offset, kpci.length, kpci.value);
            break;

        case CHIPSEC_IOC_RDMMIO:
            pmem_log("RDMMIO");
            bcopy(data, &kmmio, sizeof(mmio_msg_t));
            pmem_log("ReadMMIO(%lx, %x)", kmmio.addr, kmmio.length);
            kmmio.value = ReadMMIO(kmmio.addr, kmmio.length);
            pmem_log("val = %08llx", kmmio.value);

            bcopy(&kmmio, data, sizeof(mmio_msg_t));
            break;

        case CHIPSEC_IOC_WRMMIO:
            pmem_log("WRMMIO");
            bcopy(data, &kmmio, sizeof(mmio_msg_t));
            pmem_log("WriteMMIO(%lx, %x, %x)", kmmio.addr, kmmio.length,
                     (uint32_t) kmmio.value);

            WriteMMIO(kmmio.addr, kmmio.length, kmmio.value);
            break;

        case CHIPSEC_IOC_RDCR:
            pmem_log("RDCR");
            bcopy(data, &kcr, sizeof(cr_msg_t));
            pmem_log("ReadCR%d()", kcr.register_number);

            switch(kcr.register_number) {
                case 0:
                    kcr.value = ReadCR0();
                    break;
                case 2:
                    kcr.value = ReadCR2();
                    break;
                case 3:
                    kcr.value = ReadCR3();
                    break;
                case 4:
                    kcr.value = ReadCR4();
                    break;
                case 8:
                    kcr.value = ReadCR8();
                    break;
                default:
                    pmem_error("Incorrect CR number");
                    break;
            }
            bcopy(&kcr, data, sizeof(cr_msg_t));
            break;

        case CHIPSEC_IOC_WRCR:
            pmem_log("WRCR");
            bcopy(data, &kcr, sizeof(cr_msg_t));
            pmem_log("WriteCR%d(%x)", kcr.register_number, kcr.value);

            switch(kcr.register_number) {
                case 0:
                    WriteCR0(kcr.value);
                    break;
                case 2:
                    WriteCR2(kcr.value);
                    break;
                case 3:
                    WriteCR3(kcr.value);
                    break;
                case 4:
                    WriteCR4(kcr.value);
                    break;
                case 8:
                    WriteCR8(kcr.value);
                    break;
                default:
                    pmem_error("Incorrect CR number");
                    break;
            }
            bcopy(&kcr, data, sizeof(cr_msg_t));
            break;
            
        case CHIPSEC_IOC_RDIO:
            pmem_log("RDIO");
            bcopy(data,&kio, sizeof(io_msg_t));
            pmem_log("ReadIO %i from %x", kio.size, kio.port);
            kio.value = ReadIOPort((uint32_t)kio.port, kio.size);
            bcopy(&kio,data,sizeof(io_msg_t));
            break;
            
        case CHIPSEC_IOC_WRIO:
            pmem_log("WRIO");
            bcopy(data,&kio, sizeof(io_msg_t));
            pmem_log("WriteIO  %x to %x size %d", kio.value, kio.port,kio.size);
            WriteIOPort((uint32_t)kio.port, kio.size, (uint32_t)kio.value);
            break;
            
        case CHIPSEC_IOC_RDMSR:
            pmem_log("RDMSR");
            bcopy(data,&kmsr, sizeof(msr_msg_t));
            pmem_log("ReadMSR %x", kmsr.msr_num);
            ReadMSR(kmsr.msr_num, &kmsr.msr_lo, &kmsr.msr_hi);
            bcopy(&kmsr,data,sizeof(msr_msg_t));
            break;
            
        case CHIPSEC_IOC_WRMSR:
            pmem_log("WRMSR");
            bcopy(data,&kmsr, sizeof(msr_msg_t));
            pmem_log("WriteMSR  %x with %x%x", kmsr.msr_num, kmsr.msr_hi,kmsr.msr_lo);
            WriteMSR(kmsr.msr_num, kmsr.msr_lo, kmsr.msr_hi);
            break;
            
        case CHIPSEC_IOC_CPUID:
            pmem_log("CPUID");
            bcopy(data,&kcpuid, sizeof(cpuid_msg_t));
            pmem_log("WriteMSR  rax %x rcx %x", kcpuid.rax, kcpuid.rcx);
            chipCPUID(&kcpuid);
            bcopy(&kcpuid, data, sizeof(cpuid_msg_t));
            break;
        
        case CHIPSEC_IOC_SWSMI:
            pmem_log("SWSMI");
            bcopy(data,&kswsmi, sizeof(swsmi_msg_t));
            pmem_log("Blah");
            SWSMI(&kswsmi);
            bcopy(&kswsmi, data, sizeof(swsmi_msg_t));
            break;
            
        case CHIPSEC_IOC_HYPERCALL:
            pmem_log("HYPERCALL");
            bcopy(data,&khypercall, sizeof(hypercall_msg_t));
            pmem_log("Hypercall Data");
            khypercall.hypercall_page = (uint64_t) & hypercall_page;
            hypercall(khypercall.rdi, khypercall.rsi, khypercall.rdx, khypercall.rcx, khypercall.r8, khypercall.r9, khypercall.rax, khypercall.rbx, khypercall.r10, khypercall.r11, khypercall.xmm_buffer, khypercall.hypercall_page);
            bcopy(&khypercall,data, sizeof(hypercall_msg_t));
            break;
            
        case CHIPSEC_IOC_MSGBUS_SEND_MESSAGE:
            pmem_log("MSGBUG SEND MESSAGE");
            bcopy(data,&kmsgbus, sizeof(msgbus_msg_t));
            pmem_log("MSGBUS DATA:");
            if (kmsgbus.direction & MSGBUS_MDR_IN_MASK){
                //Write data to MDR register
                WritePCICfg(MSGBUS_BUS, MSGBUS_DEV, MSGBUS_FUN, MDR, 4, (uint32_t)kmsgbus.mdr);
            }
            //TODO investigate comment (from linux driver)
            //Write extended address to MCRX register if address is > 0xff
            if (kmsgbus.mcrx != 0){
                WritePCICfg(MSGBUS_BUS, MSGBUS_DEV, MSGBUS_FUN, MCRX, 4, (uint32_t)kmsgbus.mcrx);
            }
            //Write to MCR register to send the message on the message bus
            WritePCICfg(MSGBUS_BUS, MSGBUS_DEV, MSGBUS_FUN, MCR, 4, (uint32_t)kmsgbus.mcr);
            
            if (kmsgbus.direction & MSGBUS_MDR_OUT_MASK){
                //Read data from MDR register
                kmsgbus.mdr_out = ReadPCICfg(MSGBUS_BUS, MSGBUS_DEV, MSGBUS_FUN, MDR, 4);
            }
            bcopy(&kmsgbus, data, sizeof(msgbus_msg_t));
            break;
        
        case CHIPSEC_IOC_CPU_DESCRIPTOR_TABLE:
            descriptor_table_record kdtr;
            IOMemoryDescriptor* io_desc;
            IOMemoryMap* io_map;
            pmem_log("GET CPU DESCRIPTOR TABLE");
            bcopy(data, &kcpudes, sizeof(cpudes_msg_t));
            pmem_log("GET_CPU_DESCRIPTOR TABLE %x thread %d", kcpudes.des_table_code, kcpudes.cpu_thread_id);
            switch (kcpudes.des_table_code)
            {
                case CPU_DT_CODE_GDTR:
                    store_gdtr(&kdtr);
                    break;
                
                case CPU_DT_CODE_LDTR:
                    store_ldtr(&kdtr);
                    break;
                
                case CPU_DT_CODE_IDTR:
                    store_idtr(&kdtr);
                    break;
            
            }
            xlate_pa_va(kdtr.base, &io_desc, &io_map);
            kcpudes.limit = kdtr.limit;
            kcpudes.base_hi = (kdtr.base >> 32);
            kcpudes.base_lo = (kdtr.base & 0xFFFFFFFF);
            kcpudes.pa_hi = (io_map->getPhysicalAddress() >> 32);
            kcpudes.pa_lo = (io_map->getPhysicalAddress() & 0xFFFFFFFF);
            bcopy(&kcpudes, data, sizeof(cpudes_msg_t));
            break;
            
        case CHIPSEC_IOC_ALLOC_PHYSMEM:
            void *va;
            IOMemoryDescriptor* io_desc1;
            IOMemoryMap* io_map1;
            pmem_log("ALLOC PHYSMEM");
            bcopy(data, &kalloc_pmem, sizeof(alloc_pmem_msg_t));
            pmem_log("Allocating %x memory, with pa limit of %x", kalloc_pmem.num_bytes, kalloc_pmem.max_addr);
            va = IOMalloc((uint32_t)kalloc_pmem.num_bytes);
            if (!va){
                pmem_log("Could not allocate memory");
                return -EFAULT;
            }
            memset(va, 0, kalloc_pmem.num_bytes);
            
            if ( xlate_pa_va((addr64_t) va, &io_desc1, &io_map1) ){
                pmem_log("Could not map memory");
            }
            if (io_map1->getPhysicalAddress() > kalloc_pmem.max_addr){
                pmem_log("Allocate memory is above max_pa");
            }
            kalloc_pmem.virt_addr = (uint64_t)va;
            kalloc_pmem.phys_addr = io_map1->getPhysicalAddress();
            bcopy(&kalloc_pmem, data, sizeof(alloc_pmem_msg_t));
            break;

        default:
            pmem_error("Illegal ioctl %08lx", cmd);
            return -EFAULT;
    }
    return KERN_SUCCESS;
}