Detect sensor presence and location via motion sense host commands

FrameworkSensors/AccelerometerClient.cpp

@@ -38,6 +38,102 @@ typedef enum
     ACCELEROMETER_DATA_COUNT
 } ACCELEROMETER_DATA_INDEX;
 
+UINT8 CrosEcGetMotionSensorCount(HANDLE Handle)
+{
+    EC_REQUEST_MOTION_SENSE_DUMP req{};
+    EC_RESPONSE_MOTION_SENSE_DUMP res{};
+
+    if (Handle == INVALID_HANDLE_VALUE) {
+        TraceError("%!FUNC! Handle is invalid");
+        return 0;
+    }
+
+    req.Cmd = 0;
+    req.MaxSensorCount = 0;
+    if (0 == CrosEcSendCommand(
+        Handle,
+        EC_CMD_MOTION_SENSE,
+        1,
+        &req,
+        sizeof(req),
+        &res,
+        sizeof(res)
+    )) {
+        TraceError("%!FUNC! EC_CMD_MOTION_SENSE_DUMP failed");
+        return 0;
+    }
+
+    return res.SensorCount;
+}
+
+// Returns STATUS_NOT_FOUND if either base or lid accelerometer sensors are not found.
+NTSTATUS
+CrosEcGetAccelIndeces(HANDLE Handle, UINT8 *BaseSensor, UINT8 *LidSensor)
+{
+    EC_REQUEST_MOTION_SENSE_INFO req{};
+    EC_RESPONSE_MOTION_SENSE_INFO res{};
+    BOOLEAN FoundBase = FALSE;
+    BOOLEAN FoundLid = FALSE;
+    UINT8 SensorCount = 0;
+
+    if (Handle == INVALID_HANDLE_VALUE) {
+        TraceError("%!FUNC! Handle is invalid");
+        return STATUS_INVALID_HANDLE;
+    }
+
+    if (BaseSensor == nullptr || LidSensor == nullptr)
+    {
+        TraceError("%!FUNC! Invalid BaseSensor or LidSensor pointer");
+        return STATUS_INVALID_PARAMETER;
+    }
+
+    SensorCount = CrosEcGetMotionSensorCount(Handle);
+
+    for (UINT8 i = 0; i < SensorCount; i++)
+    {
+        req.Cmd = 1;
+        req.SensorNum = i;
+        if (0 == CrosEcSendCommand(
+            Handle,
+            EC_CMD_MOTION_SENSE,
+            1,
+            &req,
+            sizeof(req),
+            &res,
+            sizeof(res)
+        )) {
+            TraceError("%!FUNC! EC_CMD_MOTION_SENSE_INFO failed for sensor %d", i);
+            continue;
+        }
+        if (res.SensorType != MOTION_SENSE_TYPE_ACCEL) {
+            TraceError("%!FUNC! Found sensor of type %d. Not Accelerometer - ignoring.", res.SensorType);
+            continue;
+        }
+
+        switch (res.Location) {
+        case MOTION_SENSE_LOCATION_BASE:
+            TraceInformation("%!FUNC! Found base accel sensor at index: %d", i);
+            FoundBase = TRUE;
+            *BaseSensor = i;
+            break;
+        case MOTION_SENSE_LOCATION_LID:
+            TraceInformation("%!FUNC! Found lid accel sensor at index: %d", i);
+            FoundLid = TRUE;
+            *LidSensor = i;
+            break;
+        }
+    }
+
+    if (!FoundBase || !FoundLid)
+    {
+        TraceError("%!FUNC! Base or Lid accelerometer sensor not found");
+        return STATUS_NOT_FOUND;
+    }
+
+    return STATUS_SUCCESS;
+}
+
+
 //------------------------------------------------------------------------------
 // Function: Initialize
 //
@@ -57,6 +153,8 @@ AccelerometerDevice::Initialize(
     )
 {
     NTSTATUS Status = STATUS_SUCCESS;
+    UINT8 SensorCount = 0;
+    PComboDevice Context = GetContextFromSensorInstance(SensorInstance);
 
     SENSOR_FunctionEnter();
 
@@ -66,6 +164,25 @@ AccelerometerDevice::Initialize(
     m_Device = Device;
     m_SensorInstance = SensorInstance;
     m_Started = FALSE;
+    // Sensible defaults - applies to most devices
+    m_LidSensorIndex = 0;
+    m_LidBaseSensor = 1;
+
+    SensorCount = CrosEcGetMotionSensorCount(Context->m_CrosEcHandle);
+    TraceInformation("%!FUNC! Found %d Sensors on this device", SensorCount);
+    if (SensorCount == 0)
+    {
+        TraceError("%!FUNC! No Sensors available. Not initializing AccelerometerClient");
+        Status = STATUS_NOT_FOUND;
+        goto Exit;
+    }
+
+    Status = CrosEcGetAccelIndeces(Context->m_CrosEcHandle, &m_LidSensorIndex, &m_LidBaseSensor);
+    if (!NT_SUCCESS(Status))
+    {
+        TraceError("%!FUNC! Failed to get accelerometer indeces: %!STATUS!", Status);
+        goto Exit;
+    }
 
     //
     // Create Lock
@@ -411,13 +528,17 @@ AccelerometerDevice::GetData(
     UINT16 lid_angle = lid_angle_bytes[0] + (lid_angle_bytes[1] << 8);
     TraceInformation("Lid Angle Status: %dDeg%s", lid_angle, lid_angle == 500 ?  "(Unreliable)" : "");
 
+    // Lid accelerometer is relevant for screen rotation
+    // Base accelerometer is not used in this driver
+    // It's only used for lid angle in the EC firmware
+    UINT SensorOffset = 6 * m_LidSensorIndex + EC_MEMMAP_ACC_DATA + 2;
     UINT16 Sensor1[6] = {0};
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 2, (UINT8*)&Sensor1[0]);
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 3, (UINT8*)&Sensor1[1]);
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 4, (UINT8*)&Sensor1[2]);
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 5, (UINT8*)&Sensor1[3]);
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 6, (UINT8*)&Sensor1[4]);
-    CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 7, (UINT8*)&Sensor1[5]);
+    CrosEcReadMemU8(Handle, SensorOffset, (UINT8*)&Sensor1[0]);
+    CrosEcReadMemU8(Handle, SensorOffset + 1, (UINT8*)&Sensor1[1]);
+    CrosEcReadMemU8(Handle, SensorOffset + 2, (UINT8*)&Sensor1[2]);
+    CrosEcReadMemU8(Handle, SensorOffset + 3, (UINT8*)&Sensor1[3]);
+    CrosEcReadMemU8(Handle, SensorOffset + 4, (UINT8*)&Sensor1[4]);
+    CrosEcReadMemU8(Handle, SensorOffset + 5, (UINT8*)&Sensor1[5]);
     m_CachedData.Axis.X = (float) (Sensor1[0] + (Sensor1[1] << 8));
     m_CachedData.Axis.Y = (float) (Sensor1[2] + (Sensor1[3] << 8));
     m_CachedData.Axis.Z = (float) (Sensor1[4] + (Sensor1[5] << 8));

FrameworkSensors/Clients.h

@@ -16,7 +16,7 @@
 #include <cmath>
 #include <timeapi.h>
 
-#include "SensorsTrace.h"
+#include "Trace.h"
 #include <SensorsCx.h>
 #include <SensorsUtils.h>
 
@@ -221,6 +221,8 @@ typedef class _AccelerometerDevice : public _ComboDevice
     AccelerometerSample                       m_CachedThresholds;
     AccelerometerSample                       m_CachedData;
     AccelerometerSample                       m_LastSample;
+    UINT8                                     m_LidSensorIndex;
+    UINT8                                     m_LidBaseSensor;
 
 public:
 

FrameworkSensors/Device.cpp

@@ -84,38 +84,6 @@ void AllocateDeviceAtIndex(
     }
 }
 
-NTSTATUS ConnectToEc(
-	_In_ WDFDEVICE FxDevice,
-    _Inout_ HANDLE *Handle
-) {
-    SENSOR_FunctionEnter();
-    NTSTATUS Status = STATUS_SUCCESS;
-
-    UNREFERENCED_PARAMETER(FxDevice);
-
-    *Handle = CreateFileW(
-        LR"(\\.\GLOBALROOT\Device\CrosEC)",
-        GENERIC_READ | GENERIC_WRITE,
-        FILE_SHARE_READ | FILE_SHARE_WRITE,
-        NULL,
-        OPEN_EXISTING,
-        FILE_FLAG_OVERLAPPED,
-        NULL);
-
-    if (*Handle == INVALID_HANDLE_VALUE) {
-        Status = STATUS_INVALID_HANDLE;
-        TraceError("COMBO %!FUNC! CreateFileW failed %!STATUS!", Status);
-        goto Exit;
-    }
-
-Exit:
-    SENSOR_FunctionExit(Status);
-
-	return Status;
-}
-
-
-
 //------------------------------------------------------------------------------
 //
 // Function: OnDeviceAdd
@@ -258,7 +226,7 @@ OnPrepareHardware(
 
     SENSOR_FunctionEnter();
 
-    Status = ConnectToEc(Device, &Handle);
+    Status = ConnectToEc(&Handle);
     if (!NT_SUCCESS(Status)) {
         TraceError("COMBO %!FUNC! ConnectToEc failed %!STATUS!", Status);
         goto Exit;

FrameworkSensors/EcCommunication.cpp

@@ -12,13 +12,105 @@
 //
 //  Windows User-Mode Driver Framework (UMDF)
 
-#include "Clients.h"
 #include "EcCommunication.h"
 #include <windows.h>
 #include <wdf.h>
 
 #include "EcCommunication.tmh"
 
+NTSTATUS ConnectToEc(
+    _Inout_ HANDLE* Handle
+) {
+    NTSTATUS Status = STATUS_SUCCESS;
+
+    *Handle = CreateFileW(
+        LR"(\\.\GLOBALROOT\Device\CrosEC)",
+        GENERIC_READ | GENERIC_WRITE,
+        FILE_SHARE_READ | FILE_SHARE_WRITE,
+        NULL,
+        OPEN_EXISTING,
+        FILE_FLAG_OVERLAPPED,
+        NULL);
+
+    if (*Handle == INVALID_HANDLE_VALUE) {
+        TraceError("%!FUNC! CreateFileW failed %!STATUS!", Status);
+        return STATUS_INVALID_HANDLE;
+    }
+
+    return Status;
+}
+
+int CrosEcSendCommand(
+    HANDLE Handle,
+    UINT16 command,
+    UINT8 version,
+    LPVOID outdata,
+    unsigned int outlen,
+    LPVOID indata,
+    unsigned int inlen
+)
+{
+    NTSTATUS Status = STATUS_SUCCESS;
+    DWORD retb{};
+    CROSEC_COMMAND cmd{};
+
+    if (Handle == INVALID_HANDLE_VALUE) {
+        Status = STATUS_INVALID_HANDLE;
+        TraceError("%!FUNC! Invalid Handle");
+        return 0;
+    }
+
+    if (outlen > CROS_EC_CMD_MAX_REQUEST || inlen > CROS_EC_CMD_MAX_REQUEST) {
+        TraceError("%!FUNC! outlen %d or inlen %d too large", outlen, inlen);
+        return 0;
+    }
+    if (outlen == 0) {
+        TraceError("%!FUNC! outlen is 0");
+        return 0;
+    }
+    if (outdata == nullptr) {
+        TraceError("%!FUNC! Invalid outdata - NULL");
+        return 0;
+    }
+
+    cmd.command = command;
+    cmd.version = version;
+    cmd.result = 0xFF;
+    cmd.outlen = outlen;
+    cmd.inlen = CROS_EC_CMD_MAX_REQUEST - 8; // 8 is the header length
+
+    RtlCopyMemory(cmd.data, outdata, outlen);
+
+    Status = DeviceIoControl(Handle,
+        (DWORD) IOCTL_CROSEC_XCMD,
+        &cmd,
+        sizeof(cmd),
+        &cmd,
+        sizeof(cmd),
+        &retb,
+        nullptr);
+    if (!NT_SUCCESS(Status)) {
+        TraceError("%!FUNC! ConnectToEc failed %!STATUS!", Status);
+        return 0;
+    }
+
+    if (cmd.result != EC_RES_SUCCESS) {
+        TraceError("%!FUNC! Host command failed - EC result %d", cmd.result);
+        return 0;
+    }
+
+    if (inlen > 0) {
+        if (indata == nullptr) {
+            TraceError("%!FUNC! inlen is %d. But indata is NULL", inlen);
+            return 0;
+        }
+        RtlCopyMemory(indata, cmd.data, inlen);
+    }
+
+    return cmd.inlen;
+}
+
+
 int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
 {
     NTSTATUS Status = STATUS_SUCCESS;
@@ -27,7 +119,7 @@ int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
 
     if (Handle == INVALID_HANDLE_VALUE) {
         Status = STATUS_INVALID_HANDLE;
-        TraceError("COMBO %!FUNC! Invalid Handle");
+        TraceError("%!FUNC! Invalid Handle");
         return 0;
     }
 
@@ -42,11 +134,11 @@ int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
         &retb,
         nullptr);
     if (!NT_SUCCESS(Status)) {
-        TraceError("COMBO %!FUNC! ConnectToEc failed %!STATUS!", Status);
+        TraceError("%!FUNC! ConnectToEc failed %!STATUS!", Status);
         return 0;
     }
 
-    TraceInformation("COMBO %!FUNC! Successfully read %d bytes from EC memory at %02x. First one %02x. retb=%d", rm.bytes, rm.offset, rm.buffer[0], retb);
+    TraceInformation("%!FUNC! Successfully read %d bytes from EC memory at %02x. First one %02x. retb=%d", rm.bytes, rm.offset, rm.buffer[0], retb);
     *dest = rm.buffer[0];
 
     return rm.bytes;