size_t MifareSTidSTRCommands::updateBinary(unsigned char blockno, const void* buf, size_t buflen)
{
INFO_("Update binary block {0x%x(%u)} [in] buffer len {%d}", blockno, blockno, buflen);
if ((buflen >= 256) || (!buf))
{
THROW_EXCEPTION_WITH_LOG(std::invalid_argument, "Bad buffer parameter.");
}
if (d_useSKB)
{
INFO_SIMPLE_("Need to use reader memory key storage (SKB) !");
return updateBinaryIndex(d_skbIndex, blockno, buf, buflen);
}
INFO_SIMPLE_(" => Rescanning card to avoid bad authentication");
scanMifare();
INFO_SIMPLE_("Scan done ! Continue to update binary block.");
if (blockno != 0)
{
std::vector<unsigned char> command;
command.push_back(static_cast<unsigned char>(d_lastKeyType));
command.push_back(blockno);
command.insert(command.end(), (unsigned char*)buf, (unsigned char*)buf + buflen);
getSTidSTRReaderCardAdapter()->sendCommand(0x00D2, command);
}
INFO_("Returns final [out] buffer len {%d}", buflen);
return buflen;
}
size_t MifareSTidSTRCommands::readBinary(unsigned char blockno, size_t len, void* buf, size_t buflen)
{
INFO_("Read binary block {0x%x(%u)} len {%d} [out] buffer len {%d}", blockno, blockno, len, buflen);
if ((len >= 256) || (len > buflen) || (!buf))
{
THROW_EXCEPTION_WITH_LOG(std::invalid_argument, "Bad buffer parameter.");
}
if (d_useSKB)
{
INFO_SIMPLE_("Need to use reader memory key storage (SKB) !");
return readBinaryIndex(d_skbIndex, blockno, len, buf, buflen);
}
INFO_SIMPLE_(" => Rescanning card to avoid bad authentication");
scanMifare();
INFO_SIMPLE_("Scan done ! Continue to read binary block.");
std::vector<unsigned char> command;
command.push_back(static_cast<unsigned char>(d_lastKeyType));
command.push_back(blockno);
std::vector<unsigned char> sbuf = getSTidSTRReaderCardAdapter()->sendCommand(0x00B2, command);
INFO_("Read binary buffer returned %s len {%d}", BufferHelper::getHex(sbuf).c_str(), sbuf.size());
EXCEPTION_ASSERT_WITH_LOG(sbuf.size() == 16, LibLogicalAccessException, "The read value should always be 16 bytes long");
EXCEPTION_ASSERT_WITH_LOG(sbuf.size() <= buflen, LibLogicalAccessException, "Buffer is too small to get all response value");
memcpy(buf, &sbuf[0], sbuf.size());
return sbuf.size();
}
static
NTSTATUS
VideoPortForwardDeviceControl(
IN PDEVICE_OBJECT DeviceObject,
IN PIRP Irp)
{
PIO_STACK_LOCATION IrpStack;
PVIDEO_PORT_DRIVER_EXTENSION DriverExtension;
PVIDEO_PORT_DEVICE_EXTENSION DeviceExtension;
VIDEO_REQUEST_PACKET vrp;
TRACE_(VIDEOPRT, "VideoPortForwardDeviceControl\n");
IrpStack = IoGetCurrentIrpStackLocation(Irp);
DeviceExtension = DeviceObject->DeviceExtension;
DriverExtension = DeviceExtension->DriverExtension;
/* Translate the IRP to a VRP */
vrp.StatusBlock = (PSTATUS_BLOCK)&Irp->IoStatus;
vrp.IoControlCode = IrpStack->Parameters.DeviceIoControl.IoControlCode;
INFO_(VIDEOPRT, "- IoControlCode: %x\n", vrp.IoControlCode);
/* We're assuming METHOD_BUFFERED */
vrp.InputBuffer = Irp->AssociatedIrp.SystemBuffer;
vrp.InputBufferLength = IrpStack->Parameters.DeviceIoControl.InputBufferLength;
vrp.OutputBuffer = Irp->AssociatedIrp.SystemBuffer;
vrp.OutputBufferLength = IrpStack->Parameters.DeviceIoControl.OutputBufferLength;
/* Call the Miniport Driver with the VRP */
DriverExtension->InitializationData.HwStartIO(&DeviceExtension->MiniPortDeviceExtension,
&vrp);
INFO_(VIDEOPRT, "- Returned status: %x\n", Irp->IoStatus.Status);
/* Map from win32 error codes to NT status values. */
switch (Irp->IoStatus.Status)
{
case NO_ERROR:
return STATUS_SUCCESS;
case ERROR_NOT_ENOUGH_MEMORY:
return STATUS_INSUFFICIENT_RESOURCES;
case ERROR_MORE_DATA:
return STATUS_BUFFER_OVERFLOW;
case ERROR_INVALID_FUNCTION:
return STATUS_NOT_IMPLEMENTED;
case ERROR_INVALID_PARAMETER:
return STATUS_INVALID_PARAMETER;
case ERROR_INSUFFICIENT_BUFFER:
return STATUS_BUFFER_TOO_SMALL;
case ERROR_DEV_NOT_EXIST:
return STATUS_DEVICE_DOES_NOT_EXIST;
case ERROR_IO_PENDING:
return STATUS_PENDING;
default:
return STATUS_UNSUCCESSFUL;
}
}
BOOLEAN NTAPI
VideoPortDDCMonitorHelper(
PVOID HwDeviceExtension,
PVOID I2CFunctions,
PUCHAR pEdidBuffer,
ULONG EdidBufferSize
)
{
PDDC_CONTROL ddc = (PDDC_CONTROL)I2CFunctions;
PI2C_CALLBACKS i2c = &ddc->I2CCallbacks;
INT Count, i;
PUCHAR pBuffer = (PUCHAR)pEdidBuffer;
BOOL Ack;
TRACE_(VIDEOPRT, "VideoPortDDCMonitorHelper()\n");
ASSERT_IRQL_LESS_OR_EQUAL(PASSIVE_LEVEL);
if (ddc->Size != sizeof (ddc))
{
WARN_(VIDEOPRT, "ddc->Size != %d (%d)\n", sizeof (ddc), ddc->Size);
return FALSE;
}
/* select eeprom */
if (!I2CStart(HwDeviceExtension, i2c, DDC_EEPROM_ADDRESS | WRITE))
return FALSE;
/* set address */
if (!I2CWrite(HwDeviceExtension, i2c, 0x00))
return FALSE;
/* change into read mode */
if (!I2CRepStart(HwDeviceExtension, i2c, DDC_EEPROM_ADDRESS | READ))
return FALSE;
/* read eeprom */
RtlZeroMemory(pEdidBuffer, EdidBufferSize);
Count = min(128, EdidBufferSize);
for (i = 0; i < Count; i++)
{
Ack = ((i + 1) < Count);
pBuffer[i] = I2CRead(HwDeviceExtension, i2c, Ack);
}
I2CStop(HwDeviceExtension, i2c);
/* check EDID header */
if (pBuffer[0] != 0x00 || pBuffer[1] != 0xff ||
pBuffer[2] != 0xff || pBuffer[3] != 0xff ||
pBuffer[4] != 0xff || pBuffer[5] != 0xff ||
pBuffer[6] != 0xff || pBuffer[7] != 0x00)
{
WARN_(VIDEOPRT, "VideoPortDDCMonitorHelper(): Invalid EDID header!\n");
return FALSE;
}
INFO_(VIDEOPRT, "VideoPortDDCMonitorHelper(): EDID version %d rev. %d\n", pBuffer[18], pBuffer[19]);
INFO_(VIDEOPRT, "VideoPortDDCMonitorHelper() - SUCCESS!\n");
return TRUE;
}
NTSTATUS NTAPI
HwDiskChanged(PDRIVE_INFO DriveInfo, PBOOLEAN DiskChanged)
/*
* FUNCTION: Detect whether the hardware has sensed a disk change
* ARGUMENTS:
* DriveInfo: pointer to the drive that we are to check
* DiskChanged: boolean that is set with whether or not the controller thinks there has been a disk change
* RETURNS:
* STATUS_SUCCESS if the drive is successfully queried
* NOTES:
* - Does not interrupt.
* - Guessing a bit at the Model30 stuff
*/
{
UCHAR Buffer;
PCONTROLLER_INFO ControllerInfo = (PCONTROLLER_INFO) DriveInfo->ControllerInfo;
Buffer = READ_PORT_UCHAR(ControllerInfo->BaseAddress + DIGITAL_INPUT_REGISTER);
TRACE_(FLOPPY, "HwDiskChanged: read 0x%x from DIR\n", Buffer);
if(ControllerInfo->Model30)
{
if(!(Buffer & DIR_DISKETTE_CHANGE))
{
INFO_(FLOPPY, "HdDiskChanged - Model30 - returning TRUE\n");
*DiskChanged = TRUE;
}
else
{
INFO_(FLOPPY, "HdDiskChanged - Model30 - returning FALSE\n");
*DiskChanged = FALSE;
}
}
else
{
if(Buffer & DIR_DISKETTE_CHANGE)
{
INFO_(FLOPPY, "HdDiskChanged - PS2 - returning TRUE\n");
*DiskChanged = TRUE;
}
else
{
INFO_(FLOPPY, "HdDiskChanged - PS2 - returning FALSE\n");
*DiskChanged = FALSE;
}
}
return STATUS_SUCCESS;
}
/*
* IntVideoPortDispatchDeviceControl
*
* Answer requests for device control calls.
*
* Run Level
* PASSIVE_LEVEL
*/
NTSTATUS
NTAPI
IntVideoPortDispatchDeviceControl(
IN PDEVICE_OBJECT DeviceObject,
IN PIRP Irp)
{
PIO_STACK_LOCATION IrpStack;
NTSTATUS Status;
ULONG IoControlCode;
TRACE_(VIDEOPRT, "IntVideoPortDispatchDeviceControl\n");
IrpStack = IoGetCurrentIrpStackLocation(Irp);
IoControlCode = IrpStack->Parameters.DeviceIoControl.IoControlCode;
INFO_(VIDEOPRT, "- IoControlCode: %x: %s\n", IoControlCode, IoctlName(IoControlCode));
switch(IoControlCode)
{
case IOCTL_VIDEO_INIT_WIN32K_CALLBACKS:
INFO_(VIDEOPRT, "- IOCTL_VIDEO_INIT_WIN32K_CALLBACKS\n");
Status = VideoPortInitWin32kCallbacks(DeviceObject,
Irp->AssociatedIrp.SystemBuffer,
IrpStack->Parameters.DeviceIoControl.InputBufferLength,
&Irp->IoStatus.Information);
break;
case IOCTL_VIDEO_USE_DEVICE_IN_SESSION:
INFO_(VIDEOPRT, "- IOCTL_VIDEO_USE_DEVICE_IN_SESSION\n");
Status = VideoPortUseDeviceInSesion(DeviceObject,
Irp->AssociatedIrp.SystemBuffer,
IrpStack->Parameters.DeviceIoControl.InputBufferLength,
&Irp->IoStatus.Information);
break;
default:
/* Forward to the Miniport Driver */
Status = VideoPortForwardDeviceControl(DeviceObject, Irp);
break;
}
INFO_(VIDEOPRT, "- Returned status: %x\n", Irp->IoStatus.Status);
Irp->IoStatus.Status = Status;
IoCompleteRequest(Irp, IO_NO_INCREMENT);
return Status;
}
/*
* FUNCTION: Write data to a port, waiting first for it to become ready
*/
BOOLEAN
i8042Write(
IN PPORT_DEVICE_EXTENSION DeviceExtension,
IN PUCHAR addr,
IN UCHAR data)
{
ULONG Counter;
ASSERT(addr);
ASSERT(DeviceExtension->ControlPort != NULL);
Counter = DeviceExtension->Settings.PollingIterations;
while ((KBD_IBF & READ_PORT_UCHAR(DeviceExtension->ControlPort)) &&
(Counter--))
{
KeStallExecutionProcessor(50);
}
if (Counter)
{
WRITE_PORT_UCHAR(addr, data);
INFO_(I8042PRT, "Sent 0x%x to port %p\n", data, addr);
return TRUE;
}
return FALSE;
}
BOOLEAN NTAPI
IntVideoPortSetupTimer(
IN PDEVICE_OBJECT DeviceObject,
IN PVIDEO_PORT_DRIVER_EXTENSION DriverExtension)
{
NTSTATUS Status;
PVIDEO_PORT_DEVICE_EXTENSION DeviceExtension;
DeviceExtension = (PVIDEO_PORT_DEVICE_EXTENSION)DeviceObject->DeviceExtension;
if (DriverExtension->InitializationData.HwTimer != NULL)
{
INFO_(VIDEOPRT, "Initializing timer\n");
Status = IoInitializeTimer(
DeviceObject,
IntVideoPortTimerRoutine,
DeviceExtension);
if (!NT_SUCCESS(Status))
{
ERR_(VIDEOPRT, "IoInitializeTimer failed with status 0x%08x\n", Status);
return FALSE;
}
}
return TRUE;
}
void MifareSTidSTRCommands::loadKey(boost::shared_ptr<Location> location, boost::shared_ptr<Key> key, MifareKeyType keytype)
{
INFO_("Loading key... location {%s} key type {0x%x(%d)}", location->serialize().c_str(), keytype, keytype);
EXCEPTION_ASSERT_WITH_LOG(location, std::invalid_argument, "location cannot be null.");
EXCEPTION_ASSERT_WITH_LOG(key, std::invalid_argument, "key cannot be null.");
boost::shared_ptr<MifareLocation> mLocation = boost::dynamic_pointer_cast<MifareLocation>(location);
boost::shared_ptr<MifareKey> mKey = boost::dynamic_pointer_cast<MifareKey>(key);
EXCEPTION_ASSERT_WITH_LOG(mLocation, std::invalid_argument, "location must be a MifareLocation.");
EXCEPTION_ASSERT_WITH_LOG(mKey, std::invalid_argument, "key must be a MifareKey.");
boost::shared_ptr<KeyStorage> key_storage = key->getKeyStorage();
if (boost::dynamic_pointer_cast<ComputerMemoryKeyStorage>(key_storage))
{
INFO_SIMPLE_("Using computer memory key storage !");
loadKey(static_cast<unsigned char>(mLocation->sector), keytype, key->getData(), key->getLength(), true);
}
else if (boost::dynamic_pointer_cast<ReaderMemoryKeyStorage>(key_storage))
{
INFO_SIMPLE_("Using reader memory key storage !");
// Don't load the key when reader memory, except if specified
if (!key->isEmpty())
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "It's not possible to change a key at specific index through host/reader connection. Please use SKB configuration card instead.");
}
}
else
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The key storage type is not supported for this card/reader.");
}
}
static VOID
i8042DetectKeyboard(
IN PPORT_DEVICE_EXTENSION DeviceExtension)
{
NTSTATUS Status;
/* Set LEDs (that is not fatal if some error occurs) */
Status = i8042SynchWritePort(DeviceExtension, 0, KBD_CMD_SET_LEDS, TRUE);
if (NT_SUCCESS(Status))
{
Status = i8042SynchWritePort(DeviceExtension, 0, 0, TRUE);
if (!NT_SUCCESS(Status))
{
WARN_(I8042PRT, "Can't finish SET_LEDS (0x%08lx)\n", Status);
return;
}
}
else
{
WARN_(I8042PRT, "Warning: can't write SET_LEDS (0x%08lx)\n", Status);
}
/* Turn on translation and SF (Some machines don't reboot if SF is not set, see ReactOS bug #1842) */
if (!i8042ChangeMode(DeviceExtension, 0, CCB_TRANSLATE | CCB_SYSTEM_FLAG))
return;
/*
* We used to send a KBD_LINE_TEST (0xAB) command, but on at least HP
* Pavilion notebooks the response to that command was incorrect.
* So now we just assume that a keyboard is attached.
*/
DeviceExtension->Flags |= KEYBOARD_PRESENT;
INFO_(I8042PRT, "Keyboard detected\n");
}
NTSTATUS NTAPI
HwReset(PCONTROLLER_INFO ControllerInfo)
/*
* FUNCTION: Reset the controller
* ARGUMENTS:
* ControllerInfo: controller to reset
* RETURNS:
* STATUS_SUCCESS in all cases
* NOTES:
* - Generates an interrupt that must be serviced four times (one per drive)
*/
{
TRACE_(FLOPPY, "HwReset called\n");
/* Write the reset bit in the DRSR */
WRITE_PORT_UCHAR(ControllerInfo->BaseAddress + DATA_RATE_SELECT_REGISTER, DRSR_SW_RESET);
/* Check for the reset bit in the DOR and set it if necessary (see Intel doc) */
if(!(READ_PORT_UCHAR(ControllerInfo->BaseAddress + DIGITAL_OUTPUT_REGISTER) & DOR_RESET))
{
HwDumpRegisters(ControllerInfo);
INFO_(FLOPPY, "HwReset: Setting Enable bit\n");
WRITE_PORT_UCHAR(ControllerInfo->BaseAddress + DIGITAL_OUTPUT_REGISTER, DOR_DMA_IO_INTERFACE_ENABLE|DOR_RESET);
HwDumpRegisters(ControllerInfo);
if(!(READ_PORT_UCHAR(ControllerInfo->BaseAddress + DIGITAL_OUTPUT_REGISTER) & DOR_RESET))
{
WARN_(FLOPPY, "HwReset: failed to set the DOR enable bit!\n");
HwDumpRegisters(ControllerInfo);
return STATUS_UNSUCCESSFUL;
}
}
return STATUS_SUCCESS;
}
/*
* FUNCTION: Read data from port 0x60
*/
NTSTATUS
i8042ReadData(
IN PPORT_DEVICE_EXTENSION DeviceExtension,
IN UCHAR StatusFlags,
OUT PUCHAR Data)
{
UCHAR PortStatus;
NTSTATUS Status;
Status = i8042ReadStatus(DeviceExtension, &PortStatus);
if (!NT_SUCCESS(Status))
return Status;
// If data is available
if (PortStatus & StatusFlags)
{
*Data = READ_PORT_UCHAR(DeviceExtension->DataPort);
INFO_(I8042PRT, "Read: 0x%02x (status: 0x%x)\n", Data[0], PortStatus);
// If the data is valid (not timeout, not parity error)
if ((PortStatus & KBD_PERR) == 0)
return STATUS_SUCCESS;
}
return STATUS_UNSUCCESSFUL;
}
NTSTATUS NTAPI
HwGetVersion(PCONTROLLER_INFO ControllerInfo)
/*
* FUNCTION: Gets the version of the controller
* ARGUMENTS:
* ControllerInfo: controller to target with the request
* ConfigValue: Configuration value to send to the drive (see header)
* RETURNS:
* Version number returned by the command, or
* 0 on failure
* NOTE:
* - This command doesn't interrupt, so we go right to reading after
* we issue the command
*/
{
UCHAR Buffer;
PAGED_CODE();
if(Send_Byte(ControllerInfo, COMMAND_VERSION) != STATUS_SUCCESS)
{
WARN_(FLOPPY, "HwGetVersion: unable to write fifo\n");
return STATUS_UNSUCCESSFUL;
}
if(Get_Byte(ControllerInfo, &Buffer) != STATUS_SUCCESS)
{
WARN_(FLOPPY, "HwGetVersion: unable to write fifo\n");
return STATUS_UNSUCCESSFUL;
}
INFO_(FLOPPY, "HwGetVersion returning version 0x%x\n", Buffer);
return Buffer;
}
NTSTATUS NTAPI
HwSenseInterruptStatus(PCONTROLLER_INFO ControllerInfo)
/*
* FUNCTION: Send a sense interrupt status command to a controller
* ARGUMENTS:
* ControllerInfo: controller to queue the command to
* RETURNS:
* STATUS_SUCCESS if the command is queued successfully
* STATUS_UNSUCCESSFUL if not
*/
{
UCHAR Buffer[2];
int i;
PAGED_CODE();
if(Send_Byte(ControllerInfo, COMMAND_SENSE_INTERRUPT_STATUS) != STATUS_SUCCESS)
{
WARN_(FLOPPY, "HwSenseInterruptStatus: failed to write controller\n");
return STATUS_UNSUCCESSFUL;
}
for(i = 0; i < 2; i++)
{
if(Get_Byte(ControllerInfo, &Buffer[i]) != STATUS_SUCCESS)
{
WARN_(FLOPPY, "HwSenseInterruptStatus: failed to read controller\n");
return STATUS_UNSUCCESSFUL;
}
}
INFO_(FLOPPY, "HwSenseInterruptStatus returned 0x%x 0x%x\n", Buffer[0], Buffer[1]);
return STATUS_SUCCESS;
}
static BOOL
I2CWrite(PVOID HwDeviceExtension, PI2C_CALLBACKS i2c, UCHAR Data)
{
UCHAR Bit;
BOOL Ack;
/* transmit data */
for (Bit = (1 << 7); Bit != 0; Bit >>= 1)
{
WRITE_SCL(LOW);
WRITE_SDA((Data & Bit) ? HIGH : LOW);
DELAY_HALF();
WRITE_SCL(HIGH);
DELAY_HALF();
}
/* get ack */
WRITE_SCL(LOW);
WRITE_SDA(HIGH);
DELAY_HALF();
WRITE_SCL(HIGH);
do
{
DELAY_HALF();
}
while (READ_SCL() != HIGH);
Ack = (READ_SDA() == LOW);
DELAY_HALF();
INFO_(VIDEOPRT, "I2CWrite: %s\n", Ack ? "Ack" : "Nak");
return Ack;
}
VP_STATUS NTAPI
IntInt10AllocateBuffer(
IN PVOID Context,
OUT PUSHORT Seg,
OUT PUSHORT Off,
IN OUT PULONG Length)
{
PVOID MemoryAddress;
NTSTATUS Status;
PKPROCESS CallingProcess = (PKPROCESS)PsGetCurrentProcess();
KAPC_STATE ApcState;
TRACE_(VIDEOPRT, "IntInt10AllocateBuffer\n");
IntAttachToCSRSS(&CallingProcess, &ApcState);
MemoryAddress = (PVOID)0x20000;
Status = ZwAllocateVirtualMemory(NtCurrentProcess(), &MemoryAddress, 0,
Length, MEM_COMMIT, PAGE_EXECUTE_READWRITE);
if (!NT_SUCCESS(Status))
{
WARN_(VIDEOPRT, "- ZwAllocateVirtualMemory failed\n");
IntDetachFromCSRSS(&CallingProcess, &ApcState);
return ERROR_NOT_ENOUGH_MEMORY;
}
if (MemoryAddress > (PVOID)(0x100000 - *Length))
{
ZwFreeVirtualMemory(NtCurrentProcess(), &MemoryAddress, Length,
MEM_RELEASE);
WARN_(VIDEOPRT, "- Unacceptable memory allocated\n");
IntDetachFromCSRSS(&CallingProcess, &ApcState);
return ERROR_NOT_ENOUGH_MEMORY;
}
*Seg = (USHORT)((ULONG)MemoryAddress >> 4);
*Off = (USHORT)((ULONG)MemoryAddress & 0xF);
INFO_(VIDEOPRT, "- Segment: %x\n", (ULONG)MemoryAddress >> 4);
INFO_(VIDEOPRT, "- Offset: %x\n", (ULONG)MemoryAddress & 0xF);
INFO_(VIDEOPRT, "- Length: %x\n", *Length);
IntDetachFromCSRSS(&CallingProcess, &ApcState);
return NO_ERROR;
}
NTSTATUS NTAPI
HwReadIdResult(PCONTROLLER_INFO ControllerInfo,
PUCHAR CurCylinder,
PUCHAR CurHead)
/*
* FUNCTION: Get the result of a read id command
* ARGUMENTS:
* ControllerInfo: controller to query
* CurCylinder: Returns the cylinder that we're at
* CurHead: Returns the head that we're at
* RETURNS:
* STATUS_SUCCESS if the read id was a success
* STATUS_UNSUCCESSFUL otherwise
* NOTES:
* - This function tests the error conditions itself, and boils the
* whole thing down to a single SUCCESS or FAILURE result
* - Called post-interrupt; does not interrupt
* TODO
* - perhaps handle more status
*/
{
UCHAR Buffer[7] = {0,0,0,0,0,0,0};
int i;
PAGED_CODE();
for(i = 0; i < 7; i++)
if(Get_Byte(ControllerInfo, &Buffer[i]) != STATUS_SUCCESS)
{
WARN_(FLOPPY, "ReadIdResult(): can't read from the controller\n");
return STATUS_UNSUCCESSFUL;
}
/* Validate that it did what we told it to */
INFO_(FLOPPY, "ReadId results: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n", Buffer[0], Buffer[1], Buffer[2], Buffer[3],
Buffer[4], Buffer[5], Buffer[6]);
/* Last command successful? */
if((Buffer[0] & SR0_LAST_COMMAND_STATUS) != SR0_LCS_SUCCESS)
{
WARN_(FLOPPY, "ReadId didn't return last command success\n");
return STATUS_UNSUCCESSFUL;
}
/* ID mark found? */
if(Buffer[1] & SR1_CANNOT_FIND_ID_ADDRESS)
{
WARN_(FLOPPY, "ReadId didn't find an address mark\n");
return STATUS_UNSUCCESSFUL;
}
if(CurCylinder)
*CurCylinder = Buffer[3];
if(CurHead)
*CurHead = Buffer[4];
return STATUS_SUCCESS;
}
void IdOnDemandReaderUnitConfiguration::unSerialize(boost::property_tree::ptree& node)
{
INFO_SIMPLE_("Unserializing reader unit configuration...");
d_authCode = node.get_child("AuthenticateCode").get_value<std::string>();
INFO_("Authenticate Code unSerialized {%s}", d_authCode.c_str());
}
/*
* IntVideoPortDispatchOpen
*
* Answer requests for Open calls.
*
* Run Level
* PASSIVE_LEVEL
*/
NTSTATUS
NTAPI
IntVideoPortDispatchOpen(
IN PDEVICE_OBJECT DeviceObject,
IN PIRP Irp)
{
PVIDEO_PORT_DEVICE_EXTENSION DeviceExtension;
PVIDEO_PORT_DRIVER_EXTENSION DriverExtension;
TRACE_(VIDEOPRT, "IntVideoPortDispatchOpen\n");
if (CsrssInitialized == FALSE)
{
/*
* We know the first open call will be from the CSRSS process
* to let us know its handle.
*/
INFO_(VIDEOPRT, "Referencing CSRSS\n");
Csrss = (PKPROCESS)PsGetCurrentProcess();
INFO_(VIDEOPRT, "Csrss %p\n", Csrss);
IntInitializeVideoAddressSpace();
CsrssInitialized = TRUE;
}
DeviceExtension = (PVIDEO_PORT_DEVICE_EXTENSION)DeviceObject->DeviceExtension;
DriverExtension = DeviceExtension->DriverExtension;
if (DriverExtension->InitializationData.HwInitialize(&DeviceExtension->MiniPortDeviceExtension))
{
Irp->IoStatus.Status = STATUS_SUCCESS;
InterlockedIncrement((PLONG)&DeviceExtension->DeviceOpened);
}
else
{
Irp->IoStatus.Status = STATUS_UNSUCCESSFUL;
}
Irp->IoStatus.Information = FILE_OPENED;
IoCompleteRequest(Irp, IO_NO_INCREMENT);
return STATUS_SUCCESS;
}
static VOID NTAPI
i8042KbdDpcRoutine(
IN PKDPC Dpc,
IN PVOID DeferredContext,
IN PVOID SystemArgument1,
IN PVOID SystemArgument2)
{
PI8042_KEYBOARD_EXTENSION DeviceExtension;
PPORT_DEVICE_EXTENSION PortDeviceExtension;
ULONG KeysTransferred = 0;
ULONG KeysInBufferCopy;
KIRQL Irql;
UNREFERENCED_PARAMETER(Dpc);
UNREFERENCED_PARAMETER(SystemArgument1);
UNREFERENCED_PARAMETER(SystemArgument2);
__analysis_assume(DeferredContext != NULL);
DeviceExtension = DeferredContext;
PortDeviceExtension = DeviceExtension->Common.PortDeviceExtension;
if (HandlePowerKeys(DeviceExtension))
{
DeviceExtension->KeyComplete = FALSE;
return;
}
i8042PacketDpc(PortDeviceExtension);
if (!DeviceExtension->KeyComplete)
return;
/* We got the interrupt as it was being enabled, too bad */
if (!PortDeviceExtension->HighestDIRQLInterrupt)
return;
Irql = KeAcquireInterruptSpinLock(PortDeviceExtension->HighestDIRQLInterrupt);
DeviceExtension->KeyComplete = FALSE;
KeysInBufferCopy = DeviceExtension->KeysInBuffer;
KeReleaseInterruptSpinLock(PortDeviceExtension->HighestDIRQLInterrupt, Irql);
TRACE_(I8042PRT, "Send a key\n");
if (!DeviceExtension->KeyboardData.ClassService)
return;
INFO_(I8042PRT, "Sending %lu key(s)\n", KeysInBufferCopy);
(*(PSERVICE_CALLBACK_ROUTINE)DeviceExtension->KeyboardData.ClassService)(
DeviceExtension->KeyboardData.ClassDeviceObject,
DeviceExtension->KeyboardBuffer,
DeviceExtension->KeyboardBuffer + KeysInBufferCopy,
&KeysTransferred);
KeAcquireInterruptSpinLock(PortDeviceExtension->HighestDIRQLInterrupt);
DeviceExtension->KeysInBuffer -= KeysTransferred;
KeReleaseInterruptSpinLock(PortDeviceExtension->HighestDIRQLInterrupt, Irql);
}
void TcpDataTransport::disconnect()
{
if (d_socket)
{
INFO_("Disconnected.");
d_socket->close();
d_socket.reset();
}
}
void STidSTRLEDBuzzerDisplay::setGreenLed(bool status, bool deferred)
{
INFO_("Set Green LED {%d} deferred{%d}", status, deferred);
d_green_led = status;
if (!deferred)
{
setPort();
}
}
void STidSTRLEDBuzzerDisplay::setBuzzer(bool status, bool deferred)
{
INFO_("Set Buzzer {%d} deferred{%d}", status, deferred);
d_buzzer = status;
if (!deferred)
{
setPort();
}
}
void STidSTRLEDBuzzerDisplay::setPort()
{
INFO_("Sending LED/Buzzer command for: red{%d} green{%d} buzzer{%d}...", d_red_led, d_green_led, d_buzzer);
std::vector<unsigned char> data;
data.push_back(static_cast<unsigned char>((d_red_led ? 2 : 0) | (d_green_led ? 1 : 0)));
data.push_back(0xFF); // LED duration
data.push_back((d_buzzer) ? 0xFF : 0x00); // Buzzer duration
getSTidSTRReaderCardAdapter()->sendCommand(0x0007, data);
}
static NTSTATUS NTAPI
RWComputeCHS(PDRIVE_INFO IN DriveInfo,
ULONG IN DiskByteOffset,
PUCHAR OUT Cylinder,
PUCHAR OUT Head,
PUCHAR OUT Sector)
/*
* FUNCTION: Compute the CHS from the absolute byte offset on disk
* ARGUMENTS:
* DriveInfo: Drive to compute on
* DiskByteOffset: Absolute offset on disk of the starting byte
* Cylinder: Cylinder that the byte is on
* Head: Head that the byte is on
* Sector: Sector that the byte is on
* RETURNS:
* STATUS_SUCCESS if CHS are determined correctly
* STATUS_UNSUCCESSFUL otherwise
* NOTES:
* - Lots of ugly typecasts here
* - Sectors are 1-based!
* - This is really crummy code. Please FIXME.
*/
{
ULONG AbsoluteSector;
UCHAR SectorsPerCylinder = (UCHAR)DriveInfo->DiskGeometry.SectorsPerTrack * (UCHAR)DriveInfo->DiskGeometry.TracksPerCylinder;
TRACE_(FLOPPY, "RWComputeCHS: Called with offset 0x%x\n", DiskByteOffset);
/* First calculate the 1-based "absolute sector" based on the byte offset */
ASSERT(!(DiskByteOffset % DriveInfo->DiskGeometry.BytesPerSector)); /* FIXME: Only handle full sector transfers atm */
/* AbsoluteSector is zero-based to make the math a little easier */
AbsoluteSector = DiskByteOffset / DriveInfo->DiskGeometry.BytesPerSector; /* Num full sectors */
/* Cylinder number is floor(AbsoluteSector / SectorsPerCylinder) */
*Cylinder = (CHAR)(AbsoluteSector / SectorsPerCylinder);
/* Head number is 0 if the sector within the cylinder < SectorsPerTrack; 1 otherwise */
*Head = AbsoluteSector % SectorsPerCylinder < DriveInfo->DiskGeometry.SectorsPerTrack ? 0 : 1;
/*
* Sector number is the sector within the cylinder if on head 0; that minus SectorsPerTrack if it's on head 1
* (lots of casts to placate msvc). 1-based!
*/
*Sector = ((UCHAR)(AbsoluteSector % SectorsPerCylinder) + 1) - ((*Head) * (UCHAR)DriveInfo->DiskGeometry.SectorsPerTrack);
INFO_(FLOPPY, "RWComputeCHS: offset 0x%x is c:0x%x h:0x%x s:0x%x\n", DiskByteOffset, *Cylinder, *Head, *Sector);
/* Sanity checking */
ASSERT(*Cylinder <= DriveInfo->DiskGeometry.Cylinders.QuadPart);
ASSERT(*Head <= DriveInfo->DiskGeometry.TracksPerCylinder);
ASSERT(*Sector <= DriveInfo->DiskGeometry.SectorsPerTrack);
return STATUS_SUCCESS;
}
void MifareSTidSTRCommands::authenticate(unsigned char /*blockno*/, boost::shared_ptr<KeyStorage> key_storage, MifareKeyType keytype)
{
INFO_("Setting the authenticate type... key storage {%s} key type {0x%x(%d)}", key_storage->serialize().c_str(), keytype, keytype);
if (boost::dynamic_pointer_cast<ComputerMemoryKeyStorage>(key_storage))
{
INFO_SIMPLE_("Setting computer memory key storage !");
d_useSKB = false;
d_skbIndex = 0;
}
else if (boost::dynamic_pointer_cast<ReaderMemoryKeyStorage>(key_storage))
{
d_useSKB = true;
boost::shared_ptr<ReaderMemoryKeyStorage> rmks = boost::dynamic_pointer_cast<ReaderMemoryKeyStorage>(key_storage);
d_skbIndex = rmks->getKeySlot();
INFO_("Setting reader memory key storage ! SKB index {%d}", d_skbIndex);
}
else
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "The key storage type is not supported for this card/reader.");
}
d_lastKeyType = keytype;
}
size_t MifareSTidSTRCommands::updateBinaryIndex(unsigned char keyindex, unsigned char blockno, const void* buf, size_t buflen)
{
INFO_("Update binary index key index {0x%x(%u)} block {0x%x(%u)} [in] buffer len {%d}", keyindex, keyindex, blockno, blockno, buflen);
INFO_SIMPLE_(" => Rescanning card to avoid bad authentication");
scanMifare();
INFO_SIMPLE_("Scan done ! Continue to update binary index.");
if (blockno != 0)
{
std::vector<unsigned char> command;
command.push_back(static_cast<unsigned char>(d_lastKeyType));
command.push_back(keyindex);
command.push_back(blockno);
command.insert(command.end(), (unsigned char*)buf, (unsigned char*)buf + buflen);
getSTidSTRReaderCardAdapter()->sendCommand(0x00D3, command);
}
INFO_("Returns final [out] buffer len {%d}", buflen);
return buflen;
}
static NTSTATUS
i8042BasicDetect(
IN PPORT_DEVICE_EXTENSION DeviceExtension)
{
NTSTATUS Status;
ULONG ResendIterations;
UCHAR Value = 0;
/* Don't enable keyboard and mouse interrupts, disable keyboard/mouse */
i8042Flush(DeviceExtension);
if (!i8042ChangeMode(DeviceExtension, CCB_KBD_INT_ENAB | CCB_MOUSE_INT_ENAB, CCB_KBD_DISAB | CCB_MOUSE_DISAB))
return STATUS_IO_DEVICE_ERROR;
i8042Flush(DeviceExtension);
/* Issue a CTRL_SELF_TEST command to check if this is really an i8042 controller */
ResendIterations = DeviceExtension->Settings.ResendIterations + 1;
while (ResendIterations--)
{
if (!i8042Write(DeviceExtension, DeviceExtension->ControlPort, CTRL_SELF_TEST))
{
WARN_(I8042PRT, "Writing CTRL_SELF_TEST command failed\n");
return STATUS_IO_TIMEOUT;
}
Status = i8042ReadDataWait(DeviceExtension, &Value);
if (!NT_SUCCESS(Status))
{
WARN_(I8042PRT, "Failed to read CTRL_SELF_TEST response, status 0x%08lx\n", Status);
return Status;
}
if (Value == KBD_SELF_TEST_OK)
{
INFO_(I8042PRT, "CTRL_SELF_TEST completed successfully!\n");
break;
}
else if (Value == KBD_RESEND)
{
TRACE_(I8042PRT, "Resending...\n", Value);
KeStallExecutionProcessor(50);
}
else
{
WARN_(I8042PRT, "Got 0x%02x instead of 0x55\n", Value);
return STATUS_IO_DEVICE_ERROR;
}
}
return STATUS_SUCCESS;
}
/*
* These functions are callbacks for filter driver custom
* initialization routines.
*/
NTSTATUS NTAPI
i8042SynchWritePort(
IN PPORT_DEVICE_EXTENSION DeviceExtension,
IN UCHAR Port,
IN UCHAR Value,
IN BOOLEAN WaitForAck)
{
NTSTATUS Status;
UCHAR Ack;
ULONG ResendIterations;
ResendIterations = DeviceExtension->Settings.ResendIterations + 1;
do
{
if (Port)
if (!i8042Write(DeviceExtension, DeviceExtension->DataPort, Port))
{
WARN_(I8042PRT, "Failed to write Port\n");
return STATUS_IO_TIMEOUT;
}
if (!i8042Write(DeviceExtension, DeviceExtension->DataPort, Value))
{
WARN_(I8042PRT, "Failed to write Value\n");
return STATUS_IO_TIMEOUT;
}
if (WaitForAck)
{
Status = i8042ReadDataWait(DeviceExtension, &Ack);
if (!NT_SUCCESS(Status))
{
WARN_(I8042PRT, "Failed to read Ack\n");
return Status;
}
if (Ack == KBD_ACK)
return STATUS_SUCCESS;
else if (Ack == KBD_RESEND)
INFO_(I8042PRT, "i8042 asks for a data resend\n");
}
else
{
return STATUS_SUCCESS;
}
TRACE_(I8042PRT, "Reiterating\n");
ResendIterations--;
} while (ResendIterations);
return STATUS_IO_TIMEOUT;
}
VP_STATUS NTAPI
IntInt10WriteMemory(
IN PVOID Context,
IN USHORT Seg,
IN USHORT Off,
IN PVOID Buffer,
IN ULONG Length)
{
PKPROCESS CallingProcess = (PKPROCESS)PsGetCurrentProcess();
KAPC_STATE ApcState;
TRACE_(VIDEOPRT, "IntInt10WriteMemory\n");
INFO_(VIDEOPRT, "- Segment: %x\n", Seg);
INFO_(VIDEOPRT, "- Offset: %x\n", Off);
INFO_(VIDEOPRT, "- Buffer: %x\n", Buffer);
INFO_(VIDEOPRT, "- Length: %x\n", Length);
IntAttachToCSRSS(&CallingProcess, &ApcState);
RtlCopyMemory((PVOID)((Seg << 4) | Off), Buffer, Length);
IntDetachFromCSRSS(&CallingProcess, &ApcState);
return NO_ERROR;
}