NTSTATUS FilterDeleteSetting(_In_ PMS_FILTER pFilter, uint16_t aKey, int aIndex)
{
HANDLE regKey = NULL;
OBJECT_ATTRIBUTES attributes;
DECLARE_UNICODE_STRING_SIZE(Name, 20);
// Convert 'aKey' to a string
RtlIntegerToUnicodeString((ULONG)aKey, 16, &Name);
InitializeObjectAttributes(
&attributes,
&Name,
OBJ_CASE_INSENSITIVE | OBJ_KERNEL_HANDLE,
pFilter->otSettingsRegKey,
NULL);
// Open the registry key
NTSTATUS status =
ZwOpenKey(
®Key,
KEY_ALL_ACCESS,
&attributes);
if (!NT_SUCCESS(status))
{
// Key doesn't exist
goto error;
}
// If 'aIndex' is -1 then delete the whole key, otherwise delete the individual value
if (aIndex == -1)
{
// Delete the registry key
status = ZwDeleteKey(regKey);
}
else
{
UCHAR KeyInfoBuffer[128] = { 0 };
PKEY_FULL_INFORMATION pKeyInfo = (PKEY_FULL_INFORMATION)KeyInfoBuffer;
ULONG KeyInfoLength = sizeof(KeyInfoBuffer);
// When deleting an individual value, since order doesn't matter, we will actually
// copy the last value over the one being deleted and then delete the last value; so
// we maintain a contiguous list of numbered values
// Query the number of values
// Note: Can't use helper function because we already have the key open
status =
ZwQueryKey(
regKey,
KeyValueFullInformation,
pKeyInfo,
KeyInfoLength,
&KeyInfoLength);
if (!NT_SUCCESS(status))
{
LogError(DRIVER_DEFAULT, "ZwQueryKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto error;
}
if ((ULONG)aIndex >= pKeyInfo->Values)
{
// Attempt to delete beyond the end of the list
status = STATUS_OBJECT_NAME_NOT_FOUND;
goto error;
}
else if (pKeyInfo->Values == 1)
{
// Deleting the only value on the key, go ahead and delete the entire key
status = ZwDeleteKey(regKey);
}
else if (pKeyInfo->Values - 1 != (ULONG)aIndex)
{
// We aren't deleting the last value so we need to copy the last value
// over this one, and then delete the last one.
PKEY_VALUE_PARTIAL_INFORMATION pValueInfo = NULL;
ULONG ValueInfoLength = 0;
// Convert pKeyInfo->Values-1 to a string
RtlIntegerToUnicodeString(pKeyInfo->Values - 1, 16, &Name);
// Query the key data buffer size
status = ZwQueryValueKey(
regKey,
&Name,
KeyValuePartialInformation,
pValueInfo,
0,
&ValueInfoLength);
NT_ASSERT(status != STATUS_SUCCESS);
if (status != STATUS_BUFFER_TOO_SMALL)
{
LogVerbose(DRIVER_DEFAULT, "ZwQueryValueKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto error;
}
pValueInfo = FILTER_ALLOC_MEM(pFilter->FilterHandle, ValueInfoLength);
if (pValueInfo == NULL)
{
status = STATUS_INSUFFICIENT_RESOURCES;
goto error;
}
// Query the data buffer
status = ZwQueryValueKey(
regKey,
&Name,
KeyValuePartialInformation,
pValueInfo,
ValueInfoLength,
&ValueInfoLength);
if (!NT_SUCCESS(status))
{
LogError(DRIVER_DEFAULT, "ZwQueryValueKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto cleanup;
}
// Delete the registry value
status =
ZwDeleteValueKey(
regKey,
&Name);
if (!NT_SUCCESS(status))
{
LogError(DRIVER_DEFAULT, "ZwDeleteValueKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto cleanup;
}
// Convert 'aIndex' to a string
RtlIntegerToUnicodeString((ULONG)aIndex, 16, &Name);
// Write the data to the registry key we are deleting
status =
ZwSetValueKey(
regKey,
&Name,
0,
REG_BINARY,
(PVOID)pValueInfo->Data,
pValueInfo->DataLength);
if (!NT_SUCCESS(status))
{
LogError(DRIVER_DEFAULT, "ZwSetValueKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto cleanup;
}
cleanup:
if (pValueInfo) FILTER_FREE_MEM(pValueInfo);
}
else
{
// Deleting the last value in the list (but not the only value)
// Just delete the value directly. No need to copy any others.
// Convert 'aIndex' to a string
RtlIntegerToUnicodeString((ULONG)aIndex, 16, &Name);
// Delete the registry value
status =
ZwDeleteValueKey(
regKey,
&Name);
}
}
error:
if (regKey) ZwClose(regKey);
return status;
}
_Use_decl_annotations_
VOID
FilterOidRequestComplete(
NDIS_HANDLE FilterModuleContext,
PNDIS_OID_REQUEST Request,
NDIS_STATUS Status
)
/*++
Routine Description:
Notification that an OID request has been completed
If this filter sends a request down to a lower layer, and the request is
pended, the FilterOidRequestComplete routine is invoked when the request
is complete. Most requests we've sent are simply clones of requests
received from a higher layer; all we need to do is complete the original
higher request.
However, if this filter driver sends original requests down, it must not
attempt to complete a pending request to the higher layer.
Arguments:
FilterModuleContext - our filter context area
NdisRequest - the completed request
Status - completion status
--*/
{
PMS_FILTER pFilter = (PMS_FILTER)FilterModuleContext;
PNDIS_OID_REQUEST OriginalRequest;
POTLWF_REQUEST_CONTEXT Context;
BOOLEAN bFalse = FALSE;
LogFuncEntryMsg(DRIVER_OID, "Filter: %p, Request %p", FilterModuleContext, Request);
Context = (POTLWF_REQUEST_CONTEXT)(&Request->SourceReserved[0]);
OriginalRequest = (*Context);
//
// This is an internal request
//
if (OriginalRequest == NULL)
{
otLwfInternalRequestComplete(pFilter, Request, Status);
LogFuncExit(DRIVER_OID);
return;
}
FILTER_ACQUIRE_LOCK(&pFilter->PendingOidRequestLock, bFalse);
ASSERT(pFilter->PendingOidRequest == Request);
pFilter->PendingOidRequest = NULL;
FILTER_RELEASE_LOCK(&pFilter->PendingOidRequestLock, bFalse);
//
// Copy the information from the returned request to the original request
//
switch(Request->RequestType)
{
case NdisRequestMethod:
OriginalRequest->DATA.METHOD_INFORMATION.OutputBufferLength = Request->DATA.METHOD_INFORMATION.OutputBufferLength;
OriginalRequest->DATA.METHOD_INFORMATION.BytesRead = Request->DATA.METHOD_INFORMATION.BytesRead;
OriginalRequest->DATA.METHOD_INFORMATION.BytesNeeded = Request->DATA.METHOD_INFORMATION.BytesNeeded;
OriginalRequest->DATA.METHOD_INFORMATION.BytesWritten = Request->DATA.METHOD_INFORMATION.BytesWritten;
break;
case NdisRequestSetInformation:
OriginalRequest->DATA.SET_INFORMATION.BytesRead = Request->DATA.SET_INFORMATION.BytesRead;
OriginalRequest->DATA.SET_INFORMATION.BytesNeeded = Request->DATA.SET_INFORMATION.BytesNeeded;
break;
case NdisRequestQueryInformation:
case NdisRequestQueryStatistics:
default:
OriginalRequest->DATA.QUERY_INFORMATION.BytesWritten = Request->DATA.QUERY_INFORMATION.BytesWritten;
OriginalRequest->DATA.QUERY_INFORMATION.BytesNeeded = Request->DATA.QUERY_INFORMATION.BytesNeeded;
break;
}
(*Context) = NULL;
LogVerbose(DRIVER_OID, "Freeing (cloned) Oid Request %p", Request);
NdisFreeCloneOidRequest(pFilter->FilterHandle, Request);
LogVerbose(DRIVER_OID, "Completing (external) Oid Request %p", OriginalRequest);
NdisFOidRequestComplete(pFilter->FilterHandle, OriginalRequest, Status);
LogFuncExit(DRIVER_OID);
}
static FORCEINLINE NTSTATUS
__InitHypercallPage()
{
ULONG eax = 'DEAD';
ULONG ebx = 'DEAD';
ULONG ecx = 'DEAD';
ULONG edx = 'DEAD';
ULONG Index;
ULONG HypercallMsr;
NTSTATUS Status;
Status = STATUS_UNSUCCESSFUL;
for (;;) {
CHAR Signature[13] = { 0 };
CpuId(__BaseLeaf, &eax, &ebx, &ecx, &edx);
*((PULONG)(Signature + 0)) = ebx;
*((PULONG)(Signature + 4)) = ecx;
*((PULONG)(Signature + 8)) = edx;
if (strcmp(Signature, XEN_SIGNATURE) == 0 &&
eax >= __BaseLeaf + 2)
break;
__BaseLeaf += 0x100;
if (__BaseLeaf > 0x40000100)
goto fail1;
}
CpuId(__BaseLeaf + 1, &eax, NULL, NULL, NULL);
__MajorVersion = (USHORT)(eax >> 16);
__MinorVersion = (USHORT)(eax & 0xFFFF);
LogVerbose("XEN %d.%d\n", __MajorVersion, __MinorVersion);
LogVerbose("INTERFACE 0x%08x\n", __XEN_INTERFACE_VERSION__);
if ((ULONG_PTR)__HypercallSection & (PAGE_SIZE - 1))
Hypercall = (PVOID)(((ULONG_PTR)__HypercallSection + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1));
else
Hypercall = (PVOID)__HypercallSection;
ASSERT3U(((ULONG_PTR)Hypercall & (PAGE_SIZE - 1)), ==, 0);
for (Index = 0; Index < MAXIMUM_HYPERCALL_PFN_COUNT; Index++) {
PHYSICAL_ADDRESS PhysicalAddress;
PhysicalAddress = MmGetPhysicalAddress((PUCHAR)Hypercall + (Index << PAGE_SHIFT));
__Pfn[Index] = (PFN_NUMBER)(PhysicalAddress.QuadPart >> PAGE_SHIFT);
}
CpuId(__BaseLeaf + 2, &eax, &ebx, NULL, NULL);
__PfnCount = eax;
ASSERT(__PfnCount <= MAXIMUM_HYPERCALL_PFN_COUNT);
HypercallMsr = ebx;
for (Index = 0; Index < __PfnCount; Index++) {
LogVerbose("HypercallPfn[%d]: %p\n", Index, (PVOID)__Pfn[Index]);
__writemsr(HypercallMsr, (ULONG64)__Pfn[Index] << PAGE_SHIFT);
}
return STATUS_SUCCESS;
fail1:
LogError("fail1 (%08x)", Status);
return Status;
}
NTSTATUS FilterReadSetting(_In_ PMS_FILTER pFilter, uint16_t aKey, int aIndex, uint8_t *aValue, uint16_t *aValueLength)
{
HANDLE regKey = NULL;
OBJECT_ATTRIBUTES attributes;
DECLARE_UNICODE_STRING_SIZE(Name, 20);
PKEY_VALUE_PARTIAL_INFORMATION pInfo = NULL;
ULONG InfoLength = sizeof(*pInfo) + *aValueLength;
// Convert 'aKey' to a string
RtlIntegerToUnicodeString((ULONG)aKey, 16, &Name);
InitializeObjectAttributes(
&attributes,
&Name,
OBJ_CASE_INSENSITIVE | OBJ_KERNEL_HANDLE,
pFilter->otSettingsRegKey,
NULL);
// Open the registry key
NTSTATUS status =
ZwOpenKey(
®Key,
KEY_ALL_ACCESS,
&attributes);
if (!NT_SUCCESS(status))
{
// Key doesn't exist
goto error;
}
// Convert 'aIndex' to a string
RtlIntegerToUnicodeString((ULONG)aIndex, 16, &Name);
// Allocate buffer for query
pInfo = FILTER_ALLOC_MEM(pFilter->FilterHandle, InfoLength);
if (pInfo == NULL)
{
status = STATUS_INSUFFICIENT_RESOURCES;
goto error;
}
// Query the data
status = ZwQueryValueKey(
regKey,
&Name,
KeyValuePartialInformation,
pInfo,
InfoLength,
&InfoLength);
if (!NT_SUCCESS(status))
{
LogVerbose(DRIVER_DEFAULT, "ZwQueryValueKey for %S value failed, %!STATUS!", Name.Buffer, status);
goto error;
}
NT_ASSERT(*aValueLength >= pInfo->DataLength);
*aValueLength = (uint16_t)pInfo->DataLength;
if (aValue)
{
memcpy(aValue, pInfo->Data, pInfo->DataLength);
}
error:
if (pInfo) FILTER_FREE_MEM(pInfo);
if (regKey) ZwClose(regKey);
return status;
}
bool SerialPort::Open( const char *inDevName, const char *param )
{
char devName[ 40 ];
unsigned baudRate;
devName[ 0 ] = '\0';
#if 1
if ( inDevName[ 0 ] != '/' )
{
StrMaxCpy( devName, "/dev/", sizeof( devName ));
}
#endif
StrMaxCat( devName, inDevName, sizeof( devName ));
// Translate the params, if any
speed_t speed = B0;
if ( param == NULL )
{
speed = B38400;
baudRate = 38400;
}
else
{
baudRate = atoi( param );
for ( unsigned i = 0; i < ARRAY_LEN( gBaudTable ); i++ )
{
if ( gBaudTable[ i ].baudRate == baudRate )
{
speed = gBaudTable[ i ].speed;
break;
}
}
if ( speed == B0 )
{
LogError( "Unrecognized baud rate: '%s'\n", param );
return false;
}
}
LogVerbose( "Port: '%s' Baud: %d\n", devName, baudRate );
if (( m_fd = open( devName, O_RDWR | O_EXCL )) < 0 )
{
LogError( "Unable to open serial port '%s': %s\n", devName, strerror( errno ));
return false;
}
// Setup the serial port
struct termios attr;
if ( tcgetattr( m_fd, &attr ) < 0 )
{
LogError( "A: Call to tcgetattr failed: %s\n", strerror( errno ));
return false;
}
attr.c_iflag = 0;
attr.c_oflag = 0;
attr.c_cflag = CLOCAL | CREAD | CS8;
attr.c_lflag = 0;
attr.c_cc[ VTIME ] = 0; // timeout in tenths of a second
attr.c_cc[ VMIN ] = 1;
cfsetispeed( &attr, speed );
cfsetospeed( &attr, speed );
if ( tcsetattr( m_fd, TCSAFLUSH, &attr ) < 0 )
{
LogError( "Call to tcsetattr failed: %s\n", strerror( errno ));
return false;
}
return true;
} // Open
_Use_decl_annotations_
NDIS_STATUS
FilterAttach(
NDIS_HANDLE NdisFilterHandle,
NDIS_HANDLE FilterDriverContext,
PNDIS_FILTER_ATTACH_PARAMETERS AttachParameters
)
/*++
Routine Description:
Filter attach routine.
Create filter's context, allocate NetBufferLists and NetBuffer pools and any
other resources, and read configuration if needed.
Arguments:
NdisFilterHandle - Specify a handle identifying this instance of the filter. FilterAttach
should save this handle. It is a required parameter in subsequent calls
to NdisFxxx functions.
FilterDriverContext - Filter driver context passed to NdisFRegisterFilterDriver.
AttachParameters - attach parameters
Return Value:
NDIS_STATUS_SUCCESS: FilterAttach successfully allocated and initialize data structures
for this filter instance.
NDIS_STATUS_RESOURCES: FilterAttach failed due to insufficient resources.
NDIS_STATUS_FAILURE: FilterAttach could not set up this instance of this filter and it has called
NdisWriteErrorLogEntry with parameters specifying the reason for failure.
N.B.: FILTER can use NdisRegisterDeviceEx to create a device, so the upper
layer can send Irps to the filter.
--*/
{
PMS_FILTER pFilter = NULL;
NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
NTSTATUS NtStatus;
NDIS_FILTER_ATTRIBUTES FilterAttributes;
ULONG Size;
COMPARTMENT_ID OriginalCompartmentID;
OBJECT_ATTRIBUTES ObjectAttributes = {0};
const ULONG RegKeyOffset = ARRAYSIZE(L"\\REGISTRY\\MACHINE\\SYSTEM\\CurrentControlSet\\Services\\otlwf\\Parameters\\NdisAdapters\\") - 1;
DECLARE_CONST_UNICODE_STRING(RegKeyPath, L"\\REGISTRY\\MACHINE\\SYSTEM\\CurrentControlSet\\Services\\otlwf\\Parameters\\NdisAdapters\\{00000000-0000-0000-0000-000000000000}");
RtlCopyMemory(RegKeyPath.Buffer + RegKeyOffset, AttachParameters->BaseMiniportName->Buffer + 8, sizeof(L"{00000000-0000-0000-0000-000000000000}"));
LogFuncEntry(DRIVER_DEFAULT);
do
{
ASSERT(FilterDriverContext == (NDIS_HANDLE)FilterDriverObject);
if (FilterDriverContext != (NDIS_HANDLE)FilterDriverObject)
{
Status = NDIS_STATUS_INVALID_PARAMETER;
break;
}
// Verify the media type is supported. This is a last resort; the
// the filter should never have been bound to an unsupported miniport
// to begin with.
if (AttachParameters->MiniportMediaType != NdisMediumIP)
{
LogError(DRIVER_DEFAULT, "Unsupported media type, 0x%x.", (ULONG)AttachParameters->MiniportMediaType);
Status = NDIS_STATUS_INVALID_PARAMETER;
break;
}
Size = sizeof(MS_FILTER) + AttachParameters->BaseMiniportInstanceName->Length;
pFilter = (PMS_FILTER)FILTER_ALLOC_MEM(NdisFilterHandle, Size);
if (pFilter == NULL)
{
LogWarning(DRIVER_DEFAULT, "Failed to allocate context structure, 0x%x bytes", Size);
Status = NDIS_STATUS_RESOURCES;
break;
}
NdisZeroMemory(pFilter, sizeof(MS_FILTER));
LogVerbose(DRIVER_DEFAULT, "Opening interface registry key %S", RegKeyPath.Buffer);
InitializeObjectAttributes(
&ObjectAttributes,
(PUNICODE_STRING)&RegKeyPath,
OBJ_CASE_INSENSITIVE | OBJ_KERNEL_HANDLE,
NULL,
NULL);
// Open the registry key
NtStatus = ZwOpenKey(&pFilter->InterfaceRegKey, KEY_ALL_ACCESS, &ObjectAttributes);
if (!NT_SUCCESS(NtStatus))
{
LogError(DRIVER_DEFAULT, "ZwOpenKey failed to open %S, %!STATUS!", RegKeyPath.Buffer, NtStatus);
Status = NDIS_STATUS_FAILURE;
break;
}
// Format of "\DEVICE\{5BA90C49-0D7E-455B-8D3B-614F6714A212}"
AttachParameters->BaseMiniportName->Buffer += 8;
AttachParameters->BaseMiniportName->Length -= 8 * sizeof(WCHAR);
NtStatus = RtlGUIDFromString(AttachParameters->BaseMiniportName, &pFilter->InterfaceGuid);
AttachParameters->BaseMiniportName->Buffer -= 8;
AttachParameters->BaseMiniportName->Length += 8 * sizeof(WCHAR);
if (!NT_SUCCESS(NtStatus))
{
LogError(DRIVER_DEFAULT, "Failed to convert FilterModuleGuidName to a GUID, %!STATUS!", NtStatus);
Status = NDIS_STATUS_FAILURE;
break;
}
pFilter->InterfaceFriendlyName.Length = pFilter->InterfaceFriendlyName.MaximumLength = AttachParameters->BaseMiniportInstanceName->Length;
pFilter->InterfaceFriendlyName.Buffer = (PWSTR)((PUCHAR)pFilter + sizeof(MS_FILTER));
NdisMoveMemory(pFilter->InterfaceFriendlyName.Buffer,
AttachParameters->BaseMiniportInstanceName->Buffer,
pFilter->InterfaceFriendlyName.Length);
pFilter->InterfaceIndex = AttachParameters->BaseMiniportIfIndex;
pFilter->InterfaceLuid = AttachParameters->BaseMiniportNetLuid;
pFilter->InterfaceCompartmentID = UNSPECIFIED_COMPARTMENT_ID;
pFilter->FilterHandle = NdisFilterHandle;
NdisZeroMemory(&FilterAttributes, sizeof(NDIS_FILTER_ATTRIBUTES));
FilterAttributes.Header.Revision = NDIS_FILTER_ATTRIBUTES_REVISION_1;
FilterAttributes.Header.Size = sizeof(NDIS_FILTER_ATTRIBUTES);
FilterAttributes.Header.Type = NDIS_OBJECT_TYPE_FILTER_ATTRIBUTES;
FilterAttributes.Flags = 0;
NDIS_DECLARE_FILTER_MODULE_CONTEXT(MS_FILTER);
Status = NdisFSetAttributes(NdisFilterHandle, pFilter, &FilterAttributes);
if (Status != NDIS_STATUS_SUCCESS)
{
LogError(DRIVER_DEFAULT, "Failed to set attributes, %!NDIS_STATUS!", Status);
break;
}
// Filter initially in Paused state
pFilter->State = FilterPaused;
// Initialize rundowns to disabled with no active references
pFilter->ExternalRefs.Count = EX_RUNDOWN_ACTIVE;
pFilter->cmdRundown.Count = EX_RUNDOWN_ACTIVE;
// Query the compartment ID for this interface to use for the IP stack
pFilter->InterfaceCompartmentID = GetInterfaceCompartmentID(&pFilter->InterfaceLuid);
LogVerbose(DRIVER_DEFAULT, "Interface %!GUID! is in Compartment %u", &pFilter->InterfaceGuid, (ULONG)pFilter->InterfaceCompartmentID);
// Make sure we are in the right compartment
(VOID)otLwfSetCompartment(pFilter, &OriginalCompartmentID);
// Register for address changed notifications
NtStatus =
NotifyUnicastIpAddressChange(
AF_INET6,
otLwfAddressChangeCallback,
pFilter,
FALSE,
&pFilter->AddressChangeHandle
);
// Revert the compartment, now that we have the table
otLwfRevertCompartment(OriginalCompartmentID);
if (!NT_SUCCESS(NtStatus))
{
LogError(DRIVER_DEFAULT, "NotifyUnicastIpAddressChange failed, %!STATUS!", NtStatus);
Status = NDIS_STATUS_FAILURE;
break;
}
// Add Filter to global list of Thread Filters
NdisAcquireSpinLock(&FilterListLock);
InsertTailList(&FilterModuleList, &pFilter->FilterModuleLink);
NdisReleaseSpinLock(&FilterListLock);
LogVerbose(DRIVER_DEFAULT, "Created Filter: %p", pFilter);
} while (FALSE);
// Clean up on failure
if (Status != NDIS_STATUS_SUCCESS)
{
if (pFilter != NULL)
{
if (pFilter->AddressChangeHandle != NULL)
{
CancelMibChangeNotify2(pFilter->AddressChangeHandle);
pFilter->AddressChangeHandle = NULL;
}
NdisFreeMemory(pFilter, 0, 0);
}
}
LogFuncExitNDIS(DRIVER_DEFAULT, Status);
return Status;
}
bool BioloidCommandLine::ProcessLine( char *lineStr )
{
char *devTypeStr;
BLD_DevType_t *devType;
char token[ 20 ];
StrTokenizer line( lineStr, token, sizeof( token ));
unsigned devTypeIdx;
if ( m_bus == NULL )
{
LogError( "SetBus not called\n" );
return false;
}
// Pull out the device type
if (( devTypeStr = line.NextToken( gDelim )) == NULL )
{
// Empty line - ignore
return true;
}
// Check for special non-device type commands
if ( strcmp( devTypeStr, "action" ) == 0 )
{
m_bus->SendAction();
// The action command is sent as a broadcast, so no response
// is expected.
return true;
}
if ( strcmp( devTypeStr, "scan" ) == 0 )
{
uint8_t numIds;
if ( !line.NextNum( &numIds ))
{
numIds = 32;
}
if ( numIds < 100 )
{
bool servoIdFound = m_bus->Scan( DevFound, 0, numIds );
bool sensorIdFound = m_bus->Scan( DevFound, 100, numIds );
if ( !servoIdFound && !sensorIdFound )
{
Log( "No devices found\n" );
}
}
else
{
if ( !m_bus->Scan( DevFound, 0 , numIds ))
{
Log( "No devices found\n" );
}
}
return true;
}
if ( strcmp( devTypeStr, "dev-types" ) == 0 )
{
for ( devTypeIdx = 0; devTypeIdx < m_numDevTypes; devTypeIdx++ )
{
devType = m_devType[devTypeIdx];
Log( "%-10s Model: %5u %2d registers\n",
devType->devTypeStr, devType->model, devType->numRegs );
}
return true;
}
if ( strcmp( devTypeStr, "quit" ) == 0 )
{
return false;
}
// Since it's not one of those - assume it's a device type
devType = NULL;
for ( devTypeIdx = 0; devTypeIdx < m_numDevTypes; devTypeIdx++ )
{
if ( strcmp( m_devType[ devTypeIdx ]->devTypeStr, devTypeStr ) == 0 )
{
devType = m_devType[ devTypeIdx ];
break;
}
}
if ( devTypeIdx >= m_numDevTypes )
{
LogError( "Unrecognized device type: '%s'\n", devTypeStr );
return true;
}
Bioloid::ID_t id;
if ( !line.NextNum( &id ))
{
if ( strcmp( line.PrevToken(), "reg" ) == 0 )
{
DumpRegInfo( devType );
return true;
}
LogError( "Invalid ID: '%s'\n", line.PrevToken() );
return true;
}
if ( id >= Bioloid::INVALID_ID )
{
LogError( "IDs must be 254 (0xFE) or less\n" );
return true;
}
m_device.SetBusAndID( m_bus, id );
char *cmdStr;
if (( cmdStr = line.NextToken()) == NULL )
{
LogError( "No command specified for %s %u\n", devType->devTypeStr, id );
return true;
}
LogVerbose( "DevType: %s ID: %d Cmd: %s\n", devType->devTypeStr, id, cmdStr );
if ( strcmp( cmdStr, "ping" ) == 0 )
{
Bioloid::Error err;
if ( id == Bioloid::BROADCAST_ID )
{
LogError( "Broadcast ID not valid with ping command\n" );
return true;
}
if (( err = m_device.Ping()) == Bioloid::ERROR_NONE )
{
Log( "%s %d Response Received\n", devType->devTypeStr, id );
}
else
{
Log( "%s %d ", devType->devTypeStr, id );
PrintError( err );
}
}
else
if (( strcmp( cmdStr, "read-data" ) == 0 )
|| ( strcmp( cmdStr, "rd" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
if ( id == Bioloid::BROADCAST_ID )
{
LogError( "Broadcast ID not valid with read-data command\n" );
return true;
}
if ( !line.NextNum( &offset ))
{
LogError( "Invalid offset specified: '%s'\n", line.PrevToken() );
return true;
}
if ( !line.NextNum( &numBytes ))
{
LogError( "Invalid numBytes specified: '%s'\n", line.PrevToken() );
return true;
}
if ( numBytes > sizeof( gReadBuf ))
{
LogError( "Only able to a maximum of %d bytes\n", sizeof( gReadBuf ));
return true;
}
if ( PrintError( m_device.Read( offset, gReadBuf, numBytes )))
{
return true;
}
DumpMem( "Read", offset, gReadBuf, numBytes );
}
else
if (( strcmp( cmdStr, "write-data" ) == 0 )
|| ( strcmp( cmdStr, "wd" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
uint8_t data[ 20 ];
if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
{
return true;
}
PrintError( m_device.Write( offset, data, numBytes ));
}
else
if (( strcmp( cmdStr, "reg-write" ) == 0 )
|| ( strcmp( cmdStr, "rw" ) == 0 ))
{
uint8_t offset;
uint8_t numBytes;
uint8_t data[ 20 ];
if ( !ParseOffsetAndData( line, &offset, &numBytes, data, sizeof( data )))
{
return true;
}
m_device.SendDeferredWrite( offset, data, numBytes );
}
else
if ( strcmp( cmdStr, "get" ) == 0 )
{
ProcessDeviceGetCommand( devType, id, line, false );
}
else
if ( strcmp( cmdStr, "get-raw" ) == 0 )
{
ProcessDeviceGetCommand( devType, id, line, true );
}
else
if ( strcmp( cmdStr, "set" ) == 0 )
{
ProcessDeviceSetCommand( devType, id, line, false );
}
else
if ( strcmp( cmdStr, "set-raw" ) == 0 )
{
ProcessDeviceSetCommand( devType, id, line, true );
}
else
if ( strcmp( cmdStr, "reset" ) == 0 )
{
m_device.Reset();
}
else
{
LogError( "Unrecognized command: '%s'\n", cmdStr );
}
return true;
}
int doParallelSuperPMI(CommandLine::Options& o)
{
HRESULT hr = E_FAIL;
SimpleTimer st;
st.Start();
#ifndef FEATURE_PAL // TODO-Porting: handle Ctrl-C signals gracefully on Unix
// Register a ConsoleCtrlHandler
if (!SetConsoleCtrlHandler(CtrlHandler, TRUE))
{
LogError("Failed to set control handler.");
return 1;
}
#endif // !FEATURE_PAL
char tempPath[MAX_PATH];
if (!GetTempPath(MAX_PATH, tempPath))
{
LogError("Failed to get path to temp folder.");
return 1;
}
if (o.workerCount <= 0)
{
// Use the default value which is the number of processors on the machine.
SYSTEM_INFO sysinfo;
GetSystemInfo(&sysinfo);
o.workerCount = sysinfo.dwNumberOfProcessors;
// If we ever execute on a machine which has more than MAXIMUM_WAIT_OBJECTS(64) CPU cores
// we still can't spawn more than the max supported by WaitForMultipleObjects()
if (o.workerCount > MAXIMUM_WAIT_OBJECTS)
o.workerCount = MAXIMUM_WAIT_OBJECTS;
}
// Obtain the folder path of the current executable, which we will use to spawn ourself.
char* spmiFilename = new char[MAX_PATH];
if (!GetModuleFileName(NULL, spmiFilename, MAX_PATH))
{
LogError("Failed to get current exe path.");
return 1;
}
char* spmiArgs = ConstructChildProcessArgs(o);
// TODO: merge all this output to a single call to LogVerbose to avoid all the newlines.
LogVerbose("Using child (%s) with args (%s)", spmiFilename, spmiArgs);
if (o.mclFilename != nullptr)
LogVerbose(" failingMCList=%s", o.mclFilename);
if (o.diffMCLFilename != nullptr)
LogVerbose(" diffMCLFilename=%s", o.diffMCLFilename);
LogVerbose(" workerCount=%d, skipCleanup=%d.", o.workerCount, o.skipCleanup);
HANDLE* hProcesses = new HANDLE[o.workerCount];
HANDLE* hStdOutput = new HANDLE[o.workerCount];
HANDLE* hStdError = new HANDLE[o.workerCount];
char** arrFailingMCListPath = new char*[o.workerCount];
char** arrDiffMCListPath = new char*[o.workerCount];
char** arrStdOutputPath = new char*[o.workerCount];
char** arrStdErrorPath = new char*[o.workerCount];
// Add a random number to the temporary file names to allow multiple parallel SuperPMI to happen at once.
unsigned int randNumber = 0;
#ifdef FEATURE_PAL
PAL_Random(&randNumber, sizeof(randNumber));
#else // !FEATURE_PAL
rand_s(&randNumber);
#endif // !FEATURE_PAL
for (int i = 0; i < o.workerCount; i++)
{
if (o.mclFilename != nullptr)
{
arrFailingMCListPath[i] = new char[MAX_PATH];
sprintf_s(arrFailingMCListPath[i], MAX_PATH, "%sParallelSuperPMI-%u-%d.mcl", tempPath, randNumber, i);
}
else
{
arrFailingMCListPath[i] = nullptr;
}
if (o.diffMCLFilename != nullptr)
{
arrDiffMCListPath[i] = new char[MAX_PATH];
sprintf_s(arrDiffMCListPath[i], MAX_PATH, "%sParallelSuperPMI-Diff-%u-%d.mcl", tempPath, randNumber, i);
}
else
{
arrDiffMCListPath[i] = nullptr;
}
arrStdOutputPath[i] = new char[MAX_PATH];
arrStdErrorPath[i] = new char[MAX_PATH];
sprintf_s(arrStdOutputPath[i], MAX_PATH, "%sParallelSuperPMI-stdout-%u-%d.txt", tempPath, randNumber, i);
sprintf_s(arrStdErrorPath[i], MAX_PATH, "%sParallelSuperPMI-stderr-%u-%d.txt", tempPath, randNumber, i);
}
char cmdLine[MAX_CMDLINE_SIZE];
cmdLine[0] = '\0';
int bytesWritten;
for (int i = 0; i < o.workerCount; i++)
{
bytesWritten = sprintf_s(cmdLine, MAX_CMDLINE_SIZE, "%s -stride %d %d", spmiFilename, i + 1, o.workerCount);
if (o.mclFilename != nullptr)
{
bytesWritten += sprintf_s(cmdLine + bytesWritten, MAX_CMDLINE_SIZE - bytesWritten, " -failingMCList %s",
arrFailingMCListPath[i]);
}
if (o.diffMCLFilename != nullptr)
{
bytesWritten += sprintf_s(cmdLine + bytesWritten, MAX_CMDLINE_SIZE - bytesWritten, " -diffMCList %s",
arrDiffMCListPath[i]);
}
bytesWritten += sprintf_s(cmdLine + bytesWritten, MAX_CMDLINE_SIZE - bytesWritten, " -v ewmin %s", spmiArgs);
SECURITY_ATTRIBUTES sa;
sa.nLength = sizeof(sa);
sa.lpSecurityDescriptor = NULL;
sa.bInheritHandle = TRUE; // Let newly created stdout/stderr handles be inherited.
LogDebug("stdout %i=%s", i, arrStdOutputPath[i]);
hStdOutput[i] = CreateFileA(arrStdOutputPath[i], GENERIC_WRITE, FILE_SHARE_READ, &sa, CREATE_ALWAYS,
FILE_ATTRIBUTE_NORMAL, NULL);
if (hStdOutput[i] == INVALID_HANDLE_VALUE)
{
LogError("Unable to open '%s'. GetLastError()=%u", arrStdOutputPath[i], GetLastError());
return -1;
}
LogDebug("stderr %i=%s", i, arrStdErrorPath[i]);
hStdError[i] = CreateFileA(arrStdErrorPath[i], GENERIC_WRITE, FILE_SHARE_READ, &sa, CREATE_ALWAYS,
FILE_ATTRIBUTE_NORMAL, NULL);
if (hStdError[i] == INVALID_HANDLE_VALUE)
{
LogError("Unable to open '%s'. GetLastError()=%u", arrStdErrorPath[i], GetLastError());
return -1;
}
// Create a SuperPMI worker process and redirect its output to file
if (!StartProcess(cmdLine, hStdOutput[i], hStdError[i], &hProcesses[i]))
{
return -1;
}
}
WaitForMultipleObjects(o.workerCount, hProcesses, true, INFINITE);
// Close stdout/stderr
for (int i = 0; i < o.workerCount; i++)
{
CloseHandle(hStdOutput[i]);
CloseHandle(hStdError[i]);
}
SpmiResult result = SpmiResult::Success;
if (!closeRequested)
{
// Figure out the error code to use.
// Mainly, if any child returns non-zero, we want to return non-zero, to indicate failure.
for (int i = 0; i < o.workerCount; i++)
{
DWORD exitCodeTmp;
BOOL ok = GetExitCodeProcess(hProcesses[i], &exitCodeTmp);
SpmiResult childResult = (SpmiResult)exitCodeTmp;
if (ok && (childResult != result))
{
if (result == SpmiResult::Error || childResult == SpmiResult::Error)
{
result = SpmiResult::Error;
}
else if (result == SpmiResult::Diffs || childResult == SpmiResult::Diffs)
{
result = SpmiResult::Diffs;
}
else if (result == SpmiResult::Misses || childResult == SpmiResult::Misses)
{
result = SpmiResult::Misses;
}
else if (result == SpmiResult::JitFailedToInit || childResult == SpmiResult::JitFailedToInit)
{
result = SpmiResult::JitFailedToInit;
}
else
{
result = SpmiResult::GeneralFailure;
}
}
}
bool usageError = false; // variable to flag if we hit a usage error in SuperPMI
int loaded = 0, jitted = 0, failed = 0, excluded = 0, diffs = 0;
// Read the stderr files and log them as errors
// Read the stdout files and parse them for counts and log any MISSING or ISSUE errors
for (int i = 0; i < o.workerCount; i++)
{
ProcessChildStdErr(arrStdErrorPath[i]);
ProcessChildStdOut(o, arrStdOutputPath[i], &loaded, &jitted, &failed, &excluded, &diffs, &usageError);
if (usageError)
break;
}
if (o.mclFilename != nullptr && !usageError)
{
// Concat the resulting .mcl files
MergeWorkerMCLs(o.mclFilename, arrFailingMCListPath, o.workerCount);
}
if (o.diffMCLFilename != nullptr && !usageError)
{
// Concat the resulting diff .mcl files
MergeWorkerMCLs(o.diffMCLFilename, arrDiffMCListPath, o.workerCount);
}
if (!usageError)
{
if (o.applyDiff)
{
LogInfo(g_AsmDiffsSummaryFormatString, loaded, jitted, failed, excluded, diffs);
}
else
{
LogInfo(g_SummaryFormatString, loaded, jitted, failed, excluded);
}
}
st.Stop();
LogVerbose("Total time: %fms", st.GetMilliseconds());
}
if (!o.skipCleanup)
{
// Delete all temporary files generated
for (int i = 0; i < o.workerCount; i++)
{
if (arrFailingMCListPath[i] != nullptr)
{
DeleteFile(arrFailingMCListPath[i]);
}
if (arrDiffMCListPath[i] != nullptr)
{
DeleteFile(arrDiffMCListPath[i]);
}
DeleteFile(arrStdOutputPath[i]);
DeleteFile(arrStdErrorPath[i]);
}
}
return (int)result;
}
int verbTOC::DoWork(const char *nameOfInput)
{
LogVerbose("Indexing from '%s' into '%s.mct'", nameOfInput, nameOfInput);
MethodContextIterator mci;
if (!mci.Initialize(nameOfInput))
return -1;
int savedCount = 0;
TOCElementNode *head = nullptr;
TOCElementNode *curElem = nullptr;
while (mci.MoveNext())
{
MethodContext* mc = mci.Current();
TOCElementNode *nxt = new TOCElementNode(mci.MethodContextNumber(), mci.CurrentPos());
mc->dumpMethodMD5HashToBuffer(nxt->tocElement.Hash, MD5_HASH_BUFFER_SIZE);
if (curElem != nullptr)
{
curElem->Next = nxt;
}
else
{
head = nxt;
}
curElem = nxt;
savedCount++;
}
size_t maxLen = strlen(nameOfInput) + 5;
char *nameOfOutput = (char*)_alloca(maxLen);
strcpy_s(nameOfOutput, maxLen, nameOfInput);
strcat_s(nameOfOutput, maxLen, ".mct");
HANDLE hFileOut = CreateFileA(nameOfOutput, GENERIC_WRITE, FILE_SHARE_WRITE, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL);
if (hFileOut == INVALID_HANDLE_VALUE)
{
LogError("Failed to open input 1 '%s'. GetLastError()=%u", nameOfOutput, GetLastError());
return -1;
}
DWORD written;
// Write out the signature "INDX" and then the element count
LARGE_INTEGER token;
token.u.LowPart = *(const int*)"INDX"; // cuz Type Safety is for languages that have good IO facilities
token.u.HighPart = savedCount;
if (!WriteFile(hFileOut, &token, sizeof(token), &written, nullptr) || written != sizeof(token))
{
LogError("Failed to write index header. GetLastError()=%u", GetLastError());
}
// Now just dump sizeof(TOCElement) byte chunks into the file.
// I could probably do this more efficiently, but I don't think it matters
DWORD chunkSize = sizeof(TOCElement);
for (curElem = head; curElem != nullptr; curElem = curElem->Next)
{
if (!WriteFile(hFileOut, &curElem->tocElement, chunkSize, &written, nullptr) || written != chunkSize)
{
LogError("Failed to write index element '%d'. GetLastError()=%u", curElem->tocElement.Number, GetLastError());
return -1;
}
}
// Now write out a final "INDX" to flag the end of the file...
if (!WriteFile(hFileOut, &token.u.LowPart, sizeof(token.u.LowPart), &written, nullptr) || (written != sizeof(token.u.LowPart)))
{
LogError("Failed to write index terminal. GetLastError()=%u", GetLastError());
}
LogInfo("Loaded %d, added %d to Table of Contents", mci.MethodContextNumber(), savedCount);
if (CloseHandle(hFileOut) == 0)
{
LogError("CloseHandle failed. GetLastError()=%u", GetLastError());
return -1;
}
if (!mci.Destroy())
return -1;
return 0;
}
NTSTATUS
FrontendSetState(
IN PXENVBD_FRONTEND Frontend,
IN XENVBD_STATE State
)
{
NTSTATUS Status;
const ULONG TargetId = Frontend->TargetId;
BOOLEAN Failed = FALSE;
LogTrace("Target[%d] @ (%d) =====>\n", TargetId, KeGetCurrentIrql());
LogVerbose("Target[%d] : %s ----> %s\n", TargetId, __XenvbdStateName(Frontend->State),
__XenvbdStateName(State));
while (!Failed && Frontend->State != State) {
switch (Frontend->State) {
case XENVBD_INITIALIZED:
switch (State) {
case XENVBD_CLOSED:
case XENVBD_PREPARED:
case XENVBD_CONNECTED:
case XENVBD_ENABLED:
Status = FrontendClose(Frontend);
if (NT_SUCCESS(Status)) {
Frontend->State = XENVBD_CLOSED;
} else {
Failed = TRUE;
}
break;
default:
Failed = TRUE;
break;
}
break;
case XENVBD_CLOSED:
switch (State) {
case XENVBD_PREPARED:
case XENVBD_CONNECTED:
case XENVBD_ENABLED:
Status = FrontendPrepare(Frontend);
if (NT_SUCCESS(Status)) {
Frontend->State = XENVBD_PREPARED;
} else {
Status = FrontendClose(Frontend);
if (NT_SUCCESS(Status))
Frontend->State = XENVBD_CLOSED;
else
Frontend->State = XENVBD_STATE_INVALID;
Failed = TRUE;
}
break;
default:
Failed = TRUE;
break;
}
break;
case XENVBD_PREPARED:
switch (State) {
case XENVBD_CONNECTED:
case XENVBD_ENABLED:
Status = FrontendConnect(Frontend);
if (NT_SUCCESS(Status)) {
Frontend->State = XENVBD_CONNECTED;
} else {
Status = FrontendClose(Frontend);
if (NT_SUCCESS(Status))
Frontend->State = XENVBD_CLOSED;
else
Frontend->State = XENVBD_STATE_INVALID;
Failed = TRUE;
}
break;
case XENVBD_CLOSED:
Status = FrontendClose(Frontend);
if (NT_SUCCESS(Status)) {
Frontend->State = XENVBD_CLOSED;
} else {
Frontend->State = XENVBD_STATE_INVALID;
Failed = TRUE;
}
break;
default:
Failed = TRUE;
break;
}
break;
case XENVBD_CONNECTED:
switch (State) {
case XENVBD_ENABLED:
FrontendEnable(Frontend);
Frontend->State = XENVBD_ENABLED;
break;
case XENVBD_CLOSED:
case XENVBD_PREPARED:
FrontendDisconnect(Frontend);
Status = FrontendClose(Frontend);
if (NT_SUCCESS(Status)) {
Frontend->State = XENVBD_CLOSED;
} else {
Frontend->State = XENVBD_STATE_INVALID;
Failed = TRUE;
}
break;
default:
Failed = TRUE;
break;
}
break;
case XENVBD_ENABLED:
switch (State) {
case XENVBD_CLOSED:
case XENVBD_PREPARED:
case XENVBD_CONNECTED:
FrontendDisable(Frontend);
Frontend->State = XENVBD_CONNECTED;
break;
default:
Failed = TRUE;
break;
}
break;
default:
Failed = TRUE;
break;
}
LogVerbose("Target[%d] : in state %s\n", TargetId, __XenvbdStateName(Frontend->State));
}
LogTrace("Target[%d] @ (%d) <===== (%s)\n", TargetId, KeGetCurrentIrql(), Failed ? "FAILED" : "SUCCEEDED");
return Failed ? STATUS_UNSUCCESSFUL : STATUS_SUCCESS;
}
int main(int argc, char** argv)
{
int fd, n;
unsigned long loadaddr;
char* p;
char m4IsStopped = 0;
char m4IsRunning = 0;
int m4TraceFlags = 0;
int m4Retry;
char* filepath = argv[1];
int currentSoC = -1;
if (argc < 2) {
LogError(HEADER);
LogError("-- %s -- \nUsage:\n"
"%s [filename.bin] [0xLOADADDR] [--verbose] # loads new firmware\n"
"or: %s stop # holds the auxiliary core in reset\n"
"or: %s start # releases the auxiliary core from reset\n"
"or: %s kick [n] # triggers interrupt on RPMsg virtqueue n\n",
NAME_OF_UTILITY, argv[0], argv[0], argv[0], argv[0], argv[0]);
return RETURN_CODE_ARGUMENTS_ERROR;
}
currentSoC = get_board_id();
if (currentSoC == -1) {
LogError(HEADER);
LogError("Board is not supported.\n");
return RETURN_CODE_ARGUMENTS_ERROR;
}
fd = open("/dev/mem", O_RDWR | O_SYNC);
if (fd < 0) {
LogError(HEADER);
LogError("Could not open /dev/mem, are you root?\n");
return RETURN_CODE_ARGUMENTS_ERROR;
}
/* PARTIAL COMMANDS */
if (!strcmp(argv[1], "stop")) {
stop_cpu(fd, currentSoC);
return RETURN_CODE_OK;
}
else if (!strcmp(argv[1], "start")) {
start_cpu(fd, currentSoC);
return RETURN_CODE_OK;
}
else if (!strcmp(argv[1], "kick")) {
if (argc < 3) {
LogError(HEADER);
LogError("%s - Usage: %s kick {vq_id to kick}\n", NAME_OF_UTILITY, argv[0]);
return RETURN_CODE_ARGUMENTS_ERROR;
}
rpmsg_mu_kick(fd, currentSoC, strtoul(argv[2], &p, 16));
return RETURN_CODE_OK;
}
/* FW LOADING */
if (argc < 3) {
LogError(HEADER);
LogError("%s - Usage: %s [yourfwname.bin] [0xLOADADDR] [--verbose]\n", NAME_OF_UTILITY, argv[0]);
return RETURN_CODE_ARGUMENTS_ERROR;
}
if (access(filepath, F_OK) == -1) {
LogError("File %s not found.\n", argv[1]);
return RETURN_CODE_ARGUMENTS_ERROR;
}
loadaddr = strtoul(argv[2], &p, 16);
if (argc == 4) {
if (!strcmp(argv[3], "--verbose")) {
verbose = 1;
}
else {
LogError(HEADER);
LogError("%s - Usage: %s [yourfwname.bin] [0xLOADADDR] [--verbose]\n", NAME_OF_UTILITY, argv[0]);
return RETURN_CODE_ARGUMENTS_ERROR;
}
}
LogVerbose("LoadAddr is: %X\n", loadaddr);
LogVerbose("Will stop CPU now...\n");
stop_cpu(fd, currentSoC);
LogVerbose("Will ungate M4 clock source...\n");
ungate_m4_clk(fd, currentSoC);
LogVerbose("Will load M4 firmware...\n");
load_m4_fw(fd, currentSoC, filepath, loadaddr);
LogVerbose("Will start CPU now...\n");
start_cpu(fd, currentSoC);
LogVerbose("Done!\n");
close(fd);
return RETURN_CODE_OK;
}
