2 * PROJECT: ReactOS Kernel Streaming Mixer
3 * LICENSE: GPL - See COPYING in the top level directory
4 * FILE: drivers/wdm/audio/filters/kmixer/kmixer.c
5 * PURPOSE: Pin functions
6 * PROGRAMMERS: Johannes Anderwald (janderwald@reactos.org)
11 const GUID KSPROPSETID_Connection
= {0x1D58C920L
, 0xAC9B, 0x11CF, {0xA5, 0xD6, 0x28, 0xDB, 0x04, 0xC1, 0x00, 0x00}};
14 PerformSampleRateConversion(
24 KFLOATING_SAVE FloatSave
;
31 PFLOAT FloatIn
, FloatOut
;
35 DPRINT("PerformSampleRateConversion OldRate %u NewRate %u BytesPerSample %u NumChannels %u Irql %u\n", OldRate
, NewRate
, BytesPerSample
, NumChannels
, KeGetCurrentIrql());
37 ASSERT(BytesPerSample
== 1 || BytesPerSample
== 2 || BytesPerSample
== 4);
39 /* first acquire float save context */
40 Status
= KeSaveFloatingPointState(&FloatSave
);
42 if (!NT_SUCCESS(Status
))
44 DPRINT1("KeSaveFloatingPointState failed with %x\n", Status
);
48 NumSamples
= BufferLength
/ (BytesPerSample
* NumChannels
);
50 FloatIn
= ExAllocatePool(NonPagedPool
, NumSamples
* NumChannels
* sizeof(FLOAT
));
53 KeRestoreFloatingPointState(&FloatSave
);
54 return STATUS_INSUFFICIENT_RESOURCES
;
57 NewSamples
= lrintf(((FLOAT
)NumSamples
* ((FLOAT
)NewRate
/ (FLOAT
)OldRate
))) + 2;
59 FloatOut
= ExAllocatePool(NonPagedPool
, NewSamples
* NumChannels
* sizeof(FLOAT
));
63 KeRestoreFloatingPointState(&FloatSave
);
64 return STATUS_INSUFFICIENT_RESOURCES
;
67 ResultOut
= ExAllocatePool(NonPagedPool
, NewSamples
* NumChannels
* BytesPerSample
);
72 KeRestoreFloatingPointState(&FloatSave
);
73 return STATUS_INSUFFICIENT_RESOURCES
;
76 State
= src_new(SRC_SINC_FASTEST
, NumChannels
, &error
);
79 DPRINT1("KeSaveFloatingPointState failed with %x\n", Status
);
80 KeRestoreFloatingPointState(&FloatSave
);
83 ExFreePool(ResultOut
);
84 return STATUS_UNSUCCESSFUL
;
88 if (BytesPerSample
== 1)
90 for(Index
= 0; Index
< NumSamples
* NumChannels
; Index
++)
91 FloatIn
[Index
] = (float)(Buffer
[Index
] / (1.0 * 0x80));
93 else if (BytesPerSample
== 2)
95 src_short_to_float_array((short*)Buffer
, FloatIn
, NumSamples
* NumChannels
);
97 else if (BytesPerSample
== 4)
99 src_int_to_float_array((int*)Buffer
, FloatIn
, NumSamples
* NumChannels
);
102 Data
.data_in
= FloatIn
;
103 Data
.data_out
= FloatOut
;
104 Data
.input_frames
= NumSamples
;
105 Data
.output_frames
= NewSamples
;
106 Data
.src_ratio
= (double)NewRate
/ (double)OldRate
;
108 error
= src_process(State
, &Data
);
111 DPRINT1("src_process failed with %x\n", error
);
112 KeRestoreFloatingPointState(&FloatSave
);
114 ExFreePool(FloatOut
);
115 ExFreePool(ResultOut
);
116 return STATUS_UNSUCCESSFUL
;
119 if (BytesPerSample
== 1)
121 /* FIXME perform over/under clipping */
123 for(Index
= 0; Index
< Data
.output_frames_gen
* NumChannels
; Index
++)
124 ResultOut
[Index
] = (lrintf(FloatOut
[Index
]) >> 24);
126 else if (BytesPerSample
== 2)
128 PUSHORT Res
= (PUSHORT
)ResultOut
;
130 src_float_to_short_array(FloatOut
, (short*)Res
, Data
.output_frames_gen
* NumChannels
);
132 else if (BytesPerSample
== 4)
134 PULONG Res
= (PULONG
)ResultOut
;
136 src_float_to_int_array(FloatOut
, (int*)Res
, Data
.output_frames_gen
* NumChannels
);
141 *ResultLength
= Data
.output_frames_gen
* BytesPerSample
* NumChannels
;
143 ExFreePool(FloatOut
);
145 KeRestoreFloatingPointState(&FloatSave
);
146 return STATUS_SUCCESS
;
150 PerformChannelConversion(
160 ULONG NewIndex
, OldIndex
;
162 Samples
= BufferLength
/ (BitsPerSample
/ 8) / OldChannels
;
164 if (NewChannels
> OldChannels
)
166 if (BitsPerSample
== 8)
168 PUCHAR BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* NewChannels
);
170 return STATUS_INSUFFICIENT_RESOURCES
;
172 for(NewIndex
= 0, OldIndex
= 0; OldIndex
< Samples
* OldChannels
; NewIndex
+= NewChannels
, OldIndex
+= OldChannels
)
176 RtlMoveMemory(&BufferOut
[NewIndex
], &Buffer
[OldIndex
], OldChannels
* sizeof(UCHAR
));
180 /* 2 channel stretched to 4 looks like LRLR */
181 BufferOut
[NewIndex
+OldChannels
+ SubIndex
] = Buffer
[OldIndex
+ (SubIndex
% OldChannels
)];
182 }while(SubIndex
++ < NewChannels
- OldChannels
);
185 *ResultLength
= Samples
* NewChannels
;
187 else if (BitsPerSample
== 16)
189 PUSHORT BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* NewChannels
);
191 return STATUS_INSUFFICIENT_RESOURCES
;
193 for(NewIndex
= 0, OldIndex
= 0; OldIndex
< Samples
* OldChannels
; NewIndex
+= NewChannels
, OldIndex
+= OldChannels
)
197 RtlMoveMemory(&BufferOut
[NewIndex
], &Buffer
[OldIndex
], OldChannels
* sizeof(USHORT
));
201 BufferOut
[NewIndex
+OldChannels
+ SubIndex
] = Buffer
[OldIndex
+ (SubIndex
% OldChannels
)];
202 }while(SubIndex
++ < NewChannels
- OldChannels
);
205 *ResultLength
= Samples
* NewChannels
;
207 else if (BitsPerSample
== 24)
209 PUCHAR BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* NewChannels
);
211 return STATUS_INSUFFICIENT_RESOURCES
;
213 for(NewIndex
= 0, OldIndex
= 0; OldIndex
< Samples
* OldChannels
; NewIndex
+= NewChannels
, OldIndex
+= OldChannels
)
217 RtlMoveMemory(&BufferOut
[NewIndex
], &Buffer
[OldIndex
], OldChannels
* 3);
221 RtlMoveMemory(&BufferOut
[(NewIndex
+OldChannels
+ SubIndex
) * 3], &Buffer
[(OldIndex
+ (SubIndex
% OldChannels
)) * 3], 3);
222 }while(SubIndex
++ < NewChannels
- OldChannels
);
225 *ResultLength
= Samples
* NewChannels
;
227 else if (BitsPerSample
== 32)
229 PULONG BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* NewChannels
);
231 return STATUS_INSUFFICIENT_RESOURCES
;
233 for(NewIndex
= 0, OldIndex
= 0; OldIndex
< Samples
* OldChannels
; NewIndex
+= NewChannels
, OldIndex
+= OldChannels
)
237 RtlMoveMemory(&BufferOut
[NewIndex
], &Buffer
[OldIndex
], OldChannels
* sizeof(ULONG
));
241 BufferOut
[NewIndex
+OldChannels
+ SubIndex
] = Buffer
[OldIndex
+ (SubIndex
% OldChannels
)];
242 }while(SubIndex
++ < NewChannels
- OldChannels
);
245 *ResultLength
= Samples
* NewChannels
;
251 PUSHORT BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* NewChannels
);
253 return STATUS_INSUFFICIENT_RESOURCES
;
255 for(NewIndex
= 0, OldIndex
= 0; OldIndex
< Samples
* OldChannels
; NewIndex
+= NewChannels
, OldIndex
+= OldChannels
)
258 * mix stream instead of just dumping part of it ;)
260 RtlMoveMemory(&BufferOut
[NewIndex
], &Buffer
[OldIndex
], NewChannels
* (BitsPerSample
/8));
264 *ResultLength
= Samples
* NewChannels
;
266 return STATUS_SUCCESS
;
271 PerformQualityConversion(
282 ASSERT(OldWidth
!= NewWidth
);
284 Samples
= BufferLength
/ (OldWidth
/ 8);
285 //DPRINT("Samples %u BufferLength %u\n", Samples, BufferLength);
287 if (OldWidth
== 8 && NewWidth
== 16)
290 PUSHORT BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(USHORT
));
292 return STATUS_INSUFFICIENT_RESOURCES
;
294 for(Index
= 0; Index
< Samples
; Index
++)
296 Sample
= Buffer
[Index
];
299 Sample
= _byteswap_ushort(Sample
);
301 BufferOut
[Index
] = Sample
;
304 *ResultLength
= Samples
* sizeof(USHORT
);
306 else if (OldWidth
== 8 && NewWidth
== 32)
309 PULONG BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(ULONG
));
311 return STATUS_INSUFFICIENT_RESOURCES
;
313 for(Index
= 0; Index
< Samples
; Index
++)
315 Sample
= Buffer
[Index
];
318 Sample
= _byteswap_ulong(Sample
);
320 BufferOut
[Index
] = Sample
;
323 *ResultLength
= Samples
* sizeof(ULONG
);
325 else if (OldWidth
== 16 && NewWidth
== 32)
328 PUSHORT BufferIn
= (PUSHORT
)Buffer
;
329 PULONG BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(ULONG
));
331 return STATUS_INSUFFICIENT_RESOURCES
;
333 for(Index
= 0; Index
< Samples
; Index
++)
335 Sample
= BufferIn
[Index
];
338 Sample
= _byteswap_ulong(Sample
);
340 BufferOut
[Index
] = Sample
;
343 *ResultLength
= Samples
* sizeof(ULONG
);
346 else if (OldWidth
== 16 && NewWidth
== 8)
349 PUSHORT BufferIn
= (PUSHORT
)Buffer
;
350 PUCHAR BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(UCHAR
));
352 return STATUS_INSUFFICIENT_RESOURCES
;
354 for(Index
= 0; Index
< Samples
; Index
++)
356 Sample
= BufferIn
[Index
];
358 Sample
= _byteswap_ushort(Sample
);
361 BufferOut
[Index
] = (Sample
& 0xFF);
364 *ResultLength
= Samples
* sizeof(UCHAR
);
366 else if (OldWidth
== 32 && NewWidth
== 8)
369 PULONG BufferIn
= (PULONG
)Buffer
;
370 PUCHAR BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(UCHAR
));
372 return STATUS_INSUFFICIENT_RESOURCES
;
374 for(Index
= 0; Index
< Samples
; Index
++)
376 Sample
= BufferIn
[Index
];
378 Sample
= _byteswap_ulong(Sample
);
381 BufferOut
[Index
] = (Sample
& 0xFF);
384 *ResultLength
= Samples
* sizeof(UCHAR
);
386 else if (OldWidth
== 32 && NewWidth
== 16)
389 PULONG BufferIn
= (PULONG
)Buffer
;
390 PUSHORT BufferOut
= ExAllocatePool(NonPagedPool
, Samples
* sizeof(USHORT
));
392 return STATUS_INSUFFICIENT_RESOURCES
;
394 for(Index
= 0; Index
< Samples
; Index
++)
396 Sample
= BufferIn
[Index
];
398 Sample
= _byteswap_ulong(Sample
);
401 BufferOut
[Index
] = (Sample
& 0xFFFF);
404 *ResultLength
= Samples
* sizeof(USHORT
);
408 DPRINT1("Not implemented conversion OldWidth %u NewWidth %u\n", OldWidth
, NewWidth
);
409 return STATUS_NOT_IMPLEMENTED
;
412 return STATUS_SUCCESS
;
418 Pin_fnDeviceIoControl(
419 PDEVICE_OBJECT DeviceObject
,
422 PIO_STACK_LOCATION IoStack
;
424 //DPRINT1("Pin_fnDeviceIoControl called DeviceObject %p Irp %p\n", DeviceObject);
426 IoStack
= IoGetCurrentIrpStackLocation(Irp
);
428 if (IoStack
->Parameters
.DeviceIoControl
.InputBufferLength
== sizeof(KSP_PIN
) && IoStack
->Parameters
.DeviceIoControl
.OutputBufferLength
== sizeof(KSDATAFORMAT_WAVEFORMATEX
))
430 Property
= (PKSP_PIN
)IoStack
->Parameters
.DeviceIoControl
.Type3InputBuffer
;
432 if (IsEqualGUIDAligned(&Property
->Property
.Set
, &KSPROPSETID_Connection
))
434 if (Property
->Property
.Id
== KSPROPERTY_CONNECTION_DATAFORMAT
&& Property
->Property
.Flags
== KSPROPERTY_TYPE_SET
)
436 PKSDATAFORMAT_WAVEFORMATEX Formats
;
437 PKSDATAFORMAT_WAVEFORMATEX WaveFormat
;
439 Formats
= (PKSDATAFORMAT_WAVEFORMATEX
)IoStack
->FileObject
->FsContext2
;
440 WaveFormat
= (PKSDATAFORMAT_WAVEFORMATEX
)Irp
->UserBuffer
;
442 ASSERT(Property
->PinId
== 0 || Property
->PinId
== 1);
446 Formats
[Property
->PinId
].WaveFormatEx
.nChannels
= WaveFormat
->WaveFormatEx
.nChannels
;
447 Formats
[Property
->PinId
].WaveFormatEx
.wBitsPerSample
= WaveFormat
->WaveFormatEx
.wBitsPerSample
;
448 Formats
[Property
->PinId
].WaveFormatEx
.nSamplesPerSec
= WaveFormat
->WaveFormatEx
.nSamplesPerSec
;
450 Irp
->IoStatus
.Information
= 0;
451 Irp
->IoStatus
.Status
= STATUS_SUCCESS
;
452 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
453 return STATUS_SUCCESS
;
457 DPRINT1("Size %u Expected %u\n",IoStack
->Parameters
.DeviceIoControl
.OutputBufferLength
, sizeof(KSDATAFORMAT_WAVEFORMATEX
));
458 Irp
->IoStatus
.Information
= 0;
459 Irp
->IoStatus
.Status
= STATUS_UNSUCCESSFUL
;
460 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
461 return STATUS_UNSUCCESSFUL
;
467 PDEVICE_OBJECT DeviceObject
,
472 Irp
->IoStatus
.Status
= STATUS_UNSUCCESSFUL
;
473 Irp
->IoStatus
.Information
= 0;
474 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
475 return STATUS_UNSUCCESSFUL
;
481 PDEVICE_OBJECT DeviceObject
,
486 Irp
->IoStatus
.Information
= 0;
487 Irp
->IoStatus
.Status
= STATUS_SUCCESS
;
488 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
489 return STATUS_SUCCESS
;
495 PDEVICE_OBJECT DeviceObject
,
500 Irp
->IoStatus
.Status
= STATUS_UNSUCCESSFUL
;
501 Irp
->IoStatus
.Information
= 0;
502 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
503 return STATUS_UNSUCCESSFUL
;
509 PDEVICE_OBJECT DeviceObject
,
514 Irp
->IoStatus
.Status
= STATUS_SUCCESS
;
515 Irp
->IoStatus
.Information
= 0;
516 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
517 return STATUS_SUCCESS
;
523 PDEVICE_OBJECT DeviceObject
,
528 Irp
->IoStatus
.Status
= STATUS_UNSUCCESSFUL
;
529 Irp
->IoStatus
.Information
= 0;
530 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
531 return STATUS_UNSUCCESSFUL
;
537 PDEVICE_OBJECT DeviceObject
,
543 Irp
->IoStatus
.Status
= STATUS_UNSUCCESSFUL
;
544 Irp
->IoStatus
.Information
= 0;
545 IoCompleteRequest(Irp
, IO_NO_INCREMENT
);
546 return STATUS_UNSUCCESSFUL
;
551 Pin_fnFastDeviceIoControl(
552 PFILE_OBJECT FileObject
,
555 ULONG InputBufferLength
,
557 ULONG OutputBufferLength
,
559 PIO_STATUS_BLOCK IoStatus
,
560 PDEVICE_OBJECT DeviceObject
)
572 PFILE_OBJECT FileObject
,
573 PLARGE_INTEGER FileOffset
,
578 PIO_STATUS_BLOCK IoStatus
,
579 PDEVICE_OBJECT DeviceObject
)
589 PFILE_OBJECT FileObject
,
590 PLARGE_INTEGER FileOffset
,
595 PIO_STATUS_BLOCK IoStatus
,
596 PDEVICE_OBJECT DeviceObject
)
598 PKSSTREAM_HEADER StreamHeader
;
601 NTSTATUS Status
= STATUS_SUCCESS
;
602 PKSDATAFORMAT_WAVEFORMATEX Formats
;
603 PKSDATAFORMAT_WAVEFORMATEX InputFormat
, OutputFormat
;
605 DPRINT("Pin_fnFastWrite called DeviceObject %p Irp %p\n", DeviceObject
);
607 Formats
= (PKSDATAFORMAT_WAVEFORMATEX
)FileObject
->FsContext2
;
609 InputFormat
= Formats
;
610 OutputFormat
= (Formats
+ 1);
611 StreamHeader
= (PKSSTREAM_HEADER
)Buffer
;
614 DPRINT("Num Channels %u Old Channels %u\n SampleRate %u Old SampleRate %u\n BitsPerSample %u Old BitsPerSample %u\n",
615 InputFormat
->WaveFormatEx
.nChannels
, OutputFormat
->WaveFormatEx
.nChannels
,
616 InputFormat
->WaveFormatEx
.nSamplesPerSec
, OutputFormat
->WaveFormatEx
.nSamplesPerSec
,
617 InputFormat
->WaveFormatEx
.wBitsPerSample
, OutputFormat
->WaveFormatEx
.wBitsPerSample
);
619 if (InputFormat
->WaveFormatEx
.wBitsPerSample
!= OutputFormat
->WaveFormatEx
.wBitsPerSample
)
621 Status
= PerformQualityConversion(StreamHeader
->Data
,
622 StreamHeader
->DataUsed
,
623 InputFormat
->WaveFormatEx
.wBitsPerSample
,
624 OutputFormat
->WaveFormatEx
.wBitsPerSample
,
627 if (NT_SUCCESS(Status
))
629 ExFreePool(StreamHeader
->Data
);
630 StreamHeader
->Data
= BufferOut
;
631 StreamHeader
->DataUsed
= BufferLength
;
635 if (InputFormat
->WaveFormatEx
.nChannels
!= OutputFormat
->WaveFormatEx
.nChannels
)
637 Status
= PerformChannelConversion(StreamHeader
->Data
,
638 StreamHeader
->DataUsed
,
639 InputFormat
->WaveFormatEx
.nChannels
,
640 OutputFormat
->WaveFormatEx
.nChannels
,
641 OutputFormat
->WaveFormatEx
.wBitsPerSample
,
645 if (NT_SUCCESS(Status
))
647 ExFreePool(StreamHeader
->Data
);
648 StreamHeader
->Data
= BufferOut
;
649 StreamHeader
->DataUsed
= BufferLength
;
653 if (InputFormat
->WaveFormatEx
.nSamplesPerSec
!= OutputFormat
->WaveFormatEx
.nSamplesPerSec
)
655 Status
= PerformSampleRateConversion(StreamHeader
->Data
,
656 StreamHeader
->DataUsed
,
657 InputFormat
->WaveFormatEx
.nSamplesPerSec
,
658 OutputFormat
->WaveFormatEx
.nSamplesPerSec
,
659 OutputFormat
->WaveFormatEx
.wBitsPerSample
/ 8,
660 OutputFormat
->WaveFormatEx
.nChannels
,
663 if (NT_SUCCESS(Status
))
665 ExFreePool(StreamHeader
->Data
);
666 StreamHeader
->Data
= BufferOut
;
667 StreamHeader
->DataUsed
= BufferLength
;
671 IoStatus
->Status
= Status
;
673 if (NT_SUCCESS(Status
))
679 static KSDISPATCH_TABLE PinTable
=
681 Pin_fnDeviceIoControl
,
688 Pin_fnFastDeviceIoControl
,
698 KSOBJECT_HEADER ObjectHeader
;
699 PKSDATAFORMAT DataFormat
;
700 PIO_STACK_LOCATION IoStack
;
703 DataFormat
= ExAllocatePool(NonPagedPool
, sizeof(KSDATAFORMAT_WAVEFORMATEX
) * 2);
705 return STATUS_INSUFFICIENT_RESOURCES
;
707 RtlZeroMemory(DataFormat
, sizeof(KSDATAFORMAT_WAVEFORMATEX
) * 2);
709 IoStack
= IoGetCurrentIrpStackLocation(Irp
);
710 IoStack
->FileObject
->FsContext2
= (PVOID
)DataFormat
;
712 /* allocate object header */
713 Status
= KsAllocateObjectHeader(&ObjectHeader
, 0, NULL
, Irp
, &PinTable
);
717 void * calloc(size_t Elements
, size_t ElementSize
)
720 PUCHAR Block
= ExAllocatePool(NonPagedPool
, Elements
* ElementSize
);
724 for(Index
= 0; Index
< Elements
* ElementSize
; Index
++)
730 void free(PVOID Block
)
741 PUCHAR Block
= (PUCHAR
)dest
;
743 for(Index
= 0; Index
< count
; Index
++)
755 PUCHAR Src
= (PUCHAR
)src
, Dest
= (PUCHAR
)dest
;
757 for(Index
= 0; Index
< count
; Index
++)
758 Dest
[Index
] = Src
[Index
];