Skip to content

Commit bc56a83

Browse files
JohnAZoidbergJohnAZoidberg
authored
Merge pull request #16 from FrameworkComputer/motionsense
Detect sensor presence and location via motion sense host commands
2 parents 40ca7a4 + e525a53 commit bc56a83

File tree

8 files changed

+341
-63
lines changed

8 files changed

+341
-63
lines changed

‎FrameworkSensors/AccelerometerClient.cpp‎

Lines changed: 127 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,102 @@ typedef enum
3838
ACCELEROMETER_DATA_COUNT
3939
} ACCELEROMETER_DATA_INDEX;
4040

41+
UINT8 CrosEcGetMotionSensorCount(HANDLE Handle)
42+
{
43+
EC_REQUEST_MOTION_SENSE_DUMP req{};
44+
EC_RESPONSE_MOTION_SENSE_DUMP res{};
45+
46+
if (Handle == INVALID_HANDLE_VALUE) {
47+
TraceError("%!FUNC! Handle is invalid");
48+
return 0;
49+
}
50+
51+
req.Cmd = 0;
52+
req.MaxSensorCount = 0;
53+
if (0 == CrosEcSendCommand(
54+
Handle,
55+
EC_CMD_MOTION_SENSE,
56+
1,
57+
&req,
58+
sizeof(req),
59+
&res,
60+
sizeof(res)
61+
)) {
62+
TraceError("%!FUNC! EC_CMD_MOTION_SENSE_DUMP failed");
63+
return 0;
64+
}
65+
66+
return res.SensorCount;
67+
}
68+
69+
// Returns STATUS_NOT_FOUND if either base or lid accelerometer sensors are not found.
70+
NTSTATUS
71+
CrosEcGetAccelIndeces(HANDLE Handle, UINT8 *BaseSensor, UINT8 *LidSensor)
72+
{
73+
EC_REQUEST_MOTION_SENSE_INFO req{};
74+
EC_RESPONSE_MOTION_SENSE_INFO res{};
75+
BOOLEAN FoundBase = FALSE;
76+
BOOLEAN FoundLid = FALSE;
77+
UINT8 SensorCount = 0;
78+
79+
if (Handle == INVALID_HANDLE_VALUE) {
80+
TraceError("%!FUNC! Handle is invalid");
81+
return STATUS_INVALID_HANDLE;
82+
}
83+
84+
if (BaseSensor == nullptr || LidSensor == nullptr)
85+
{
86+
TraceError("%!FUNC! Invalid BaseSensor or LidSensor pointer");
87+
return STATUS_INVALID_PARAMETER;
88+
}
89+
90+
SensorCount = CrosEcGetMotionSensorCount(Handle);
91+
92+
for (UINT8 i = 0; i < SensorCount; i++)
93+
{
94+
req.Cmd = 1;
95+
req.SensorNum = i;
96+
if (0 == CrosEcSendCommand(
97+
Handle,
98+
EC_CMD_MOTION_SENSE,
99+
1,
100+
&req,
101+
sizeof(req),
102+
&res,
103+
sizeof(res)
104+
)) {
105+
TraceError("%!FUNC! EC_CMD_MOTION_SENSE_INFO failed for sensor %d", i);
106+
continue;
107+
}
108+
if (res.SensorType != MOTION_SENSE_TYPE_ACCEL) {
109+
TraceError("%!FUNC! Found sensor of type %d. Not Accelerometer - ignoring.", res.SensorType);
110+
continue;
111+
}
112+
113+
switch (res.Location) {
114+
case MOTION_SENSE_LOCATION_BASE:
115+
TraceInformation("%!FUNC! Found base accel sensor at index: %d", i);
116+
FoundBase = TRUE;
117+
*BaseSensor = i;
118+
break;
119+
case MOTION_SENSE_LOCATION_LID:
120+
TraceInformation("%!FUNC! Found lid accel sensor at index: %d", i);
121+
FoundLid = TRUE;
122+
*LidSensor = i;
123+
break;
124+
}
125+
}
126+
127+
if (!FoundBase || !FoundLid)
128+
{
129+
TraceError("%!FUNC! Base or Lid accelerometer sensor not found");
130+
return STATUS_NOT_FOUND;
131+
}
132+
133+
return STATUS_SUCCESS;
134+
}
135+
136+
41137
//------------------------------------------------------------------------------
42138
// Function: Initialize
43139
//
@@ -57,6 +153,8 @@ AccelerometerDevice::Initialize(
57153
)
58154
{
59155
NTSTATUS Status = STATUS_SUCCESS;
156+
UINT8 SensorCount = 0;
157+
PComboDevice Context = GetContextFromSensorInstance(SensorInstance);
60158

61159
SENSOR_FunctionEnter();
62160

@@ -66,6 +164,25 @@ AccelerometerDevice::Initialize(
66164
m_Device = Device;
67165
m_SensorInstance = SensorInstance;
68166
m_Started = FALSE;
167+
// Sensible defaults - applies to most devices
168+
m_LidSensorIndex = 0;
169+
m_LidBaseSensor = 1;
170+
171+
SensorCount = CrosEcGetMotionSensorCount(Context->m_CrosEcHandle);
172+
TraceInformation("%!FUNC! Found %d Sensors on this device", SensorCount);
173+
if (SensorCount == 0)
174+
{
175+
TraceError("%!FUNC! No Sensors available. Not initializing AccelerometerClient");
176+
Status = STATUS_NOT_FOUND;
177+
goto Exit;
178+
}
179+
180+
Status = CrosEcGetAccelIndeces(Context->m_CrosEcHandle, &m_LidSensorIndex, &m_LidBaseSensor);
181+
if (!NT_SUCCESS(Status))
182+
{
183+
TraceError("%!FUNC! Failed to get accelerometer indeces: %!STATUS!", Status);
184+
goto Exit;
185+
}
69186

70187
//
71188
// Create Lock
@@ -411,13 +528,17 @@ AccelerometerDevice::GetData(
411528
UINT16 lid_angle = lid_angle_bytes[0] + (lid_angle_bytes[1] << 8);
412529
TraceInformation("Lid Angle Status: %dDeg%s", lid_angle, lid_angle == 500 ? "(Unreliable)" : "");
413530

531+
// Lid accelerometer is relevant for screen rotation
532+
// Base accelerometer is not used in this driver
533+
// It's only used for lid angle in the EC firmware
534+
UINT SensorOffset = 6 * m_LidSensorIndex + EC_MEMMAP_ACC_DATA + 2;
414535
UINT16 Sensor1[6] = {0};
415-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 2, (UINT8*)&Sensor1[0]);
416-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 3, (UINT8*)&Sensor1[1]);
417-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 4, (UINT8*)&Sensor1[2]);
418-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 5, (UINT8*)&Sensor1[3]);
419-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 6, (UINT8*)&Sensor1[4]);
420-
CrosEcReadMemU8(Handle, EC_MEMMAP_ACC_DATA + 7, (UINT8*)&Sensor1[5]);
536+
CrosEcReadMemU8(Handle, SensorOffset, (UINT8*)&Sensor1[0]);
537+
CrosEcReadMemU8(Handle, SensorOffset + 1, (UINT8*)&Sensor1[1]);
538+
CrosEcReadMemU8(Handle, SensorOffset + 2, (UINT8*)&Sensor1[2]);
539+
CrosEcReadMemU8(Handle, SensorOffset + 3, (UINT8*)&Sensor1[3]);
540+
CrosEcReadMemU8(Handle, SensorOffset + 4, (UINT8*)&Sensor1[4]);
541+
CrosEcReadMemU8(Handle, SensorOffset + 5, (UINT8*)&Sensor1[5]);
421542
m_CachedData.Axis.X = (float) (Sensor1[0] + (Sensor1[1] << 8));
422543
m_CachedData.Axis.Y = (float) (Sensor1[2] + (Sensor1[3] << 8));
423544
m_CachedData.Axis.Z = (float) (Sensor1[4] + (Sensor1[5] << 8));

‎FrameworkSensors/Clients.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
#include <cmath>
1717
#include <timeapi.h>
1818

19-
#include "SensorsTrace.h"
19+
#include "Trace.h"
2020
#include <SensorsCx.h>
2121
#include <SensorsUtils.h>
2222

@@ -221,6 +221,8 @@ typedef class _AccelerometerDevice : public _ComboDevice
221221
AccelerometerSample m_CachedThresholds;
222222
AccelerometerSample m_CachedData;
223223
AccelerometerSample m_LastSample;
224+
UINT8 m_LidSensorIndex;
225+
UINT8 m_LidBaseSensor;
224226

225227
public:
226228

‎FrameworkSensors/Device.cpp‎

Lines changed: 1 addition & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -84,38 +84,6 @@ void AllocateDeviceAtIndex(
8484
}
8585
}
8686

87-
NTSTATUS ConnectToEc(
88-
_In_ WDFDEVICE FxDevice,
89-
_Inout_ HANDLE *Handle
90-
) {
91-
SENSOR_FunctionEnter();
92-
NTSTATUS Status = STATUS_SUCCESS;
93-
94-
UNREFERENCED_PARAMETER(FxDevice);
95-
96-
*Handle = CreateFileW(
97-
LR"(\\.\GLOBALROOT\Device\CrosEC)",
98-
GENERIC_READ | GENERIC_WRITE,
99-
FILE_SHARE_READ | FILE_SHARE_WRITE,
100-
NULL,
101-
OPEN_EXISTING,
102-
FILE_FLAG_OVERLAPPED,
103-
NULL);
104-
105-
if (*Handle == INVALID_HANDLE_VALUE) {
106-
Status = STATUS_INVALID_HANDLE;
107-
TraceError("COMBO %!FUNC! CreateFileW failed %!STATUS!", Status);
108-
goto Exit;
109-
}
110-
111-
Exit:
112-
SENSOR_FunctionExit(Status);
113-
114-
return Status;
115-
}
116-
117-
118-
11987
//------------------------------------------------------------------------------
12088
//
12189
// Function: OnDeviceAdd
@@ -258,7 +226,7 @@ OnPrepareHardware(
258226

259227
SENSOR_FunctionEnter();
260228

261-
Status = ConnectToEc(Device, &Handle);
229+
Status = ConnectToEc(&Handle);
262230
if (!NT_SUCCESS(Status)) {
263231
TraceError("COMBO %!FUNC! ConnectToEc failed %!STATUS!", Status);
264232
goto Exit;

‎FrameworkSensors/EcCommunication.cpp‎

Lines changed: 96 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,105 @@
1212
//
1313
// Windows User-Mode Driver Framework (UMDF)
1414

15-
#include "Clients.h"
1615
#include "EcCommunication.h"
1716
#include <windows.h>
1817
#include <wdf.h>
1918

2019
#include "EcCommunication.tmh"
2120

21+
NTSTATUS ConnectToEc(
22+
_Inout_ HANDLE* Handle
23+
) {
24+
NTSTATUS Status = STATUS_SUCCESS;
25+
26+
*Handle = CreateFileW(
27+
LR"(\\.\GLOBALROOT\Device\CrosEC)",
28+
GENERIC_READ | GENERIC_WRITE,
29+
FILE_SHARE_READ | FILE_SHARE_WRITE,
30+
NULL,
31+
OPEN_EXISTING,
32+
FILE_FLAG_OVERLAPPED,
33+
NULL);
34+
35+
if (*Handle == INVALID_HANDLE_VALUE) {
36+
TraceError("%!FUNC! CreateFileW failed %!STATUS!", Status);
37+
return STATUS_INVALID_HANDLE;
38+
}
39+
40+
return Status;
41+
}
42+
43+
int CrosEcSendCommand(
44+
HANDLE Handle,
45+
UINT16 command,
46+
UINT8 version,
47+
LPVOID outdata,
48+
unsigned int outlen,
49+
LPVOID indata,
50+
unsigned int inlen
51+
)
52+
{
53+
NTSTATUS Status = STATUS_SUCCESS;
54+
DWORD retb{};
55+
CROSEC_COMMAND cmd{};
56+
57+
if (Handle == INVALID_HANDLE_VALUE) {
58+
Status = STATUS_INVALID_HANDLE;
59+
TraceError("%!FUNC! Invalid Handle");
60+
return 0;
61+
}
62+
63+
if (outlen > CROS_EC_CMD_MAX_REQUEST || inlen > CROS_EC_CMD_MAX_REQUEST) {
64+
TraceError("%!FUNC! outlen %d or inlen %d too large", outlen, inlen);
65+
return 0;
66+
}
67+
if (outlen == 0) {
68+
TraceError("%!FUNC! outlen is 0");
69+
return 0;
70+
}
71+
if (outdata == nullptr) {
72+
TraceError("%!FUNC! Invalid outdata - NULL");
73+
return 0;
74+
}
75+
76+
cmd.command = command;
77+
cmd.version = version;
78+
cmd.result = 0xFF;
79+
cmd.outlen = outlen;
80+
cmd.inlen = CROS_EC_CMD_MAX_REQUEST - 8; // 8 is the header length
81+
82+
RtlCopyMemory(cmd.data, outdata, outlen);
83+
84+
Status = DeviceIoControl(Handle,
85+
(DWORD) IOCTL_CROSEC_XCMD,
86+
&cmd,
87+
sizeof(cmd),
88+
&cmd,
89+
sizeof(cmd),
90+
&retb,
91+
nullptr);
92+
if (!NT_SUCCESS(Status)) {
93+
TraceError("%!FUNC! ConnectToEc failed %!STATUS!", Status);
94+
return 0;
95+
}
96+
97+
if (cmd.result != EC_RES_SUCCESS) {
98+
TraceError("%!FUNC! Host command failed - EC result %d", cmd.result);
99+
return 0;
100+
}
101+
102+
if (inlen > 0) {
103+
if (indata == nullptr) {
104+
TraceError("%!FUNC! inlen is %d. But indata is NULL", inlen);
105+
return 0;
106+
}
107+
RtlCopyMemory(indata, cmd.data, inlen);
108+
}
109+
110+
return cmd.inlen;
111+
}
112+
113+
22114
int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
23115
{
24116
NTSTATUS Status = STATUS_SUCCESS;
@@ -27,7 +119,7 @@ int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
27119

28120
if (Handle == INVALID_HANDLE_VALUE) {
29121
Status = STATUS_INVALID_HANDLE;
30-
TraceError("COMBO %!FUNC! Invalid Handle");
122+
TraceError("%!FUNC! Invalid Handle");
31123
return 0;
32124
}
33125

@@ -42,11 +134,11 @@ int CrosEcReadMemU8(HANDLE Handle, unsigned int offset, UINT8* dest)
42134
&retb,
43135
nullptr);
44136
if (!NT_SUCCESS(Status)) {
45-
TraceError("COMBO %!FUNC! ConnectToEc failed %!STATUS!", Status);
137+
TraceError("%!FUNC! ConnectToEc failed %!STATUS!", Status);
46138
return 0;
47139
}
48140

49-
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);
141+
TraceInformation("%!FUNC! Successfully read %d bytes from EC memory at %02x. First one %02x. retb=%d", rm.bytes, rm.offset, rm.buffer[0], retb);
50142
*dest = rm.buffer[0];
51143

52144
return rm.bytes;

0 commit comments

Comments
 (0)