Skip to content

Commit a6ba4b8

Browse files
janderwaldjanderwald
committed
[USBVIDEO]
- Calculate alt setting interface number correctly - Dynamically calculate transfer settings - Search stream for SOF bit
1 parent 393802c commit a6ba4b8

6 files changed

Lines changed: 150 additions & 32 deletions

File tree

‎drivers/usb/usbvideo/descriptor.c‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -80,10 +80,13 @@ USBVideoFindStreamingInterfaceDescriptor(
8080
EndpointDescriptor = (PUSB_ENDPOINT_DESCRIPTOR)((ULONG_PTR)Descriptor + Descriptor->bLength);
8181
ASSERT(EndpointDescriptor->bDescriptorType == 0x05);
8282
ASSERT(EndpointDescriptor->bLength == 0x07);
83-
if ((EndpointDescriptor->wMaxPacketSize & 0x3FF) >=dwMaxPayloadTransferSize)
83+
USHORT wMaxPacketSize = EndpointDescriptor->wMaxPacketSize;
84+
ULONG MaxPacketSize = (wMaxPacketSize & 0x7FF) * (((wMaxPacketSize >> 11) & 3) + 1);
85+
86+
if (MaxPacketSize >= dwMaxPayloadTransferSize)
8487
{
8588
/* found alternate setting */
86-
DPRINT1("wMaxPacketSize %x\n", EndpointDescriptor->wMaxPacketSize);
89+
DPRINT1("wMaxPacketSize %x AltSetting %x\n", EndpointDescriptor->wMaxPacketSize, AltSetting);
8790
*AlternateSetting = AltSetting;
8891
return STATUS_SUCCESS;
8992
}

‎drivers/usb/usbvideo/iso.c‎

Lines changed: 84 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -53,15 +53,14 @@ USBVideoQueueIsoRead(
5353
Urb->UrbIsochronousTransfer.Hdr.Length = GET_ISO_URB_SIZE(DeviceExtension->IsoPacketCount);
5454
Urb->UrbIsochronousTransfer.PipeHandle = DeviceExtension->hPipe;
5555
Urb->UrbIsochronousTransfer.TransferFlags = USBD_TRANSFER_DIRECTION_IN | USBD_START_ISO_TRANSFER_ASAP;
56-
Urb->UrbIsochronousTransfer.TransferBufferLength = DeviceExtension->dwMaxPayloadTransferSize * DeviceExtension->IsoPacketCount;
56+
Urb->UrbIsochronousTransfer.TransferBufferLength = (DeviceExtension->dwMaxPayloadTransferSize) * DeviceExtension->IsoPacketCount;
5757
Urb->UrbIsochronousTransfer.TransferBuffer = TransferBuffer;
5858
Urb->UrbIsochronousTransfer.NumberOfPackets = DeviceExtension->IsoPacketCount;
5959
Urb->UrbIsochronousTransfer.StartFrame = 0;
6060

6161
for (Index = 0; Index < DeviceExtension->IsoPacketCount; Index++)
6262
{
63-
Urb->UrbIsochronousTransfer.IsoPacket[Index].Offset = Index * DeviceExtension->dwMaxPayloadTransferSize;
64-
Urb->UrbIsochronousTransfer.IsoPacket[Index].Length = DeviceExtension->dwMaxPayloadTransferSize;
63+
Urb->UrbIsochronousTransfer.IsoPacket[Index].Offset = Index * (DeviceExtension->dwMaxPayloadTransferSize);
6564
}
6665
/* setup next stack location */
6766
IoStack = IoGetNextIrpStackLocation(Irp);
@@ -109,16 +108,64 @@ USBVideoIsoReadComplete(
109108
return STATUS_MORE_PROCESSING_REQUIRED;
110109
}
111110
BytesReceived = 0;
111+
UCHAR FirstFID = 0xFF, NewFid = 0xFF;
112+
ULONG PacketFIDChange = -1;
113+
Data = (PUCHAR)Urb->UrbIsochronousTransfer.TransferBuffer;
112114

113115
for(Index = 0; Index < Urb->UrbIsochronousTransfer.NumberOfPackets; Index++)
114116
{
115117
//DPRINT1("Index %u Length %u\n", Index, Urb->UrbIsochronousTransfer.IsoPacket[Index].Length);
116118
BytesReceived += Urb->UrbIsochronousTransfer.IsoPacket[Index].Length;
119+
if (Urb->UrbIsochronousTransfer.IsoPacket[Index].Status != USBD_STATUS_SUCCESS)
120+
{
121+
DPRINT("Status %x failed for packet %u\n", Urb->UrbIsochronousTransfer.IsoPacket[Index].Status, Index);
122+
continue;
123+
}
124+
Offset = Urb->UrbIsochronousTransfer.IsoPacket[Index].Offset;
125+
Hdr = (PUVC_PAYLOAD_HEADER)(Data + Offset);
126+
HeaderLen = Hdr->bHeaderLength;
127+
/* validate header length */
128+
if (HeaderLen < 2 || HeaderLen > 12 || Offset + HeaderLen > BytesReceived)
129+
{
130+
DPRINT("Invalid packet atIso Index %u Offset %u HeaderLen %u BytesReceived %u\n", Index,
131+
Offset, HeaderLen, BytesReceived);
132+
continue;
133+
}
134+
if (FirstFID == 0xFF)
135+
{
136+
/* valid first packet */
137+
FirstFID = Hdr->FID;
138+
continue;
139+
}
140+
if (FirstFID != Hdr->FID)
141+
{
142+
if (PacketFIDChange == (ULONG)-1)
143+
{
144+
PUCHAR DataBuffer = (PUCHAR)(Data + Offset + HeaderLen);
145+
if (DataBuffer[0] == 0xFF && DataBuffer[1] == 0xD8)
146+
{
147+
/* fid changed at packet */
148+
PacketFIDChange = Index;
149+
NewFid = Hdr->FID;
150+
}
151+
}
152+
}
117153
}
118154

119-
Data = (PUCHAR)Urb->UrbIsochronousTransfer.TransferBuffer;
120155
Offset = 0;
121-
for(Index = 0; Index < Urb->UrbIsochronousTransfer.NumberOfPackets; Index++)
156+
if (PacketFIDChange != (ULONG)-1)
157+
{
158+
Index = PacketFIDChange;
159+
Frame->LastFid = NewFid;
160+
Frame->FrameSize = 0;
161+
Frame->FrameStarted = TRUE;
162+
//DPRINT1("FIDChange at Packet Index %u\n", Index);
163+
}
164+
else
165+
{
166+
Index = Urb->UrbIsochronousTransfer.NumberOfPackets + 1; // throw away packet
167+
}
168+
for(; Index < Urb->UrbIsochronousTransfer.NumberOfPackets; Index++)
122169
{
123170
if (Urb->UrbIsochronousTransfer.IsoPacket[Index].Status != USBD_STATUS_SUCCESS)
124171
{
@@ -136,19 +183,28 @@ USBVideoIsoReadComplete(
136183
Offset, HeaderLen, BytesReceived);
137184
continue;
138185
}
139-
#if 0
186+
187+
/* discard packet if EOH bit is not set */
188+
if (Hdr->EOH == 0) {
189+
DPRINT1("EOH bit not set at IsoPacket Index %u\n", Index);//, Value[0], Status);
190+
continue;
191+
}
192+
193+
/* discard packet if RES bit is set */
194+
if (Hdr->RES) {
195+
DPRINT1("RES bit set at IsoPacket Index %u\n", Index);//, Value[0], Status);
196+
continue;
197+
}
198+
140199
/* discard frame if err bit is set */
141200
if (Hdr->ERR) {
142-
//PUCHAR Value = AllocFunction(sizeof(UCHAR)*2);
143-
//NTSTATUS Status = USBVideoTransferControlPacket(Pin, Value, sizeof(UCHAR)*2, USBD_TRANSFER_DIRECTION_IN, 0x81, 0x0200, 0x00);
144201
DPRINT1("ERR bit set at IsoPacket Index %u\n", Index);//, Value[0], Status);
145202
Frame->FrameStarted = FALSE;
146203
Frame->FrameSize = 0;
147-
break;
204+
continue;
148205
}
149-
#endif
150206
/* check FID */
151-
if (Frame->FrameStarted && (Hdr->FID != Frame->LastFid || Hdr->EOF)) {
207+
if (Frame->FrameStarted && Frame->FrameSize > 0 && (Hdr->FID != Frame->LastFid)) {
152208
USBVideoDeliverFrame(DeviceExtension,
153209
Frame->FrameBuffer,
154210
Frame->FrameSize);
@@ -159,12 +215,13 @@ USBVideoIsoReadComplete(
159215

160216
/* copy payload */
161217
ULONG PayloadDataOffset = Offset + HeaderLen;
162-
ULONG PayloadDataLen = DeviceExtension->dwMaxPayloadTransferSize - HeaderLen;
163-
218+
ULONG Length = Urb->UrbIsochronousTransfer.IsoPacket[Index].Length;
219+
ULONG PayloadDataLen = Length - HeaderLen;
220+
#if 0
164221
/* validate payload length*/
165222
if (PayloadDataOffset + PayloadDataLen > BytesReceived)
166223
PayloadDataLen = BytesReceived - PayloadDataOffset;
167-
224+
#endif
168225
if (PayloadDataLen > 0 && Frame->FrameSize + PayloadDataLen <= Frame->MaxFrameSize) {
169226

170227
RtlCopyMemory(
@@ -177,6 +234,19 @@ USBVideoIsoReadComplete(
177234
Frame->FrameStarted = TRUE;
178235
}
179236

237+
/* check FID */
238+
if (Frame->FrameStarted && Frame->FrameSize > 2 && Hdr->EOF) {
239+
if (Frame->FrameBuffer[Frame->FrameSize-2] == 0xFF &&
240+
Frame->FrameBuffer[Frame->FrameSize-1] == 0xD9)
241+
{
242+
USBVideoDeliverFrame(DeviceExtension,
243+
Frame->FrameBuffer,
244+
Frame->FrameSize);
245+
Frame->FrameSize = 0;
246+
Frame->FrameStarted = FALSE;
247+
}
248+
}
249+
180250
if (Frame->FrameSize == Frame->MaxFrameSize)
181251
{
182252
DPRINT1("Buffer full discarding\n");

‎drivers/usb/usbvideo/pin.c‎

Lines changed: 45 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -200,13 +200,15 @@ USBVideoSetStreamingDefaults(
200200
}
201201
else
202202
{
203-
ULONG TransferSize = 650 * 1024; // FIXME determine
203+
ULONG TransferSize = DeviceExtension->dwMaxVideoFrameSize;
204+
ULONG Payload = (DeviceExtension->dwMaxPayloadTransferSize);
205+
TransferSize = ROUND_UP(TransferSize, Payload);
204206
DeviceExtension->IsoTransferSize = TransferSize;
205-
DeviceExtension->FrameContextCount = 1;
207+
DeviceExtension->FrameContextCount = 3;
206208
DeviceExtension->FrameContextSize = DeviceExtension->IsoTransferSize;
207-
DeviceExtension->UrbPoolCount = 1;
208-
DeviceExtension->IsoPacketCount = DeviceExtension->IsoTransferSize / DeviceExtension->dwMaxPayloadTransferSize;
209-
if (DeviceExtension->IsoTransferSize % DeviceExtension->dwMaxPayloadTransferSize != 0)
209+
DeviceExtension->UrbPoolCount = 3;
210+
DeviceExtension->IsoPacketCount = DeviceExtension->IsoTransferSize / Payload;
211+
if (DeviceExtension->IsoTransferSize % Payload != 0)
210212
{
211213
DeviceExtension->IsoPacketCount++;
212214
}
@@ -268,7 +270,7 @@ USBVideoSetStreamingFormat(
268270
{
269271
case 0x0100: // 1.0
270272
Length = 26;
271-
break;
273+
//break;
272274
case 0x0110: // 1.1
273275
Length = 34;
274276
break;
@@ -282,11 +284,12 @@ USBVideoSetStreamingFormat(
282284
break;
283285
}
284286
RtlZeroMemory(&ProbeCommit, sizeof(VS_PROBE_COMMIT_CONTROL));
285-
ProbeCommit.bmHint = 0x0001;
287+
ProbeCommit.bmHint = 0x09;
286288
ProbeCommit.bFormatIndex = FormatIndex;
287289
ProbeCommit.bFrameIndex = FrameIndex;
288290
ProbeCommit.dwFrameInterval = dwFrameInterval;
289-
//ProbeCommit.wCompQuality = 10000;
291+
ProbeCommit.wCompQuality = 10000;
292+
ProbeCommit.bmFramingInfo = 0x3;
290293

291294
UCHAR ProbeControl = 0x01;
292295
USHORT ProbeValue = 0x100;
@@ -306,8 +309,20 @@ USBVideoSetStreamingFormat(
306309
return Status;
307310
}
308311
/* todo check if format matches request */
312+
DPRINT1("bmHint %x\n", ProbeCommit.bmHint);
313+
DPRINT1("bFormatIndex %x\n", ProbeCommit.bFormatIndex);
314+
DPRINT1("bFrameIndex %x\n", ProbeCommit.bFrameIndex);
315+
DPRINT1("dwFrameInterval %x\n", ProbeCommit.dwFrameInterval);
316+
DPRINT1("wKeyFrameRate %x\n", ProbeCommit.wKeyFrameRate);
317+
DPRINT1("wPFrameRate %x\n", ProbeCommit.wPFrameRate);
318+
DPRINT1("wCompQuality %x\n", ProbeCommit.wCompQuality);
319+
DPRINT1("wCompWindowSize %x\n", ProbeCommit.wCompWindowSize);
320+
DPRINT1("wDelay %x\n", ProbeCommit.wDelay);
321+
DPRINT1("dwMaxVideoFrameSize %x\n", ProbeCommit.dwMaxVideoFrameSize);
309322
DPRINT1("dwMaxPayloadTransferSize %x\n", ProbeCommit.dwMaxPayloadTransferSize);
323+
DeviceExtension->dwMaxVideoFrameSize = ProbeCommit.dwMaxVideoFrameSize;
310324
DeviceExtension->dwMaxPayloadTransferSize = ProbeCommit.dwMaxPayloadTransferSize;
325+
311326
/* set format */
312327
UCHAR SetFormat = 0x01;
313328
USHORT CommitControl = 0x200;
@@ -352,6 +367,7 @@ USBVideoSetStreamingFormat(
352367
DeviceExtension->PipeType = Urb->UrbSelectInterface.Interface.Pipes[0].PipeType;
353368
DeviceExtension->hPipe = Urb->UrbSelectInterface.Interface.Pipes[0].PipeHandle;
354369
DeviceExtension->MaximumPacketSize = Urb->UrbSelectInterface.Interface.Pipes[0].MaximumPacketSize;
370+
DPRINT1("MaximumPacketSize %x\n", DeviceExtension->MaximumPacketSize);
355371
USBVideoSetStreamingDefaults(Pin, ConnectionFormat);
356372
DPRINT1("USBVideoSetFormat success\n");
357373
return Status;
@@ -696,7 +712,27 @@ USBVideoPinClose(
696712
FreeFunction(Urb);
697713
DPRINT1("USBVideoPinClose Status %x\n", Status);
698714

699-
/* todo cleanup irps, urbs, etc */
715+
for(ULONG i = 0; i < DeviceExtension->FrameContextCount; i++)
716+
{
717+
FreeFunction(DeviceExtension->FrameCtx[i].FrameBuffer);
718+
}
719+
720+
for (ULONG i = 0; i < DeviceExtension->UrbPoolCount; i++)
721+
{
722+
FreeFunction(DeviceExtension->Irp[i]);
723+
FreeFunction(DeviceExtension->Urb[i]);
724+
FreeFunction(DeviceExtension->Buffer[i]);
725+
}
726+
FreeFunction(DeviceExtension->FrameCtx);
727+
FreeFunction(DeviceExtension->Irp);
728+
FreeFunction(DeviceExtension->Urb);
729+
FreeFunction(DeviceExtension->Buffer);
730+
731+
DeviceExtension->FrameCtx = NULL;
732+
DeviceExtension->Urb = NULL;
733+
DeviceExtension->Irp = NULL;
734+
DeviceExtension->Buffer = NULL;
735+
700736
Irp->IoStatus.Status = Status;
701737
return Status;
702738
}

‎drivers/usb/usbvideo/usbvideo.c‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -279,6 +279,7 @@ USBVideoPnPStart(
279279
Device->Context = DeviceExtension;
280280
DeviceExtension->LowerDevice = Device->NextDeviceObject;
281281
KeInitializeEvent(&DeviceExtension->StoppedStreamingEvent, NotificationEvent, FALSE);
282+
KeInitializeSpinLock(&DeviceExtension->StreamingLock);
282283

283284
/* add to object bag*/
284285
KsAddItemToObjectBag(Device->Bag, Device->Context, ExFreePool);

‎drivers/usb/usbvideo/usbvideo.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,14 +27,12 @@ typedef struct _VS_PROBE_COMMIT_CONTROL {
2727
USHORT wDelay;
2828
ULONG dwMaxVideoFrameSize;
2929
ULONG dwMaxPayloadTransferSize;
30-
#if 0
3130
// UVC 1.1+
3231
DWORD dwClockFrequency;
3332
UCHAR bmFramingInfo;
3433
UCHAR bPreferedVersion;
3534
UCHAR bMinVersion;
3635
UCHAR bMaxVersion;
37-
#endif
3836
}VS_PROBE_COMMIT_CONTROL;
3937

4038
typedef struct _JFIF_APP0 {
@@ -308,6 +306,8 @@ typedef struct
308306

309307
#include <poppack.h>
310308

309+
#define ROUND_DOWN(n, align) (((ULONG)n) & ~((align) - 1l))
310+
#define ROUND_UP(n, align) ROUND_DOWN(((ULONG)n) + (align) - 1, (align))
311311

312312
typedef struct
313313
{
@@ -324,6 +324,7 @@ typedef struct
324324
USBD_PIPE_TYPE PipeType; /* pipe type */
325325
ULONG MaximumPacketSize; /* max packet size */
326326
PFRAME_CONTEXT FrameCtx;
327+
ULONG dwMaxVideoFrameSize; /* max video frame size */
327328
ULONG dwMaxPayloadTransferSize;
328329
PVIDEO_FORMAT_INFO VideoFormatInfo; /* video format lookup info */
329330
PUCHAR * Buffer; /* buffer array for irp queue */
@@ -333,7 +334,7 @@ typedef struct
333334
ULONG StopStreaming; /* stops streaming */
334335
ULONG StoppedStreamingIrps; /* stopped streaming irp count */
335336
KEVENT StoppedStreamingEvent; /* stopped streaming event */
336-
337+
KSPIN_LOCK StreamingLock;
337338
ULONG BulkTransferSize;
338339
ULONG IsoTransferSize;
339340
ULONG FrameContextCount;

‎drivers/usb/usbvideo/uvc.c‎

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -106,8 +106,8 @@ UvcPatchAvi1ToJfif(
106106
pFrame + restOffset,
107107
restSize);
108108
// mark header as done
109-
pOut[2 + sizeof(JFIF_APP0) + restSize + 1] = 0xFF;
110-
pOut[2 + sizeof(JFIF_APP0) + restSize + 2] = 0xD9;
109+
pOut[2 + sizeof(JFIF_APP0) + restSize] = 0xFF;
110+
pOut[2 + sizeof(JFIF_APP0) + restSize + 1] = 0xD9;
111111

112112
*pOutSize = outSize;
113113
return STATUS_SUCCESS;
@@ -123,6 +123,7 @@ USBVideoDeliverFrame(
123123
PKSSTREAM_POINTER StreamPointer;
124124
PKSPIN Pin;
125125
NTSTATUS Status;
126+
KIRQL OldLevel;
126127

127128
if (!DeviceExtension || !DeviceExtension->Pin)
128129
{
@@ -151,11 +152,16 @@ USBVideoDeliverFrame(
151152
ULONG BytesToCopy = min(StreamPointer->Offset->Remaining, FrameSize);
152153
if (BytesToCopy > 0)
153154
{
155+
ASSERT(BytesToCopy == FrameSize);
156+
ASSERT(FrameSize < StreamPointer->Offset->Remaining);
154157
ULONG Offset = StreamPointer->Offset->Count - StreamPointer->Offset->Remaining;
155158
DPRINT("USBVideoDeliverFrame: Copying %u bytes at offset %u user buffer (frame size %u)\n",
156159
BytesToCopy,
157160
Offset,
158161
FrameSize);
162+
163+
KeAcquireSpinLock(&DeviceExtension->StreamingLock, &OldLevel);
164+
159165
if (DeviceExtension->NeedFramePatching)
160166
{
161167
Status = UvcPatchAvi1ToJfif(FrameBuffer, BytesToCopy, StreamPointer->StreamHeader->Data, &BytesToCopy);
@@ -166,11 +172,12 @@ USBVideoDeliverFrame(
166172
Status = STATUS_SUCCESS;
167173
}
168174

175+
KeReleaseSpinLock(&DeviceExtension->StreamingLock, OldLevel);
176+
169177
if (NT_SUCCESS(Status))
170178
{
171179
/* Advance the stream pointer to deliver the buffer */
172-
KsStreamPointerAdvanceOffsetsAndUnlock(StreamPointer, 0, BytesToCopy, FALSE);
173-
KsStreamPointerUnlock(StreamPointer, FALSE);
180+
KsStreamPointerAdvanceOffsetsAndUnlock(StreamPointer, 0, BytesToCopy, TRUE);
174181
}
175182
else
176183
{

0 commit comments

Comments
 (0)