From 576ad34f955323058a067f3f6e04da94313cc4ad Mon Sep 17 00:00:00 2001 From: Daniel Hammer Date: Tue, 10 Mar 2026 17:14:33 +0100 Subject: [PATCH] feat: Intel HDA audio driver, audio streaming syscalls, userspace Music app, fixes and improvements, rudimentary Bluetooth support --- kernel/src/Api/Audio.hpp | 90 + kernel/src/Api/BluetoothSyscall.hpp | 104 + kernel/src/Api/Device.hpp | 122 +- kernel/src/Api/Syscall.cpp | 20 + kernel/src/Api/Syscall.hpp | 54 +- kernel/src/Drivers/Audio/IntelHda.cpp | 1150 ++++++++++ kernel/src/Drivers/Audio/IntelHda.hpp | 284 +++ kernel/src/Drivers/Init.cpp | 42 + kernel/src/Drivers/Init.hpp | 6 +- kernel/src/Drivers/Storage/Nvme.cpp | 829 ++++++++ kernel/src/Drivers/Storage/Nvme.hpp | 193 ++ kernel/src/Drivers/USB/Bluetooth/A2dp.cpp | 500 +++++ kernel/src/Drivers/USB/Bluetooth/A2dp.hpp | 60 + .../src/Drivers/USB/Bluetooth/Bluetooth.cpp | 329 +++ .../src/Drivers/USB/Bluetooth/Bluetooth.hpp | 39 + kernel/src/Drivers/USB/Bluetooth/Hci.cpp | 818 ++++++++ kernel/src/Drivers/USB/Bluetooth/Hci.hpp | 251 +++ kernel/src/Drivers/USB/Bluetooth/L2cap.cpp | 422 ++++ kernel/src/Drivers/USB/Bluetooth/L2cap.hpp | 111 + kernel/src/Drivers/USB/Bluetooth/Sbc.cpp | 416 ++++ kernel/src/Drivers/USB/Bluetooth/Sbc.hpp | 90 + kernel/src/Drivers/USB/UsbDevice.cpp | 243 ++- kernel/src/Drivers/USB/UsbDevice.hpp | 8 + kernel/src/Drivers/USB/Xhci.cpp | 198 +- kernel/src/Drivers/USB/Xhci.hpp | 34 + kernel/src/Fs/Ext2.cpp | 24 +- kernel/src/Fs/Fat32.cpp | 15 +- kernel/src/Main.cpp | 1 + programs/GNUmakefile | 23 +- programs/include/Api/Syscall.hpp | 54 +- programs/include/gui/desktop.hpp | 7 + programs/include/libc/string.h | 1 + programs/include/montauk/syscall.h | 55 + programs/lib/libc/libc.c | 10 + programs/lib/libc/obj/libc.o | Bin 102208 -> 103296 bytes programs/src/bluetooth/Makefile | 86 + programs/src/bluetooth/main.cpp | 670 ++++++ programs/src/bluetooth/manifest.toml | 8 + programs/src/bluetooth/stb_truetype_impl.cpp | 35 + programs/src/desktop/apps/app_filemanager.cpp | 103 +- programs/src/desktop/main.cpp | 23 + programs/src/desktop/window.cpp | 7 +- programs/src/disks/actions.cpp | 65 +- programs/src/disks/disks.h | 19 + programs/src/disks/main.cpp | 71 +- programs/src/disks/render.cpp | 55 + programs/src/music/Makefile | 87 + programs/src/music/main.cpp | 1147 ++++++++++ programs/src/music/manifest.toml | 8 + programs/src/music/minimp3.h | 1865 +++++++++++++++++ programs/src/music/minimp3_impl.cpp | 16 + programs/src/music/stb_truetype_impl.cpp | 35 + programs/src/volume/Makefile | 86 + programs/src/volume/main.cpp | 368 ++++ programs/src/volume/manifest.toml | 8 + programs/src/volume/stb_truetype_impl.cpp | 35 + scripts/copy_icons.sh | 8 + scripts/install_apps.sh | 4 + 58 files changed, 11275 insertions(+), 137 deletions(-) create mode 100644 kernel/src/Api/Audio.hpp create mode 100644 kernel/src/Api/BluetoothSyscall.hpp create mode 100644 kernel/src/Drivers/Audio/IntelHda.cpp create mode 100644 kernel/src/Drivers/Audio/IntelHda.hpp create mode 100644 kernel/src/Drivers/Storage/Nvme.cpp create mode 100644 kernel/src/Drivers/Storage/Nvme.hpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/A2dp.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/A2dp.hpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Hci.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Hci.hpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/L2cap.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/L2cap.hpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Sbc.cpp create mode 100644 kernel/src/Drivers/USB/Bluetooth/Sbc.hpp create mode 100644 programs/src/bluetooth/Makefile create mode 100644 programs/src/bluetooth/main.cpp create mode 100644 programs/src/bluetooth/manifest.toml create mode 100644 programs/src/bluetooth/stb_truetype_impl.cpp create mode 100644 programs/src/music/Makefile create mode 100644 programs/src/music/main.cpp create mode 100644 programs/src/music/manifest.toml create mode 100644 programs/src/music/minimp3.h create mode 100644 programs/src/music/minimp3_impl.cpp create mode 100644 programs/src/music/stb_truetype_impl.cpp create mode 100644 programs/src/volume/Makefile create mode 100644 programs/src/volume/main.cpp create mode 100644 programs/src/volume/manifest.toml create mode 100644 programs/src/volume/stb_truetype_impl.cpp diff --git a/kernel/src/Api/Audio.hpp b/kernel/src/Api/Audio.hpp new file mode 100644 index 0000000..7d3eadb --- /dev/null +++ b/kernel/src/Api/Audio.hpp @@ -0,0 +1,90 @@ +/* + * Audio.hpp + * Audio syscall implementations + * Routes audio to Intel HDA or Bluetooth A2DP based on handle + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include +#include +#include +#include + +#include "Syscall.hpp" + +namespace Montauk { + + // Audio handle convention: + // 0x00 - 0x0F : Intel HDA handles + // 0x100 : Bluetooth A2DP audio output + + static constexpr int AUDIO_HANDLE_BT = 0x100; + + static int64_t Sys_AudioOpen(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { + // If HDA is available, use it (primary output) + if (Drivers::Audio::IntelHda::IsInitialized()) { + return (int64_t)Drivers::Audio::IntelHda::Open(sampleRate, channels, bitsPerSample); + } + + // Fallback: try Bluetooth audio if available + if (Drivers::USB::Bluetooth::IsInitialized()) { + auto state = Drivers::USB::Bluetooth::A2dp::GetState(); + if (state == Drivers::USB::Bluetooth::A2dp::State::Open || + state == Drivers::USB::Bluetooth::A2dp::State::Streaming || + state == Drivers::USB::Bluetooth::A2dp::State::Configured) { + Drivers::USB::Bluetooth::A2dp::ConfigureStream(sampleRate, channels, bitsPerSample); + Drivers::USB::Bluetooth::A2dp::StartStream(); + return AUDIO_HANDLE_BT; + } + } + + return -1; + } + + static int64_t Sys_AudioClose(int handle) { + if (handle == AUDIO_HANDLE_BT) { + Drivers::USB::Bluetooth::A2dp::StopStream(); + return 0; + } + Drivers::Audio::IntelHda::Close(handle); + return 0; + } + + static int64_t Sys_AudioWrite(int handle, const uint8_t* data, uint32_t size) { + if (handle == AUDIO_HANDLE_BT) { + return (int64_t)Drivers::USB::Bluetooth::A2dp::WriteAudio(data, size); + } + return (int64_t)Drivers::Audio::IntelHda::Write(handle, data, size); + } + + static int64_t Sys_AudioCtl(int handle, int cmd, int value) { + if (handle == AUDIO_HANDLE_BT) { + switch (cmd) { + case AUDIO_CTL_SET_VOLUME: + Drivers::USB::Bluetooth::A2dp::SetVolume(value); + return 0; + case AUDIO_CTL_GET_VOLUME: + return Drivers::USB::Bluetooth::A2dp::GetVolume(); + case AUDIO_CTL_PAUSE: + if (value) Drivers::USB::Bluetooth::A2dp::StopStream(); + else Drivers::USB::Bluetooth::A2dp::StartStream(); + return 0; + case AUDIO_CTL_GET_OUTPUT: + return 1; // Bluetooth + default: + return -1; + } + } + + // Additional control commands for audio routing + if (cmd == AUDIO_CTL_GET_OUTPUT) return 0; // HDA + if (cmd == AUDIO_CTL_BT_STATUS) { + if (!Drivers::USB::Bluetooth::IsInitialized()) return 0; + return (int64_t)Drivers::USB::Bluetooth::A2dp::GetState(); + } + + return (int64_t)Drivers::Audio::IntelHda::Control(handle, cmd, value); + } + +}; diff --git a/kernel/src/Api/BluetoothSyscall.hpp b/kernel/src/Api/BluetoothSyscall.hpp new file mode 100644 index 0000000..b9f28a9 --- /dev/null +++ b/kernel/src/Api/BluetoothSyscall.hpp @@ -0,0 +1,104 @@ +/* + * BluetoothSyscall.hpp + * SYS_BTSCAN, SYS_BTCONNECT, SYS_BTDISCONNECT, SYS_BTLIST, SYS_BTINFO + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include +#include +#include +#include + +#include "Syscall.hpp" + +namespace Montauk { + + static int64_t Sys_BtScan(BtScanResult* buf, int maxCount, uint32_t timeoutMs) { + if (!buf || maxCount <= 0) return -1; + if (!Drivers::USB::Bluetooth::IsInitialized()) return -1; + + // Use a stack buffer for kernel-side results (max 16) + Drivers::USB::Bluetooth::Hci::InquiryDevice tmpBuf[16]; + int kMax = maxCount; + if (kMax > 16) kMax = 16; + + int found = Drivers::USB::Bluetooth::Scan(tmpBuf, kMax, timeoutMs); + if (found < 0) return found; + + // Convert kernel InquiryDevice to userspace BtScanResult + for (int i = 0; i < found; i++) { + memcpy(buf[i].bdAddr, tmpBuf[i].BdAddr, 6); + buf[i]._pad[0] = 0; + buf[i]._pad[1] = 0; + buf[i].classOfDevice = tmpBuf[i].ClassOfDevice; + buf[i].rssi = tmpBuf[i].Rssi; + buf[i]._pad2[0] = 0; + buf[i]._pad2[1] = 0; + buf[i]._pad2[2] = 0; + memcpy(buf[i].name, tmpBuf[i].Name, 64); + } + + return (int64_t)found; + } + + static int64_t Sys_BtConnect(const uint8_t* bdAddr) { + if (!bdAddr) return -1; + if (!Drivers::USB::Bluetooth::IsInitialized()) return -1; + + return (int64_t)Drivers::USB::Bluetooth::Connect(bdAddr); + } + + static int64_t Sys_BtDisconnect(const uint8_t* bdAddr) { + if (!bdAddr) return -1; + if (!Drivers::USB::Bluetooth::IsInitialized()) return -1; + + return (int64_t)Drivers::USB::Bluetooth::Disconnect(bdAddr); + } + + static int64_t Sys_BtList(BtDevInfo* buf, int maxCount) { + if (!buf || maxCount <= 0) return 0; + if (!Drivers::USB::Bluetooth::IsInitialized()) return 0; + + Drivers::USB::Bluetooth::Hci::ConnectionInfo tmpBuf[4]; + int kMax = maxCount; + if (kMax > 4) kMax = 4; + + int count = Drivers::USB::Bluetooth::ListConnected(tmpBuf, kMax); + + for (int i = 0; i < count; i++) { + memcpy(buf[i].bdAddr, tmpBuf[i].BdAddr, 6); + buf[i].connected = tmpBuf[i].Active ? 1 : 0; + buf[i].encrypted = tmpBuf[i].Encrypted ? 1 : 0; + buf[i].handle = tmpBuf[i].Handle; + buf[i].linkType = tmpBuf[i].LinkType; + buf[i]._pad = 0; + } + + return (int64_t)count; + } + + static int64_t Sys_BtInfo(BtAdapterInfo* buf) { + if (!buf) return -1; + + memset(buf, 0, sizeof(BtAdapterInfo)); + + if (!Drivers::USB::Bluetooth::IsInitialized()) { + buf->initialized = 0; + return -1; + } + + buf->initialized = 1; + memcpy(buf->bdAddr, Drivers::USB::Bluetooth::GetBdAddr(), 6); + buf->scanning = Drivers::USB::Bluetooth::Hci::IsInquiryActive() ? 1 : 0; + + // Copy adapter name + const char* name = "MontaukOS"; + int i = 0; + for (; i < 63 && name[i]; i++) buf->name[i] = name[i]; + buf->name[i] = '\0'; + + return 0; + } + +} diff --git a/kernel/src/Api/Device.hpp b/kernel/src/Api/Device.hpp index 0d5bfa8..e51d011 100644 --- a/kernel/src/Api/Device.hpp +++ b/kernel/src/Api/Device.hpp @@ -9,13 +9,18 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include "Syscall.hpp" +#include +#include namespace Montauk { @@ -92,10 +97,12 @@ namespace Montauk { auto* dev = Drivers::USB::Xhci::GetDevice(slot); if (!dev || !dev->Active) continue; const char* devName = "USB Device"; - if (dev->InterfaceClass == 3) { + if (dev->InterfaceClass == Drivers::USB::UsbDevice::CLASS_HID) { if (dev->InterfaceProtocol == 1) devName = "USB HID Keyboard"; else if (dev->InterfaceProtocol == 2) devName = "USB HID Mouse"; else devName = "USB HID Device"; + } else if (dev->InterfaceClass == Drivers::USB::UsbDevice::CLASS_WIRELESS) { + devName = "Bluetooth Adapter"; } else if (dev->InterfaceClass == 8) { devName = "USB Mass Storage"; } else if (dev->InterfaceClass == 9) { @@ -129,6 +136,22 @@ namespace Montauk { } } + // Audio (category 9) + if (Drivers::Audio::IntelHda::IsInitialized()) { + uint32_t codecId = Drivers::Audio::IntelHda::GetCodecVendorId(); + char detail[48]; + int p = 0; + p = dl_append(detail, p, "HDA Codec ", 48); + p = dl_append_hex(detail, p, codecId >> 16, 4, 48); + p = dl_append(detail, p, ":", 48); + p = dl_append_hex(detail, p, codecId & 0xFFFF, 4, 48); + add(9, "Intel HDA", detail); + } + + if (Drivers::USB::Bluetooth::IsInitialized()) { + add(9, "Bluetooth Audio", "A2DP (SBC)"); + } + // Storage (category 7) if (Drivers::Storage::Ahci::IsInitialized()) { for (int port = 0; port < 32 && count < maxCount; port++) { @@ -152,6 +175,27 @@ namespace Montauk { } } + if (Drivers::Storage::Nvme::IsInitialized()) { + for (int ns = 0; ns < Drivers::Storage::Nvme::GetNamespaceCount() && count < maxCount; ns++) { + auto* nsInfo = Drivers::Storage::Nvme::GetNamespaceInfo(ns); + if (!nsInfo || !nsInfo->Active) continue; + uint64_t sectors = nsInfo->SectorCount; + uint64_t sizeMB = (sectors * nsInfo->SectorSize) / (1024 * 1024); + uint64_t sizeGB = sizeMB / 1024; + char detail[48]; + int p = 0; + if (sizeGB > 0) { + p = dl_append_dec(detail, p, (int)sizeGB, 48); + p = dl_append(detail, p, " GiB, NVMe ns ", 48); + } else { + p = dl_append_dec(detail, p, (int)sizeMB, 48); + p = dl_append(detail, p, " MiB, NVMe ns ", 48); + } + p = dl_append_dec(detail, p, ns, 48); + add(7, nsInfo->Model, detail); + } + } + // PCI devices (category 8) auto& pciDevs = Pci::GetDevices(); for (int i = 0; i < (int)pciDevs.size() && count < maxCount; i++) { @@ -174,30 +218,64 @@ namespace Montauk { return count; } - static int Sys_DiskInfo(DiskInfo* buf, int port) { + static int Sys_DiskInfo(DiskInfo* buf, int blockDevIndex) { if (buf == nullptr) return -1; - if (!Drivers::Storage::Ahci::IsInitialized()) return -1; - auto* info = Drivers::Storage::Ahci::GetPortInfo(port); - if (!info) return -1; + auto* bdev = Drivers::Storage::GetBlockDevice(blockDevIndex); + if (!bdev) return -1; - buf->port = info->PortIndex; - buf->type = (uint8_t)info->Type; - buf->sataGen = (uint8_t)info->SataGen; - buf->sectorCount = info->SectorCount; - buf->sectorSizeLog = info->SectorSizeLog; - buf->sectorSizePhys = info->SectorSizePhys; - buf->rpm = info->Rpm; - buf->ncqDepth = info->NcqDepth; - buf->supportsLba48 = info->SupportsLba48 ? 1 : 0; - buf->supportsNcq = info->SupportsNcq ? 1 : 0; - buf->supportsTrim = info->SupportsTrim ? 1 : 0; - buf->supportsSmart = info->SupportsSmart ? 1 : 0; - buf->supportsWriteCache = info->SupportsWriteCache ? 1 : 0; - buf->supportsReadAhead = info->SupportsReadAhead ? 1 : 0; - dl_strcpy(buf->model, info->Model, 41); - dl_strcpy(buf->serial, info->Serial, 21); - dl_strcpy(buf->firmware, info->Firmware, 9); + // Zero the struct first so SATA-specific fields default to 0 + for (unsigned i = 0; i < sizeof(DiskInfo); i++) + ((uint8_t*)buf)[i] = 0; + + buf->port = (uint8_t)blockDevIndex; + buf->sectorCount = bdev->SectorCount; + buf->sectorSizeLog = bdev->SectorSize; + buf->sectorSizePhys = bdev->SectorSize; + dl_strcpy(buf->model, bdev->Model, 41); + + // Try to fill AHCI-specific fields if this is an AHCI device + if (Drivers::Storage::Ahci::IsInitialized()) { + for (int p = 0; p < 32; p++) { + auto* info = Drivers::Storage::Ahci::GetPortInfo(p); + if (!info) continue; + // Match by sector count and model name + if (info->SectorCount == bdev->SectorCount && + info->Model[0] == bdev->Model[0]) { + buf->type = (uint8_t)info->Type; + buf->sataGen = (uint8_t)info->SataGen; + buf->sectorSizeLog = info->SectorSizeLog; + buf->sectorSizePhys = info->SectorSizePhys; + buf->rpm = info->Rpm; + buf->ncqDepth = info->NcqDepth; + buf->supportsLba48 = info->SupportsLba48 ? 1 : 0; + buf->supportsNcq = info->SupportsNcq ? 1 : 0; + buf->supportsTrim = info->SupportsTrim ? 1 : 0; + buf->supportsSmart = info->SupportsSmart ? 1 : 0; + buf->supportsWriteCache = info->SupportsWriteCache ? 1 : 0; + buf->supportsReadAhead = info->SupportsReadAhead ? 1 : 0; + dl_strcpy(buf->serial, info->Serial, 21); + dl_strcpy(buf->firmware, info->Firmware, 9); + break; + } + } + } + + // For NVMe devices: set type=3 (NVMe), rpm=1 (SSD) + if (buf->type == 0 && Drivers::Storage::Nvme::IsInitialized()) { + for (int ns = 0; ns < Drivers::Storage::Nvme::GetNamespaceCount(); ns++) { + auto* nsInfo = Drivers::Storage::Nvme::GetNamespaceInfo(ns); + if (!nsInfo) continue; + if (nsInfo->SectorCount == bdev->SectorCount) { + buf->type = 3; // NVMe + buf->rpm = 1; // SSD / non-rotating + break; + } + } + } + + // If type is still 0, this block device is not recognized + if (buf->type == 0) return -1; return 0; } diff --git a/kernel/src/Api/Syscall.cpp b/kernel/src/Api/Syscall.cpp index 84d10d6..354d7bd 100644 --- a/kernel/src/Api/Syscall.cpp +++ b/kernel/src/Api/Syscall.cpp @@ -30,6 +30,8 @@ #include "Device.hpp" // SYS_DEVLIST, SYS_DISKINFO #include "Storage.hpp" // SYS_PARTLIST, SYS_DISKREAD, SYS_DISKWRITE #include "Window.hpp" // SYS_WINCREATE, SYS_WINDESTROY, SYS_WINPRESENT, SYS_WINPOLL, SYS_WINENUM, SYS_WINMAP, SYS_WINSENDEVENT, SYS_WINRESIZE, SYS_WINSETSCALE, SYS_WINGETSCALE +#include "Audio.hpp" // SYS_AUDIOOPEN, SYS_AUDIOCLOSE, SYS_AUDIOWRITE, SYS_AUDIOCTL +#include "BluetoothSyscall.hpp" // SYS_BTSCAN, SYS_BTCONNECT, SYS_BTDISCONNECT, SYS_BTLIST, SYS_BTINFO // Assembly entry point extern "C" void SyscallEntry(); @@ -243,6 +245,24 @@ namespace Montauk { return (int64_t)Sys_FsMount((int)frame->arg1, (int)frame->arg2); case SYS_FSFORMAT: return (int64_t)Sys_FsFormat((const FsFormatParams*)frame->arg1); + case SYS_AUDIOOPEN: + return Sys_AudioOpen((uint32_t)frame->arg1, (uint8_t)frame->arg2, (uint8_t)frame->arg3); + case SYS_AUDIOCLOSE: + return Sys_AudioClose((int)frame->arg1); + case SYS_AUDIOWRITE: + return Sys_AudioWrite((int)frame->arg1, (const uint8_t*)frame->arg2, (uint32_t)frame->arg3); + case SYS_AUDIOCTL: + return Sys_AudioCtl((int)frame->arg1, (int)frame->arg2, (int)frame->arg3); + case SYS_BTSCAN: + return Sys_BtScan((BtScanResult*)frame->arg1, (int)frame->arg2, (uint32_t)frame->arg3); + case SYS_BTCONNECT: + return Sys_BtConnect((const uint8_t*)frame->arg1); + case SYS_BTDISCONNECT: + return Sys_BtDisconnect((const uint8_t*)frame->arg1); + case SYS_BTLIST: + return Sys_BtList((BtDevInfo*)frame->arg1, (int)frame->arg2); + case SYS_BTINFO: + return Sys_BtInfo((BtAdapterInfo*)frame->arg1); default: return -1; } diff --git a/kernel/src/Api/Syscall.hpp b/kernel/src/Api/Syscall.hpp index 972c794..7a865f1 100644 --- a/kernel/src/Api/Syscall.hpp +++ b/kernel/src/Api/Syscall.hpp @@ -149,6 +149,28 @@ namespace Montauk { static constexpr uint64_t SYS_FSMOUNT = 75; static constexpr uint64_t SYS_FSFORMAT = 76; + /* Audio.hpp */ + static constexpr uint64_t SYS_AUDIOOPEN = 80; + static constexpr uint64_t SYS_AUDIOCLOSE = 81; + static constexpr uint64_t SYS_AUDIOWRITE = 82; + static constexpr uint64_t SYS_AUDIOCTL = 83; + + // Audio control commands (for SYS_AUDIOCTL) + static constexpr int AUDIO_CTL_SET_VOLUME = 0; + static constexpr int AUDIO_CTL_GET_VOLUME = 1; + static constexpr int AUDIO_CTL_GET_POS = 2; + static constexpr int AUDIO_CTL_PAUSE = 3; + static constexpr int AUDIO_CTL_GET_OUTPUT = 4; // 0=HDA, 1=Bluetooth + static constexpr int AUDIO_CTL_SET_OUTPUT = 5; // Switch audio output + static constexpr int AUDIO_CTL_BT_STATUS = 6; // Get Bluetooth connection status + + /* Bluetooth.hpp */ + static constexpr uint64_t SYS_BTSCAN = 84; + static constexpr uint64_t SYS_BTCONNECT = 85; + static constexpr uint64_t SYS_BTDISCONNECT = 86; + static constexpr uint64_t SYS_BTLIST = 87; + static constexpr uint64_t SYS_BTINFO = 88; + static constexpr int SOCK_TCP = 1; static constexpr int SOCK_UDP = 2; @@ -244,8 +266,8 @@ namespace Montauk { }; struct DiskInfo { - uint8_t port; // AHCI port index - uint8_t type; // 0=none, 1=SATA, 2=SATAPI + uint8_t port; // block device index + uint8_t type; // 0=none, 1=SATA, 2=SATAPI, 3=NVMe uint8_t sataGen; // SATA gen (1/2/3) uint8_t _pad0; uint64_t sectorCount; // Total user-addressable sectors @@ -314,6 +336,34 @@ namespace Montauk { uint64_t heapUsed; // heapNext - UserHeapBase (bytes) }; + // Bluetooth scan result (returned by SYS_BTSCAN) + struct BtScanResult { + uint8_t bdAddr[6]; + uint8_t _pad[2]; + uint32_t classOfDevice; + int8_t rssi; + uint8_t _pad2[3]; + char name[64]; + }; + + // Bluetooth connected device info (returned by SYS_BTLIST) + struct BtDevInfo { + uint8_t bdAddr[6]; + uint8_t connected; + uint8_t encrypted; + uint16_t handle; + uint8_t linkType; + uint8_t _pad; + }; + + // Bluetooth adapter info (returned by SYS_BTINFO) + struct BtAdapterInfo { + uint8_t bdAddr[6]; + uint8_t initialized; + uint8_t scanning; + char name[64]; + }; + // Stack frame pushed by SyscallEntry.asm struct SyscallFrame { uint64_t r15, r14, r13, r12, rbp, rbx; // callee-saved diff --git a/kernel/src/Drivers/Audio/IntelHda.cpp b/kernel/src/Drivers/Audio/IntelHda.cpp new file mode 100644 index 0000000..57f6edc --- /dev/null +++ b/kernel/src/Drivers/Audio/IntelHda.cpp @@ -0,0 +1,1150 @@ +/* + * IntelHda.cpp + * Intel High Definition Audio controller driver + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "IntelHda.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Drivers::Audio::IntelHda { + + using namespace Kt; + + // ========================================================================= + // Driver state + // ========================================================================= + + static bool g_initialized = false; + + static volatile uint8_t* g_mmioBase = nullptr; + static uint8_t g_bus, g_dev, g_func; + + // CORB/RIRB + static volatile uint32_t* g_corb = nullptr; + static volatile RirbEntry* g_rirb = nullptr; + static uint64_t g_corbPhys = 0; + static uint64_t g_rirbPhys = 0; + static uint16_t g_corbSize = 0; // Actual entries in use + static uint16_t g_rirbSize = 0; + static uint16_t g_rirbReadPtr = 0; + + // Codec info + static uint8_t g_codecAddr = 0; // First codec found + static bool g_codecFound = false; + static uint32_t g_codecVendorId = 0; // Vendor (upper 16) | Device (lower 16) + + // Output path nodes discovered during codec enumeration + static uint8_t g_dacNid = 0; // DAC node + static uint8_t g_speakerNid = 0; // Speaker / Line Out pin node + static uint8_t g_hpNid = 0; // Headphone pin node (0 if none) + static uint8_t g_pinNid = 0; // Currently active output pin + static uint8_t g_mixerNid = 0; // Mixer node (0 if direct DAC→Pin) + static bool g_hpPresenceDetect = false; // HP pin supports presence detect + static bool g_hpPlugged = false; // Current jack state + + // Number of input/output streams from GCAP + static uint8_t g_numInputStreams = 0; + static uint8_t g_numOutputStreams = 0; + + // DMA buffers for output + static volatile BdlEntry* g_bdl = nullptr; + static uint64_t g_bdlPhys = 0; + static uint8_t* g_dmaBuffer = nullptr; // Virtual (HHDM) + static uint64_t g_dmaBufferPhys = 0; + + // DMA position buffer + static volatile uint32_t* g_dmaPos = nullptr; + static uint64_t g_dmaPosPhys = 0; + + // Active stream + static AudioStream g_stream = {}; + + // Volume (0-127 for HDA, exposed as 0-100 to userspace) + static int g_volume = 80; // Default 80% + + // Set when an unsolicited response is detected (e.g. jack plug/unplug) + static volatile bool g_jackEventPending = false; + + // Debounce: after a switch, ignore jack events for this many Write() calls. + // Pin sense is unreliable while the pin widget is settling after reconfiguration. + static uint32_t g_jackDebounce = 0; + static constexpr uint32_t JACK_DEBOUNCE_COUNT = 512; + + // ========================================================================= + // MMIO register access + // ========================================================================= + + static uint8_t Read8(uint32_t reg) { + return *(volatile uint8_t*)(g_mmioBase + reg); + } + + static uint16_t Read16(uint32_t reg) { + return *(volatile uint16_t*)(g_mmioBase + reg); + } + + static uint32_t Read32(uint32_t reg) { + return *(volatile uint32_t*)(g_mmioBase + reg); + } + + static void Write8(uint32_t reg, uint8_t val) { + *(volatile uint8_t*)(g_mmioBase + reg) = val; + } + + static void Write16(uint32_t reg, uint16_t val) { + *(volatile uint16_t*)(g_mmioBase + reg) = val; + } + + static void Write32(uint32_t reg, uint32_t val) { + *(volatile uint32_t*)(g_mmioBase + reg) = val; + } + + // Stream descriptor register access + static uint32_t StreamBase(uint8_t streamIndex) { + return SD_BASE + (uint32_t)streamIndex * SD_SIZE; + } + + static uint8_t ReadSD8(uint8_t stream, uint32_t offset) { + return Read8(StreamBase(stream) + offset); + } + + static uint16_t ReadSD16(uint8_t stream, uint32_t offset) { + return Read16(StreamBase(stream) + offset); + } + + static uint32_t ReadSD32(uint8_t stream, uint32_t offset) { + return Read32(StreamBase(stream) + offset); + } + + static void WriteSD8(uint8_t stream, uint32_t offset, uint8_t val) { + Write8(StreamBase(stream) + offset, val); + } + + static void WriteSD16(uint8_t stream, uint32_t offset, uint16_t val) { + Write16(StreamBase(stream) + offset, val); + } + + static void WriteSD32(uint8_t stream, uint32_t offset, uint32_t val) { + Write32(StreamBase(stream) + offset, val); + } + + // ========================================================================= + // Delay helper + // ========================================================================= + + static void MicroDelay(int us) { + // Simple busy-wait; us is approximate + for (volatile int i = 0; i < us * 100; i++) { + asm volatile("pause"); + } + } + + // ========================================================================= + // CORB/RIRB command interface + // ========================================================================= + + static bool SendVerb(uint32_t verb) { + // Read current write pointer + uint16_t wp = Read16(REG_CORBWP) & 0xFF; + + // Advance write pointer + wp = (wp + 1) % g_corbSize; + + // Write verb to CORB + g_corb[wp] = verb; + + // Update write pointer + Write16(REG_CORBWP, wp); + + return true; + } + + static bool ReadResponse(uint32_t* response, uint32_t* responseEx, int timeoutUs = 100000) { + for (int i = 0; i < timeoutUs / 10; i++) { + uint16_t wp = Read16(REG_RIRBWP) & 0xFF; + + if (g_rirbReadPtr != wp) { + g_rirbReadPtr = (g_rirbReadPtr + 1) % g_rirbSize; + + uint32_t ex = g_rirb[g_rirbReadPtr].ResponseEx; + + // Bit 4 of ResponseEx = unsolicited response. + // Skip it and flag for deferred jack detection. + if (ex & (1u << 4)) { + g_jackEventPending = true; + continue; + } + + if (response) + *response = g_rirb[g_rirbReadPtr].Response; + if (responseEx) + *responseEx = ex; + + return true; + } + + MicroDelay(10); + } + + return false; + } + + // Send a verb and wait for the response + static uint32_t CodecCommand(uint8_t codec, uint8_t nid, uint32_t verb) { + uint32_t fullVerb = ((uint32_t)codec << 28) | ((uint32_t)nid << 20) | verb; + SendVerb(fullVerb); + + uint32_t response = 0; + if (!ReadResponse(&response, nullptr)) { + KernelLogStream(WARNING, "HDA") << "Verb timeout: codec=" << base::dec + << (uint64_t)codec << " nid=" << (uint64_t)nid + << " verb=" << base::hex << (uint64_t)verb; + return 0; + } + return response; + } + + static uint32_t GetParameter(uint8_t codec, uint8_t nid, uint32_t paramId) { + return CodecCommand(codec, nid, 0xF0000 | paramId); + } + + // ========================================================================= + // Controller reset + // ========================================================================= + + static bool ResetController() { + // Enter reset: clear CRST + uint32_t gctl = Read32(REG_GCTL); + gctl &= ~GCTL_CRST; + Write32(REG_GCTL, gctl); + + // Wait for reset to take effect + for (int i = 0; i < 1000; i++) { + if ((Read32(REG_GCTL) & GCTL_CRST) == 0) + break; + MicroDelay(100); + } + if (Read32(REG_GCTL) & GCTL_CRST) { + KernelLogStream(ERROR, "HDA") << "Controller failed to enter reset"; + return false; + } + + MicroDelay(1000); + + // Exit reset: set CRST + gctl = Read32(REG_GCTL); + gctl |= GCTL_CRST; + Write32(REG_GCTL, gctl); + + // Wait for controller to come out of reset + for (int i = 0; i < 1000; i++) { + if (Read32(REG_GCTL) & GCTL_CRST) + break; + MicroDelay(100); + } + if (!(Read32(REG_GCTL) & GCTL_CRST)) { + KernelLogStream(ERROR, "HDA") << "Controller failed to exit reset"; + return false; + } + + // Enable acceptance of unsolicited responses (for jack detect events) + gctl = Read32(REG_GCTL); + gctl |= GCTL_UNSOL; + Write32(REG_GCTL, gctl); + + // Wait for codecs to initialize (spec says 521us minimum after CRST) + MicroDelay(1000); + + return true; + } + + // ========================================================================= + // CORB/RIRB initialization + // ========================================================================= + + static uint16_t DecodeSizeCapability(uint8_t sizeReg) { + // Size register bits 7:4 = size capability, bits 1:0 = size select + // Capability: bit 6 = 256 entries, bit 5 = 16 entries, bit 4 = 2 entries + // We prefer 256 entries + if (sizeReg & (1 << 6)) return 256; + if (sizeReg & (1 << 5)) return 16; + if (sizeReg & (1 << 4)) return 2; + return 2; + } + + static uint8_t SizeSelectBits(uint16_t entries) { + if (entries == 256) return 0x02; + if (entries == 16) return 0x01; + return 0x00; // 2 entries + } + + static bool InitCorbRirb() { + // Stop CORB and RIRB + Write8(REG_CORBCTL, 0); + Write8(REG_RIRBCTL, 0); + MicroDelay(100); + + // Determine CORB size + uint8_t corbSizeReg = Read8(REG_CORBSIZE); + g_corbSize = DecodeSizeCapability(corbSizeReg); + + // Allocate CORB buffer (aligned to 128 bytes, we allocate a full page) + void* corbVirt = Memory::g_pfa->AllocateZeroed(); + g_corbPhys = Memory::SubHHDM(corbVirt); + g_corb = (volatile uint32_t*)corbVirt; + + // Set CORB base address + Write32(REG_CORBLBASE, (uint32_t)(g_corbPhys & 0xFFFFFFFF)); + Write32(REG_CORBUBASE, (uint32_t)(g_corbPhys >> 32)); + + // Set CORB size + Write8(REG_CORBSIZE, (corbSizeReg & 0xF0) | SizeSelectBits(g_corbSize)); + + // Reset CORB read pointer + Write16(REG_CORBRP, CORBRP_RST); + MicroDelay(100); + // Clear reset bit (some controllers need this) + Write16(REG_CORBRP, 0); + for (int i = 0; i < 1000; i++) { + if ((Read16(REG_CORBRP) & CORBRP_RST) == 0) + break; + MicroDelay(10); + } + + // Reset CORB write pointer + Write16(REG_CORBWP, 0); + + // Determine RIRB size + uint8_t rirbSizeReg = Read8(REG_RIRBSIZE); + g_rirbSize = DecodeSizeCapability(rirbSizeReg); + + // Allocate RIRB buffer (8 bytes per entry) + void* rirbVirt = Memory::g_pfa->AllocateZeroed(); + g_rirbPhys = Memory::SubHHDM(rirbVirt); + g_rirb = (volatile RirbEntry*)rirbVirt; + + // Set RIRB base address + Write32(REG_RIRBLBASE, (uint32_t)(g_rirbPhys & 0xFFFFFFFF)); + Write32(REG_RIRBUBASE, (uint32_t)(g_rirbPhys >> 32)); + + // Set RIRB size + Write8(REG_RIRBSIZE, (rirbSizeReg & 0xF0) | SizeSelectBits(g_rirbSize)); + + // Reset RIRB write pointer + Write16(REG_RIRBWP, RIRBWP_RST); + + // Set response interrupt count + Write16(REG_RINTCNT, 1); + + // Initialize our read pointer to match HW state + g_rirbReadPtr = 0; + + // Start CORB and RIRB + Write8(REG_CORBCTL, CORBCTL_RUN); + Write8(REG_RIRBCTL, RIRBCTL_RUN | RIRBCTL_RINTCTL); + + MicroDelay(100); + + KernelLogStream(OK, "HDA") << "CORB: " << base::dec << (uint64_t)g_corbSize + << " entries, RIRB: " << (uint64_t)g_rirbSize << " entries"; + + return true; + } + + // ========================================================================= + // Codec discovery and output path enumeration + // ========================================================================= + + static bool DiscoverCodecs() { + uint16_t statests = Read16(REG_STATESTS); + KernelLogStream(INFO, "HDA") << "STATESTS=" << base::hex << (uint64_t)statests; + + for (uint8_t addr = 0; addr < 15; addr++) { + if (statests & (1 << addr)) { + uint32_t vendorId = GetParameter(addr, 0, PARAM_VENDOR_ID); + if (vendorId == 0 || vendorId == 0xFFFFFFFF) + continue; + + g_codecAddr = addr; + g_codecFound = true; + g_codecVendorId = vendorId; + + KernelLogStream(OK, "HDA") << "Codec " << base::dec << (uint64_t)addr + << ": vendor=" << base::hex << (uint64_t)(vendorId >> 16) + << " device=" << (uint64_t)(vendorId & 0xFFFF); + + // Clear state change status + Write16(REG_STATESTS, statests); + return true; + } + } + + KernelLogStream(ERROR, "HDA") << "No codecs found"; + return false; + } + + static bool EnumerateOutputPath() { + // Get subordinate node count for root node (node 0) + uint32_t subNodes = GetParameter(g_codecAddr, 0, PARAM_SUB_NODE_COUNT); + uint8_t startNid = (subNodes >> 16) & 0xFF; + uint8_t nodeCount = subNodes & 0xFF; + + KernelLogStream(INFO, "HDA") << "Root: startNid=" << base::dec << (uint64_t)startNid + << " count=" << (uint64_t)nodeCount; + + // Find Audio Function Group + uint8_t afg = 0; + for (uint8_t i = 0; i < nodeCount; i++) { + uint8_t nid = startNid + i; + uint32_t fgType = GetParameter(g_codecAddr, nid, PARAM_FN_GROUP_TYPE); + if ((fgType & 0xFF) == 0x01) { // Audio Function Group + afg = nid; + KernelLogStream(INFO, "HDA") << "Audio Function Group at NID " << (uint64_t)nid; + break; + } + } + + if (afg == 0) { + KernelLogStream(ERROR, "HDA") << "No Audio Function Group found"; + return false; + } + + // Power on the AFG + CodecCommand(g_codecAddr, afg, 0x70500 | 0x00); // D0 (fully on) + + // Enumerate widgets in the AFG + subNodes = GetParameter(g_codecAddr, afg, PARAM_SUB_NODE_COUNT); + startNid = (subNodes >> 16) & 0xFF; + nodeCount = subNodes & 0xFF; + + KernelLogStream(INFO, "HDA") << "AFG widgets: startNid=" << base::dec + << (uint64_t)startNid << " count=" << (uint64_t)nodeCount; + + // Find DAC, output pins, and mixer + uint8_t firstDac = 0; + uint8_t speakerPin = 0; // Best speaker/line-out pin + uint8_t speakerPri = 0xFF; + uint8_t hpPin = 0; // Best headphone pin + + for (uint8_t i = 0; i < nodeCount; i++) { + uint8_t nid = startNid + i; + uint32_t widgetCap = GetParameter(g_codecAddr, nid, PARAM_AUDIO_WIDGET_CAP); + uint8_t widgetType = (widgetCap >> 20) & 0xF; + + if (widgetType == WIDGET_AUDIO_OUTPUT && firstDac == 0) { + firstDac = nid; + KernelLogStream(INFO, "HDA") << "DAC found at NID " << base::dec << (uint64_t)nid; + } + + if (widgetType == WIDGET_PIN_COMPLEX) { + uint32_t pinCfg = CodecCommand(g_codecAddr, nid, 0xF1C00); + uint8_t connectivity = (pinCfg >> 30) & 0x03; + uint8_t devType = (pinCfg >> 20) & 0x0F; + + if (connectivity == 1) // No physical connection + continue; + + KernelLogStream(INFO, "HDA") << "Pin NID " << base::dec << (uint64_t)nid + << " type=" << (uint64_t)devType + << " connectivity=" << (uint64_t)connectivity; + + if (devType == PIN_DEV_HP_OUT) { + if (hpPin == 0) hpPin = nid; + } else if (devType == PIN_DEV_SPEAKER || devType == PIN_DEV_LINE_OUT) { + uint8_t pri = (devType == PIN_DEV_SPEAKER) ? 0 : 1; + if (pri < speakerPri) { + speakerPin = nid; + speakerPri = pri; + } + } + } + + if (widgetType == WIDGET_AUDIO_MIXER) { + if (g_mixerNid == 0) g_mixerNid = nid; + } + } + + if (firstDac == 0) { + KernelLogStream(ERROR, "HDA") << "No DAC found"; + return false; + } + if (speakerPin == 0 && hpPin == 0) { + KernelLogStream(ERROR, "HDA") << "No output pin found"; + return false; + } + + g_dacNid = firstDac; + g_speakerNid = speakerPin; + g_hpNid = hpPin; + + // Default to speaker; if no speaker, use HP + g_pinNid = g_speakerNid ? g_speakerNid : g_hpNid; + + // Check if HP pin supports presence detect + if (g_hpNid) { + uint32_t pinCaps = GetParameter(g_codecAddr, g_hpNid, PARAM_PIN_CAPS); + g_hpPresenceDetect = (pinCaps & (1 << 2)) != 0; // Bit 2 = Presence Detect Capable + KernelLogStream(INFO, "HDA") << "HP pin NID " << base::dec << (uint64_t)g_hpNid + << (g_hpPresenceDetect ? " (presence detect)" : " (no presence detect)"); + + // Enable unsolicited responses on the HP pin for jack events + if (g_hpPresenceDetect) { + // Set Unsolicited Response: enable (bit 7) | tag 0x01 + CodecCommand(g_codecAddr, g_hpNid, 0x70800 | (1 << 7) | 0x01); + } + } + + KernelLogStream(OK, "HDA") << "Output path: DAC=" << base::dec << (uint64_t)g_dacNid + << (g_mixerNid ? " Mixer=" : "") << (g_mixerNid ? (uint64_t)g_mixerNid : 0) + << " Speaker=" << (uint64_t)g_speakerNid + << " HP=" << (uint64_t)g_hpNid; + + return true; + } + + // ========================================================================= + // Stream format encoding + // ========================================================================= + + static uint16_t EncodeFormat(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { + uint16_t fmt = 0; + + // Base rate and multiplier/divisor + switch (sampleRate) { + case 8000: fmt = FMT_BASE_48K | (0 << 11) | (5 << 8); break; // 48k / 6 + case 11025: fmt = FMT_BASE_44K | (0 << 11) | (3 << 8); break; // 44.1k / 4 + case 16000: fmt = FMT_BASE_48K | (0 << 11) | (2 << 8); break; // 48k / 3 + case 22050: fmt = FMT_BASE_44K | (0 << 11) | (1 << 8); break; // 44.1k / 2 + case 32000: fmt = FMT_BASE_48K | (1 << 11) | (2 << 8); break; // 48k * 2 / 3 + case 44100: fmt = FMT_BASE_44K | (0 << 11) | (0 << 8); break; // 44.1k + case 48000: fmt = FMT_BASE_48K | (0 << 11) | (0 << 8); break; // 48k + case 88200: fmt = FMT_BASE_44K | (1 << 11) | (0 << 8); break; // 44.1k * 2 + case 96000: fmt = FMT_BASE_48K | (1 << 11) | (0 << 8); break; // 48k * 2 + case 176400: fmt = FMT_BASE_44K | (3 << 11) | (0 << 8); break; // 44.1k * 4 + case 192000: fmt = FMT_BASE_48K | (3 << 11) | (0 << 8); break; // 48k * 4 + default: fmt = FMT_BASE_48K | (0 << 11) | (0 << 8); break; // Default 48k + } + + // Bits per sample + switch (bitsPerSample) { + case 8: fmt |= (0 << 4); break; + case 16: fmt |= (1 << 4); break; + case 20: fmt |= (2 << 4); break; + case 24: fmt |= (3 << 4); break; + case 32: fmt |= (4 << 4); break; + default: fmt |= (1 << 4); break; // Default 16-bit + } + + // Channels (0-based) + fmt |= (channels - 1) & 0x0F; + + return fmt; + } + + // ========================================================================= + // Volume control + // ========================================================================= + + static void SetOutputVolume(int percent) { + if (percent < 0) percent = 0; + if (percent > 100) percent = 100; + g_volume = percent; + + // Convert 0-100 to HDA 0-127 gain (7-bit) + uint8_t gain = (uint8_t)((percent * 127) / 100); + uint16_t ampPayload = AMP_SET_OUTPUT | AMP_SET_LEFT | AMP_SET_RIGHT | gain; + + // Set DAC output amp + CodecCommand(g_codecAddr, g_dacNid, 0x30000 | ampPayload); + + // Set mixer output amp if present + if (g_mixerNid) { + CodecCommand(g_codecAddr, g_mixerNid, 0x30000 | ampPayload); + } + } + + // ========================================================================= + // Pin enable / disable helpers + // ========================================================================= + + static void EnablePin(uint8_t nid) { + if (nid == 0) return; + + // Power on + CodecCommand(g_codecAddr, nid, 0x70500 | 0x00); // D0 + + // Read pin config to determine type + uint32_t pinCfg = CodecCommand(g_codecAddr, nid, 0xF1C00); + uint8_t devType = (pinCfg >> 20) & 0x0F; + + uint8_t pinCtl = PIN_CTL_ENABLE_OUTPUT; + if (devType == PIN_DEV_HP_OUT) + pinCtl |= PIN_CTL_ENABLE_HP; + + CodecCommand(g_codecAddr, nid, 0x70700 | pinCtl); + + // Enable EAPD if supported + uint32_t pinCaps = GetParameter(g_codecAddr, nid, PARAM_PIN_CAPS); + if (pinCaps & (1 << 16)) + CodecCommand(g_codecAddr, nid, 0x70C00 | EAPD_ENABLE); + + // Unmute output amp + CodecCommand(g_codecAddr, nid, + 0x30000 | AMP_SET_OUTPUT | AMP_SET_LEFT | AMP_SET_RIGHT | 127); + } + + static void DisablePin(uint8_t nid) { + if (nid == 0) return; + + // Mute output amp + CodecCommand(g_codecAddr, nid, + 0x30000 | AMP_SET_OUTPUT | AMP_SET_LEFT | AMP_SET_RIGHT | AMP_MUTE); + + // Disable pin output + CodecCommand(g_codecAddr, nid, 0x70700 | 0x00); + } + + // ========================================================================= + // Jack detection and output switching + // ========================================================================= + + static bool IsHpPlugged() { + if (!g_hpNid || !g_hpPresenceDetect) + return false; + // Get Pin Sense (verb 0xF0900): bit 31 = presence detected + uint32_t sense = CodecCommand(g_codecAddr, g_hpNid, 0xF0900); + return (sense & (1u << 31)) != 0; + } + + static void SwitchOutput(bool toHeadphones) { + if (toHeadphones) { + // Mute speaker, enable HP + DisablePin(g_speakerNid); + EnablePin(g_hpNid); + g_pinNid = g_hpNid; + KernelLogStream(INFO, "HDA") << "Switched to headphone output"; + } else { + // Mute HP, enable speaker + DisablePin(g_hpNid); + EnablePin(g_speakerNid); + g_pinNid = g_speakerNid; + KernelLogStream(INFO, "HDA") << "Switched to speaker output"; + } + // Re-apply volume on the now-active pin + SetOutputVolume(g_volume); + + // Start debounce: pin sense is unreliable right after reconfiguration, + // and the pin change itself may trigger spurious unsolicited responses. + g_jackDebounce = JACK_DEBOUNCE_COUNT; + g_jackEventPending = false; + } + + // Drain any unsolicited responses sitting in the RIRB. + // During normal playback no CodecCommands are sent, so ReadResponse() + // never runs and unsolicited responses (jack events) pile up unread. + // This consumes them and sets g_jackEventPending if any are found. + static void DrainUnsolicitedResponses() { + for (;;) { + uint16_t wp = Read16(REG_RIRBWP) & 0xFF; + if (g_rirbReadPtr == wp) + break; + + uint16_t next = (g_rirbReadPtr + 1) % g_rirbSize; + uint32_t ex = g_rirb[next].ResponseEx; + + if (!(ex & (1u << 4))) + break; // Solicited response — leave for ReadResponse() + + g_rirbReadPtr = next; + g_jackEventPending = true; + } + } + + // Poll jack state; call when an unsolicited response fires. + // Switches output if state changed. + static void PollJackState() { + if (!g_hpNid || !g_hpPresenceDetect || !g_speakerNid) + return; + + bool plugged = IsHpPlugged(); + if (plugged != g_hpPlugged) { + g_hpPlugged = plugged; + SwitchOutput(plugged); + } + } + + // ========================================================================= + // Configure output path (DAC -> [Mixer ->] Pin) + // ========================================================================= + + static void ConfigureOutputPath(uint16_t streamFormat, uint8_t streamTag) { + // Power on DAC and mixer + CodecCommand(g_codecAddr, g_dacNid, 0x70500 | 0x00); // D0 + if (g_mixerNid) CodecCommand(g_codecAddr, g_mixerNid, 0x70500 | 0x00); + + // Set converter stream/channel on DAC: stream tag in bits 7:4, channel 0 in bits 3:0 + CodecCommand(g_codecAddr, g_dacNid, 0x70600 | ((uint32_t)streamTag << 4) | 0); + + // Set converter format on DAC + CodecCommand(g_codecAddr, g_dacNid, 0x20000 | streamFormat); + + // Unmute DAC output amp + SetOutputVolume(g_volume); + + // If there's a mixer, unmute it + if (g_mixerNid) { + CodecCommand(g_codecAddr, g_mixerNid, + 0x30000 | AMP_SET_INPUT | AMP_SET_LEFT | AMP_SET_RIGHT | 127); + } + + // Check jack state and enable the correct output pin + if (g_hpNid && g_hpPresenceDetect && g_speakerNid) { + g_hpPlugged = IsHpPlugged(); + if (g_hpPlugged) { + EnablePin(g_hpNid); + DisablePin(g_speakerNid); + g_pinNid = g_hpNid; + } else { + EnablePin(g_speakerNid); + DisablePin(g_hpNid); + g_pinNid = g_speakerNid; + } + } else { + // Only one output available, just enable it + EnablePin(g_pinNid); + } + } + + // ========================================================================= + // Output stream setup + // ========================================================================= + + static bool SetupOutputStream(uint8_t streamIndex, uint16_t streamFormat) { + // Reset stream + WriteSD8(streamIndex, SD_CTL, SD_CTL0_SRST); + for (int i = 0; i < 1000; i++) { + if (ReadSD8(streamIndex, SD_CTL) & SD_CTL0_SRST) + break; + MicroDelay(10); + } + + // Clear reset + WriteSD8(streamIndex, SD_CTL, 0); + for (int i = 0; i < 1000; i++) { + if ((ReadSD8(streamIndex, SD_CTL) & SD_CTL0_SRST) == 0) + break; + MicroDelay(10); + } + + // Clear status bits + WriteSD8(streamIndex, SD_STS, SD_STS_BCIS | SD_STS_FIFOE | SD_STS_DESE); + + // Set up BDL (double-buffered) + for (int i = 0; i < BUFFER_COUNT; i++) { + g_bdl[i].Address = g_dmaBufferPhys + (uint64_t)(i * BUFFER_SIZE); + g_bdl[i].Length = BUFFER_SIZE; + g_bdl[i].Ioc = 1; // Interrupt on completion for each buffer + } + + // Set BDL address + WriteSD32(streamIndex, SD_BDPL, (uint32_t)(g_bdlPhys & 0xFFFFFFFF)); + WriteSD32(streamIndex, SD_BDPU, (uint32_t)(g_bdlPhys >> 32)); + + // Set cyclic buffer length (total bytes across all BDL entries) + WriteSD32(streamIndex, SD_CBL, TOTAL_BUFFER_SIZE); + + // Set last valid index (0-based) + WriteSD16(streamIndex, SD_LVI, BUFFER_COUNT - 1); + + // Set stream format + WriteSD16(streamIndex, SD_FMT, streamFormat); + + // Set stream tag in CTL byte 2 (bits 23:20) + uint8_t streamTag = 1; // Use stream tag 1 + WriteSD8(streamIndex, SD_CTL + 2, (streamTag << 4)); + + return true; + } + + static void StartStream(uint8_t streamIndex) { + // Enable interrupts and start running + uint8_t ctl = ReadSD8(streamIndex, SD_CTL); + ctl |= SD_CTL0_RUN | SD_CTL0_IOCE; + WriteSD8(streamIndex, SD_CTL, ctl); + } + + static void StopStream(uint8_t streamIndex) { + uint8_t ctl = ReadSD8(streamIndex, SD_CTL); + ctl &= ~(SD_CTL0_RUN | SD_CTL0_IOCE); + WriteSD8(streamIndex, SD_CTL, ctl); + + // Wait for stream to stop + for (int i = 0; i < 1000; i++) { + if ((ReadSD8(streamIndex, SD_CTL) & SD_CTL0_RUN) == 0) + break; + MicroDelay(10); + } + } + + // ========================================================================= + // MSI setup + // ========================================================================= + + static void HandleInterrupt(uint8_t irq); + + static bool SetupMsi(uint8_t bus, uint8_t dev, uint8_t func) { + uint8_t cap = Pci::FindCapability(bus, dev, func, Pci::PCI_CAP_MSI); + if (cap == 0) { + KernelLogStream(INFO, "HDA") << "MSI capability not found"; + return false; + } + + uint16_t msgCtrl = Pci::LegacyRead16(bus, dev, func, cap + 2); + bool is64bit = (msgCtrl & (1 << 7)) != 0; + + Pci::LegacyWrite32(bus, dev, func, cap + 4, MSI_ADDR_BASE); + + if (is64bit) { + Pci::LegacyWrite32(bus, dev, func, cap + 8, 0); + Pci::LegacyWrite16(bus, dev, func, cap + 12, MSI_VECTOR); + } else { + Pci::LegacyWrite16(bus, dev, func, cap + 8, MSI_VECTOR); + } + + msgCtrl &= ~(0x70); + msgCtrl |= (1 << 0); + Pci::LegacyWrite16(bus, dev, func, cap + 2, msgCtrl); + + uint16_t pciCmd = Pci::LegacyRead16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND); + pciCmd |= Pci::PCI_CMD_INTX_DISABLE; + Pci::LegacyWrite16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND, pciCmd); + + Hal::RegisterIrqHandler(MSI_IRQ, HandleInterrupt); + + KernelLogStream(OK, "HDA") << "MSI enabled: vector " << base::dec << (uint64_t)MSI_VECTOR + << " (IRQ slot " << (uint64_t)MSI_IRQ << ")" << (is64bit ? " [64-bit]" : " [32-bit]"); + + return true; + } + + // ========================================================================= + // Interrupt handler + // ========================================================================= + + static void HandleInterrupt(uint8_t /*irq*/) { + uint32_t intsts = Read32(REG_INTSTS); + + // Handle stream interrupts (bits 0-29 correspond to stream descriptors) + if (g_stream.Active) { + uint8_t si = g_stream.StreamIndex; + if (intsts & (1u << si)) { + uint8_t sts = ReadSD8(si, SD_STS); + WriteSD8(si, SD_STS, sts); + } + } + + // Handle RIRB interrupt (controller interrupt enable bit 30) + // Do NOT advance g_rirbReadPtr here — ReadResponse() owns it. + // Just clear status. Unsolicited responses are detected and flagged + // in ReadResponse() by checking ResponseEx bit 4. + if (intsts & (1u << 30)) { + uint8_t rirbSts = Read8(REG_RIRBSTS); + Write8(REG_RIRBSTS, rirbSts); + } + } + + // ========================================================================= + // DMA buffer allocation + // ========================================================================= + + static bool AllocateDmaBuffers() { + // Allocate BDL (needs 128-byte alignment, page-aligned is fine) + void* bdlVirt = Memory::g_pfa->AllocateZeroed(); + g_bdlPhys = Memory::SubHHDM(bdlVirt); + g_bdl = (volatile BdlEntry*)bdlVirt; + + // Allocate DMA audio buffers (BUFFER_COUNT * BUFFER_SIZE = 32 KiB = 8 pages) + int pages = TOTAL_BUFFER_SIZE / 0x1000; + void* bufVirt = Memory::g_pfa->ReallocConsecutive(nullptr, pages); + if (!bufVirt) { + KernelLogStream(ERROR, "HDA") << "Failed to allocate DMA buffer (" << base::dec << pages << " pages)"; + return false; + } + memset(bufVirt, 0, TOTAL_BUFFER_SIZE); + g_dmaBufferPhys = Memory::SubHHDM(bufVirt); + g_dmaBuffer = (uint8_t*)bufVirt; + + // Allocate DMA position buffer (one page) + void* posVirt = Memory::g_pfa->AllocateZeroed(); + g_dmaPosPhys = Memory::SubHHDM(posVirt); + g_dmaPos = (volatile uint32_t*)posVirt; + + KernelLogStream(OK, "HDA") << "DMA buffers allocated: " << base::dec + << BUFFER_COUNT << "x" << BUFFER_SIZE << " bytes"; + + return true; + } + + // ========================================================================= + // Probe (called by PCI driver matching) + // ========================================================================= + + bool Probe(const Pci::PciDevice& dev) { + g_bus = dev.Bus; + g_dev = dev.Device; + g_func = dev.Function; + + KernelLogStream(INFO, "HDA") << "Probing Intel HDA at " + << base::hex << (uint64_t)g_bus << ":" << (uint64_t)g_dev << "." << base::dec << (uint64_t)g_func + << " (device=" << base::hex << (uint64_t)dev.DeviceId << ")"; + + // Enable memory space and bus mastering + Pci::EnableBusMaster(g_bus, g_dev, g_func); + + // Read BAR0 + uint64_t mmioPhys = Pci::ReadBar0(g_bus, g_dev, g_func); + if (mmioPhys == 0) { + KernelLogStream(ERROR, "HDA") << "BAR0 is zero"; + return false; + } + + KernelLogStream(INFO, "HDA") << "MMIO base: " << base::hex << mmioPhys; + + // Map MMIO region (HDA MMIO is typically 16 KiB) + for (uint64_t offset = 0; offset < 0x4000; offset += 0x1000) { + Memory::VMM::g_paging->MapMMIO(mmioPhys + offset, + Memory::HHDM(mmioPhys + offset)); + } + g_mmioBase = (volatile uint8_t*)Memory::HHDM(mmioPhys); + + // Read capabilities + uint16_t gcap = Read16(REG_GCAP); + uint8_t vmin = Read8(REG_VMIN); + uint8_t vmaj = Read8(REG_VMAJ); + + g_numOutputStreams = (gcap >> 12) & 0xF; + g_numInputStreams = (gcap >> 8) & 0xF; + uint8_t numBidiStreams = (gcap >> 3) & 0x1F; + bool ok64bit = (gcap & 0x01) != 0; + + KernelLogStream(INFO, "HDA") << "HDA v" << base::dec << (uint64_t)vmaj << "." << (uint64_t)vmin + << " | ISS=" << (uint64_t)g_numInputStreams + << " OSS=" << (uint64_t)g_numOutputStreams + << " BSS=" << (uint64_t)numBidiStreams + << (ok64bit ? " 64-bit" : " 32-bit"); + + if (g_numOutputStreams == 0) { + KernelLogStream(ERROR, "HDA") << "No output streams available"; + return false; + } + + // Reset the controller + if (!ResetController()) return false; + + // Allocate DMA buffers + if (!AllocateDmaBuffers()) return false; + + // Set up DMA position buffer + Write32(REG_DPIBLBASE, (uint32_t)(g_dmaPosPhys & 0xFFFFFFFF) | 1); // Bit 0 = enable + Write32(REG_DPIBUBASE, (uint32_t)(g_dmaPosPhys >> 32)); + + // Initialize CORB/RIRB + if (!InitCorbRirb()) return false; + + // Set up MSI (with legacy fallback) + bool msiOk = SetupMsi(g_bus, g_dev, g_func); + if (!msiOk) { + uint8_t irqLine = Pci::LegacyRead8(g_bus, g_dev, g_func, (uint8_t)Pci::PCI_REG_INTERRUPT); + if (irqLine != 0xFF) { + KernelLogStream(INFO, "HDA") << "Falling back to legacy IRQ " << base::dec << (uint64_t)irqLine; + Hal::RegisterIrqHandler(irqLine, HandleInterrupt); + } else { + KernelLogStream(WARNING, "HDA") << "No interrupt available, polling only"; + } + } + + // Enable interrupts + uint32_t intctl = INTCTL_GIE | INTCTL_CIE; + // Enable interrupt for all output streams + for (uint8_t i = 0; i < g_numOutputStreams; i++) { + intctl |= (1u << (g_numInputStreams + i)); + } + Write32(REG_INTCTL, intctl); + + // Discover codecs + if (!DiscoverCodecs()) return false; + + // Enumerate output path + if (!EnumerateOutputPath()) return false; + + // Read initial jack state + if (g_hpNid && g_hpPresenceDetect) { + g_hpPlugged = IsHpPlugged(); + KernelLogStream(INFO, "HDA") << "Headphone jack: " + << (g_hpPlugged ? "plugged" : "unplugged"); + } + + g_initialized = true; + KernelLogStream(OK, "HDA") << "Intel HDA initialized successfully"; + + return true; + } + + // ========================================================================= + // Public API + // ========================================================================= + + bool IsInitialized() { + return g_initialized; + } + + uint32_t GetCodecVendorId() { + return g_codecVendorId; + } + + int Open(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { + if (!g_initialized) return -1; + if (g_stream.Active) return -1; // Only one stream at a time + if (channels < 1 || channels > 8) return -1; + if (bitsPerSample != 8 && bitsPerSample != 16 && bitsPerSample != 20 + && bitsPerSample != 24 && bitsPerSample != 32) return -1; + + // Output stream index = numInputStreams (first output stream) + uint8_t streamIndex = g_numInputStreams; + uint8_t streamTag = 1; + + // Encode the stream format + uint16_t fmt = EncodeFormat(sampleRate, channels, bitsPerSample); + + // Configure the codec output path + ConfigureOutputPath(fmt, streamTag); + + // Set up the output stream DMA + if (!SetupOutputStream(streamIndex, fmt)) return -1; + + // Zero the DMA buffer + memset(g_dmaBuffer, 0, TOTAL_BUFFER_SIZE); + + // Record stream state + g_stream.Active = true; + g_stream.SampleRate = sampleRate; + g_stream.Channels = channels; + g_stream.BitsPerSample = bitsPerSample; + g_stream.StreamIndex = streamIndex; + g_stream.StreamTag = streamTag; + g_stream.WritePos = 0; + + // Start the stream + StartStream(streamIndex); + + KernelLogStream(OK, "HDA") << "Stream opened: " << base::dec + << (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit " + << (uint64_t)channels << "ch"; + + return 0; // Handle 0 + } + + void Close(int handle) { + if (handle != 0 || !g_stream.Active) return; + + StopStream(g_stream.StreamIndex); + + // Mute output + CodecCommand(g_codecAddr, g_dacNid, + 0x30000 | AMP_SET_OUTPUT | AMP_SET_LEFT | AMP_SET_RIGHT | AMP_MUTE); + + g_stream.Active = false; + + KernelLogStream(OK, "HDA") << "Stream closed"; + } + + int Write(int handle, const uint8_t* data, uint32_t size) { + if (handle != 0 || !g_stream.Active || !data || size == 0) + return -1; + + // Drain unsolicited responses from the RIRB — during playback no + // CodecCommands are sent, so ReadResponse() never runs and jack + // events would otherwise go unnoticed. + if (g_hpPresenceDetect && g_speakerNid) { + DrainUnsolicitedResponses(); + + if (g_jackDebounce > 0) { + g_jackDebounce--; + g_jackEventPending = false; + } else if (g_jackEventPending) { + g_jackEventPending = false; + PollJackState(); + } + } + + // Get current hardware playback position from DMA position buffer + // Each stream's position is at dmaPos[streamIndex * 2] + uint32_t hwPos = g_dmaPos[g_stream.StreamIndex * 2]; + uint32_t writePos = g_stream.WritePos; + + // Calculate available space in the ring buffer + uint32_t available; + if (writePos >= hwPos) { + available = TOTAL_BUFFER_SIZE - (writePos - hwPos); + } else { + available = hwPos - writePos; + } + + // Leave a small gap to avoid write pointer catching up to read pointer + if (available > 64) available -= 64; + else available = 0; + + if (size > available) size = available; + if (size == 0) return 0; + + // Write data to DMA buffer (handle wrap-around) + uint32_t firstChunk = TOTAL_BUFFER_SIZE - writePos; + if (firstChunk > size) firstChunk = size; + + memcpy(g_dmaBuffer + writePos, data, firstChunk); + if (size > firstChunk) { + memcpy(g_dmaBuffer, data + firstChunk, size - firstChunk); + } + + g_stream.WritePos = (writePos + size) % TOTAL_BUFFER_SIZE; + + return (int)size; + } + + int Control(int handle, int cmd, int value) { + if (handle != 0) return -1; + + switch (cmd) { + case AUDIO_CTL_SET_VOLUME: + if (!g_initialized) { g_volume = value; return 0; } + SetOutputVolume(value); + return 0; + + case AUDIO_CTL_GET_VOLUME: + return g_volume; + + case AUDIO_CTL_GET_POS: + if (!g_stream.Active) return 0; + return (int)g_dmaPos[g_stream.StreamIndex * 2]; + + case AUDIO_CTL_PAUSE: + if (!g_stream.Active) return -1; + if (value) + StopStream(g_stream.StreamIndex); + else + StartStream(g_stream.StreamIndex); + return 0; + + default: + return -1; + } + } + +}; diff --git a/kernel/src/Drivers/Audio/IntelHda.hpp b/kernel/src/Drivers/Audio/IntelHda.hpp new file mode 100644 index 0000000..5bcecc0 --- /dev/null +++ b/kernel/src/Drivers/Audio/IntelHda.hpp @@ -0,0 +1,284 @@ +/* + * IntelHda.hpp + * Intel High Definition Audio controller driver + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include +#include + +namespace Drivers::Audio::IntelHda { + + // ========================================================================= + // HDA controller registers (memory-mapped via BAR0) + // Ref: Intel High Definition Audio Specification, Rev 1.0a + // ========================================================================= + + // Global registers + constexpr uint32_t REG_GCAP = 0x00; // Global Capabilities (16-bit) + constexpr uint32_t REG_VMIN = 0x02; // Minor Version (8-bit) + constexpr uint32_t REG_VMAJ = 0x03; // Major Version (8-bit) + constexpr uint32_t REG_OUTPAY = 0x04; // Output Payload Capability (16-bit) + constexpr uint32_t REG_INPAY = 0x06; // Input Payload Capability (16-bit) + constexpr uint32_t REG_GCTL = 0x08; // Global Control (32-bit) + constexpr uint32_t REG_WAKEEN = 0x0C; // Wake Enable (16-bit) + constexpr uint32_t REG_STATESTS = 0x0E; // State Change Status (16-bit) + constexpr uint32_t REG_GSTS = 0x10; // Global Status (16-bit) + constexpr uint32_t REG_INTCTL = 0x20; // Interrupt Control (32-bit) + constexpr uint32_t REG_INTSTS = 0x24; // Interrupt Status (32-bit) + constexpr uint32_t REG_WALCLK = 0x30; // Wall Clock Counter (32-bit) + constexpr uint32_t REG_SSYNC = 0x38; // Stream Synchronization (32-bit) + + // CORB registers + constexpr uint32_t REG_CORBLBASE = 0x40; // CORB Lower Base Address (32-bit) + constexpr uint32_t REG_CORBUBASE = 0x44; // CORB Upper Base Address (32-bit) + constexpr uint32_t REG_CORBWP = 0x48; // CORB Write Pointer (16-bit) + constexpr uint32_t REG_CORBRP = 0x4A; // CORB Read Pointer (16-bit) + constexpr uint32_t REG_CORBCTL = 0x4C; // CORB Control (8-bit) + constexpr uint32_t REG_CORBSTS = 0x4D; // CORB Status (8-bit) + constexpr uint32_t REG_CORBSIZE = 0x4E; // CORB Size (8-bit) + + // RIRB registers + constexpr uint32_t REG_RIRBLBASE = 0x50; // RIRB Lower Base Address (32-bit) + constexpr uint32_t REG_RIRBUBASE = 0x54; // RIRB Upper Base Address (32-bit) + constexpr uint32_t REG_RIRBWP = 0x58; // RIRB Write Pointer (16-bit) + constexpr uint32_t REG_RINTCNT = 0x5A; // Response Interrupt Count (16-bit) + constexpr uint32_t REG_RIRBCTL = 0x5C; // RIRB Control (8-bit) + constexpr uint32_t REG_RIRBSTS = 0x5D; // RIRB Status (8-bit) + constexpr uint32_t REG_RIRBSIZE = 0x5E; // RIRB Size (8-bit) + + // Immediate command interface + constexpr uint32_t REG_ICW = 0x60; // Immediate Command Write (32-bit) + constexpr uint32_t REG_IRR = 0x64; // Immediate Response Read (32-bit) + constexpr uint32_t REG_ICS = 0x68; // Immediate Command Status (16-bit) + + // DMA Position Buffer + constexpr uint32_t REG_DPIBLBASE = 0x70; // DMA Position Lower Base (32-bit) + constexpr uint32_t REG_DPIBUBASE = 0x74; // DMA Position Upper Base (32-bit) + + // GCTL register bits + constexpr uint32_t GCTL_CRST = (1u << 0); // Controller Reset + constexpr uint32_t GCTL_FCNTRL = (1u << 1); // Flush Control + constexpr uint32_t GCTL_UNSOL = (1u << 8); // Accept Unsolicited Responses + + // INTCTL register bits + constexpr uint32_t INTCTL_GIE = (1u << 31); // Global Interrupt Enable + constexpr uint32_t INTCTL_CIE = (1u << 30); // Controller Interrupt Enable + + // CORBCTL register bits + constexpr uint8_t CORBCTL_RUN = (1u << 1); // CORB DMA Engine Run + constexpr uint8_t CORBCTL_MEIE = (1u << 0); // Memory Error Interrupt Enable + + // RIRBCTL register bits + constexpr uint8_t RIRBCTL_RUN = (1u << 1); // RIRB DMA Engine Run + constexpr uint8_t RIRBCTL_RINTCTL = (1u << 0); // Response Interrupt Control + + // RIRBSTS register bits + constexpr uint8_t RIRBSTS_RINTFL = (1u << 0); // Response Interrupt + + // CORBRP register bits + constexpr uint16_t CORBRP_RST = (1u << 15); // CORB Read Pointer Reset + + // RIRBWP register bits + constexpr uint16_t RIRBWP_RST = (1u << 15); // RIRB Write Pointer Reset + + // ========================================================================= + // Stream Descriptor registers (offset = 0x80 + streamIndex * 0x20) + // ========================================================================= + + constexpr uint32_t SD_BASE = 0x80; + constexpr uint32_t SD_SIZE = 0x20; + + // Stream descriptor register offsets (relative to stream base) + constexpr uint32_t SD_CTL = 0x00; // Control (24-bit: bytes 0,1,2) + constexpr uint32_t SD_STS = 0x03; // Status (8-bit) + constexpr uint32_t SD_LPIB = 0x04; // Link Position in Current Buffer (32-bit) + constexpr uint32_t SD_CBL = 0x08; // Cyclic Buffer Length (32-bit) + constexpr uint32_t SD_LVI = 0x0C; // Last Valid Index (16-bit) + constexpr uint32_t SD_FIFOS = 0x10; // FIFO Size (16-bit, read-only) + constexpr uint32_t SD_FMT = 0x12; // Format (16-bit) + constexpr uint32_t SD_BDPL = 0x18; // BDL Pointer Lower (32-bit) + constexpr uint32_t SD_BDPU = 0x1C; // BDL Pointer Upper (32-bit) + + // Stream control bits (CTL is 24-bit, accessed as 3 bytes) + constexpr uint8_t SD_CTL0_SRST = (1u << 0); // Stream Reset + constexpr uint8_t SD_CTL0_RUN = (1u << 1); // Stream Run + constexpr uint8_t SD_CTL0_IOCE = (1u << 2); // Interrupt On Completion Enable + constexpr uint8_t SD_CTL0_FEIE = (1u << 3); // FIFO Error Interrupt Enable + constexpr uint8_t SD_CTL0_DEIE = (1u << 4); // Descriptor Error Interrupt Enable + // CTL byte 2 (offset +2): bits 23:20 = Stream Number (1-15) + + // Stream status bits + constexpr uint8_t SD_STS_BCIS = (1u << 2); // Buffer Completion Interrupt Status + constexpr uint8_t SD_STS_FIFOE = (1u << 3); // FIFO Error + constexpr uint8_t SD_STS_DESE = (1u << 4); // Descriptor Error + + // ========================================================================= + // Stream format register encoding (SD_FMT, 16-bit) + // ========================================================================= + + // Bit 15: Stream Type (0=PCM, 1=non-PCM) + // Bits 14: Base Rate (0=48kHz, 1=44.1kHz) + // Bits 13:11: Sample Rate Multiplier (0=x1, 1=x2, 2=x3, 3=x4) + // Bits 10:8: Sample Rate Divisor (0=/1, 1=/2, 2=/3, ..., 7=/8) + // Bits 7:4: Bits Per Sample (000=8, 001=16, 010=20, 011=24, 100=32) + // Bits 3:0: Number of Channels - 1 + + constexpr uint16_t FMT_BASE_44K = (1u << 14); + constexpr uint16_t FMT_BASE_48K = 0; + + // ========================================================================= + // Buffer Descriptor List Entry (16 bytes) + // ========================================================================= + + struct BdlEntry { + uint64_t Address; // Physical address of buffer + uint32_t Length; // Length in bytes + uint32_t Ioc; // Bit 0: Interrupt on Completion + } __attribute__((packed)); + + static_assert(sizeof(BdlEntry) == 16, "BDL entry must be 16 bytes"); + + // ========================================================================= + // RIRB response entry (8 bytes) + // ========================================================================= + + struct RirbEntry { + uint32_t Response; + uint32_t ResponseEx; // Bits 3:0 = Codec Address, Bit 4 = Unsolicited + } __attribute__((packed)); + + static_assert(sizeof(RirbEntry) == 8, "RIRB entry must be 8 bytes"); + + // ========================================================================= + // HDA Codec verbs + // ========================================================================= + + // Verb construction: (codec << 28) | (nid << 20) | verb + // Get Parameter: verb = 0xF0000 | paramId + // Set Converter Format: verb = 0x20000 | format + // Set Amp Gain/Mute: verb = 0x30000 | payload + // Set Converter Stream/Channel: verb = 0x70600 | (stream << 4) | channel + // Set Pin Widget Control: verb = 0x70700 | value + // Set EAPD/BTL Enable: verb = 0x70C00 | value + // Set Power State: verb = 0x70500 | state + // Get Config Default: verb = 0xF1C00 + // Set Connection Select: verb = 0x70100 | index + + // Parameter IDs + constexpr uint32_t PARAM_VENDOR_ID = 0x00; + constexpr uint32_t PARAM_REVISION_ID = 0x02; + constexpr uint32_t PARAM_SUB_NODE_COUNT = 0x04; + constexpr uint32_t PARAM_FN_GROUP_TYPE = 0x05; + constexpr uint32_t PARAM_AUDIO_WIDGET_CAP = 0x09; + constexpr uint32_t PARAM_PCM_RATES = 0x0A; + constexpr uint32_t PARAM_STREAM_FORMATS = 0x0B; + constexpr uint32_t PARAM_PIN_CAPS = 0x0C; + constexpr uint32_t PARAM_INPUT_AMP_CAP = 0x0D; + constexpr uint32_t PARAM_CONN_LIST_LEN = 0x0E; + constexpr uint32_t PARAM_POWER_STATES = 0x0F; + constexpr uint32_t PARAM_OUTPUT_AMP_CAP = 0x12; + + // Widget types (bits 23:20 of Audio Widget Capabilities) + constexpr uint8_t WIDGET_AUDIO_OUTPUT = 0x0; + constexpr uint8_t WIDGET_AUDIO_INPUT = 0x1; + constexpr uint8_t WIDGET_AUDIO_MIXER = 0x2; + constexpr uint8_t WIDGET_AUDIO_SELECTOR = 0x3; + constexpr uint8_t WIDGET_PIN_COMPLEX = 0x4; + constexpr uint8_t WIDGET_POWER = 0x5; + constexpr uint8_t WIDGET_VOLUME_KNOB = 0x6; + constexpr uint8_t WIDGET_BEEP_GEN = 0x7; + constexpr uint8_t WIDGET_VENDOR_DEFINED = 0xF; + + // Pin default config: device type (bits 23:20) + constexpr uint8_t PIN_DEV_LINE_OUT = 0x0; + constexpr uint8_t PIN_DEV_SPEAKER = 0x1; + constexpr uint8_t PIN_DEV_HP_OUT = 0x2; + constexpr uint8_t PIN_DEV_CD = 0x3; + constexpr uint8_t PIN_DEV_SPDIF_OUT = 0x4; + constexpr uint8_t PIN_DEV_LINE_IN = 0x8; + constexpr uint8_t PIN_DEV_MIC_IN = 0xA; + + // Pin widget control bits + constexpr uint8_t PIN_CTL_ENABLE_OUTPUT = (1u << 6); // OUT Enable + constexpr uint8_t PIN_CTL_ENABLE_INPUT = (1u << 5); // IN Enable + constexpr uint8_t PIN_CTL_ENABLE_HP = (1u << 7); // Headphone enable + + // EAPD/BTL bits + constexpr uint8_t EAPD_ENABLE = (1u << 1); + + // Amp gain/mute verb payload bits + constexpr uint16_t AMP_SET_OUTPUT = (1u << 15); + constexpr uint16_t AMP_SET_INPUT = (1u << 14); + constexpr uint16_t AMP_SET_LEFT = (1u << 13); + constexpr uint16_t AMP_SET_RIGHT = (1u << 12); + constexpr uint16_t AMP_MUTE = (1u << 7); + + // ========================================================================= + // Ring buffer sizes + // ========================================================================= + + constexpr int CORB_ENTRIES = 256; // 256 entries * 4 bytes = 1024 bytes + constexpr int RIRB_ENTRIES = 256; // 256 entries * 8 bytes = 2048 bytes + constexpr int BDL_MAX_ENTRIES = 256; // Max BDL entries per stream + + // ========================================================================= + // DMA buffer configuration + // ========================================================================= + + constexpr int BUFFER_COUNT = 2; // Double-buffered + constexpr int BUFFER_SIZE = 0x4000; // 16 KiB per buffer segment + constexpr int TOTAL_BUFFER_SIZE = BUFFER_COUNT * BUFFER_SIZE; + + // ========================================================================= + // MSI configuration + // ========================================================================= + + constexpr uint8_t MSI_IRQ = 27; // IRQ slot 27 = vector 59 + constexpr uint32_t MSI_VECTOR = 59; + constexpr uint32_t MSI_ADDR_BASE = 0xFEE00000; + + // ========================================================================= + // Audio stream state (one active output stream) + // ========================================================================= + + struct AudioStream { + bool Active; + uint32_t SampleRate; + uint8_t Channels; + uint8_t BitsPerSample; + uint8_t StreamIndex; // HDA stream index + uint8_t StreamTag; // HDA stream tag (1-15) + volatile uint32_t WritePos; // Write position in ring buffer (bytes) + }; + + // ========================================================================= + // Public API + // ========================================================================= + + bool Probe(const Pci::PciDevice& dev); + bool IsInitialized(); + + // Returns the codec vendor/device ID (vendor in upper 16 bits, device in lower 16). + // Returns 0 if no codec was found. + uint32_t GetCodecVendorId(); + + // Open an output stream. Returns stream handle (0) or -1 on failure. + int Open(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample); + + // Close an output stream. + void Close(int handle); + + // Write PCM sample data to the stream buffer. + // Returns number of bytes written (may be less than requested if buffer full). + int Write(int handle, const uint8_t* data, uint32_t size); + + // Control commands + constexpr int AUDIO_CTL_SET_VOLUME = 0; // value: 0-100 + constexpr int AUDIO_CTL_GET_VOLUME = 1; + constexpr int AUDIO_CTL_GET_POS = 2; // returns playback position in bytes + constexpr int AUDIO_CTL_PAUSE = 3; // value: 1=pause, 0=resume + int Control(int handle, int cmd, int value); + +}; diff --git a/kernel/src/Drivers/Init.cpp b/kernel/src/Drivers/Init.cpp index a66acef..55cafdf 100644 --- a/kernel/src/Drivers/Init.cpp +++ b/kernel/src/Drivers/Init.cpp @@ -11,7 +11,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -65,6 +67,14 @@ namespace Drivers { return Storage::Ahci::Probe(dev); } + static bool ProbeNvme(const Pci::PciDevice& dev) { + return Storage::Nvme::Probe(dev); + } + + static bool ProbeIntelHda(const Pci::PciDevice& dev) { + return Audio::IntelHda::Probe(dev); + } + // ------------------------------------------------------------------------- // Driver table // ------------------------------------------------------------------------- @@ -126,6 +136,32 @@ namespace Drivers { Pci::ProbePhase::Normal, ProbeAhci, }, + // Order 6: NVMe — Normal phase, match class=0x01/0x08/0x02 (NVM Express) + { + "NVMe", + 0, // VendorId (any) + 0x01, // ClassCode (Mass Storage) + 0x08, // SubClass (Non-Volatile Memory) + 0x02, // ProgIf (NVM Express) + nullptr, + 0, + Pci::ProbePhase::Normal, + ProbeNvme, + }, + // Order 7: Intel HDA — Normal phase, match vendor=0x8086 + class=0x04 (Multimedia) + // SubClass 0x01 = "Multimedia audio controller" (most Intel HDA) + // SubClass 0x03 = "Audio device" (HDA-compatible) + { + "IntelHDA", + 0x8086, // VendorId (Intel) + 0x04, // ClassCode (Multimedia) + 0xFF, // SubClass (any — covers both 0x01 and 0x03) + 0xFF, // ProgIf (any) + nullptr, + 0, + Pci::ProbePhase::Normal, + ProbeIntelHda, + }, }; static constexpr uint16_t g_driverTableCount = sizeof(g_driverTable) / sizeof(g_driverTable[0]); @@ -163,4 +199,10 @@ namespace Drivers { Storage::Gpt::ProbeAll(); } + void InitializeAudio() { + // HDA driver initializes during ProbeNormal(). + // Bluetooth audio (A2DP) initializes automatically during USB enumeration + // when a Bluetooth adapter is detected by the xHCI driver. + } + } diff --git a/kernel/src/Drivers/Init.hpp b/kernel/src/Drivers/Init.hpp index 3ac080c..d19d60f 100644 --- a/kernel/src/Drivers/Init.hpp +++ b/kernel/src/Drivers/Init.hpp @@ -14,7 +14,8 @@ namespace Drivers { // Post-probe: wire up GPU framebuffer to cursor subsystem. void InitializeGraphics(); - // Probe PCI devices for Normal-phase drivers (xHCI, E1000, E1000E, AHCI). + // Probe PCI devices for Normal-phase drivers (xHCI, E1000, E1000E, AHCI, NVMe, IntelHDA). + // USB class drivers (Bluetooth, etc.) initialize automatically during xHCI enumeration. void ProbeNormal(); // Post-probe: initialize network stack. @@ -23,4 +24,7 @@ namespace Drivers { // Post-probe: register SATA drives with VFS. void InitializeStorage(); + // Post-probe: initialize audio subsystem. + void InitializeAudio(); + } diff --git a/kernel/src/Drivers/Storage/Nvme.cpp b/kernel/src/Drivers/Storage/Nvme.cpp new file mode 100644 index 0000000..e0fcd13 --- /dev/null +++ b/kernel/src/Drivers/Storage/Nvme.cpp @@ -0,0 +1,829 @@ +/* + * Nvme.cpp + * NVM Express (NVMe) storage driver + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "Nvme.hpp" +#include "BlockDevice.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Kt; + +namespace Drivers::Storage::Nvme { + + // ------------------------------------------------------------------------- + // Driver state + // ------------------------------------------------------------------------- + + static bool g_initialized = false; + static volatile uint8_t* g_mmioBase = nullptr; + static uint32_t g_doorbellStride = 0; // in bytes (4 << DSTRD) + static uint16_t g_maxQueueEntries = 0; // CAP.MQES + 1 + + // Admin queue state + static SqEntry* g_adminSq = nullptr; + static CqEntry* g_adminCq = nullptr; + static uint64_t g_adminSqPhys = 0; + static uint64_t g_adminCqPhys = 0; + static uint16_t g_adminSqTail = 0; + static uint16_t g_adminCqHead = 0; + static uint8_t g_adminCqPhase = 1; // Expected phase bit starts at 1 + static uint16_t g_adminCmdId = 0; + + // I/O queue state (single I/O queue pair) + static SqEntry* g_ioSq = nullptr; + static CqEntry* g_ioCq = nullptr; + static uint64_t g_ioSqPhys = 0; + static uint64_t g_ioCqPhys = 0; + static uint16_t g_ioSqTail = 0; + static uint16_t g_ioCqHead = 0; + static uint8_t g_ioCqPhase = 1; + static uint16_t g_ioCmdId = 0; + static uint16_t g_ioSqDepth = 0; + static uint16_t g_ioCqDepth = 0; + + // Namespace state + static int g_nsCount = 0; + static NamespaceInfo g_namespaces[MAX_NAMESPACES] = {}; + + // Controller identify data + static uint32_t g_mdts = 0; // Max Data Transfer Size in pages (0 = unlimited) + + // ------------------------------------------------------------------------- + // Register access + // ------------------------------------------------------------------------- + + static void WriteReg32(uint32_t reg, uint32_t value) { + *(volatile uint32_t*)(g_mmioBase + reg) = value; + } + + static uint32_t ReadReg32(uint32_t reg) { + return *(volatile uint32_t*)(g_mmioBase + reg); + } + + static void WriteReg64(uint32_t reg, uint64_t value) { + *(volatile uint32_t*)(g_mmioBase + reg) = (uint32_t)(value & 0xFFFFFFFF); + *(volatile uint32_t*)(g_mmioBase + reg + 4) = (uint32_t)(value >> 32); + } + + static uint64_t ReadReg64(uint32_t reg) { + uint32_t lo = *(volatile uint32_t*)(g_mmioBase + reg); + uint32_t hi = *(volatile uint32_t*)(g_mmioBase + reg + 4); + return (uint64_t)lo | ((uint64_t)hi << 32); + } + + // ------------------------------------------------------------------------- + // Doorbell registers + // NVMe spec: SQ y Tail Doorbell offset = 0x1000 + (2y * doorbellStride) + // CQ y Head Doorbell offset = 0x1000 + ((2y+1) * doorbellStride) + // ------------------------------------------------------------------------- + + static void WriteSqTailDoorbell(uint16_t queueId, uint16_t value) { + uint32_t offset = 0x1000 + (2 * queueId) * g_doorbellStride; + *(volatile uint32_t*)(g_mmioBase + offset) = value; + } + + static void WriteCqHeadDoorbell(uint16_t queueId, uint16_t value) { + uint32_t offset = 0x1000 + (2 * queueId + 1) * g_doorbellStride; + *(volatile uint32_t*)(g_mmioBase + offset) = value; + } + + // ------------------------------------------------------------------------- + // DMA buffer allocation + // ------------------------------------------------------------------------- + + static void* AllocateDmaBuffer(uint64_t& outPhys, int pages = 1) { + void* virt; + if (pages == 1) { + virt = Memory::g_pfa->AllocateZeroed(); + } else { + virt = Memory::g_pfa->ReallocConsecutive(nullptr, pages); + memset(virt, 0, pages * 0x1000); + } + outPhys = Memory::SubHHDM(virt); + return virt; + } + + // ------------------------------------------------------------------------- + // Admin command submission + // ------------------------------------------------------------------------- + + static void SubmitAdminCommand(SqEntry& cmd) { + cmd.CommandId = g_adminCmdId++; + g_adminSq[g_adminSqTail] = cmd; + g_adminSqTail = (g_adminSqTail + 1) % ADMIN_QUEUE_DEPTH; + WriteSqTailDoorbell(0, g_adminSqTail); + } + + static bool WaitAdminCompletion(CqEntry& out) { + for (int i = 0; i < 5000000; i++) { + CqEntry* cqe = &g_adminCq[g_adminCqHead]; + uint16_t status = cqe->Status; + + // Check phase bit matches expected + if ((status & CQE_PHASE_BIT) == g_adminCqPhase) { + out = *cqe; + + // Advance CQ head + g_adminCqHead++; + if (g_adminCqHead >= ADMIN_QUEUE_DEPTH) { + g_adminCqHead = 0; + g_adminCqPhase ^= 1; // Toggle expected phase + } + WriteCqHeadDoorbell(0, g_adminCqHead); + + // Check status code (bits 15:1, 0 = success) + if (status & CQE_STATUS_MASK) { + KernelLogStream(ERROR, "NVMe") << "Admin command failed, status=" + << base::hex << (uint64_t)(status >> 1); + return false; + } + return true; + } + asm volatile("" ::: "memory"); + } + + KernelLogStream(ERROR, "NVMe") << "Admin command timeout"; + return false; + } + + // Submit an admin command and wait for completion + static bool AdminCommand(SqEntry& cmd, CqEntry& cqe) { + SubmitAdminCommand(cmd); + return WaitAdminCompletion(cqe); + } + + // ------------------------------------------------------------------------- + // I/O command submission + // ------------------------------------------------------------------------- + + static void SubmitIoCommand(SqEntry& cmd) { + cmd.CommandId = g_ioCmdId++; + g_ioSq[g_ioSqTail] = cmd; + g_ioSqTail = (g_ioSqTail + 1) % g_ioSqDepth; + WriteSqTailDoorbell(1, g_ioSqTail); + } + + static bool WaitIoCompletion(CqEntry& out) { + for (int i = 0; i < 5000000; i++) { + CqEntry* cqe = &g_ioCq[g_ioCqHead]; + uint16_t status = cqe->Status; + + if ((status & CQE_PHASE_BIT) == g_ioCqPhase) { + out = *cqe; + + g_ioCqHead++; + if (g_ioCqHead >= g_ioCqDepth) { + g_ioCqHead = 0; + g_ioCqPhase ^= 1; + } + WriteCqHeadDoorbell(1, g_ioCqHead); + + if (status & CQE_STATUS_MASK) { + KernelLogStream(ERROR, "NVMe") << "I/O command failed, status=" + << base::hex << (uint64_t)(status >> 1); + return false; + } + return true; + } + asm volatile("" ::: "memory"); + } + + KernelLogStream(ERROR, "NVMe") << "I/O command timeout"; + return false; + } + + // ------------------------------------------------------------------------- + // Controller disable/enable + // ------------------------------------------------------------------------- + + static bool DisableController() { + uint32_t cc = ReadReg32(REG_CC); + cc &= ~CC_EN; + WriteReg32(REG_CC, cc); + + // Wait for CSTS.RDY to become 0 + for (int i = 0; i < 5000000; i++) { + uint32_t csts = ReadReg32(REG_CSTS); + if (csts & CSTS_CFS) { + KernelLogStream(ERROR, "NVMe") << "Controller fatal status during disable"; + return false; + } + if (!(csts & CSTS_RDY)) { + return true; + } + asm volatile("" ::: "memory"); + } + + KernelLogStream(ERROR, "NVMe") << "Controller disable timeout"; + return false; + } + + static bool EnableController() { + // Configure CC: enable, NVM command set, 4 KiB pages, + // SQ entry size = 64 bytes (2^6), CQ entry size = 16 bytes (2^4) + uint32_t cc = CC_EN | CC_CSS_NVM | CC_AMS_RR | CC_SHN_NONE; + cc |= (0u << CC_MPS_SHIFT); // MPS = 0 => 4 KiB pages + cc |= (6u << CC_IOSQES_SHIFT); // SQ entry = 2^6 = 64 bytes + cc |= (4u << CC_IOCQES_SHIFT); // CQ entry = 2^4 = 16 bytes + WriteReg32(REG_CC, cc); + + // Wait for CSTS.RDY + for (int i = 0; i < 5000000; i++) { + uint32_t csts = ReadReg32(REG_CSTS); + if (csts & CSTS_CFS) { + KernelLogStream(ERROR, "NVMe") << "Controller fatal status during enable"; + return false; + } + if (csts & CSTS_RDY) { + return true; + } + asm volatile("" ::: "memory"); + } + + KernelLogStream(ERROR, "NVMe") << "Controller enable timeout"; + return false; + } + + // ------------------------------------------------------------------------- + // Admin queue setup + // ------------------------------------------------------------------------- + + static bool SetupAdminQueues() { + // Allocate Admin Submission Queue (ADMIN_QUEUE_DEPTH * 64 bytes) + // At 32 entries * 64 bytes = 2048 bytes, fits in 1 page + g_adminSq = (SqEntry*)AllocateDmaBuffer(g_adminSqPhys); + + // Allocate Admin Completion Queue (ADMIN_QUEUE_DEPTH * 16 bytes) + // At 32 entries * 16 bytes = 512 bytes, fits in 1 page + g_adminCq = (CqEntry*)AllocateDmaBuffer(g_adminCqPhys); + + g_adminSqTail = 0; + g_adminCqHead = 0; + g_adminCqPhase = 1; + g_adminCmdId = 0; + + // Set Admin Queue Attributes + // AQA: bits 27:16 = ACQS (CQ size - 1), bits 11:0 = ASQS (SQ size - 1) + uint32_t aqa = ((ADMIN_QUEUE_DEPTH - 1) << 16) | (ADMIN_QUEUE_DEPTH - 1); + WriteReg32(REG_AQA, aqa); + + // Set Admin SQ and CQ base addresses + WriteReg64(REG_ASQ, g_adminSqPhys); + WriteReg64(REG_ACQ, g_adminCqPhys); + + return true; + } + + // ------------------------------------------------------------------------- + // Identify Controller + // ------------------------------------------------------------------------- + + static bool IdentifyController() { + uint64_t identPhys; + uint8_t* identData = (uint8_t*)AllocateDmaBuffer(identPhys); + + SqEntry cmd = {}; + cmd.Opcode = ADMIN_IDENTIFY; + cmd.Nsid = 0; + cmd.Prp1 = identPhys; + cmd.Prp2 = 0; + cmd.Cdw10 = IDENTIFY_CNS_CONTROLLER; + + CqEntry cqe; + if (!AdminCommand(cmd, cqe)) { + KernelLogStream(ERROR, "NVMe") << "Identify Controller failed"; + Memory::g_pfa->Free(identData); + return false; + } + + // Parse controller data (4096 bytes) + // Bytes 24-63: Serial Number (20 bytes, space-padded ASCII) + // Bytes 64-103: Model Number (40 bytes, space-padded ASCII) + // Bytes 104-111: Firmware Revision (8 bytes) + // Byte 77: MDTS (Maximum Data Transfer Size, in units of CAP.MPSMIN pages) + + // Model string + char model[41]; + memcpy(model, identData + 24, 40); + model[40] = '\0'; + // Trim trailing spaces + for (int i = 39; i >= 0; i--) { + if (model[i] == ' ' || model[i] == '\0') { + model[i] = '\0'; + } else { + break; + } + } + + // MDTS + uint8_t mdtsPower = identData[77]; + if (mdtsPower > 0) { + g_mdts = (1u << mdtsPower); // In minimum page size units (4 KiB pages) + } else { + g_mdts = 0; // No limit + } + + // NN (Number of Namespaces): bytes 516-519 + uint32_t nn = *(uint32_t*)(identData + 516); + + KernelLogStream(OK, "NVMe") << "Controller: " << model; + KernelLogStream(INFO, "NVMe") << "MDTS: " << (g_mdts ? (uint64_t)(g_mdts * 4) : 0) + << " KiB, Namespaces: " << (uint64_t)nn; + + // Copy model to all namespaces (will be used during registration) + for (int i = 0; i < MAX_NAMESPACES; i++) { + memcpy(g_namespaces[i].Model, model, 41); + } + + // Enumerate active namespaces + if (nn > (uint32_t)MAX_NAMESPACES) nn = MAX_NAMESPACES; + + for (uint32_t nsid = 1; nsid <= nn; nsid++) { + // Identify Namespace + uint64_t nsIdentPhys; + uint8_t* nsIdentData = (uint8_t*)AllocateDmaBuffer(nsIdentPhys); + + SqEntry nsCmd = {}; + nsCmd.Opcode = ADMIN_IDENTIFY; + nsCmd.Nsid = nsid; + nsCmd.Prp1 = nsIdentPhys; + nsCmd.Prp2 = 0; + nsCmd.Cdw10 = IDENTIFY_CNS_NAMESPACE; + + CqEntry nsCqe; + if (!AdminCommand(nsCmd, nsCqe)) { + KernelLogStream(WARNING, "NVMe") << "Identify Namespace " << (uint64_t)nsid << " failed"; + Memory::g_pfa->Free(nsIdentData); + continue; + } + + // Bytes 0-7: NSZE (Namespace Size in LBAs) + uint64_t nsze = *(uint64_t*)(nsIdentData + 0); + + if (nsze == 0) { + Memory::g_pfa->Free(nsIdentData); + continue; + } + + // Byte 26: NLBAF (Number of LBA Formats, 0-based) + // Byte 25:24 - FLBAS (Formatted LBA Size) + // bits 3:0 = index of the LBA format in use + uint8_t flbas = nsIdentData[26]; + uint8_t lbaFmtIdx = flbas & 0x0F; + + // LBA Format descriptors start at byte 128, each is 4 bytes + // Bits 23:16 = LBADS (LBA Data Size as power of 2) + uint32_t lbaFmt = *(uint32_t*)(nsIdentData + 128 + lbaFmtIdx * 4); + uint8_t lbads = (lbaFmt >> 16) & 0xFF; + uint32_t sectorSize = (1u << lbads); + + int idx = g_nsCount; + g_namespaces[idx].Active = true; + g_namespaces[idx].Nsid = nsid; + g_namespaces[idx].SectorCount = nsze; + g_namespaces[idx].SectorSize = sectorSize; + + // Compute max transfer in blocks + if (g_mdts > 0) { + g_namespaces[idx].MaxTransferBlocks = (g_mdts * 0x1000) / sectorSize; + } else { + // Conservative default: 128 sectors (64 KiB for 512-byte sectors) + g_namespaces[idx].MaxTransferBlocks = 128; + } + + g_nsCount++; + + uint64_t sizeBytes = nsze * sectorSize; + uint64_t sizeMB = sizeBytes / (1024 * 1024); + uint64_t sizeGB = sizeMB / 1024; + + if (sizeGB > 0) { + KernelLogStream(OK, "NVMe") << "Namespace " << (uint64_t)nsid + << ": " << sizeGB << " GiB (" << (uint64_t)sectorSize << " B/sector)"; + } else { + KernelLogStream(OK, "NVMe") << "Namespace " << (uint64_t)nsid + << ": " << sizeMB << " MiB (" << (uint64_t)sectorSize << " B/sector)"; + } + + Memory::g_pfa->Free(nsIdentData); + } + + Memory::g_pfa->Free(identData); + return g_nsCount > 0; + } + + // ------------------------------------------------------------------------- + // Create I/O queues + // ------------------------------------------------------------------------- + + static bool SetNumberOfQueues(uint16_t& sqCount, uint16_t& cqCount) { + SqEntry cmd = {}; + cmd.Opcode = ADMIN_SET_FEATURES; + cmd.Cdw10 = FEATURE_NUM_QUEUES; + // CDW11: bits 31:16 = number of CQs requested (0-based) + // bits 15:0 = number of SQs requested (0-based) + cmd.Cdw11 = ((uint32_t)(cqCount - 1) << 16) | (sqCount - 1); + + CqEntry cqe; + if (!AdminCommand(cmd, cqe)) { + return false; + } + + // Result DW0 contains allocated counts (0-based) + sqCount = (cqe.Result & 0xFFFF) + 1; + cqCount = ((cqe.Result >> 16) & 0xFFFF) + 1; + return true; + } + + static bool CreateIoQueues() { + // Request 1 SQ + 1 CQ + uint16_t sqCount = 1; + uint16_t cqCount = 1; + if (!SetNumberOfQueues(sqCount, cqCount)) { + KernelLogStream(ERROR, "NVMe") << "Set Number of Queues failed"; + return false; + } + + KernelLogStream(INFO, "NVMe") << "Allocated " << (uint64_t)sqCount + << " SQ(s), " << (uint64_t)cqCount << " CQ(s)"; + + // Determine queue depths (capped by controller max) + g_ioSqDepth = IO_QUEUE_DEPTH; + g_ioCqDepth = IO_QUEUE_DEPTH; + if (g_ioSqDepth > g_maxQueueEntries) g_ioSqDepth = g_maxQueueEntries; + if (g_ioCqDepth > g_maxQueueEntries) g_ioCqDepth = g_maxQueueEntries; + + // Allocate I/O CQ + int cqPages = ((uint32_t)g_ioCqDepth * sizeof(CqEntry) + 0xFFF) / 0x1000; + g_ioCq = (CqEntry*)AllocateDmaBuffer(g_ioCqPhys, cqPages); + g_ioCqHead = 0; + g_ioCqPhase = 1; + + // Create I/O Completion Queue (queue ID = 1) + { + SqEntry cmd = {}; + cmd.Opcode = ADMIN_CREATE_IO_CQ; + cmd.Prp1 = g_ioCqPhys; + // CDW10: bits 31:16 = queue size (0-based), bits 15:0 = queue ID + cmd.Cdw10 = ((uint32_t)(g_ioCqDepth - 1) << 16) | 1; + // CDW11: bit 0 = physically contiguous, bit 1 = interrupts enabled + // bits 31:16 = interrupt vector + cmd.Cdw11 = (1u << 0) | (1u << 1) | (0u << 16); + + CqEntry cqe; + if (!AdminCommand(cmd, cqe)) { + KernelLogStream(ERROR, "NVMe") << "Create I/O CQ failed"; + return false; + } + } + + // Allocate I/O SQ + int sqPages = ((uint32_t)g_ioSqDepth * sizeof(SqEntry) + 0xFFF) / 0x1000; + g_ioSq = (SqEntry*)AllocateDmaBuffer(g_ioSqPhys, sqPages); + g_ioSqTail = 0; + g_ioCmdId = 0; + + // Create I/O Submission Queue (queue ID = 1, linked to CQ ID = 1) + { + SqEntry cmd = {}; + cmd.Opcode = ADMIN_CREATE_IO_SQ; + cmd.Prp1 = g_ioSqPhys; + // CDW10: bits 31:16 = queue size (0-based), bits 15:0 = queue ID + cmd.Cdw10 = ((uint32_t)(g_ioSqDepth - 1) << 16) | 1; + // CDW11: bit 0 = physically contiguous, bits 31:16 = CQ ID + cmd.Cdw11 = (1u << 0) | (1u << 16); + + CqEntry cqe; + if (!AdminCommand(cmd, cqe)) { + KernelLogStream(ERROR, "NVMe") << "Create I/O SQ failed"; + return false; + } + } + + KernelLogStream(OK, "NVMe") << "I/O queues created (depth " << (uint64_t)g_ioSqDepth << ")"; + return true; + } + + // ------------------------------------------------------------------------- + // Interrupt handler + // ------------------------------------------------------------------------- + + static void HandleInterrupt(uint8_t irq) { + (void)irq; + // NVMe uses polling-based completion in this driver. + // The interrupt handler just acknowledges the interrupt. + // Completions are consumed in the polling loops above. + } + + // ------------------------------------------------------------------------- + // MSI setup + // ------------------------------------------------------------------------- + + static bool SetupMsi(uint8_t bus, uint8_t dev, uint8_t func) { + uint8_t cap = Pci::FindCapability(bus, dev, func, Pci::PCI_CAP_MSI); + if (cap == 0) { + KernelLogStream(INFO, "NVMe") << "MSI capability not found"; + return false; + } + + uint16_t msgCtrl = Pci::LegacyRead16(bus, dev, func, cap + 2); + bool is64bit = (msgCtrl & (1 << 7)) != 0; + + Pci::LegacyWrite32(bus, dev, func, cap + 4, MSI_ADDR_BASE); + + if (is64bit) { + Pci::LegacyWrite32(bus, dev, func, cap + 8, 0); + Pci::LegacyWrite16(bus, dev, func, cap + 12, MSI_VECTOR); + } else { + Pci::LegacyWrite16(bus, dev, func, cap + 8, MSI_VECTOR); + } + + msgCtrl &= ~(0x70); // Single message + msgCtrl |= (1 << 0); // MSI Enable + Pci::LegacyWrite16(bus, dev, func, cap + 2, msgCtrl); + + uint16_t pciCmd = Pci::LegacyRead16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND); + pciCmd |= Pci::PCI_CMD_INTX_DISABLE; + Pci::LegacyWrite16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND, pciCmd); + + Hal::RegisterIrqHandler(MSI_IRQ, HandleInterrupt); + + KernelLogStream(OK, "NVMe") << "MSI enabled: vector " << base::dec << (uint64_t)MSI_VECTOR; + return true; + } + + // ------------------------------------------------------------------------- + // Probe (PCI driver entry point) + // ------------------------------------------------------------------------- + + bool Probe(const Pci::PciDevice& dev) { + if (g_initialized) return false; + + KernelLogStream(OK, "NVMe") << "Found NVMe controller at PCI " + << base::hex << (uint64_t)dev.Bus << ":" + << (uint64_t)dev.Device << "." << (uint64_t)dev.Function + << " (" << (uint64_t)dev.VendorId << ":" << (uint64_t)dev.DeviceId << ")"; + + // NVMe uses BAR0 for MMIO registers + uint64_t mmioPhys = Pci::ReadBar0(dev.Bus, dev.Device, dev.Function); + if (mmioPhys == 0) { + KernelLogStream(ERROR, "NVMe") << "BAR0 is zero"; + return false; + } + + KernelLogStream(INFO, "NVMe") << "BAR0 physical: " << base::hex << mmioPhys; + + // Map MMIO region. NVMe requires at least the controller registers (0x1000) + // plus doorbell registers. Map 16 KiB to cover admin + a few I/O doorbells. + constexpr uint64_t MmioSize = 0x4000; + for (uint64_t offset = 0; offset < MmioSize; offset += 0x1000) { + Memory::VMM::g_paging->MapMMIO(mmioPhys + offset, Memory::HHDM(mmioPhys + offset)); + } + + g_mmioBase = (volatile uint8_t*)Memory::HHDM(mmioPhys); + + // Enable bus mastering and memory space + Pci::EnableBusMaster(dev.Bus, dev.Device, dev.Function); + + // Read capabilities + uint64_t cap = ReadReg64(REG_CAP); + g_maxQueueEntries = (uint16_t)((cap & CAP_MQES_MASK) + 1); + uint32_t dstrd = (uint32_t)((cap & CAP_DSTRD_MASK) >> CAP_DSTRD_SHIFT); + g_doorbellStride = 4u << dstrd; + + // Read version + uint32_t vs = ReadReg32(REG_VS); + uint32_t vsMajor = (vs >> 16) & 0xFFFF; + uint32_t vsMinor = (vs >> 8) & 0xFF; + uint32_t vsTertiary = vs & 0xFF; + + KernelLogStream(INFO, "NVMe") << "Version: " << base::dec + << (uint64_t)vsMajor << "." << (uint64_t)vsMinor << "." << (uint64_t)vsTertiary; + KernelLogStream(INFO, "NVMe") << "Max queue entries: " << (uint64_t)g_maxQueueEntries + << ", Doorbell stride: " << (uint64_t)g_doorbellStride << " bytes"; + + // Step 1: Disable controller + if (!DisableController()) { + KernelLogStream(ERROR, "NVMe") << "Failed to disable controller"; + return false; + } + + // Step 2: Set up MSI + bool hasMsi = SetupMsi(dev.Bus, dev.Device, dev.Function); + if (!hasMsi) { + uint8_t irqLine = Pci::LegacyRead8(dev.Bus, dev.Device, dev.Function, + (uint8_t)Pci::PCI_REG_INTERRUPT); + if (irqLine != 0xFF) { + KernelLogStream(INFO, "NVMe") << "Using legacy IRQ " << base::dec << (uint64_t)irqLine; + Hal::RegisterIrqHandler(irqLine, HandleInterrupt); + Hal::IoApic::UnmaskIrq(Hal::IoApic::GetGsiForIrq(irqLine)); + } + } + + // Step 3: Set up admin queues + if (!SetupAdminQueues()) { + KernelLogStream(ERROR, "NVMe") << "Failed to set up admin queues"; + return false; + } + + // Step 4: Enable controller + if (!EnableController()) { + KernelLogStream(ERROR, "NVMe") << "Failed to enable controller"; + return false; + } + + KernelLogStream(OK, "NVMe") << "Controller enabled and ready"; + + // Step 5: Identify controller and namespaces + if (!IdentifyController()) { + KernelLogStream(ERROR, "NVMe") << "Failed to identify controller/namespaces"; + return false; + } + + // Step 6: Create I/O queues + if (!CreateIoQueues()) { + KernelLogStream(ERROR, "NVMe") << "Failed to create I/O queues"; + return false; + } + + // Step 7: Register namespaces as block devices + for (int i = 0; i < g_nsCount; i++) { + Storage::BlockDevice bdev = {}; + bdev.ReadSectors = [](void* ctx, uint64_t lba, uint32_t count, void* buffer) -> bool { + return ReadSectors((int)(uintptr_t)ctx, lba, count, buffer); + }; + bdev.WriteSectors = [](void* ctx, uint64_t lba, uint32_t count, const void* buffer) -> bool { + return WriteSectors((int)(uintptr_t)ctx, lba, count, buffer); + }; + bdev.Ctx = (void*)(uintptr_t)i; + bdev.SectorCount = g_namespaces[i].SectorCount; + bdev.SectorSize = (uint16_t)g_namespaces[i].SectorSize; + memcpy(bdev.Model, g_namespaces[i].Model, 41); + Storage::RegisterBlockDevice(bdev); + } + + g_initialized = true; + + KernelLogStream(OK, "NVMe") << "Initialization complete: " + << base::dec << (uint64_t)g_nsCount << " namespace(s) ready"; + + return true; + } + + // ------------------------------------------------------------------------- + // Public API: Read/Write + // ------------------------------------------------------------------------- + + bool IsInitialized() { + return g_initialized; + } + + int GetNamespaceCount() { + return g_nsCount; + } + + const NamespaceInfo* GetNamespaceInfo(int ns) { + if (ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) { + return nullptr; + } + return &g_namespaces[ns]; + } + + bool ReadSectors(int ns, uint64_t lba, uint32_t count, void* buffer) { + if (!g_initialized || ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) { + return false; + } + if (count == 0 || buffer == nullptr) return false; + + // Limit to max transfer size + uint32_t maxBlocks = g_namespaces[ns].MaxTransferBlocks; + if (count > maxBlocks) { + KernelLogStream(ERROR, "NVMe") << "ReadSectors: count " << count + << " exceeds max " << maxBlocks; + return false; + } + + uint32_t sectorSize = g_namespaces[ns].SectorSize; + uint32_t totalBytes = count * sectorSize; + int pagesNeeded = (totalBytes + 0xFFF) / 0x1000; + + uint64_t dmaPhys; + void* dmaVirt = AllocateDmaBuffer(dmaPhys, pagesNeeded); + + // Build NVMe Read command + SqEntry cmd = {}; + cmd.Opcode = IO_CMD_READ; + cmd.Nsid = g_namespaces[ns].Nsid; + cmd.Prp1 = dmaPhys; + + // PRP2: If transfer spans more than one page, set PRP2 + if (totalBytes > 0x1000) { + // For transfers spanning exactly 2 pages, PRP2 = second page address + // For transfers spanning more pages, PRP2 should point to a PRP list. + // Since our DMA buffer is physically contiguous, we can point to the + // second page directly for 2-page transfers. For larger transfers, + // we build a PRP list. + if (pagesNeeded == 2) { + cmd.Prp2 = dmaPhys + 0x1000; + } else { + // Build a PRP list (array of physical page addresses) + uint64_t prpListPhys; + uint64_t* prpList = (uint64_t*)AllocateDmaBuffer(prpListPhys); + for (int i = 1; i < pagesNeeded; i++) { + prpList[i - 1] = dmaPhys + (uint64_t)i * 0x1000; + } + cmd.Prp2 = prpListPhys; + } + } + + // CDW10-11: Starting LBA (64-bit) + cmd.Cdw10 = (uint32_t)(lba & 0xFFFFFFFF); + cmd.Cdw11 = (uint32_t)(lba >> 32); + // CDW12: bits 15:0 = Number of Logical Blocks (0-based) + cmd.Cdw12 = count - 1; + + SubmitIoCommand(cmd); + + CqEntry cqe; + bool ok = WaitIoCompletion(cqe); + if (ok) { + memcpy(buffer, dmaVirt, totalBytes); + } + + // Free PRP list if we allocated one + if (pagesNeeded > 2 && cmd.Prp2 != 0 && cmd.Prp2 != dmaPhys + 0x1000) { + Memory::g_pfa->Free((void*)Memory::HHDM(cmd.Prp2)); + } + + Memory::g_pfa->Free(dmaVirt, pagesNeeded); + return ok; + } + + bool WriteSectors(int ns, uint64_t lba, uint32_t count, const void* buffer) { + if (!g_initialized || ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) { + return false; + } + if (count == 0 || buffer == nullptr) return false; + + uint32_t maxBlocks = g_namespaces[ns].MaxTransferBlocks; + if (count > maxBlocks) { + KernelLogStream(ERROR, "NVMe") << "WriteSectors: count " << count + << " exceeds max " << maxBlocks; + return false; + } + + uint32_t sectorSize = g_namespaces[ns].SectorSize; + uint32_t totalBytes = count * sectorSize; + int pagesNeeded = (totalBytes + 0xFFF) / 0x1000; + + uint64_t dmaPhys; + void* dmaVirt = AllocateDmaBuffer(dmaPhys, pagesNeeded); + memcpy(dmaVirt, buffer, totalBytes); + + // Build NVMe Write command + SqEntry cmd = {}; + cmd.Opcode = IO_CMD_WRITE; + cmd.Nsid = g_namespaces[ns].Nsid; + cmd.Prp1 = dmaPhys; + + if (totalBytes > 0x1000) { + if (pagesNeeded == 2) { + cmd.Prp2 = dmaPhys + 0x1000; + } else { + uint64_t prpListPhys; + uint64_t* prpList = (uint64_t*)AllocateDmaBuffer(prpListPhys); + for (int i = 1; i < pagesNeeded; i++) { + prpList[i - 1] = dmaPhys + (uint64_t)i * 0x1000; + } + cmd.Prp2 = prpListPhys; + } + } + + cmd.Cdw10 = (uint32_t)(lba & 0xFFFFFFFF); + cmd.Cdw11 = (uint32_t)(lba >> 32); + cmd.Cdw12 = count - 1; + + SubmitIoCommand(cmd); + + CqEntry cqe; + bool ok = WaitIoCompletion(cqe); + + if (pagesNeeded > 2 && cmd.Prp2 != 0 && cmd.Prp2 != dmaPhys + 0x1000) { + Memory::g_pfa->Free((void*)Memory::HHDM(cmd.Prp2)); + } + + Memory::g_pfa->Free(dmaVirt, pagesNeeded); + return ok; + } + +}; diff --git a/kernel/src/Drivers/Storage/Nvme.hpp b/kernel/src/Drivers/Storage/Nvme.hpp new file mode 100644 index 0000000..e2f87a5 --- /dev/null +++ b/kernel/src/Drivers/Storage/Nvme.hpp @@ -0,0 +1,193 @@ +/* + * Nvme.hpp + * NVM Express (NVMe) storage driver + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include +#include + +namespace Drivers::Storage::Nvme { + + // ========================================================================= + // NVMe controller registers (memory-mapped via BAR0) + // Ref: NVM Express Base Specification 2.0 + // ========================================================================= + + constexpr uint32_t REG_CAP = 0x00; // Controller Capabilities (64-bit) + constexpr uint32_t REG_VS = 0x08; // Version + constexpr uint32_t REG_INTMS = 0x0C; // Interrupt Mask Set + constexpr uint32_t REG_INTMC = 0x10; // Interrupt Mask Clear + constexpr uint32_t REG_CC = 0x14; // Controller Configuration + constexpr uint32_t REG_CSTS = 0x1C; // Controller Status + constexpr uint32_t REG_AQA = 0x24; // Admin Queue Attributes + constexpr uint32_t REG_ASQ = 0x28; // Admin SQ Base Address (64-bit) + constexpr uint32_t REG_ACQ = 0x30; // Admin CQ Base Address (64-bit) + + // CAP register fields (64-bit) + constexpr uint64_t CAP_MQES_MASK = 0xFFFF; // Max Queue Entries Supported (0-based) + constexpr int CAP_DSTRD_SHIFT = 32; // Doorbell Stride (2 ^ (2 + DSTRD)) + constexpr uint64_t CAP_DSTRD_MASK = 0xFULL << 32; + constexpr int CAP_MPSMIN_SHIFT = 48; // Memory Page Size Minimum + constexpr uint64_t CAP_MPSMIN_MASK = 0xFULL << 48; + constexpr int CAP_MPSMAX_SHIFT = 52; // Memory Page Size Maximum + constexpr uint64_t CAP_MPSMAX_MASK = 0xFULL << 52; + constexpr uint64_t CAP_CSS_NVM = (1ULL << 37); // NVM Command Set supported + + // CC register fields + constexpr uint32_t CC_EN = (1u << 0); // Enable + constexpr uint32_t CC_CSS_NVM = (0u << 4); // NVM Command Set + constexpr uint32_t CC_MPS_SHIFT = 7; // Memory Page Size (2 ^ (12 + MPS)) + constexpr uint32_t CC_AMS_RR = (0u << 11); // Arbitration: Round Robin + constexpr uint32_t CC_SHN_NONE = (0u << 14); // No shutdown notification + constexpr uint32_t CC_SHN_NORMAL = (1u << 14); // Normal shutdown + constexpr uint32_t CC_IOSQES_SHIFT = 16; // I/O SQ Entry Size (2^n) + constexpr uint32_t CC_IOCQES_SHIFT = 20; // I/O CQ Entry Size (2^n) + + // CSTS register fields + constexpr uint32_t CSTS_RDY = (1u << 0); // Ready + constexpr uint32_t CSTS_CFS = (1u << 1); // Controller Fatal Status + constexpr uint32_t CSTS_SHST_MASK = (3u << 2); // Shutdown Status + constexpr uint32_t CSTS_SHST_NORMAL = (0u << 2); + constexpr uint32_t CSTS_SHST_COMPLETE = (2u << 2); + + // ========================================================================= + // Submission Queue Entry (64 bytes) + // ========================================================================= + + struct SqEntry { + // Dword 0: Command Dword 0 + uint8_t Opcode; + uint8_t Flags; // Fused (1:0), PSDT (7:6) + uint16_t CommandId; + + // Dword 1 + uint32_t Nsid; // Namespace ID + + // Dwords 2-3 + uint64_t Reserved; + + // Dwords 4-5: Metadata Pointer + uint64_t Mptr; + + // Dwords 6-9: Data Pointer (PRP1, PRP2) + uint64_t Prp1; + uint64_t Prp2; + + // Dwords 10-15: Command specific + uint32_t Cdw10; + uint32_t Cdw11; + uint32_t Cdw12; + uint32_t Cdw13; + uint32_t Cdw14; + uint32_t Cdw15; + } __attribute__((packed)); + + static_assert(sizeof(SqEntry) == 64, "SqEntry must be 64 bytes"); + + // ========================================================================= + // Completion Queue Entry (16 bytes) + // ========================================================================= + + struct CqEntry { + uint32_t Result; // Command-specific result (DW0) + uint32_t Reserved; + uint16_t SqHead; // SQ Head Pointer + uint16_t SqId; // SQ Identifier + uint16_t CommandId; // Command Identifier + uint16_t Status; // Status Field (bit 0 = Phase Tag) + } __attribute__((packed)); + + static_assert(sizeof(CqEntry) == 16, "CqEntry must be 16 bytes"); + + // Status field: Phase bit is bit 0; status code is bits 15:1 + constexpr uint16_t CQE_PHASE_BIT = (1u << 0); + constexpr uint16_t CQE_STATUS_MASK = 0xFFFE; // bits 15:1 + + // ========================================================================= + // Admin opcodes + // ========================================================================= + + constexpr uint8_t ADMIN_DELETE_IO_SQ = 0x00; + constexpr uint8_t ADMIN_CREATE_IO_SQ = 0x01; + constexpr uint8_t ADMIN_DELETE_IO_CQ = 0x04; + constexpr uint8_t ADMIN_CREATE_IO_CQ = 0x05; + constexpr uint8_t ADMIN_IDENTIFY = 0x06; + constexpr uint8_t ADMIN_SET_FEATURES = 0x09; + + // ========================================================================= + // NVM I/O opcodes + // ========================================================================= + + constexpr uint8_t IO_CMD_READ = 0x02; + constexpr uint8_t IO_CMD_WRITE = 0x01; + + // ========================================================================= + // Identify CNS values + // ========================================================================= + + constexpr uint32_t IDENTIFY_CNS_NAMESPACE = 0x00; + constexpr uint32_t IDENTIFY_CNS_CONTROLLER = 0x01; + + // ========================================================================= + // Feature identifiers + // ========================================================================= + + constexpr uint32_t FEATURE_NUM_QUEUES = 0x07; + + // ========================================================================= + // Queue parameters + // ========================================================================= + + constexpr int ADMIN_QUEUE_DEPTH = 32; // Admin queue entries + constexpr int IO_QUEUE_DEPTH = 64; // I/O queue entries + constexpr int MAX_NAMESPACES = 8; + + // ========================================================================= + // Namespace info + // ========================================================================= + + struct NamespaceInfo { + bool Active; + uint32_t Nsid; + uint64_t SectorCount; // Total LBAs (NSZE) + uint32_t SectorSize; // Bytes per LBA + uint32_t MaxTransferBlocks; // MDTS in blocks + char Model[41]; + }; + + // ========================================================================= + // MSI configuration (use a different IRQ slot than AHCI) + // ========================================================================= + + constexpr uint8_t MSI_IRQ = 26; // IRQ slot 26 = vector 58 + constexpr uint32_t MSI_VECTOR = 58; + constexpr uint32_t MSI_ADDR_BASE = 0xFEE00000; + + // ========================================================================= + // Public API + // ========================================================================= + + // Probe a PCI device (called by driver matching framework) + bool Probe(const Pci::PciDevice& dev); + + // Check if the driver was initialized + bool IsInitialized(); + + // Get number of active namespaces + int GetNamespaceCount(); + + // Read sectors from an NVMe namespace + // ns: namespace index, lba: starting LBA, count: sector count (max 128) + // buffer: destination buffer + // Returns true on success + bool ReadSectors(int ns, uint64_t lba, uint32_t count, void* buffer); + + // Write sectors to an NVMe namespace + bool WriteSectors(int ns, uint64_t lba, uint32_t count, const void* buffer); + + // Get info about a specific namespace + const NamespaceInfo* GetNamespaceInfo(int ns); + +}; diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp new file mode 100644 index 0000000..8192b70 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp @@ -0,0 +1,500 @@ +/* + * A2dp.cpp + * Bluetooth A2DP / AVDTP implementation + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "A2dp.hpp" +#include "Sbc.hpp" +#include "L2cap.hpp" +#include "Hci.hpp" +#include +#include +#include +#include +#include + +using namespace Kt; + +namespace Drivers::USB::Bluetooth::A2dp { + + // ========================================================================= + // AVDTP constants + // ========================================================================= + + // AVDTP signal IDs + constexpr uint8_t AVDTP_DISCOVER = 0x01; + constexpr uint8_t AVDTP_GET_CAPABILITIES = 0x02; + constexpr uint8_t AVDTP_SET_CONFIGURATION = 0x03; + constexpr uint8_t AVDTP_GET_CONFIGURATION = 0x04; + constexpr uint8_t AVDTP_RECONFIGURE = 0x05; + constexpr uint8_t AVDTP_OPEN = 0x06; + constexpr uint8_t AVDTP_START = 0x07; + constexpr uint8_t AVDTP_CLOSE = 0x08; + constexpr uint8_t AVDTP_SUSPEND = 0x09; + constexpr uint8_t AVDTP_ABORT = 0x0A; + + // AVDTP message types + constexpr uint8_t MSG_COMMAND = 0x00; + constexpr uint8_t MSG_GENERAL_REJECT = 0x01; + constexpr uint8_t MSG_RESPONSE_ACCEPT = 0x02; + constexpr uint8_t MSG_RESPONSE_REJECT = 0x03; + + // AVDTP packet types + constexpr uint8_t PKT_SINGLE = 0x00; + + // Service category IDs + constexpr uint8_t CAT_MEDIA_TRANSPORT = 0x01; + constexpr uint8_t CAT_MEDIA_CODEC = 0x07; + + // Media type + constexpr uint8_t MEDIA_AUDIO = 0x00; + + // Codec type + constexpr uint8_t CODEC_SBC = 0x00; + + // SBC capability octets + // Octet 0: Sampling Frequency (bits 7-4) | Channel Mode (bits 3-0) + // Octet 1: Block Length (bits 7-4) | Subbands (bits 3-2) | Alloc Method (bits 1-0) + // Octet 2: Min Bitpool + // Octet 3: Max Bitpool + + // ========================================================================= + // State + // ========================================================================= + + static State g_state = State::Idle; + static uint16_t g_sigCid = 0; // L2CAP CID for AVDTP signaling + static uint16_t g_mediaCid = 0; // L2CAP CID for AVDTP media transport + static uint8_t g_txLabel = 1; + static uint8_t g_remoteSeid = 0; // Remote stream endpoint ID + static uint8_t g_localSeid = 1; // Our local SEID + + // SBC encoder + static Sbc::SbcEncoder g_sbcEncoder = {}; + static bool g_sbcInitialized = false; + + // Media packet state + static uint16_t g_seqNum = 0; + static uint32_t g_timestamp = 0; + + // Volume + static int g_volume = 80; + + // AVDTP response tracking + static volatile bool g_avdtpResponseReady = false; + static uint8_t g_avdtpResponseBuf[128] = {}; + static uint32_t g_avdtpResponseLen = 0; + + // ========================================================================= + // AVDTP signaling helpers + // ========================================================================= + + static void SendAvdtpCommand(uint8_t signalId, const uint8_t* payload, uint16_t len) { + uint8_t buf[128] = {}; + + // AVDTP single packet header + buf[0] = (g_txLabel << 4) | (PKT_SINGLE << 2) | MSG_COMMAND; + buf[1] = signalId; + g_txLabel = (g_txLabel + 1) & 0x0F; + + if (payload && len > 0) { + memcpy(&buf[2], payload, len); + } + + L2cap::SendData(g_sigCid, buf, 2 + len); + } + + static void SendAvdtpResponse(uint8_t txLabel, uint8_t signalId, + const uint8_t* payload, uint16_t len) { + uint8_t buf[128] = {}; + + buf[0] = (txLabel << 4) | (PKT_SINGLE << 2) | MSG_RESPONSE_ACCEPT; + buf[1] = signalId; + + if (payload && len > 0) { + memcpy(&buf[2], payload, len); + } + + L2cap::SendData(g_sigCid, buf, 2 + len); + } + + // ========================================================================= + // WaitAvdtpResponse + // ========================================================================= + + static bool WaitAvdtpResponse(uint32_t timeoutMs = 3000) { + g_avdtpResponseReady = false; + uint64_t start = Timekeeping::GetMilliseconds(); + + while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + Xhci::PollEvents(); + if (g_avdtpResponseReady) return true; + for (int j = 0; j < 100; j++) { + asm volatile("" ::: "memory"); + } + } + return false; + } + + // ========================================================================= + // AVDTP signaling procedures + // ========================================================================= + + static bool AvdtpDiscover() { + SendAvdtpCommand(AVDTP_DISCOVER, nullptr, 0); + + if (!WaitAvdtpResponse()) { + KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout"; + return false; + } + + // Parse discover response to find audio sink SEID + // Response format: each SEP is 2 bytes: + // Byte 0: SEID(6) | InUse(1) | Rsvd(1) + // Byte 1: MediaType(4) | SEPType(4) (SEPType: 0=Source, 1=Sink) + if (g_avdtpResponseLen >= 4) { + for (uint32_t i = 2; i + 1 < g_avdtpResponseLen; i += 2) { + uint8_t seid = (g_avdtpResponseBuf[i] >> 2) & 0x3F; + bool inUse = (g_avdtpResponseBuf[i] >> 1) & 1; + uint8_t mediaType = (g_avdtpResponseBuf[i + 1] >> 4) & 0x0F; + uint8_t sepType = g_avdtpResponseBuf[i + 1] & 0x0F; + + if (mediaType == MEDIA_AUDIO && sepType == 0x01 && !inUse) { + g_remoteSeid = seid; + KernelLogStream(INFO, "BT-A2DP") << "Found audio sink SEID=" + << (uint64_t)seid; + return true; + } + } + } + + KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found"; + return false; + } + + static bool AvdtpGetCapabilities() { + uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; + SendAvdtpCommand(AVDTP_GET_CAPABILITIES, payload, 1); + return WaitAvdtpResponse(); + } + + static bool AvdtpSetConfiguration() { + // Set Configuration payload: + // ACP SEID (1 byte) | INT SEID (1 byte) | Service Capabilities... + uint8_t payload[12] = {}; + payload[0] = (g_remoteSeid << 2); // ACP SEID + payload[1] = (g_localSeid << 2); // INT SEID + + // Media Transport capability (no data) + payload[2] = CAT_MEDIA_TRANSPORT; // Category + payload[3] = 0; // Length + + // Media Codec capability (SBC) + payload[4] = CAT_MEDIA_CODEC; // Category + payload[5] = 6; // Length + payload[6] = (MEDIA_AUDIO << 4); // Media Type + payload[7] = CODEC_SBC; // Codec Type + // SBC codec info (4 bytes) + // Sampling: 44.1kHz (bit 5), Channel mode: Joint Stereo (bit 0) + payload[8] = 0x21; // 44.1kHz | Joint Stereo + // Block length: 16 (bit 7), Subbands: 8 (bit 1), Alloc: Loudness (bit 0) + payload[9] = 0x83; // 16 blocks | 8 subbands | Loudness + payload[10] = 2; // Min bitpool + payload[11] = 53; // Max bitpool + + SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, 12); + + if (!WaitAvdtpResponse()) { + KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration timeout"; + return false; + } + + g_state = State::Configured; + KernelLogStream(OK, "BT-A2DP") << "Stream configured"; + return true; + } + + static bool AvdtpOpen() { + uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; + SendAvdtpCommand(AVDTP_OPEN, payload, 1); + + if (!WaitAvdtpResponse()) { + KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open timeout"; + return false; + } + + g_state = State::Open; + KernelLogStream(OK, "BT-A2DP") << "Stream opened"; + return true; + } + + static bool AvdtpStart() { + uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; + SendAvdtpCommand(AVDTP_START, payload, 1); + + if (!WaitAvdtpResponse()) { + KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout"; + return false; + } + + g_state = State::Streaming; + KernelLogStream(OK, "BT-A2DP") << "Streaming started"; + return true; + } + + // ========================================================================= + // OnChannelReady — called by L2CAP when an AVDTP channel is configured + // ========================================================================= + + void OnChannelReady(uint16_t l2capCid) { + if (g_sigCid == 0) { + // First AVDTP channel is signaling + g_sigCid = l2capCid; + KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID=" + << (uint64_t)l2capCid; + + // Auto-discover remote SEPs + g_state = State::Discovering; + if (AvdtpDiscover()) { + AvdtpGetCapabilities(); + AvdtpSetConfiguration(); + } + } else if (g_mediaCid == 0) { + // Second AVDTP channel is media transport + g_mediaCid = l2capCid; + KernelLogStream(OK, "BT-A2DP") << "AVDTP media channel ready: CID=" + << (uint64_t)l2capCid; + } + } + + // ========================================================================= + // ProcessAvdtp — handle AVDTP signaling packets + // ========================================================================= + + void ProcessAvdtp(const uint8_t* data, uint16_t len) { + if (len < 2) return; + + uint8_t txLabel = (data[0] >> 4) & 0x0F; + uint8_t pktType = (data[0] >> 2) & 0x03; + uint8_t msgType = data[0] & 0x03; + uint8_t signalId = data[1] & 0x3F; + + if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT) { + // This is a response to our command + memcpy(g_avdtpResponseBuf, data, len > sizeof(g_avdtpResponseBuf) ? sizeof(g_avdtpResponseBuf) : len); + g_avdtpResponseLen = len; + g_avdtpResponseReady = true; + return; + } + + // Handle incoming commands + if (msgType == MSG_COMMAND) { + switch (signalId) { + case AVDTP_DISCOVER: { + // Respond with our local SEP (audio source) + uint8_t rsp[2] = {}; + rsp[0] = (g_localSeid << 2); // SEID, not in use + rsp[1] = (MEDIA_AUDIO << 4) | 0x00; // Audio, Source + SendAvdtpResponse(txLabel, AVDTP_DISCOVER, rsp, 2); + break; + } + + case AVDTP_GET_CAPABILITIES: { + // Respond with our SBC capabilities + uint8_t rsp[10] = {}; + rsp[0] = CAT_MEDIA_TRANSPORT; + rsp[1] = 0; + rsp[2] = CAT_MEDIA_CODEC; + rsp[3] = 6; + rsp[4] = (MEDIA_AUDIO << 4); + rsp[5] = CODEC_SBC; + rsp[6] = 0x21; // 44.1kHz | Joint Stereo + rsp[7] = 0x83; // 16 blocks | 8 subbands | Loudness + rsp[8] = 2; // Min bitpool + rsp[9] = 53; // Max bitpool + SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10); + break; + } + + case AVDTP_SET_CONFIGURATION: { + // Accept configuration from remote + if (len >= 4) { + g_remoteSeid = (data[2] >> 2) & 0x3F; + g_state = State::Configured; + SendAvdtpResponse(txLabel, AVDTP_SET_CONFIGURATION, nullptr, 0); + KernelLogStream(OK, "BT-A2DP") << "Remote configured stream, SEID=" + << (uint64_t)g_remoteSeid; + } + break; + } + + case AVDTP_OPEN: { + g_state = State::Open; + SendAvdtpResponse(txLabel, AVDTP_OPEN, nullptr, 0); + KernelLogStream(OK, "BT-A2DP") << "Remote opened stream"; + + // The media transport channel will be set up via L2CAP after this + break; + } + + case AVDTP_START: { + g_state = State::Streaming; + SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0); + KernelLogStream(OK, "BT-A2DP") << "Remote started streaming"; + break; + } + + case AVDTP_CLOSE: { + g_state = State::Idle; + SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0); + break; + } + + case AVDTP_SUSPEND: { + g_state = State::Open; + SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0); + break; + } + + case AVDTP_ABORT: { + g_state = State::Idle; + SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0); + break; + } + + default: + break; + } + } + } + + // ========================================================================= + // ConfigureStream + // ========================================================================= + + bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { + Sbc::Init(&g_sbcEncoder, sampleRate, channels, bitsPerSample); + g_sbcInitialized = true; + g_seqNum = 0; + g_timestamp = 0; + + KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: " + << (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit " + << (uint64_t)channels << "ch"; + + return true; + } + + // ========================================================================= + // StartStream / StopStream + // ========================================================================= + + bool StartStream() { + if (g_state == State::Open || g_state == State::Configured) { + if (g_state == State::Configured) { + if (!AvdtpOpen()) return false; + } + return AvdtpStart(); + } + return (g_state == State::Streaming); + } + + bool StopStream() { + if (g_state == State::Streaming) { + uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; + SendAvdtpCommand(AVDTP_SUSPEND, payload, 1); + WaitAvdtpResponse(1000); + g_state = State::Open; + } + return true; + } + + // ========================================================================= + // WriteAudio — encode PCM to SBC and stream over Bluetooth + // ========================================================================= + + int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) { + if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) { + return -1; + } + + uint32_t samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder); + uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; // 16-bit samples + uint32_t sbcFrameSize = Sbc::GetFrameSize(&g_sbcEncoder); + + // Apply volume scaling to PCM data + // We work on a local copy for volume adjustment + int16_t scaledPcm[512]; // Max ~128 samples * 2 channels = 256 samples + if (bytesPerFrame > sizeof(scaledPcm)) return -1; + + uint32_t consumed = 0; + + while (consumed + bytesPerFrame <= pcmLen) { + // Copy and scale by volume + const int16_t* src = (const int16_t*)(pcmData + consumed); + uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels; + for (uint32_t i = 0; i < numSamples; i++) { + scaledPcm[i] = (int16_t)(((int32_t)src[i] * g_volume) / 100); + } + + // Build media packet: RTP-like header (12 bytes) + SBC payload header (1 byte) + SBC frames + uint8_t mediaPkt[256] = {}; + + // Simplified media packet header (AVDTP media packet) + // Byte 0: V=2, P=0, X=0, CC=0 -> 0x80 + // Byte 1: M=0, PT=96 -> 0x60 + // Bytes 2-3: Sequence number + // Bytes 4-7: Timestamp + // Bytes 8-11: SSRC + // Byte 12: SBC payload header (number of SBC frames) + mediaPkt[0] = 0x80; + mediaPkt[1] = 0x60; + mediaPkt[2] = (uint8_t)(g_seqNum >> 8); + mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF); + mediaPkt[4] = (uint8_t)(g_timestamp >> 24); + mediaPkt[5] = (uint8_t)(g_timestamp >> 16); + mediaPkt[6] = (uint8_t)(g_timestamp >> 8); + mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF); + mediaPkt[8] = 0; mediaPkt[9] = 0; mediaPkt[10] = 0; mediaPkt[11] = 0x01; // SSRC + mediaPkt[12] = 1; // Number of SBC frames in this packet + + // Encode SBC frame + uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, scaledPcm, &mediaPkt[13]); + + uint32_t totalLen = 13 + encodedSize; + + // Send via L2CAP on media channel + L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)totalLen); + + g_seqNum++; + g_timestamp += samplesPerFrame; + consumed += bytesPerFrame; + } + + return (int)consumed; + } + + // ========================================================================= + // State queries + // ========================================================================= + + State GetState() { + return g_state; + } + + bool IsStreaming() { + return (g_state == State::Streaming); + } + + int GetVolume() { + return g_volume; + } + + void SetVolume(int percent) { + if (percent < 0) percent = 0; + if (percent > 100) percent = 100; + g_volume = percent; + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp new file mode 100644 index 0000000..1b295df --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp @@ -0,0 +1,60 @@ +/* + * A2dp.hpp + * Bluetooth A2DP (Advanced Audio Distribution Profile) + * AVDTP signaling and SBC audio streaming + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include + +namespace Drivers::USB::Bluetooth::A2dp { + + // ========================================================================= + // A2DP stream states + // ========================================================================= + + enum class State { + Idle, // No A2DP connection + Discovering, // AVDTP discover in progress + Configured, // Stream endpoint configured + Open, // Stream open, ready for audio + Streaming // Actively streaming audio + }; + + // ========================================================================= + // Public API + // ========================================================================= + + // Called by L2CAP when an AVDTP channel becomes ready + void OnChannelReady(uint16_t l2capCid); + + // Process an AVDTP signaling packet + void ProcessAvdtp(const uint8_t* data, uint16_t len); + + // Configure a stream for the given PCM parameters + bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample); + + // Start streaming + bool StartStream(); + + // Stop streaming + bool StopStream(); + + // Write PCM audio data to the Bluetooth audio stream + // Returns number of bytes consumed + int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen); + + // Get current state + State GetState(); + + // Check if currently streaming + bool IsStreaming(); + + // Get volume (0-100) + int GetVolume(); + + // Set volume (0-100) + void SetVolume(int percent); + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp new file mode 100644 index 0000000..8d8235a --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp @@ -0,0 +1,329 @@ +/* + * Bluetooth.cpp + * Top-level Bluetooth subsystem — adapter registration and Intel BT initialization + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "Bluetooth.hpp" +#include "Hci.hpp" +#include "A2dp.hpp" +#include +#include +#include +#include +#include +#include + +using namespace Kt; + +namespace Drivers::USB::Bluetooth { + + // ========================================================================= + // State + // ========================================================================= + + static bool g_initialized = false; + static uint8_t g_slotId = 0; + static uint8_t g_bdAddr[6] = {}; + + // Intel Bluetooth device IDs + static bool IsIntelBt(uint16_t vid, uint16_t pid) { + if (vid != 0x8087) return false; + // Known Intel Bluetooth USB product IDs + switch (pid) { + case 0x0032: // AX211 variant + case 0x0033: // AX211 + case 0x0036: // AX211 variant + case 0x0038: // AX211 variant + case 0x0AAA: // AX200 + case 0x0026: // AX201 + case 0x0029: // AX201 variant + case 0x0025: // 9560 + case 0x0A2B: // 8265 + case 0x0A2A: // 8260 + case 0x07DC: // 8265 variant + case 0x0AA7: // AX200 variant + return true; + default: + return false; + } + } + + // ========================================================================= + // Intel Bluetooth firmware detection + // ========================================================================= + + static bool InitIntelBluetooth(uint8_t slotId) { + KernelLogStream(INFO, "BT") << "Intel Bluetooth adapter detected"; + + // Intel BT controllers require HCI Reset before they respond to + // vendor-specific commands. This mirrors the Linux btintel driver + // sequence: Reset → Read Version → (firmware load) → Reset. + if (!Hci::Reset()) { + KernelLogStream(ERROR, "BT") << "Initial HCI Reset failed"; + return false; + } + + // Read standard HCI version -- if this fails, the controller is likely + // in bootloader mode where only vendor commands are accepted. + Hci::LocalVersion lver = {}; + bool hciVersionOk = Hci::ReadLocalVersion(&lver); + if (hciVersionOk) { + KernelLogStream(INFO, "BT") << "HCI version=" << (uint64_t)lver.HciVersion + << " rev=" << base::hex << (uint64_t)lver.HciRevision + << " LMP=" << (uint64_t)lver.LmpVersion + << " manufacturer=" << (uint64_t)lver.Manufacturer + << " subver=" << (uint64_t)lver.LmpSubversion << base::dec; + } + + // Read Intel version to check firmware state + Hci::IntelVersion ver = {}; + if (!Hci::ReadIntelVersion(&ver)) { + KernelLogStream(WARNING, "BT") << "Failed to read Intel BT version"; + } else { + KernelLogStream(INFO, "BT") << "Intel BT: HW variant=" << (uint64_t)ver.HwVariant + << " FW variant=" << base::hex << (uint64_t)ver.FwVariant + << " FW rev=" << (uint64_t)ver.FwRevision << "." + << (uint64_t)ver.FwBuildNum << base::dec; + + if (ver.FwVariant == 0x23) { + KernelLogStream(OK, "BT") << "Intel BT firmware already loaded (operational mode)"; + } else if (ver.FwVariant == 0x06) { + KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode, firmware not loaded"; + KernelLogStream(WARNING, "BT") << "Bluetooth will have limited functionality without firmware"; + } else if (!hciVersionOk) { + // Standard HCI commands failed AND Intel version is zeros/unknown + // -> controller is in bootloader mode, needs firmware download + KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode (FW not loaded by UEFI)"; + KernelLogStream(WARNING, "BT") << "Bluetooth requires firmware download for full functionality"; + } else { + KernelLogStream(INFO, "BT") << "Intel BT firmware variant: " + << base::hex << (uint64_t)ver.FwVariant; + } + } + + return true; + } + + // ========================================================================= + // RegisterAdapter — entry point from USB enumeration + // ========================================================================= + + void RegisterAdapter(uint8_t slotId) { + if (g_initialized) { + KernelLogStream(WARNING, "BT") << "Bluetooth adapter already registered"; + return; + } + + g_slotId = slotId; + + // Initialize HCI transport (allocates DMA buffers, registers callback) + // NOTE: Does NOT queue receive transfers yet — device isn't ready + Hci::Initialize(slotId); + + auto* dev = Xhci::GetDevice(slotId); + if (!dev) return; + + // Wait for the USB device to be ready after SET_CONFIGURATION + // Intel BT controllers need 200-500ms after config before accepting HCI + uint64_t start = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - start < 200) { + Xhci::PollEvents(); + asm volatile("pause" ::: "memory"); + } + + // Start the event pipe BEFORE sending any HCI commands. + // HCI command responses arrive as events on the interrupt IN endpoint, + // so it must be queued to receive them. + Hci::StartEventPipe(); + + // Intel-specific initialization (includes HCI Reset) + bool didReset = false; + if (IsIntelBt(dev->VendorId, dev->ProductId)) { + if (InitIntelBluetooth(slotId)) { + didReset = true; // InitIntelBluetooth already sent HCI Reset + } else { + KernelLogStream(WARNING, "BT") << "Intel BT init failed, continuing with basic HCI"; + } + } + + // Standard HCI Reset (skip if Intel init already did one) + if (!didReset) { + if (!Hci::Reset()) { + KernelLogStream(ERROR, "BT") << "HCI Reset failed"; + return; + } + } + + // Read BD_ADDR + if (Hci::ReadBdAddr(g_bdAddr)) { + KernelLogStream(OK, "BT") << "BD_ADDR: " + << base::hex + << (uint64_t)g_bdAddr[5] << ":" << (uint64_t)g_bdAddr[4] << ":" + << (uint64_t)g_bdAddr[3] << ":" << (uint64_t)g_bdAddr[2] << ":" + << (uint64_t)g_bdAddr[1] << ":" << (uint64_t)g_bdAddr[0] << base::dec; + } + + // Read buffer size + uint16_t aclLen = 0, aclNum = 0; + uint8_t scoLen = 0; + uint16_t scoNum = 0; + if (Hci::ReadBufferSize(&aclLen, &scoLen, &aclNum, &scoNum)) { + KernelLogStream(INFO, "BT") << "ACL buffer: " << (uint64_t)aclLen + << " bytes x " << (uint64_t)aclNum; + } + + // Set local name + Hci::WriteLocalName("MontaukOS"); + + // Set class of device: Audio (Major Service: Audio, Major Class: Audio/Video) + // CoD: 0x240404 = Rendering | Audio | Wearable Headset (common for audio devices) + // For a computer acting as audio source: + // 0x200408 = Audio service | Audio/Video class | Portable Audio + Hci::WriteClassOfDevice(0x200408); + + // Enable Simple Secure Pairing + Hci::WriteSSPMode(1); + + // Set event mask to receive relevant events + uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, 0x00, 0x20}; + Hci::SendCommand(Hci::OP_SET_EVENT_MASK, eventMask, 8); + Hci::WaitCommandComplete(Hci::OP_SET_EVENT_MASK); + + // Enable inquiry + page scan (discoverable and connectable) + Hci::WriteScanEnable(0x03); + + g_initialized = true; + KernelLogStream(OK, "BT") << "Bluetooth adapter initialized successfully"; + } + + // ========================================================================= + // Public queries + // ========================================================================= + + bool IsInitialized() { + return g_initialized; + } + + uint8_t GetSlotId() { + return g_slotId; + } + + const uint8_t* GetBdAddr() { + return g_bdAddr; + } + + // ========================================================================= + // Scan — blocking inquiry + // ========================================================================= + + int Scan(Hci::InquiryDevice* buf, int maxCount, uint32_t timeoutMs) { + if (!g_initialized || !buf || maxCount <= 0) return -1; + + Hci::ClearInquiryResults(); + + // Convert timeout to 1.28s units (min 1, max 30) + uint8_t duration = (uint8_t)(timeoutMs / 1280); + if (duration < 1) duration = 1; + if (duration > 30) duration = 30; + + if (!Hci::StartInquiry(duration)) return -1; + + // Poll until inquiry completes or timeout + uint64_t start = Timekeeping::GetMilliseconds(); + while (Hci::IsInquiryActive() && (Timekeeping::GetMilliseconds() - start < timeoutMs)) { + Xhci::PollEvents(); + Hci::DrainEvents(); + + for (int j = 0; j < 200; j++) { + asm volatile("pause" ::: "memory"); + } + } + + // Cancel if still running + if (Hci::IsInquiryActive()) { + Hci::CancelInquiry(); + } + + return Hci::GetInquiryResults(buf, maxCount); + } + + // ========================================================================= + // Connect — initiate ACL connection + // ========================================================================= + + int Connect(const uint8_t* bdAddr, uint32_t timeoutMs) { + if (!g_initialized || !bdAddr) return -1; + + if (!Hci::CreateConnection(bdAddr)) return -1; + + // Wait for Connection Complete event + uint64_t start = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + Xhci::PollEvents(); + Hci::DrainEvents(); + + // Check connection table for matching BD_ADDR + for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { + auto* conn = Hci::GetConnectionByIndex(i); + if (conn && conn->Active) { + bool match = true; + for (int j = 0; j < 6; j++) { + if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; } + } + if (match) return 0; + } + } + + for (int j = 0; j < 200; j++) { + asm volatile("pause" ::: "memory"); + } + } + + return -1; // Timeout + } + + // ========================================================================= + // Disconnect — disconnect a device by BD_ADDR + // ========================================================================= + + int Disconnect(const uint8_t* bdAddr) { + if (!g_initialized || !bdAddr) return -1; + + // Find connection with matching BD_ADDR + for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { + auto* conn = Hci::GetConnectionByIndex(i); + if (conn && conn->Active) { + bool match = true; + for (int j = 0; j < 6; j++) { + if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; } + } + if (match) { + Hci::Disconnect(conn->Handle, 0x13); // 0x13 = Remote User Terminated + return 0; + } + } + } + + return -1; // Not found + } + + // ========================================================================= + // ListConnected — list active connections + // ========================================================================= + + int ListConnected(Hci::ConnectionInfo* buf, int maxCount) { + if (!g_initialized || !buf || maxCount <= 0) return 0; + + int count = 0; + for (int i = 0; i < Hci::MAX_CONNECTIONS && count < maxCount; i++) { + auto* conn = Hci::GetConnectionByIndex(i); + if (conn && conn->Active) { + buf[count] = *conn; + count++; + } + } + return count; + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp new file mode 100644 index 0000000..6804379 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp @@ -0,0 +1,39 @@ +/* + * Bluetooth.hpp + * Top-level Bluetooth subsystem header + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include +#include "Hci.hpp" + +namespace Drivers::USB::Bluetooth { + + // Called by USB enumeration when a Bluetooth adapter is detected + void RegisterAdapter(uint8_t slotId); + + // Query adapter state + bool IsInitialized(); + uint8_t GetSlotId(); + + // Get Bluetooth device address (6 bytes) + const uint8_t* GetBdAddr(); + + // Scan for nearby devices (blocking, up to timeoutMs) + // Returns number of devices found; results written to buf + int Scan(Hci::InquiryDevice* buf, int maxCount, uint32_t timeoutMs); + + // Initiate connection to a remote device by BD_ADDR + // Returns 0 on success (connection established), -1 on failure + int Connect(const uint8_t* bdAddr, uint32_t timeoutMs = 10000); + + // Disconnect a device by BD_ADDR + // Returns 0 on success, -1 if not connected + int Disconnect(const uint8_t* bdAddr); + + // List connected devices + // Returns number of connected devices; info written to buf + int ListConnected(Hci::ConnectionInfo* buf, int maxCount); + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.cpp b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp new file mode 100644 index 0000000..f6b6596 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp @@ -0,0 +1,818 @@ +/* + * Hci.cpp + * Bluetooth HCI transport over USB + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "Hci.hpp" +#include "L2cap.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Kt; + +namespace Drivers::USB::Bluetooth::Hci { + + // ========================================================================= + // State + // ========================================================================= + + static uint8_t g_slotId = 0; + static bool g_initialized = false; + + // Event receive buffer (filled by xHCI interrupt IN callback) + static uint8_t g_eventBuf[256] = {}; + static volatile uint32_t g_eventLen = 0; + static volatile bool g_eventReady = false; + + // ACL receive buffer + static uint8_t g_aclRxBuf[1024] = {}; + static volatile uint32_t g_aclRxLen = 0; + static volatile bool g_aclRxReady = false; + + // ACL transmit DMA buffer + static uint8_t* g_aclTxBuf = nullptr; + static uint64_t g_aclTxBufPhys = 0; + + // HCI command DMA buffer (separate from ACL to avoid conflicts) + static uint8_t* g_cmdDmaBuf = nullptr; + static uint64_t g_cmdDmaBufPhys = 0; + + // Connection table + static ConnectionInfo g_connections[MAX_CONNECTIONS] = {}; + + // ACL buffer size (from controller) + static uint16_t g_aclMaxLen = 0; + static uint16_t g_aclMaxNum = 0; + static volatile uint16_t g_aclPendingCount = 0; + + // Inquiry results + static InquiryDevice g_inquiryResults[MAX_INQUIRY_RESULTS] = {}; + static volatile int g_inquiryResultCount = 0; + static volatile bool g_inquiryActive = false; + + // ========================================================================= + // USB transfer callback + // ========================================================================= + + static void TransferCallback(uint8_t slotId, uint8_t epDci, + const uint8_t* data, uint32_t length, + uint32_t completionCode) { + if (slotId != g_slotId) return; + + auto* dev = Xhci::GetDevice(slotId); + if (!dev) return; + + uint8_t intDci = dev->InterruptEpNum ? (dev->InterruptEpNum * 2 + 1) : 0; + uint8_t bulkInDci = dev->BulkInEpNum ? (dev->BulkInEpNum * 2 + 1) : 0; + + if (epDci == intDci && data && length > 0) { + // HCI Event received on interrupt IN. + // Dispatch asynchronous events (inquiry results, connection events, + // etc.) immediately so they are never lost. Only buffer + // Command Complete / Command Status events — those are consumed + // by WaitCommandComplete / WaitCommandStatus. + uint8_t evtCode = (length >= 1) ? data[0] : 0; + + if (evtCode == EVT_COMMAND_COMPLETE || evtCode == EVT_COMMAND_STATUS) { + uint32_t copyLen = length; + if (copyLen > sizeof(g_eventBuf)) copyLen = sizeof(g_eventBuf); + memcpy(g_eventBuf, data, copyLen); + g_eventLen = copyLen; + g_eventReady = true; + } else { + // Process immediately (inquiry results, connection events, etc.) + ProcessEvent(data, length); + } + + // Re-queue interrupt transfer for next event + Xhci::QueueInterruptTransfer(slotId); + } else if (epDci == bulkInDci && data && length > 0) { + // ACL data received on bulk IN + uint32_t copyLen = length; + if (copyLen > sizeof(g_aclRxBuf)) copyLen = sizeof(g_aclRxBuf); + memcpy(g_aclRxBuf, data, copyLen); + g_aclRxLen = copyLen; + g_aclRxReady = true; + + // Re-queue bulk IN transfer + Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket); + } else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) { + // Bulk OUT completion — decrement pending count + if (g_aclPendingCount > 0) g_aclPendingCount--; + } + } + + // ========================================================================= + // Busy wait with event polling + // ========================================================================= + + static void BusyWaitMs(uint64_t ms) { + uint64_t start = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - start < ms) { + asm volatile("pause" ::: "memory"); + } + } + + // Poll for events while waiting + static void PollWait(uint32_t ms) { + uint64_t start = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - start < ms) { + Xhci::PollEvents(); + for (int j = 0; j < 100; j++) { + asm volatile("" ::: "memory"); + } + } + } + + // ========================================================================= + // Initialize + // ========================================================================= + + void Initialize(uint8_t slotId) { + g_slotId = slotId; + + // Register our transfer callback + Xhci::RegisterTransferCallback(slotId, TransferCallback); + + // Allocate DMA buffers for HCI commands and ACL data + g_cmdDmaBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed(); + g_cmdDmaBufPhys = Memory::SubHHDM(g_cmdDmaBuf); + + g_aclTxBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed(); + g_aclTxBufPhys = Memory::SubHHDM(g_aclTxBuf); + + // NOTE: Do NOT queue interrupt IN or bulk IN transfers here. + // The BT controller is not yet HCI-initialized and may misbehave. + // Call StartEventPipe() after HCI Reset and initial setup. + + g_initialized = true; + KernelLogStream(OK, "BT-HCI") << "HCI transport initialized on slot " << (uint64_t)slotId; + } + + // Start receiving HCI events and ACL data — call after HCI init sequence + void StartEventPipe() { + if (!g_initialized) return; + + // Queue initial interrupt IN transfer for HCI events + Xhci::QueueInterruptTransfer(g_slotId); + + // Queue initial bulk IN transfer for ACL data + auto* dev = Xhci::GetDevice(g_slotId); + if (dev && dev->BulkInEpNum) { + Xhci::QueueBulkInTransfer(g_slotId, nullptr, 0, dev->BulkInMaxPacket); + } + + KernelLogStream(INFO, "BT-HCI") << "Event pipe started (interrupt IN + bulk IN)"; + } + + // ========================================================================= + // SendCommand — via USB control transfer on EP0 + // ========================================================================= + + bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen) { + if (!g_initialized || !g_cmdDmaBuf) return false; + + // HCI command packet: opcode (2) + paramLen (1) + params + // USB-BT spec: HCI commands are sent via control transfer + // bmRequestType = 0x20 (Host-to-device, Class, Device) + // bRequest = 0x00 + // wValue = 0, wIndex = 0 + // wLength = sizeof(CommandHeader) + paramLen + + // Use DMA-allocated buffer (not stack) for the command data. + // xHCI reads from this buffer via DMA for OUT transfers. + memset(g_cmdDmaBuf, 0, 512); + g_cmdDmaBuf[0] = (uint8_t)(opcode & 0xFF); + g_cmdDmaBuf[1] = (uint8_t)(opcode >> 8); + g_cmdDmaBuf[2] = paramLen; + if (params && paramLen > 0) { + memcpy(&g_cmdDmaBuf[3], params, paramLen); + } + + uint16_t totalLen = 3 + paramLen; + + uint32_t cc = Xhci::ControlTransfer(g_slotId, + 0x20, // bmRequestType: Host-to-device, Class, Device + 0x00, // bRequest: 0 + 0x0000, // wValue + 0x0000, // wIndex + totalLen, + g_cmdDmaBuf, + false); // dirIn = false (host to device) + + if (cc != Xhci::CC_SUCCESS) { + KernelLogStream(WARNING, "BT-HCI") << "SendCommand failed, opcode=" + << base::hex << (uint64_t)opcode << " cc=" << base::dec << (uint64_t)cc; + return false; + } + + return true; + } + + // ========================================================================= + // WaitCommandComplete + // ========================================================================= + + bool WaitCommandComplete(uint16_t opcode, uint8_t* outParams, + uint8_t maxLen, uint32_t timeoutMs) { + uint64_t start = Timekeeping::GetMilliseconds(); + + while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + Xhci::PollEvents(); + + if (g_eventReady) { + g_eventReady = false; + + if (g_eventLen >= 2) { + uint8_t evtCode = g_eventBuf[0]; + uint8_t evtParamLen = g_eventBuf[1]; + + if (evtCode == EVT_COMMAND_COMPLETE && evtParamLen >= 3) { + // Command Complete: NumPkts(1) + Opcode(2) + Status(1) + Params + uint16_t evtOpcode = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8); + if (evtOpcode == opcode) { + if (outParams && maxLen > 0) { + // Copy params starting after the status byte + uint8_t availLen = (evtParamLen > 4) ? (evtParamLen - 4) : 0; + uint8_t copyLen = (availLen < maxLen) ? availLen : maxLen; + // Include status byte + return params + copyLen = (evtParamLen > 3) ? (evtParamLen - 3) : 0; + if (copyLen > maxLen) copyLen = maxLen; + memcpy(outParams, &g_eventBuf[5], copyLen); + } + // Check status + uint8_t status = g_eventBuf[5]; + if (status != 0) { + KernelLogStream(WARNING, "BT-HCI") << "Command Complete status=" + << (uint64_t)status << " opcode=" << base::hex << (uint64_t)opcode; + } + return true; + } + } + + } + } + + for (int j = 0; j < 100; j++) { + asm volatile("" ::: "memory"); + } + } + + KernelLogStream(WARNING, "BT-HCI") << "WaitCommandComplete timeout, opcode=" + << base::hex << (uint64_t)opcode; + return false; + } + + // ========================================================================= + // WaitCommandStatus + // ========================================================================= + + bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs) { + uint64_t start = Timekeeping::GetMilliseconds(); + + while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + Xhci::PollEvents(); + + if (g_eventReady) { + g_eventReady = false; + + if (g_eventLen >= 2) { + uint8_t evtCode = g_eventBuf[0]; + uint8_t evtParamLen = g_eventBuf[1]; + + if (evtCode == EVT_COMMAND_STATUS && evtParamLen >= 4) { + uint8_t status = g_eventBuf[2]; + uint16_t evtOpcode = (uint16_t)g_eventBuf[4] | ((uint16_t)g_eventBuf[5] << 8); + if (evtOpcode == opcode) { + return (status == 0); + } + } + + } + } + + for (int j = 0; j < 100; j++) { + asm volatile("" ::: "memory"); + } + } + + return false; + } + + // ========================================================================= + // SendAcl — via USB bulk OUT + // ========================================================================= + + bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len) { + if (!g_initialized || !g_aclTxBuf) return false; + if (len + sizeof(AclHeader) > 4096) return false; // Single page DMA buffer + + // Build ACL packet in DMA buffer + auto* hdr = (AclHeader*)g_aclTxBuf; + hdr->HandleFlags = (handle & 0x0FFF) | pbFlag; + hdr->DataLength = len; + if (data && len > 0) { + memcpy(g_aclTxBuf + sizeof(AclHeader), data, len); + } + + uint32_t totalLen = sizeof(AclHeader) + len; + + g_aclPendingCount++; + Xhci::QueueBulkOutTransfer(g_slotId, g_aclTxBuf, g_aclTxBufPhys, totalLen); + + return true; + } + + // ========================================================================= + // ProcessEvent — handle HCI events + // ========================================================================= + + void ProcessEvent(const uint8_t* data, uint32_t len) { + if (len < 2) return; + + uint8_t evtCode = data[0]; + uint8_t evtParamLen = data[1]; + const uint8_t* params = data + 2; + + switch (evtCode) { + case EVT_CONNECTION_COMPLETE: { + if (evtParamLen >= 11) { + uint8_t status = params[0]; + uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); + const uint8_t* bdAddr = ¶ms[3]; + uint8_t linkType = params[9]; + + KernelLogStream(INFO, "BT-HCI") << "Connection Complete: status=" + << (uint64_t)status << " handle=" << (uint64_t)handle + << " link=" << (uint64_t)linkType; + + if (status == 0) { + // Find empty connection slot + for (int i = 0; i < MAX_CONNECTIONS; i++) { + if (!g_connections[i].Active) { + g_connections[i].Active = true; + g_connections[i].Handle = handle; + memcpy(g_connections[i].BdAddr, bdAddr, 6); + g_connections[i].LinkType = linkType; + g_connections[i].Encrypted = false; + break; + } + } + + // Initialize L2CAP for this connection + L2cap::Initialize(handle); + } + } + break; + } + + case EVT_DISCONNECTION_COMPLETE: { + if (evtParamLen >= 4) { + uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); + uint8_t reason = params[3]; + + KernelLogStream(INFO, "BT-HCI") << "Disconnection: handle=" + << (uint64_t)handle << " reason=" << (uint64_t)reason; + + for (int i = 0; i < MAX_CONNECTIONS; i++) { + if (g_connections[i].Active && g_connections[i].Handle == handle) { + g_connections[i].Active = false; + break; + } + } + } + break; + } + + case EVT_CONNECTION_REQUEST: { + if (evtParamLen >= 10) { + const uint8_t* bdAddr = ¶ms[0]; + uint8_t linkType = params[9]; + + KernelLogStream(INFO, "BT-HCI") << "Connection Request: link=" + << (uint64_t)linkType; + + // Auto-accept ACL connections + if (linkType == 0x01) { + AcceptConnection(bdAddr, 0x01); // Role = slave + } + } + break; + } + + case EVT_NUM_COMPLETED_PACKETS: { + if (evtParamLen >= 1) { + uint8_t numHandles = params[0]; + for (int i = 0; i < numHandles && (3 + i * 4) < evtParamLen; i++) { + uint16_t completed = (uint16_t)params[3 + i * 4] + | ((uint16_t)params[4 + i * 4] << 8); + if (g_aclPendingCount >= completed) { + g_aclPendingCount -= completed; + } else { + g_aclPendingCount = 0; + } + } + } + break; + } + + case EVT_IO_CAPABILITY_REQUEST: { + if (evtParamLen >= 6) { + // Reply with NoInputNoOutput for simple pairing + uint8_t reply[9] = {}; + memcpy(reply, ¶ms[0], 6); // BD_ADDR + reply[6] = 0x03; // IO Capability: NoInputNoOutput + reply[7] = 0x00; // OOB data not present + reply[8] = 0x00; // Authentication requirements: MITM not required + SendCommand(OP_IO_CAPABILITY_REPLY, reply, 9); + WaitCommandComplete(OP_IO_CAPABILITY_REPLY, nullptr, 0, 1000); + } + break; + } + + case EVT_USER_CONFIRM_REQUEST: { + if (evtParamLen >= 6) { + // Auto-confirm + SendCommand(OP_USER_CONFIRM_REPLY, ¶ms[0], 6); + WaitCommandComplete(OP_USER_CONFIRM_REPLY, nullptr, 0, 1000); + } + break; + } + + case EVT_INQUIRY_COMPLETE: { + g_inquiryActive = false; + KernelLogStream(INFO, "BT-HCI") << "Inquiry complete, " + << (uint64_t)g_inquiryResultCount << " device(s) found"; + break; + } + + case EVT_INQUIRY_RESULT: { + // Standard inquiry result: NumResp(1) + per-device(14 bytes each) + if (evtParamLen >= 1) { + uint8_t numResp = params[0]; + for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) { + const uint8_t* entry = ¶ms[1 + i * 14]; + auto& dev = g_inquiryResults[g_inquiryResultCount]; + memset(&dev, 0, sizeof(dev)); + memcpy(dev.BdAddr, entry, 6); + dev.ClassOfDevice = (uint32_t)entry[9] + | ((uint32_t)entry[10] << 8) + | ((uint32_t)entry[11] << 16); + dev.Rssi = -128; // Unknown for standard inquiry + g_inquiryResultCount++; + } + } + break; + } + + case EVT_INQUIRY_RESULT_RSSI: { + // Inquiry Result with RSSI: NumResp(1) + per-device(15 bytes each) + if (evtParamLen >= 1) { + uint8_t numResp = params[0]; + for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) { + const uint8_t* entry = ¶ms[1 + i * 15]; + auto& dev = g_inquiryResults[g_inquiryResultCount]; + memset(&dev, 0, sizeof(dev)); + memcpy(dev.BdAddr, entry, 6); + dev.ClassOfDevice = (uint32_t)entry[9] + | ((uint32_t)entry[10] << 8) + | ((uint32_t)entry[11] << 16); + dev.Rssi = (int8_t)entry[14]; + g_inquiryResultCount++; + } + } + break; + } + + case EVT_EXTENDED_INQUIRY_RESULT: { + // Extended Inquiry Result: NumResp(1) + BD_ADDR(6) + PSRM(1) + reserved(1) + // + CoD(3) + ClockOff(2) + RSSI(1) + EIR(240) + if (evtParamLen >= 15 && g_inquiryResultCount < MAX_INQUIRY_RESULTS) { + auto& dev = g_inquiryResults[g_inquiryResultCount]; + memset(&dev, 0, sizeof(dev)); + memcpy(dev.BdAddr, ¶ms[1], 6); + dev.ClassOfDevice = (uint32_t)params[9] + | ((uint32_t)params[10] << 8) + | ((uint32_t)params[11] << 16); + dev.Rssi = (int8_t)params[14]; + + // Parse EIR data for device name + const uint8_t* eir = ¶ms[15]; + int eirLen = evtParamLen - 15; + int pos = 0; + while (pos < eirLen && pos < 240) { + uint8_t len = eir[pos]; + if (len == 0) break; + if (pos + 1 + len > eirLen) break; + uint8_t type = eir[pos + 1]; + // Type 0x08 = Shortened Local Name, 0x09 = Complete Local Name + if (type == 0x08 || type == 0x09) { + int nameLen = len - 1; + if (nameLen > 63) nameLen = 63; + memcpy(dev.Name, &eir[pos + 2], nameLen); + dev.Name[nameLen] = '\0'; + } + pos += 1 + len; + } + + g_inquiryResultCount++; + } + break; + } + + case EVT_ENCRYPT_CHANGE: { + if (evtParamLen >= 4) { + uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); + uint8_t encryption = params[3]; + + for (int i = 0; i < MAX_CONNECTIONS; i++) { + if (g_connections[i].Active && g_connections[i].Handle == handle) { + g_connections[i].Encrypted = (encryption != 0); + break; + } + } + } + break; + } + + default: + break; + } + } + + // ========================================================================= + // ProcessAcl — handle incoming ACL data + // ========================================================================= + + void ProcessAcl(const uint8_t* data, uint32_t len) { + if (len < sizeof(AclHeader)) return; + + auto* hdr = (const AclHeader*)data; + uint16_t handle = hdr->HandleFlags & 0x0FFF; + uint16_t pbFlag = hdr->HandleFlags & 0x3000; + uint16_t dataLen = hdr->DataLength; + + if (dataLen + sizeof(AclHeader) > len) return; + + // Dispatch to L2CAP + L2cap::ProcessPacket(handle, data + sizeof(AclHeader), dataLen); + } + + // ========================================================================= + // Connection management + // ========================================================================= + + ConnectionInfo* GetConnection(uint16_t handle) { + for (int i = 0; i < MAX_CONNECTIONS; i++) { + if (g_connections[i].Active && g_connections[i].Handle == handle) { + return &g_connections[i]; + } + } + return nullptr; + } + + ConnectionInfo* GetActiveConnection() { + for (int i = 0; i < MAX_CONNECTIONS; i++) { + if (g_connections[i].Active) { + return &g_connections[i]; + } + } + return nullptr; + } + + ConnectionInfo* GetConnectionByIndex(int index) { + if (index < 0 || index >= MAX_CONNECTIONS) return nullptr; + return &g_connections[index]; + } + + // ========================================================================= + // Convenience HCI commands + // ========================================================================= + + bool Reset() { + if (!SendCommand(OP_RESET, nullptr, 0)) return false; + BusyWaitMs(100); + return WaitCommandComplete(OP_RESET, nullptr, 0, 5000); + } + + bool ReadBdAddr(uint8_t* addr) { + if (!SendCommand(OP_READ_BD_ADDR, nullptr, 0)) return false; + uint8_t params[7] = {}; + if (!WaitCommandComplete(OP_READ_BD_ADDR, params, sizeof(params))) return false; + // params[0] = status, params[1..6] = BD_ADDR + if (params[0] != 0) return false; + memcpy(addr, ¶ms[1], 6); + return true; + } + + bool ReadLocalVersion(LocalVersion* ver) { + if (!SendCommand(OP_READ_LOCAL_VERSION, nullptr, 0)) return false; + uint8_t params[9] = {}; + if (!WaitCommandComplete(OP_READ_LOCAL_VERSION, params, sizeof(params))) return false; + if (params[0] != 0) return false; + if (ver) { + ver->Status = params[0]; + ver->HciVersion = params[1]; + ver->HciRevision = (uint16_t)params[2] | ((uint16_t)params[3] << 8); + ver->LmpVersion = params[4]; + ver->Manufacturer = (uint16_t)params[5] | ((uint16_t)params[6] << 8); + ver->LmpSubversion = (uint16_t)params[7] | ((uint16_t)params[8] << 8); + } + return true; + } + + bool ReadIntelVersion(IntelVersion* ver) { + // Newer Intel BT controllers (AX200/AX201/AX211, THP+) require a + // parameter byte of 0xFF for 0xFC05 to return the full version in + // TLV format. Try with the parameter first; fall back to the + // legacy (no-param) format if the command fails. + uint8_t param = 0xFF; + bool sent = SendCommand(OP_INTEL_READ_VERSION, ¶m, 1); + if (!sent) { + // Fallback: legacy format (no parameter) + sent = SendCommand(OP_INTEL_READ_VERSION, nullptr, 0); + } + if (!sent) return false; + + uint8_t params[32] = {}; + if (!WaitCommandComplete(OP_INTEL_READ_VERSION, params, sizeof(params))) return false; + + // Log raw response for diagnostics + KernelLogStream(INFO, "BT-HCI") << "Intel version raw: " + << base::hex + << (uint64_t)params[0] << " " << (uint64_t)params[1] << " " + << (uint64_t)params[2] << " " << (uint64_t)params[3] << " " + << (uint64_t)params[4] << " " << (uint64_t)params[5] << " " + << (uint64_t)params[6] << " " << (uint64_t)params[7] << " " + << (uint64_t)params[8] << " " << (uint64_t)params[9] + << base::dec; + + if (ver) memcpy(ver, params, sizeof(IntelVersion)); + return true; + } + + bool WriteLocalName(const char* name) { + uint8_t params[248] = {}; + int i = 0; + for (; i < 247 && name[i]; i++) params[i] = name[i]; + params[i] = '\0'; + if (!SendCommand(OP_WRITE_LOCAL_NAME, params, 248)) return false; + return WaitCommandComplete(OP_WRITE_LOCAL_NAME); + } + + bool WriteClassOfDevice(uint32_t cod) { + uint8_t params[3] = { + (uint8_t)(cod & 0xFF), + (uint8_t)((cod >> 8) & 0xFF), + (uint8_t)((cod >> 16) & 0xFF) + }; + if (!SendCommand(OP_WRITE_CLASS_OF_DEVICE, params, 3)) return false; + return WaitCommandComplete(OP_WRITE_CLASS_OF_DEVICE); + } + + bool WriteScanEnable(uint8_t mode) { + if (!SendCommand(OP_WRITE_SCAN_ENABLE, &mode, 1)) return false; + return WaitCommandComplete(OP_WRITE_SCAN_ENABLE); + } + + bool WriteSSPMode(uint8_t mode) { + if (!SendCommand(OP_WRITE_SSP_MODE, &mode, 1)) return false; + return WaitCommandComplete(OP_WRITE_SSP_MODE); + } + + bool AcceptConnection(const uint8_t* bdAddr, uint8_t role) { + uint8_t params[7]; + memcpy(params, bdAddr, 6); + params[6] = role; + if (!SendCommand(OP_ACCEPT_CONN_REQ, params, 7)) return false; + return WaitCommandStatus(OP_ACCEPT_CONN_REQ); + } + + bool Disconnect(uint16_t handle, uint8_t reason) { + uint8_t params[3] = { + (uint8_t)(handle & 0xFF), + (uint8_t)((handle >> 8) & 0xFF), + reason + }; + if (!SendCommand(OP_DISCONNECT, params, 3)) return false; + return WaitCommandStatus(OP_DISCONNECT); + } + + bool ReadBufferSize(uint16_t* aclLen, uint8_t* scoLen, + uint16_t* aclNum, uint16_t* scoNum) { + if (!SendCommand(OP_READ_BUFFER_SIZE, nullptr, 0)) return false; + uint8_t params[8] = {}; + if (!WaitCommandComplete(OP_READ_BUFFER_SIZE, params, sizeof(params))) return false; + if (params[0] != 0) return false; + if (aclLen) *aclLen = (uint16_t)params[1] | ((uint16_t)params[2] << 8); + if (scoLen) *scoLen = params[3]; + if (aclNum) *aclNum = (uint16_t)params[4] | ((uint16_t)params[5] << 8); + if (scoNum) *scoNum = (uint16_t)params[6] | ((uint16_t)params[7] << 8); + g_aclMaxLen = (uint16_t)params[1] | ((uint16_t)params[2] << 8); + g_aclMaxNum = (uint16_t)params[4] | ((uint16_t)params[5] << 8); + return true; + } + + // ========================================================================= + // Inquiry (device discovery) + // ========================================================================= + + bool StartInquiry(uint8_t durationUnits) { + g_inquiryResultCount = 0; + g_inquiryActive = true; + + // HCI Inquiry: LAP(3) + InquiryLength(1) + NumResponses(1) + // GIAC LAP = 0x9E8B33 + uint8_t params[5] = { + 0x33, 0x8B, 0x9E, // LAP (General Inquiry Access Code) + durationUnits, // Duration in 1.28s units + 0x00 // Unlimited responses + }; + + if (!SendCommand(OP_INQUIRY, params, 5)) { + g_inquiryActive = false; + return false; + } + + // Inquiry uses Command Status (not Command Complete) + if (!WaitCommandStatus(OP_INQUIRY)) { + g_inquiryActive = false; + return false; + } + + return true; + } + + bool CancelInquiry() { + if (!g_inquiryActive) return true; + if (!SendCommand(OP_INQUIRY_CANCEL, nullptr, 0)) return false; + WaitCommandComplete(OP_INQUIRY_CANCEL, nullptr, 0, 2000); + g_inquiryActive = false; + return true; + } + + int GetInquiryResults(InquiryDevice* buf, int maxCount) { + int count = g_inquiryResultCount; + if (count > maxCount) count = maxCount; + if (buf && count > 0) { + memcpy(buf, g_inquiryResults, count * sizeof(InquiryDevice)); + } + return count; + } + + void ClearInquiryResults() { + g_inquiryResultCount = 0; + } + + bool IsInquiryActive() { + return g_inquiryActive; + } + + // ========================================================================= + // Create ACL connection + // ========================================================================= + + void DrainEvents() { + // Discard any unconsumed Command Complete/Status events that weren't + // picked up by WaitCommandComplete/WaitCommandStatus. + if (g_eventReady) { + g_eventReady = false; + } + + // Drain ACL data + if (g_aclRxReady) { + g_aclRxReady = false; + if (g_aclRxLen > 0) { + ProcessAcl(g_aclRxBuf, g_aclRxLen); + } + } + } + + bool CreateConnection(const uint8_t* bdAddr) { + // HCI Create Connection: + // BD_ADDR(6) + PacketType(2) + PSRM(1) + reserved(1) + ClockOffset(2) + AllowRoleSwitch(1) + uint8_t params[13] = {}; + memcpy(params, bdAddr, 6); + // Packet types: DM1, DH1, DM3, DH3, DM5, DH5 + params[6] = 0x18; // CC18 = allow DM1, DH1, DM3, DH3, DM5, DH5 + params[7] = 0xCC; + params[8] = 0x02; // Page Scan Repetition Mode R2 + params[9] = 0x00; // Reserved + params[10] = 0x00; // Clock offset + params[11] = 0x00; + params[12] = 0x01; // Allow role switch + + if (!SendCommand(OP_CREATE_CONNECTION, params, 13)) return false; + + // Create Connection uses Command Status, then Connection Complete event + return WaitCommandStatus(OP_CREATE_CONNECTION, 5000); + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.hpp b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp new file mode 100644 index 0000000..47e4e2b --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp @@ -0,0 +1,251 @@ +/* + * Hci.hpp + * Bluetooth HCI (Host Controller Interface) layer + * HCI transport over USB bulk/interrupt/control endpoints + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include + +namespace Drivers::USB::Bluetooth::Hci { + + // ========================================================================= + // HCI packet types (for USB transport) + // ========================================================================= + + // USB transport uses different endpoints for each packet type: + // Commands -> Control EP0 (class request) + // ACL data -> Bulk OUT / Bulk IN + // Events -> Interrupt IN + + // ========================================================================= + // HCI command opcodes (OGF << 10 | OCF) + // ========================================================================= + + // Link Control (OGF 0x01) + constexpr uint16_t OP_INQUIRY = 0x0401; + constexpr uint16_t OP_INQUIRY_CANCEL = 0x0402; + constexpr uint16_t OP_CREATE_CONNECTION = 0x0405; + constexpr uint16_t OP_DISCONNECT = 0x0406; + constexpr uint16_t OP_ACCEPT_CONN_REQ = 0x0409; + constexpr uint16_t OP_REJECT_CONN_REQ = 0x040A; + constexpr uint16_t OP_AUTH_REQUESTED = 0x0411; + constexpr uint16_t OP_SET_CONN_ENCRYPT = 0x0413; + constexpr uint16_t OP_IO_CAPABILITY_REPLY = 0x042B; + constexpr uint16_t OP_USER_CONFIRM_REPLY = 0x042C; + + // Link Policy (OGF 0x02) + constexpr uint16_t OP_WRITE_DEFAULT_LP = 0x080F; + constexpr uint16_t OP_SNIFF_MODE = 0x0803; + + // Controller & Baseband (OGF 0x03) + constexpr uint16_t OP_RESET = 0x0C03; + constexpr uint16_t OP_SET_EVENT_FILTER = 0x0C05; + constexpr uint16_t OP_WRITE_LOCAL_NAME = 0x0C13; + constexpr uint16_t OP_READ_LOCAL_NAME = 0x0C14; + constexpr uint16_t OP_WRITE_SCAN_ENABLE = 0x0C1A; + constexpr uint16_t OP_WRITE_CLASS_OF_DEVICE = 0x0C24; + constexpr uint16_t OP_WRITE_SSP_MODE = 0x0C56; + constexpr uint16_t OP_WRITE_INQUIRY_MODE = 0x0C45; + constexpr uint16_t OP_WRITE_PAGE_TIMEOUT = 0x0C18; + constexpr uint16_t OP_WRITE_AUTH_ENABLE = 0x0C20; + constexpr uint16_t OP_SET_EVENT_MASK = 0x0C01; + + // Informational Parameters (OGF 0x04) + constexpr uint16_t OP_READ_BD_ADDR = 0x1009; + constexpr uint16_t OP_READ_LOCAL_VERSION = 0x1001; + constexpr uint16_t OP_READ_LOCAL_FEATURES = 0x1003; + constexpr uint16_t OP_READ_BUFFER_SIZE = 0x1005; + + // Intel vendor commands (OGF 0x3F) + constexpr uint16_t OP_INTEL_READ_VERSION = 0xFC05; + constexpr uint16_t OP_INTEL_RESET = 0xFC01; + constexpr uint16_t OP_INTEL_SET_EVENT_MASK = 0xFC52; + constexpr uint16_t OP_INTEL_DDC_CONFIG_WRITE = 0xFC8B; + + // ========================================================================= + // HCI event codes + // ========================================================================= + + constexpr uint8_t EVT_INQUIRY_COMPLETE = 0x01; + constexpr uint8_t EVT_INQUIRY_RESULT = 0x02; + constexpr uint8_t EVT_CONNECTION_COMPLETE = 0x03; + constexpr uint8_t EVT_CONNECTION_REQUEST = 0x04; + constexpr uint8_t EVT_DISCONNECTION_COMPLETE = 0x05; + constexpr uint8_t EVT_AUTH_COMPLETE = 0x06; + constexpr uint8_t EVT_ENCRYPT_CHANGE = 0x08; + constexpr uint8_t EVT_COMMAND_COMPLETE = 0x0E; + constexpr uint8_t EVT_COMMAND_STATUS = 0x0F; + constexpr uint8_t EVT_NUM_COMPLETED_PACKETS = 0x13; + constexpr uint8_t EVT_IO_CAPABILITY_REQUEST = 0x31; + constexpr uint8_t EVT_IO_CAPABILITY_RESPONSE = 0x32; + constexpr uint8_t EVT_USER_CONFIRM_REQUEST = 0x33; + constexpr uint8_t EVT_SIMPLE_PAIRING_COMPLETE = 0x36; + constexpr uint8_t EVT_INQUIRY_RESULT_RSSI = 0x22; + constexpr uint8_t EVT_EXTENDED_INQUIRY_RESULT = 0x2F; + constexpr uint8_t EVT_VENDOR_SPECIFIC = 0xFF; + + // ========================================================================= + // Inquiry result storage + // ========================================================================= + + struct InquiryDevice { + uint8_t BdAddr[6]; + uint8_t _pad[2]; + uint32_t ClassOfDevice; + int8_t Rssi; + uint8_t _pad2[3]; + char Name[64]; // From Extended Inquiry Result or Remote Name Request + }; + + constexpr int MAX_INQUIRY_RESULTS = 16; + + // ========================================================================= + // HCI packet headers + // ========================================================================= + + struct CommandHeader { + uint16_t Opcode; + uint8_t ParamLength; + } __attribute__((packed)); + + struct EventHeader { + uint8_t EventCode; + uint8_t ParamLength; + } __attribute__((packed)); + + struct AclHeader { + uint16_t HandleFlags; // bits 11:0 = handle, 13:12 = PB flag, 15:14 = BC flag + uint16_t DataLength; + } __attribute__((packed)); + + // ACL PB (Packet Boundary) flag values + constexpr uint16_t ACL_PB_FIRST_NON_FLUSH = 0x0000; // First non-auto-flushable + constexpr uint16_t ACL_PB_CONTINUING = 0x1000; // Continuing fragment + constexpr uint16_t ACL_PB_FIRST_FLUSH = 0x2000; // First auto-flushable + + // ========================================================================= + // HCI connection info + // ========================================================================= + + struct ConnectionInfo { + bool Active; + uint16_t Handle; + uint8_t BdAddr[6]; + uint8_t LinkType; // 0x01 = ACL + bool Encrypted; + }; + + constexpr int MAX_CONNECTIONS = 4; + + // ========================================================================= + // Intel Bluetooth version info + // ========================================================================= + + struct IntelVersion { + uint8_t Status; + uint8_t HwPlatform; + uint8_t HwVariant; + uint8_t HwRevision; + uint8_t FwVariant; // 0x06 = bootloader, 0x23 = operational + uint8_t FwRevision; + uint8_t FwBuildNum; + uint8_t FwBuildWw; + uint8_t FwBuildYy; + uint8_t FwPatchNum; + } __attribute__((packed)); + + // ========================================================================= + // Public API + // ========================================================================= + + // Initialize HCI transport over USB for the given slot + void Initialize(uint8_t slotId); + + // Start receiving HCI events and ACL data (call after HCI init sequence) + void StartEventPipe(); + + // Send an HCI command via USB control transfer (EP0) + bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen); + + // Wait for a Command Complete event matching the given opcode + // Returns true if received within timeout, fills outParams (excluding status byte) + bool WaitCommandComplete(uint16_t opcode, uint8_t* outParams = nullptr, + uint8_t maxLen = 0, uint32_t timeoutMs = 2000); + + // Wait for a Command Status event matching the given opcode + bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs = 2000); + + // Send ACL data via USB bulk OUT + bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len); + + // Process an HCI event received on the interrupt IN endpoint + void ProcessEvent(const uint8_t* data, uint32_t len); + + // Process ACL data received on the bulk IN endpoint + void ProcessAcl(const uint8_t* data, uint32_t len); + + // Get connection info + ConnectionInfo* GetConnection(uint16_t handle); + ConnectionInfo* GetActiveConnection(); + ConnectionInfo* GetConnectionByIndex(int index); // 0..MAX_CONNECTIONS-1 + + // HCI Reset command + bool Reset(); + + // Read local BD_ADDR + bool ReadBdAddr(uint8_t* addr); + + // Read standard HCI local version info + struct LocalVersion { + uint8_t Status; + uint8_t HciVersion; + uint16_t HciRevision; + uint8_t LmpVersion; + uint16_t Manufacturer; + uint16_t LmpSubversion; + } __attribute__((packed)); + + bool ReadLocalVersion(LocalVersion* ver); + + // Read Intel-specific version info + bool ReadIntelVersion(IntelVersion* ver); + + // Set local name + bool WriteLocalName(const char* name); + + // Set class of device + bool WriteClassOfDevice(uint32_t cod); + + // Enable scan (inquiry + page) + bool WriteScanEnable(uint8_t mode); + + // Write Simple Secure Pairing mode + bool WriteSSPMode(uint8_t mode); + + // Accept an incoming connection + bool AcceptConnection(const uint8_t* bdAddr, uint8_t role); + + // Disconnect a connection + bool Disconnect(uint16_t handle, uint8_t reason); + + // Read ACL buffer size from controller + bool ReadBufferSize(uint16_t* aclLen, uint8_t* scoLen, + uint16_t* aclNum, uint16_t* scoNum); + + // Inquiry (device discovery) + bool StartInquiry(uint8_t durationUnits); // duration in 1.28s units (e.g., 8 = ~10s) + bool CancelInquiry(); + int GetInquiryResults(InquiryDevice* buf, int maxCount); + void ClearInquiryResults(); + bool IsInquiryActive(); + + // Create ACL connection to a remote device + bool CreateConnection(const uint8_t* bdAddr); + + // Drain any pending HCI events (call in poll loops that aren't inside + // WaitCommandComplete/WaitCommandStatus) + void DrainEvents(); + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp new file mode 100644 index 0000000..8596e4a --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp @@ -0,0 +1,422 @@ +/* + * L2cap.cpp + * Bluetooth L2CAP implementation + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "L2cap.hpp" +#include "Hci.hpp" +#include "A2dp.hpp" +#include +#include +#include +#include +#include + +using namespace Kt; + +namespace Drivers::USB::Bluetooth::L2cap { + + // ========================================================================= + // State + // ========================================================================= + + static uint16_t g_aclHandle = 0; + static bool g_initialized = false; + static uint8_t g_sigIdentifier = 1; + + // Channel table + static ChannelInfo g_channels[MAX_CHANNELS] = {}; + static uint16_t g_nextCid = CID_DYNAMIC_START; + + // Signaling response tracking + static volatile bool g_sigResponseReady = false; + static uint8_t g_sigResponseBuf[64] = {}; + static uint32_t g_sigResponseLen = 0; + + // ========================================================================= + // Helpers + // ========================================================================= + + static uint16_t AllocCid() { + return g_nextCid++; + } + + static ChannelInfo* AllocChannel(uint16_t psm) { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (!g_channels[i].Active) { + g_channels[i].Active = true; + g_channels[i].LocalCid = AllocCid(); + g_channels[i].RemoteCid = 0; + g_channels[i].Psm = psm; + g_channels[i].RemoteMtu = 672; // Default L2CAP MTU + g_channels[i].Configured = false; + g_channels[i].LocalConfigDone = false; + g_channels[i].RemoteConfigDone = false; + return &g_channels[i]; + } + } + return nullptr; + } + + // Send L2CAP signaling command + static void SendSignal(uint8_t code, uint8_t identifier, + const uint8_t* payload, uint16_t payloadLen) { + // L2CAP header + Signal header + payload + uint16_t sigLen = sizeof(SignalHeader) + payloadLen; + uint16_t totalPayload = sizeof(L2capHeader) + sigLen; + + uint8_t buf[128] = {}; + auto* l2hdr = (L2capHeader*)buf; + l2hdr->Length = sigLen; + l2hdr->ChannelId = CID_SIGNALING; + + auto* sig = (SignalHeader*)(buf + sizeof(L2capHeader)); + sig->Code = code; + sig->Identifier = identifier; + sig->Length = payloadLen; + + if (payload && payloadLen > 0) { + memcpy(buf + sizeof(L2capHeader) + sizeof(SignalHeader), payload, payloadLen); + } + + Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH, + buf, totalPayload); + } + + // ========================================================================= + // Initialize + // ========================================================================= + + void Initialize(uint16_t aclHandle) { + g_aclHandle = aclHandle; + g_initialized = true; + g_sigIdentifier = 1; + g_nextCid = CID_DYNAMIC_START; + + for (int i = 0; i < MAX_CHANNELS; i++) { + g_channels[i].Active = false; + } + + KernelLogStream(OK, "BT-L2CAP") << "Initialized for ACL handle " << (uint64_t)aclHandle; + } + + // ========================================================================= + // ProcessPacket + // ========================================================================= + + void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len) { + if (len < sizeof(L2capHeader)) return; + + auto* l2hdr = (const L2capHeader*)data; + uint16_t l2len = l2hdr->Length; + uint16_t cid = l2hdr->ChannelId; + const uint8_t* payload = data + sizeof(L2capHeader); + + if (l2len + sizeof(L2capHeader) > len) return; + + if (cid == CID_SIGNALING) { + // L2CAP signaling channel + if (l2len < sizeof(SignalHeader)) return; + + auto* sig = (const SignalHeader*)payload; + const uint8_t* sigPayload = payload + sizeof(SignalHeader); + uint16_t sigPayloadLen = sig->Length; + + switch (sig->Code) { + case SIG_CONN_REQ: { + if (sigPayloadLen >= 4) { + uint16_t psm = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8); + + KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM=" + << base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid; + + // Accept connections for AVDTP + if (psm == PSM_AVDTP || psm == PSM_SDP) { + auto* ch = AllocChannel(psm); + if (ch) { + ch->RemoteCid = srcCid; + + // Send Connection Response (success) + uint8_t rsp[8] = {}; + rsp[0] = (uint8_t)(ch->LocalCid & 0xFF); + rsp[1] = (uint8_t)(ch->LocalCid >> 8); + rsp[2] = (uint8_t)(srcCid & 0xFF); + rsp[3] = (uint8_t)(srcCid >> 8); + rsp[4] = 0; rsp[5] = 0; // Result: success + rsp[6] = 0; rsp[7] = 0; // Status: no info + SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8); + } + } else { + // Reject: PSM not supported + uint8_t rsp[8] = {}; + rsp[0] = 0; rsp[1] = 0; // Dest CID = 0 + rsp[2] = (uint8_t)(srcCid & 0xFF); + rsp[3] = (uint8_t)(srcCid >> 8); + rsp[4] = 0x02; rsp[5] = 0; // Result: PSM not supported + rsp[6] = 0; rsp[7] = 0; + SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8); + } + } + break; + } + + case SIG_CONN_RSP: { + if (sigPayloadLen >= 8) { + uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8); + uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8); + + KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID=" + << base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result; + + if (result == CONN_SUCCESS) { + // Find our channel by srcCid (which is our local CID) + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) { + g_channels[i].RemoteCid = dstCid; + + // Send Configuration Request + uint8_t cfgReq[4] = {}; + cfgReq[0] = (uint8_t)(dstCid & 0xFF); + cfgReq[1] = (uint8_t)(dstCid >> 8); + cfgReq[2] = 0; cfgReq[3] = 0; // Flags + SendSignal(SIG_CONFIG_REQ, g_sigIdentifier++, cfgReq, 4); + break; + } + } + } + } + break; + } + + case SIG_CONFIG_REQ: { + if (sigPayloadLen >= 4) { + uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + + // Find channel + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) { + g_channels[i].RemoteConfigDone = true; + + // Parse MTU option if present + uint16_t cfgOffset = 4; // Skip dstCid + flags + while (cfgOffset + 2 <= sigPayloadLen) { + uint8_t optType = sigPayload[cfgOffset]; + uint8_t optLen = sigPayload[cfgOffset + 1]; + if (optType == 0x01 && optLen == 2 && cfgOffset + 4 <= sigPayloadLen) { + g_channels[i].RemoteMtu = (uint16_t)sigPayload[cfgOffset + 2] + | ((uint16_t)sigPayload[cfgOffset + 3] << 8); + } + cfgOffset += 2 + optLen; + } + + // Send Config Response (success) + uint8_t rsp[6] = {}; + rsp[0] = (uint8_t)(g_channels[i].RemoteCid & 0xFF); + rsp[1] = (uint8_t)(g_channels[i].RemoteCid >> 8); + rsp[2] = 0; rsp[3] = 0; // Flags + rsp[4] = 0; rsp[5] = 0; // Result: success + SendSignal(SIG_CONFIG_RSP, sig->Identifier, rsp, 6); + + if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) { + g_channels[i].Configured = true; + KernelLogStream(OK, "BT-L2CAP") << "Channel " + << (uint64_t)g_channels[i].LocalCid << " configured"; + + // Notify A2DP if this is an AVDTP channel + if (g_channels[i].Psm == PSM_AVDTP) { + A2dp::OnChannelReady(g_channels[i].LocalCid); + } + } + break; + } + } + } + break; + } + + case SIG_CONFIG_RSP: { + if (sigPayloadLen >= 6) { + uint16_t srcCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8); + + if (result == CFG_SUCCESS) { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].RemoteCid == srcCid) { + g_channels[i].LocalConfigDone = true; + if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) { + g_channels[i].Configured = true; + KernelLogStream(OK, "BT-L2CAP") << "Channel " + << (uint64_t)g_channels[i].LocalCid << " configured"; + + if (g_channels[i].Psm == PSM_AVDTP) { + A2dp::OnChannelReady(g_channels[i].LocalCid); + } + } + break; + } + } + } + } + break; + } + + case SIG_DISCONN_REQ: { + if (sigPayloadLen >= 4) { + uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8); + + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) { + g_channels[i].Active = false; + break; + } + } + + // Send Disconnect Response + uint8_t rsp[4] = {}; + rsp[0] = (uint8_t)(dstCid & 0xFF); + rsp[1] = (uint8_t)(dstCid >> 8); + rsp[2] = (uint8_t)(srcCid & 0xFF); + rsp[3] = (uint8_t)(srcCid >> 8); + SendSignal(SIG_DISCONN_RSP, sig->Identifier, rsp, 4); + } + break; + } + + case SIG_INFO_REQ: { + if (sigPayloadLen >= 2) { + uint16_t infoType = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8); + + if (infoType == 0x0002) { + // Extended features mask + uint8_t rsp[8] = {}; + rsp[0] = 0x02; rsp[1] = 0x00; // InfoType + rsp[2] = 0x00; rsp[3] = 0x00; // Result: success + rsp[4] = 0x00; rsp[5] = 0x00; // Features: none + rsp[6] = 0x00; rsp[7] = 0x00; + SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 8); + } else { + // Not supported + uint8_t rsp[4] = {}; + rsp[0] = (uint8_t)(infoType & 0xFF); + rsp[1] = (uint8_t)(infoType >> 8); + rsp[2] = 0x01; rsp[3] = 0x00; // Result: not supported + SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 4); + } + } + break; + } + + default: + break; + } + } else { + // Data on a dynamic channel + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == cid) { + if (g_channels[i].Psm == PSM_AVDTP) { + A2dp::ProcessAvdtp(payload, l2len); + } + break; + } + } + } + } + + // ========================================================================= + // Connect + // ========================================================================= + + uint16_t Connect(uint16_t psm) { + if (!g_initialized) return 0; + + auto* ch = AllocChannel(psm); + if (!ch) return 0; + + // Send Connection Request + uint8_t req[4] = {}; + req[0] = (uint8_t)(psm & 0xFF); + req[1] = (uint8_t)(psm >> 8); + req[2] = (uint8_t)(ch->LocalCid & 0xFF); + req[3] = (uint8_t)(ch->LocalCid >> 8); + SendSignal(SIG_CONN_REQ, g_sigIdentifier++, req, 4); + + return ch->LocalCid; + } + + // ========================================================================= + // WaitConfigured + // ========================================================================= + + bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs) { + uint64_t start = Timekeeping::GetMilliseconds(); + + while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + Xhci::PollEvents(); + + auto* ch = GetChannel(localCid); + if (ch && ch->Configured) return true; + + for (int j = 0; j < 100; j++) { + asm volatile("" ::: "memory"); + } + } + return false; + } + + // ========================================================================= + // SendData + // ========================================================================= + + bool SendData(uint16_t localCid, const uint8_t* data, uint16_t len) { + if (!g_initialized) return false; + + auto* ch = GetChannel(localCid); + if (!ch || !ch->Configured) return false; + + // Build L2CAP packet + uint16_t totalLen = sizeof(L2capHeader) + len; + uint8_t buf[1024] = {}; + if (totalLen > sizeof(buf)) return false; + + auto* l2hdr = (L2capHeader*)buf; + l2hdr->Length = len; + l2hdr->ChannelId = ch->RemoteCid; + + if (data && len > 0) { + memcpy(buf + sizeof(L2capHeader), data, len); + } + + return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH, buf, totalLen); + } + + // ========================================================================= + // Channel queries + // ========================================================================= + + ChannelInfo* GetChannel(uint16_t localCid) { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == localCid) { + return &g_channels[i]; + } + } + return nullptr; + } + + ChannelInfo* FindChannelByPsm(uint16_t psm) { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].Psm == psm) { + return &g_channels[i]; + } + } + return nullptr; + } + + uint16_t GetAclHandle() { + return g_aclHandle; + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp new file mode 100644 index 0000000..f1c414c --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp @@ -0,0 +1,111 @@ +/* + * L2cap.hpp + * Bluetooth L2CAP (Logical Link Control and Adaptation Protocol) + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include + +namespace Drivers::USB::Bluetooth::L2cap { + + // ========================================================================= + // L2CAP CIDs (Channel Identifiers) + // ========================================================================= + + constexpr uint16_t CID_SIGNALING = 0x0001; // L2CAP signaling + constexpr uint16_t CID_CONNLESS = 0x0002; // Connectionless reception + constexpr uint16_t CID_DYNAMIC_START = 0x0040; // First dynamic CID + + // ========================================================================= + // L2CAP PSMs (Protocol/Service Multiplexers) + // ========================================================================= + + constexpr uint16_t PSM_SDP = 0x0001; // Service Discovery Protocol + constexpr uint16_t PSM_AVDTP = 0x0019; // Audio/Video Distribution Transport + + // ========================================================================= + // L2CAP packet header + // ========================================================================= + + struct L2capHeader { + uint16_t Length; + uint16_t ChannelId; + } __attribute__((packed)); + + // ========================================================================= + // L2CAP signaling command header + // ========================================================================= + + struct SignalHeader { + uint8_t Code; + uint8_t Identifier; + uint16_t Length; + } __attribute__((packed)); + + // Signaling command codes + constexpr uint8_t SIG_COMMAND_REJECT = 0x01; + constexpr uint8_t SIG_CONN_REQ = 0x02; + constexpr uint8_t SIG_CONN_RSP = 0x03; + constexpr uint8_t SIG_CONFIG_REQ = 0x04; + constexpr uint8_t SIG_CONFIG_RSP = 0x05; + constexpr uint8_t SIG_DISCONN_REQ = 0x06; + constexpr uint8_t SIG_DISCONN_RSP = 0x07; + constexpr uint8_t SIG_INFO_REQ = 0x0A; + constexpr uint8_t SIG_INFO_RSP = 0x0B; + + // Connection response results + constexpr uint16_t CONN_SUCCESS = 0x0000; + constexpr uint16_t CONN_PENDING = 0x0001; + constexpr uint16_t CONN_REFUSED_PSM = 0x0002; + + // Configuration response results + constexpr uint16_t CFG_SUCCESS = 0x0000; + + // ========================================================================= + // L2CAP channel info + // ========================================================================= + + struct ChannelInfo { + bool Active; + uint16_t LocalCid; + uint16_t RemoteCid; + uint16_t Psm; + uint16_t RemoteMtu; + bool Configured; // Both sides configured + bool LocalConfigDone; + bool RemoteConfigDone; + }; + + constexpr int MAX_CHANNELS = 8; + + // ========================================================================= + // Public API + // ========================================================================= + + // Initialize L2CAP for a new HCI connection + void Initialize(uint16_t aclHandle); + + // Process an L2CAP packet (called from HCI ACL processing) + void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len); + + // Connect to a remote PSM (initiate L2CAP connection) + // Returns local CID, or 0 on failure + uint16_t Connect(uint16_t psm); + + // Wait for connection to be configured + bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs = 5000); + + // Send data on an L2CAP channel + bool SendData(uint16_t localCid, const uint8_t* data, uint16_t len); + + // Get channel info + ChannelInfo* GetChannel(uint16_t localCid); + + // Find channel by PSM + ChannelInfo* FindChannelByPsm(uint16_t psm); + + // Get the ACL handle + uint16_t GetAclHandle(); + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp new file mode 100644 index 0000000..be79ceb --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp @@ -0,0 +1,416 @@ +/* + * Sbc.cpp + * SBC (Sub-Band Codec) encoder — fixed-point implementation + * Based on the Bluetooth SIG specification (A2DP v1.3, Appendix B) + * All arithmetic is 32-bit fixed-point (Q15.16 or Q1.30) + * Copyright (c) 2026 Daniel Hammer +*/ + +#include "Sbc.hpp" +#include + +namespace Drivers::USB::Bluetooth::Sbc { + + // ========================================================================= + // Fixed-point constants (Q1.30 format for filter coefficients) + // ========================================================================= + + // Scale factor: 1.0 = (1 << 15) in Q15.16 + constexpr int32_t FP_ONE = (1 << 15); + + // SBC 8-subband analysis filter prototype coefficients (Q1.30) + // These are the 80 windowed prototype coefficients from the SBC spec + // Scaled to Q15.16 for our fixed-point arithmetic + static const int32_t g_proto8[80] = { + 0x0002, 0x0005, 0x000A, 0x0014, 0x0023, 0x0038, 0x0054, 0x0078, + 0x00A5, 0x00DB, 0x011B, 0x0164, 0x01B5, 0x020E, 0x026D, 0x02D0, + 0x0335, 0x039A, 0x03FC, 0x0458, 0x04AB, 0x04F1, 0x0527, 0x054A, + 0x0557, 0x054A, 0x0527, 0x04F1, 0x04AB, 0x0458, 0x03FC, 0x039A, + 0x0335, 0x02D0, 0x026D, 0x020E, 0x01B5, 0x0164, 0x011B, 0x00DB, + 0x00A5, 0x0078, 0x0054, 0x0038, 0x0023, 0x0014, 0x000A, 0x0005, + 0x0002, -0x0002, -0x0005, -0x000A, -0x0014, -0x0023, -0x0038, -0x0054, + -0x0078, -0x00A5, -0x00DB, -0x011B, -0x0164, -0x01B5, -0x020E, -0x026D, + -0x02D0, -0x0335, -0x039A, -0x03FC, -0x0458, -0x04AB, -0x04F1, -0x0527, + -0x054A, -0x0557, -0x054A, -0x0527, -0x04F1, -0x04AB, -0x0458, -0x03FC, + }; + + // Cosine matrix for 8-subband DCT-II (Q15.16) + // cos_matrix[k][i] = cos((k + 0.5) * (2*i + 1) * PI / 16) * FP_ONE + static const int32_t g_cosMatrix8[8][16] = { + { 32138, 31650, 30679, 29246, 27381, 25126, 22529, 19644, 16531, 13254, 9882, 6484, 3134, -199, -3509, -6758}, + { 30679, 25126, 16531, 6484, -3509,-13254,-22529,-29246,-32138,-31650,-27381,-19644, -9882, 199, 9882, 19644}, + { 27381, 13254, -3509,-19644,-30679,-32138,-22529, -6484, 9882, 25126, 32138, 29246, 16531, -199,-16531,-29246}, + { 22529, -199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199}, + { 16531,-13254,-30679, 6484, 32138, -199,-32138, -6484, 30679, 13254,-16531,-25126, 3509, 29246, 9882,-27381}, + { 9882,-25126,-16531, 29246, 3509,-32138, 9882, 25126,-16531,-29246, 3509, 32138, -9882,-25126, 16531, 29246}, + { 3134,-31650, 27381, -6484,-22529, 32138,-13254, -9882, 30679,-29246, 6484, 19644,-32138, 16531, 3509,-25126}, + { -3509, 32138,-22529, -6484, 30679,-27381, 3509, 25126,-32138, 16531, 9882,-30679, 22529, -199,-25126, 32138}, + }; + + // ========================================================================= + // CRC-8 table (SBC spec CRC polynomial: x^8 + x^4 + x^3 + x^2 + 1) + // ========================================================================= + + static uint8_t SbcCrc8(const uint8_t* data, uint32_t len, uint8_t bits_last_byte) { + uint8_t crc = 0x0F; + for (uint32_t i = 0; i < len; i++) { + uint8_t byte = data[i]; + uint8_t nbits = (i == len - 1) ? bits_last_byte : 8; + for (uint8_t bit = 0; bit < nbits; bit++) { + uint8_t msb = (crc >> 7) & 1; + crc <<= 1; + if (((byte >> (7 - bit)) & 1) ^ msb) { + crc ^= 0x1D; + } + } + } + return crc; + } + + // ========================================================================= + // Init + // ========================================================================= + + void Init(SbcEncoder* enc, uint32_t sampleRate, uint8_t channels, uint8_t /*bitsPerSample*/) { + memset(enc, 0, sizeof(SbcEncoder)); + + enc->Subbands = SBC_SUBBANDS; + enc->Blocks = SBC_BLOCKS; + enc->Bitpool = SBC_BITPOOL; + enc->AllocMethod = ALLOC_LOUDNESS; + + if (channels >= 2) { + enc->Channels = 2; + enc->ChannelMode = MODE_JOINT_STEREO; + } else { + enc->Channels = 1; + enc->ChannelMode = MODE_MONO; + } + + switch (sampleRate) { + case 16000: enc->Frequency = FREQ_16000; break; + case 32000: enc->Frequency = FREQ_32000; break; + case 44100: enc->Frequency = FREQ_44100; break; + default: enc->Frequency = FREQ_48000; break; + } + + enc->SamplesPerFrame = enc->Blocks * enc->Subbands; + + // Calculate frame size + // For joint stereo: 4 + (4 * subbands * channels) / 8 + ceil(blocks * bitpool / 8) + subbands/8 + uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels); + if (enc->ChannelMode == MODE_JOINT_STEREO) { + headerBits += enc->Subbands; // join bits + } + uint32_t dataBits = enc->Blocks * enc->Bitpool; + enc->FrameSize = (headerBits + dataBits + 7) / 8; + } + + // ========================================================================= + // Analysis filter bank (8 subbands) + // ========================================================================= + + static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch, + int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) { + for (int blk = 0; blk < enc->Blocks; blk++) { + // Shift in new samples + int pos = enc->XPos[ch]; + for (int i = enc->Subbands - 1; i >= 0; i--) { + pos = (pos + 1) % (enc->Subbands * 10); + enc->X[ch][pos] = (int32_t)pcm[blk * enc->Subbands * enc->Channels + i * enc->Channels + ch]; + } + enc->XPos[ch] = pos; + + // Windowing and partial calculation + int32_t Z[2 * SBC_SUBBANDS]; + for (int i = 0; i < 2 * enc->Subbands; i++) { + Z[i] = 0; + for (int j = 0; j < 5; j++) { + int idx = (pos + i + j * 2 * enc->Subbands) % (enc->Subbands * 10); + int protoIdx = i + j * 2 * enc->Subbands; + if (protoIdx < 80) { + Z[i] += (int32_t)(((int64_t)enc->X[ch][idx] * g_proto8[protoIdx]) >> 15); + } + } + } + + // Matrixing (DCT) + for (int k = 0; k < enc->Subbands; k++) { + int32_t sum = 0; + for (int i = 0; i < 2 * enc->Subbands; i++) { + sum += (int32_t)(((int64_t)Z[i] * g_cosMatrix8[k][i]) >> 15); + } + sb_samples[blk][k] = sum; + } + } + } + + // ========================================================================= + // Bit allocation (Loudness method) + // ========================================================================= + + static void BitAllocation(SbcEncoder* enc, + int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS], + int ch, + int32_t scale_factors[SBC_SUBBANDS], + uint8_t bits[SBC_SUBBANDS]) { + // Compute scale factors + for (int sb = 0; sb < enc->Subbands; sb++) { + int32_t maxVal = 0; + for (int blk = 0; blk < enc->Blocks; blk++) { + int32_t val = sb_samples[blk][sb]; + if (val < 0) val = -val; + if (val > maxVal) maxVal = val; + } + + // Find scale factor (highest bit position) + scale_factors[sb] = 0; + int32_t tmp = maxVal; + while (tmp > 0) { + scale_factors[sb]++; + tmp >>= 1; + } + } + + // Loudness offset table for 8 subbands (from SBC spec) + static const int8_t loudness_offset_8[4][8] = { + {-2, 0, 0, 0, 0, 0, 0, 1}, // 16kHz + {-3, 0, 0, 0, 0, 0, 1, 2}, // 32kHz + {-4, 0, 0, 0, 0, 0, 1, 2}, // 44.1kHz + {-4, 0, 0, 0, 0, 0, 1, 2}, // 48kHz + }; + + // Compute bitneed + int32_t bitneed[SBC_SUBBANDS]; + for (int sb = 0; sb < enc->Subbands; sb++) { + if (enc->AllocMethod == ALLOC_LOUDNESS) { + bitneed[sb] = scale_factors[sb] - loudness_offset_8[enc->Frequency][sb]; + } else { + bitneed[sb] = scale_factors[sb]; + } + } + + // Bit allocation loop + int32_t bitcount = 0; + int32_t slicecount = 0; + int32_t bitslice = (int32_t)(scale_factors[0] > 0 ? scale_factors[0] : 1); + + // Find max bitneed + for (int sb = 0; sb < enc->Subbands; sb++) { + if (bitneed[sb] > bitslice) bitslice = bitneed[sb]; + } + bitslice++; + + // Iterative allocation + for (int sb = 0; sb < enc->Subbands; sb++) bits[sb] = 0; + + while (true) { + bitslice--; + bitcount = 0; + slicecount = 0; + for (int sb = 0; sb < enc->Subbands; sb++) { + if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) { + if (bitneed[sb] == bitslice + 1) { + bitcount += 2; + slicecount++; + } else { + bitcount++; + slicecount++; + } + } + } + if (bitcount + slicecount >= enc->Bitpool) break; + if (bitslice <= -16) break; + + for (int sb = 0; sb < enc->Subbands; sb++) { + if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) { + if (bitneed[sb] == bitslice + 1) { + bits[sb] = 2; + } else if (bits[sb] < 16) { + bits[sb]++; + } + } + } + } + + // Distribute remaining bits + int32_t remaining = enc->Bitpool - bitcount; + for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) { + if (bits[sb] >= 2 && bits[sb] < 16) { + bits[sb]++; + remaining--; + } else if (bitneed[sb] == bitslice && bits[sb] == 0) { + bits[sb] = 2; + remaining -= 2; + if (remaining < 0) { bits[sb] = 0; break; } + } + } + + for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) { + if (bits[sb] < 16) { + bits[sb]++; + remaining--; + } + } + } + + // ========================================================================= + // Bit packing helpers + // ========================================================================= + + struct BitWriter { + uint8_t* Data; + uint32_t BitPos; + }; + + static void WriteBits(BitWriter* bw, uint32_t value, uint8_t nbits) { + for (int i = nbits - 1; i >= 0; i--) { + uint32_t bytePos = bw->BitPos / 8; + uint8_t bitOff = 7 - (bw->BitPos % 8); + if (value & (1u << i)) { + bw->Data[bytePos] |= (1u << bitOff); + } + bw->BitPos++; + } + } + + // ========================================================================= + // Encode + // ========================================================================= + + uint32_t Encode(SbcEncoder* enc, const int16_t* pcm, uint8_t* out) { + int32_t sb_samples[SBC_CHANNELS][SBC_BLOCKS][SBC_SUBBANDS]; + int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS]; + uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS]; + + // Clear output + memset(out, 0, enc->FrameSize); + + // Analysis filter for each channel + for (int ch = 0; ch < enc->Channels; ch++) { + AnalysisFilter(enc, pcm, ch, sb_samples[ch]); + BitAllocation(enc, sb_samples[ch], ch, scale_factors[ch], bits[ch]); + } + + // Joint stereo processing + uint8_t joint = 0; + if (enc->ChannelMode == MODE_JOINT_STEREO) { + for (int sb = 0; sb < enc->Subbands - 1; sb++) { + // Simple heuristic: use joint coding if it saves bits + int32_t maxMid = 0, maxSide = 0; + for (int blk = 0; blk < enc->Blocks; blk++) { + int32_t mid = (sb_samples[0][blk][sb] + sb_samples[1][blk][sb]) / 2; + int32_t side = (sb_samples[0][blk][sb] - sb_samples[1][blk][sb]) / 2; + if (mid < 0) mid = -mid; + if (side < 0) side = -side; + if (mid > maxMid) maxMid = mid; + if (side > maxSide) maxSide = side; + } + int32_t maxOrig = 0; + for (int blk = 0; blk < enc->Blocks; blk++) { + int32_t v0 = sb_samples[0][blk][sb]; if (v0 < 0) v0 = -v0; + int32_t v1 = sb_samples[1][blk][sb]; if (v1 < 0) v1 = -v1; + if (v0 > maxOrig) maxOrig = v0; + if (v1 > maxOrig) maxOrig = v1; + } + if (maxMid + maxSide < maxOrig) { + joint |= (1 << (enc->Subbands - 1 - sb)); + for (int blk = 0; blk < enc->Blocks; blk++) { + int32_t l = sb_samples[0][blk][sb]; + int32_t r = sb_samples[1][blk][sb]; + sb_samples[0][blk][sb] = (l + r) / 2; + sb_samples[1][blk][sb] = (l - r) / 2; + } + // Recalculate scale factors for joint channels + for (int ch = 0; ch < 2; ch++) { + int32_t maxVal = 0; + for (int blk = 0; blk < enc->Blocks; blk++) { + int32_t val = sb_samples[ch][blk][sb]; + if (val < 0) val = -val; + if (val > maxVal) maxVal = val; + } + scale_factors[ch][sb] = 0; + int32_t tmp = maxVal; + while (tmp > 0) { scale_factors[ch][sb]++; tmp >>= 1; } + } + } + } + } + + // Pack SBC frame header + out[0] = 0x9C; // Sync word + out[1] = (enc->Frequency << 6) | ((enc->Blocks == 4 ? 0 : enc->Blocks == 8 ? 1 : enc->Blocks == 12 ? 2 : 3) << 4) + | (enc->ChannelMode << 2) | (enc->AllocMethod << 1) | (enc->Subbands == 8 ? 1 : 0); + out[2] = enc->Bitpool; + + // CRC (computed over header bytes 1-2 and scale factors) + // Will be filled after scale factors are packed + + BitWriter bw = {out, 32}; // Start after 4-byte header + + // Joint stereo flags + if (enc->ChannelMode == MODE_JOINT_STEREO) { + WriteBits(&bw, joint, enc->Subbands); + } + + // Pack scale factors (4 bits each) + for (int ch = 0; ch < enc->Channels; ch++) { + for (int sb = 0; sb < enc->Subbands; sb++) { + uint32_t sf = scale_factors[ch][sb]; + if (sf > 15) sf = 15; + WriteBits(&bw, sf, 4); + } + } + + // Compute CRC (over bytes 1, 2, and scale factor bits) + uint32_t crcBits = 16 + (enc->Channels * enc->Subbands * 4); + if (enc->ChannelMode == MODE_JOINT_STEREO) crcBits += enc->Subbands; + out[3] = SbcCrc8(&out[1], (crcBits + 7) / 8, crcBits % 8 ? crcBits % 8 : 8); + + // Pack audio samples + for (int blk = 0; blk < enc->Blocks; blk++) { + for (int ch = 0; ch < enc->Channels; ch++) { + for (int sb = 0; sb < enc->Subbands; sb++) { + if (bits[ch][sb] == 0) continue; + + int32_t sf = scale_factors[ch][sb]; + int32_t sample = sb_samples[ch][blk][sb]; + + // Quantize: levels = (1 << bits) - 1 + uint32_t levels = (1u << bits[ch][sb]) - 1; + int32_t quantized; + + if (sf > 0) { + // Normalize and quantize + int32_t maxRange = (1 << sf); + quantized = (int32_t)(((int64_t)(sample + maxRange) * levels) / (2 * maxRange)); + } else { + quantized = levels / 2; + } + + if (quantized < 0) quantized = 0; + if (quantized > (int32_t)levels) quantized = (int32_t)levels; + + WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]); + } + } + } + + // Pad to byte boundary + uint32_t totalBytes = (bw.BitPos + 7) / 8; + return totalBytes > enc->FrameSize ? enc->FrameSize : totalBytes; + } + + // ========================================================================= + // Queries + // ========================================================================= + + uint32_t GetFrameSize(const SbcEncoder* enc) { + return enc->FrameSize; + } + + uint32_t GetSamplesPerFrame(const SbcEncoder* enc) { + return enc->SamplesPerFrame; + } + +} diff --git a/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp b/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp new file mode 100644 index 0000000..d88dd60 --- /dev/null +++ b/kernel/src/Drivers/USB/Bluetooth/Sbc.hpp @@ -0,0 +1,90 @@ +/* + * Sbc.hpp + * SBC (Sub-Band Codec) encoder for Bluetooth A2DP + * Fixed-point implementation (no FPU/SSE required) + * Copyright (c) 2026 Daniel Hammer +*/ + +#pragma once +#include + +namespace Drivers::USB::Bluetooth::Sbc { + + // ========================================================================= + // SBC configuration + // ========================================================================= + + // Standard SBC parameters for A2DP + constexpr int SBC_SUBBANDS = 8; + constexpr int SBC_BLOCKS = 16; + constexpr int SBC_CHANNELS = 2; // Stereo + constexpr int SBC_BITPOOL = 53; // Standard quality + + // Allocation method + constexpr uint8_t ALLOC_SNR = 0; + constexpr uint8_t ALLOC_LOUDNESS = 1; + + // Channel mode + constexpr uint8_t MODE_MONO = 0; + constexpr uint8_t MODE_DUAL_CHANNEL = 1; + constexpr uint8_t MODE_STEREO = 2; + constexpr uint8_t MODE_JOINT_STEREO = 3; + + // Sampling frequency + constexpr uint8_t FREQ_16000 = 0; + constexpr uint8_t FREQ_32000 = 1; + constexpr uint8_t FREQ_44100 = 2; + constexpr uint8_t FREQ_48000 = 3; + + // SBC frame header + struct SbcHeader { + uint8_t SyncWord; // 0x9C + uint8_t Config; // freq(2) | blocks(2) | mode(2) | alloc(1) | subbands(1) + uint8_t Bitpool; + uint8_t Crc; + } __attribute__((packed)); + + // ========================================================================= + // Encoder state + // ========================================================================= + + struct SbcEncoder { + uint8_t Frequency; + uint8_t Blocks; + uint8_t ChannelMode; + uint8_t AllocMethod; + uint8_t Subbands; + uint8_t Bitpool; + uint8_t Channels; + + // Analysis filter state (per-channel windowed buffer) + int32_t X[SBC_CHANNELS][SBC_SUBBANDS * 10]; + int XPos[SBC_CHANNELS]; + + // Computed frame size in bytes + uint32_t FrameSize; + + // Samples per frame + uint32_t SamplesPerFrame; // blocks * subbands + }; + + // ========================================================================= + // Public API + // ========================================================================= + + // Initialize encoder with given parameters + void Init(SbcEncoder* enc, uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample); + + // Encode one SBC frame from PCM data + // pcm: interleaved 16-bit signed PCM, length = blocks * subbands * channels + // out: output buffer for SBC frame + // Returns number of bytes written to out + uint32_t Encode(SbcEncoder* enc, const int16_t* pcm, uint8_t* out); + + // Get the frame size in bytes for the current configuration + uint32_t GetFrameSize(const SbcEncoder* enc); + + // Get the number of PCM samples consumed per frame (per channel) + uint32_t GetSamplesPerFrame(const SbcEncoder* enc); + +} diff --git a/kernel/src/Drivers/USB/UsbDevice.cpp b/kernel/src/Drivers/USB/UsbDevice.cpp index f8fb39b..d709f8f 100644 --- a/kernel/src/Drivers/USB/UsbDevice.cpp +++ b/kernel/src/Drivers/USB/UsbDevice.cpp @@ -8,6 +8,7 @@ #include "Xhci.hpp" #include "HidKeyboard.hpp" #include "HidMouse.hpp" +#include "Bluetooth/Bluetooth.hpp" #include #include #include @@ -300,8 +301,9 @@ namespace Drivers::USB::UsbDevice { return 0; } - dev->VendorId = devDesc.idVendor; - dev->ProductId = devDesc.idProduct; + dev->VendorId = devDesc.idVendor; + dev->ProductId = devDesc.idProduct; + dev->DeviceClass = devDesc.bDeviceClass; KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId << ": VID:PID = " << base::hex << (uint64_t)devDesc.idVendor @@ -338,7 +340,10 @@ namespace Drivers::USB::UsbDevice { // ----------------------------------------------------------------- uint16_t offset = 0; bool foundHid = false; + bool foundBt = false; bool foundEp = false; + bool foundBulkIn = false; + bool foundBulkOut = false; uint16_t hidReportDescLen = 0; while (offset + 2 <= totalLen) { @@ -348,9 +353,10 @@ namespace Drivers::USB::UsbDevice { if (type == DESC_INTERFACE && offset + sizeof(InterfaceDescriptor) <= totalLen) { auto* iface = (InterfaceDescriptor*)&cfgBuf[offset]; - // Reset foundHid at each new interface boundary so we don't - // accidentally pick up endpoints from a different interface. + // Reset at each new interface boundary foundHid = false; + foundBt = false; + if (!foundEp && iface->bInterfaceClass == CLASS_HID && iface->bInterfaceSubClass == SUBCLASS_BOOT) { @@ -359,6 +365,18 @@ namespace Drivers::USB::UsbDevice { dev->InterfaceProtocol = iface->bInterfaceProtocol; foundHid = true; } + + // Bluetooth HCI interface (class 0xE0, subclass 0x01, protocol 0x01) + if (iface->bInterfaceClass == CLASS_WIRELESS && + iface->bInterfaceSubClass == SUBCLASS_RF && + iface->bInterfaceProtocol == PROTOCOL_BLUETOOTH) { + if (dev->InterfaceClass == 0 || dev->InterfaceClass == CLASS_WIRELESS) { + dev->InterfaceClass = iface->bInterfaceClass; + dev->InterfaceSubClass = iface->bInterfaceSubClass; + dev->InterfaceProtocol = iface->bInterfaceProtocol; + } + foundBt = true; + } } // HID descriptor (0x21): extract report descriptor length @@ -367,21 +385,55 @@ namespace Drivers::USB::UsbDevice { | ((uint16_t)cfgBuf[offset + 8] << 8); } - if (type == DESC_ENDPOINT && foundHid && !foundEp && - offset + sizeof(EndpointDescriptor) <= totalLen) { + if (type == DESC_ENDPOINT && offset + sizeof(EndpointDescriptor) <= totalLen) { auto* ep = (EndpointDescriptor*)&cfgBuf[offset]; - if ((ep->bEndpointAddress & EP_DIR_IN) && - (ep->bmAttributes & EP_XFER_TYPE_MASK) == EP_XFER_INTERRUPT) { + uint8_t xferType = ep->bmAttributes & EP_XFER_TYPE_MASK; + bool isIn = (ep->bEndpointAddress & EP_DIR_IN) != 0; + + // HID interrupt IN endpoint + if (foundHid && !foundEp && isIn && xferType == EP_XFER_INTERRUPT) { dev->InterruptEpNum = ep->bEndpointAddress & 0x0F; dev->InterruptMaxPacket = ep->wMaxPacketSize & 0x7FF; dev->InterruptInterval = ep->bInterval; foundEp = true; } + + // Bluetooth endpoints + if (foundBt) { + if (isIn && xferType == EP_XFER_INTERRUPT && !foundEp) { + // HCI event pipe (interrupt IN) + dev->InterruptEpNum = ep->bEndpointAddress & 0x0F; + dev->InterruptMaxPacket = ep->wMaxPacketSize & 0x7FF; + dev->InterruptInterval = ep->bInterval; + foundEp = true; + } else if (isIn && xferType == EP_XFER_BULK && !foundBulkIn) { + // ACL data IN pipe (bulk IN) + dev->BulkInEpNum = ep->bEndpointAddress & 0x0F; + dev->BulkInMaxPacket = ep->wMaxPacketSize & 0x7FF; + foundBulkIn = true; + } else if (!isIn && xferType == EP_XFER_BULK && !foundBulkOut) { + // ACL data OUT pipe (bulk OUT) + dev->BulkOutEpNum = ep->bEndpointAddress & 0x0F; + dev->BulkOutMaxPacket = ep->wMaxPacketSize & 0x7FF; + foundBulkOut = true; + } + } } offset += len; } + // For Bluetooth devices, also check device class for correct identification + // Some BT adapters use bDeviceClass=0xE0 at device level + if (!foundBt && devDesc.bDeviceClass == CLASS_WIRELESS && + devDesc.bDeviceSubClass == SUBCLASS_RF && + devDesc.bDeviceProtocol == PROTOCOL_BLUETOOTH) { + dev->InterfaceClass = CLASS_WIRELESS; + dev->InterfaceSubClass = SUBCLASS_RF; + dev->InterfaceProtocol = PROTOCOL_BLUETOOTH; + foundBt = true; + } + // ----------------------------------------------------------------- // Step 8: SET_CONFIGURATION // ----------------------------------------------------------------- @@ -394,57 +446,106 @@ namespace Drivers::USB::UsbDevice { } // ----------------------------------------------------------------- - // Step 9: Configure Endpoint (if HID interrupt endpoint was found) + // Step 9: Configure Endpoints // ----------------------------------------------------------------- - if (foundEp) { - // Device Context Index for an IN endpoint: DCI = EpNum * 2 + 1 - uint8_t dci = dev->InterruptEpNum * 2 + 1; - + if (foundEp || foundBulkIn || foundBulkOut) { auto* inputCtx2 = (Xhci::InputContext*)Memory::g_pfa->AllocateZeroed(); - // ICC: Add slot context (bit 0) and the interrupt endpoint (bit dci) - inputCtx2->ICC.AddFlags = (1 << 0) | (1 << dci); + // Start with slot context + inputCtx2->ICC.AddFlags = (1 << 0); // Copy the current slot context from the output context inputCtx2->Slot = dev->OutputContext->Slot; - // Update Context Entries in slot context to at least cover this DCI - uint32_t newCtxEntries = dci; + // Track the highest DCI to set Context Entries + uint32_t maxDci = 0; + + // --- Configure Interrupt IN endpoint --- + if (foundEp) { + uint8_t dci = dev->InterruptEpNum * 2 + 1; + inputCtx2->ICC.AddFlags |= (1 << dci); + if (dci > maxDci) maxDci = dci; + + // Allocate interrupt transfer ring + auto* intRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed(); + dev->InterruptRing = intRing; + dev->InterruptRingPhys = Memory::SubHHDM(intRing); + dev->InterruptRingEnqueue = 0; + dev->InterruptRingCCS = true; + + Xhci::TRB& intLink = intRing[Xhci::XFER_RING_SIZE - 1]; + intLink.Parameter0 = (uint32_t)(dev->InterruptRingPhys & 0xFFFFFFFF); + intLink.Parameter1 = (uint32_t)(dev->InterruptRingPhys >> 32); + intLink.Status = 0; + intLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT; + + auto& epCtx = inputCtx2->EP[dci - 1]; + uint32_t xhciInterval = ConvertInterval(speed, dev->InterruptInterval); + epCtx.Field0 = (xhciInterval << 16); + epCtx.Field1 = (3 << 1) + | (Xhci::EP_TYPE_INTERRUPT_IN << 3) + | ((uint32_t)dev->InterruptMaxPacket << 16); + epCtx.TRDequeuePtr = dev->InterruptRingPhys | 1; + epCtx.Field2 = dev->InterruptMaxPacket; + } + + // --- Configure Bulk IN endpoint --- + if (foundBulkIn) { + uint8_t dci = dev->BulkInEpNum * 2 + 1; + inputCtx2->ICC.AddFlags |= (1 << dci); + if (dci > maxDci) maxDci = dci; + + auto* bulkInRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed(); + dev->BulkInRing = bulkInRing; + dev->BulkInRingPhys = Memory::SubHHDM(bulkInRing); + dev->BulkInRingEnqueue = 0; + dev->BulkInRingCCS = true; + + Xhci::TRB& biLink = bulkInRing[Xhci::XFER_RING_SIZE - 1]; + biLink.Parameter0 = (uint32_t)(dev->BulkInRingPhys & 0xFFFFFFFF); + biLink.Parameter1 = (uint32_t)(dev->BulkInRingPhys >> 32); + biLink.Status = 0; + biLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT; + + auto& epCtx = inputCtx2->EP[dci - 1]; + epCtx.Field0 = 0; + epCtx.Field1 = (3 << 1) + | (Xhci::EP_TYPE_BULK_IN << 3) + | ((uint32_t)dev->BulkInMaxPacket << 16); + epCtx.TRDequeuePtr = dev->BulkInRingPhys | 1; + epCtx.Field2 = dev->BulkInMaxPacket; + } + + // --- Configure Bulk OUT endpoint --- + if (foundBulkOut) { + uint8_t dci = dev->BulkOutEpNum * 2; + inputCtx2->ICC.AddFlags |= (1 << dci); + if (dci > maxDci) maxDci = dci; + + auto* bulkOutRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed(); + dev->BulkOutRing = bulkOutRing; + dev->BulkOutRingPhys = Memory::SubHHDM(bulkOutRing); + dev->BulkOutRingEnqueue = 0; + dev->BulkOutRingCCS = true; + + Xhci::TRB& boLink = bulkOutRing[Xhci::XFER_RING_SIZE - 1]; + boLink.Parameter0 = (uint32_t)(dev->BulkOutRingPhys & 0xFFFFFFFF); + boLink.Parameter1 = (uint32_t)(dev->BulkOutRingPhys >> 32); + boLink.Status = 0; + boLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT; + + auto& epCtx = inputCtx2->EP[dci - 1]; + epCtx.Field0 = 0; + epCtx.Field1 = (3 << 1) + | (Xhci::EP_TYPE_BULK_OUT << 3) + | ((uint32_t)dev->BulkOutMaxPacket << 16); + epCtx.TRDequeuePtr = dev->BulkOutRingPhys | 1; + epCtx.Field2 = dev->BulkOutMaxPacket; + } + + // Update Context Entries to cover the highest DCI inputCtx2->Slot.Field0 = (inputCtx2->Slot.Field0 & ~(0x1Fu << 27)) - | (newCtxEntries << 27); - - // Allocate interrupt transfer ring - auto* intRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed(); - dev->InterruptRing = intRing; - dev->InterruptRingPhys = Memory::SubHHDM(intRing); - dev->InterruptRingEnqueue = 0; - dev->InterruptRingCCS = true; - - // Set up Link TRB at last position - Xhci::TRB& intLink = intRing[Xhci::XFER_RING_SIZE - 1]; - intLink.Parameter0 = (uint32_t)(dev->InterruptRingPhys & 0xFFFFFFFF); - intLink.Parameter1 = (uint32_t)(dev->InterruptRingPhys >> 32); - intLink.Status = 0; - intLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT; - - // Endpoint Context for the interrupt IN endpoint - auto& epCtx = inputCtx2->EP[dci - 1]; // EP array is 0-indexed, DCI 1 = EP[0] - - // Field0: Interval (bits 23:16) — convert bInterval to xHCI encoding - uint32_t xhciInterval = ConvertInterval(speed, dev->InterruptInterval); - epCtx.Field0 = (xhciInterval << 16); - - // Field1: CErr=3 (bits 2:1), EP Type=Interrupt IN=7 (bits 5:3), - // Max Packet Size (bits 31:16) - epCtx.Field1 = (3 << 1) - | (Xhci::EP_TYPE_INTERRUPT_IN << 3) - | ((uint32_t)dev->InterruptMaxPacket << 16); - - // TR Dequeue Pointer with DCS=1 - epCtx.TRDequeuePtr = dev->InterruptRingPhys | 1; - - // Average TRB Length - epCtx.Field2 = dev->InterruptMaxPacket; + | (maxDci << 27); // Send Configure Endpoint command Xhci::TRB cfgTrb = {}; @@ -462,9 +563,18 @@ namespace Drivers::USB::UsbDevice { return 0; } - KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId - << ": Interrupt EP " << (uint64_t)dev->InterruptEpNum - << " configured (DCI " << (uint64_t)dci << ")"; + if (foundEp) { + KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId + << ": Interrupt EP " << (uint64_t)dev->InterruptEpNum << " configured"; + } + if (foundBulkIn) { + KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId + << ": Bulk IN EP " << (uint64_t)dev->BulkInEpNum << " configured"; + } + if (foundBulkOut) { + KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId + << ": Bulk OUT EP " << (uint64_t)dev->BulkOutEpNum << " configured"; + } } // ----------------------------------------------------------------- @@ -473,7 +583,7 @@ namespace Drivers::USB::UsbDevice { // Set Boot Protocol for keyboards only. // Mice stay in Report Protocol (the default) for scroll wheel support; // HidMouse parses the HID Report Descriptor to handle variable formats. - if (foundEp && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { + if (foundEp && dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { cc = Xhci::ControlTransfer(slotId, REQTYPE_CLASS_IFACE, REQ_SET_PROTOCOL, 0, 0, 0, nullptr, false); if (cc != Xhci::CC_SUCCESS) { @@ -503,7 +613,7 @@ namespace Drivers::USB::UsbDevice { // ----------------------------------------------------------------- // Step 11: SET_IDLE(0) -- only report on changes (no idle reports) // ----------------------------------------------------------------- - if (foundEp && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { + if (foundEp && dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { // wValue upper byte = duration (0 = indefinite), lower byte = report ID cc = Xhci::ControlTransfer(slotId, REQTYPE_CLASS_IFACE, REQ_SET_IDLE, (0 << 8), 0, 0, nullptr, false); @@ -514,24 +624,33 @@ namespace Drivers::USB::UsbDevice { } // ----------------------------------------------------------------- - // Step 12: Queue first interrupt transfer + // Step 12: Queue first interrupt transfer (HID only) + // Bluetooth manages its own interrupt/bulk transfers via StartEventPipe() // ----------------------------------------------------------------- - if (foundEp) { + if (foundEp && !foundBt) { Xhci::QueueInterruptTransfer(slotId); } // ----------------------------------------------------------------- - // Step 13: Register with the appropriate HID driver + // Step 13: Register with the appropriate class driver // ----------------------------------------------------------------- - if (dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { + if (dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) { HidKeyboard::RegisterDevice(slotId); KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": HID Boot Keyboard"; - } else if (dev->InterfaceProtocol == PROTOCOL_MOUSE) { + } else if (dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_MOUSE) { HidMouse::RegisterDevice(slotId); KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": HID Boot Mouse"; + } else if (dev->InterfaceClass == CLASS_WIRELESS && + dev->InterfaceSubClass == SUBCLASS_RF && + dev->InterfaceProtocol == PROTOCOL_BLUETOOTH) { + Bluetooth::RegisterAdapter(slotId); + KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": Bluetooth Adapter" + << " VID:" << base::hex << (uint64_t)dev->VendorId + << " PID:" << (uint64_t)dev->ProductId << base::dec; } else if (foundEp) { KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId - << ": HID device, protocol=" << (uint64_t)dev->InterfaceProtocol; + << ": USB device, class=" << (uint64_t)dev->InterfaceClass + << " protocol=" << (uint64_t)dev->InterfaceProtocol; } else { KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId << ": Non-HID device, class=" << (uint64_t)devDesc.bDeviceClass; diff --git a/kernel/src/Drivers/USB/UsbDevice.hpp b/kernel/src/Drivers/USB/UsbDevice.hpp index d7a9e0d..b5aedfb 100644 --- a/kernel/src/Drivers/USB/UsbDevice.hpp +++ b/kernel/src/Drivers/USB/UsbDevice.hpp @@ -80,6 +80,11 @@ namespace Drivers::USB::UsbDevice { constexpr uint8_t PROTOCOL_KEYBOARD = 0x01; constexpr uint8_t PROTOCOL_MOUSE = 0x02; + // Wireless Controller class (Bluetooth) + constexpr uint8_t CLASS_WIRELESS = 0xE0; + constexpr uint8_t SUBCLASS_RF = 0x01; + constexpr uint8_t PROTOCOL_BLUETOOTH = 0x01; + // USB standard requests (bRequest) constexpr uint8_t REQ_GET_DESCRIPTOR = 0x06; constexpr uint8_t REQ_SET_CONFIGURATION = 0x09; @@ -100,6 +105,9 @@ namespace Drivers::USB::UsbDevice { // Endpoint transfer type mask constexpr uint8_t EP_XFER_TYPE_MASK = 0x03; + constexpr uint8_t EP_XFER_CONTROL = 0x00; + constexpr uint8_t EP_XFER_ISOCH = 0x01; + constexpr uint8_t EP_XFER_BULK = 0x02; constexpr uint8_t EP_XFER_INTERRUPT = 0x03; // --------------------------------------------------------------------------- diff --git a/kernel/src/Drivers/USB/Xhci.cpp b/kernel/src/Drivers/USB/Xhci.cpp index 440091e..872e202 100644 --- a/kernel/src/Drivers/USB/Xhci.cpp +++ b/kernel/src/Drivers/USB/Xhci.cpp @@ -98,6 +98,13 @@ namespace Drivers::USB::Xhci { static uint8_t* g_interruptDataBuf[MAX_SLOTS + 1] = {}; static uint64_t g_interruptDataBufPhys[MAX_SLOTS + 1] = {}; + // Bulk IN transfer data buffers (per slot) + static uint8_t* g_bulkInDataBuf[MAX_SLOTS + 1] = {}; + static uint64_t g_bulkInDataBufPhys[MAX_SLOTS + 1] = {}; + + // Transfer callbacks for non-HID class drivers (per slot) + static TransferCallback g_transferCallbacks[MAX_SLOTS + 1] = {}; + // Scratchpad buffer array static uint64_t* g_scratchpadBufs = nullptr; @@ -174,6 +181,36 @@ namespace Drivers::USB::Xhci { } } + // Advance bulk IN ring enqueue pointer, activating Link TRB when reached + static void AdvanceBulkInRing(UsbDeviceInfo& dev) { + dev.BulkInRingEnqueue++; + if (dev.BulkInRingEnqueue >= XFER_RING_SIZE - 1) { + TRB& link = dev.BulkInRing[XFER_RING_SIZE - 1]; + if (dev.BulkInRingCCS) { + link.Control |= TRB_CYCLE_BIT; + } else { + link.Control &= ~TRB_CYCLE_BIT; + } + dev.BulkInRingCCS = !dev.BulkInRingCCS; + dev.BulkInRingEnqueue = 0; + } + } + + // Advance bulk OUT ring enqueue pointer, activating Link TRB when reached + static void AdvanceBulkOutRing(UsbDeviceInfo& dev) { + dev.BulkOutRingEnqueue++; + if (dev.BulkOutRingEnqueue >= XFER_RING_SIZE - 1) { + TRB& link = dev.BulkOutRing[XFER_RING_SIZE - 1]; + if (dev.BulkOutRingCCS) { + link.Control |= TRB_CYCLE_BIT; + } else { + link.Control &= ~TRB_CYCLE_BIT; + } + dev.BulkOutRingCCS = !dev.BulkOutRingCCS; + dev.BulkOutRingEnqueue = 0; + } + } + // ------------------------------------------------------------------------- // Forward declarations // ------------------------------------------------------------------------- @@ -279,37 +316,62 @@ namespace Drivers::USB::Xhci { g_xferCompletionCode = completionCode; g_xferCompleted = true; } else if (slotId > 0 && slotId <= MAX_SLOTS && g_devices[slotId].Active) { - // Interrupt IN endpoint completion UsbDeviceInfo& dev = g_devices[slotId]; if (completionCode == CC_SUCCESS || completionCode == CC_SHORT_PACKET) { - uint16_t len = dev.InterruptMaxPacket; - - // Residual byte count: bits 23:0 of Status gives residual + // Compute actual transfer length from residual uint32_t residual = evt.Status & 0x00FFFFFF; - if (residual < len) { - len = dev.InterruptMaxPacket - (uint16_t)residual; - } - // Dispatch to HID driver based on interface protocol - if (dev.InterfaceClass == UsbDevice::CLASS_HID) { - if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_KEYBOARD) { - HidKeyboard::ProcessReport(g_interruptDataBuf[slotId], len); - } else if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_MOUSE) { - HidMouse::ProcessReport(g_interruptDataBuf[slotId], len); + // Check if this is a bulk IN endpoint completion + uint8_t bulkInDci = dev.BulkInEpNum ? (dev.BulkInEpNum * 2 + 1) : 0; + uint8_t bulkOutDci = dev.BulkOutEpNum ? (dev.BulkOutEpNum * 2) : 0; + uint8_t intDci = dev.InterruptEpNum ? (dev.InterruptEpNum * 2 + 1) : 0; + + if (epDci == bulkInDci && g_transferCallbacks[slotId]) { + // Bulk IN — dispatch via registered callback + uint16_t len = dev.BulkInMaxPacket; + if (residual < len) len = dev.BulkInMaxPacket - (uint16_t)residual; + g_transferCallbacks[slotId](slotId, epDci, + g_bulkInDataBuf[slotId], len, completionCode); + } else if (epDci == bulkOutDci && g_transferCallbacks[slotId]) { + // Bulk OUT completion — notify callback + g_transferCallbacks[slotId](slotId, epDci, + nullptr, 0, completionCode); + } else if (epDci == intDci) { + // Interrupt IN — HID or callback dispatch + uint16_t len = dev.InterruptMaxPacket; + if (residual < len) len = dev.InterruptMaxPacket - (uint16_t)residual; + + if (dev.InterfaceClass == UsbDevice::CLASS_HID) { + if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_KEYBOARD) { + HidKeyboard::ProcessReport(g_interruptDataBuf[slotId], len); + } else if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_MOUSE) { + HidMouse::ProcessReport(g_interruptDataBuf[slotId], len); + } + // Re-queue for next HID report + QueueInterruptTransfer(slotId); + } else if (g_transferCallbacks[slotId]) { + // Callback is responsible for re-queuing + g_transferCallbacks[slotId](slotId, epDci, + g_interruptDataBuf[slotId], len, completionCode); } + } else if (g_transferCallbacks[slotId]) { + // Unknown endpoint — try callback + g_transferCallbacks[slotId](slotId, epDci, + nullptr, 0, completionCode); } } else { KernelLogStream(WARNING, "xHCI") << "Transfer error on slot " << base::dec << (uint64_t)slotId << " ep " << (uint64_t)epDci << " cc=" << (uint64_t)completionCode; + + // Notify callback of errors too + if (g_transferCallbacks[slotId]) { + g_transferCallbacks[slotId](slotId, epDci, + nullptr, 0, completionCode); + } } - // Always re-queue the next interrupt transfer so the - // device can continue delivering reports. Transient - // errors (stall, babble, etc.) must not permanently - // kill the transfer chain. - QueueInterruptTransfer(slotId); } break; } @@ -495,6 +557,31 @@ namespace Drivers::USB::Xhci { } KernelLogStream(WARNING, "xHCI") << "Control transfer timeout on slot " << base::dec << (uint64_t)slotId; + + // Recover EP0 ring: Stop Endpoint, then Set TR Dequeue Pointer + // so that subsequent control transfers on this slot still work. + TRB stopTrb = {}; + stopTrb.Control = (TRB_STOP_ENDPOINT << TRB_TYPE_SHIFT) + | ((uint32_t)slotId << 24) + | (1 << 16); // DCI=1 (EP0) in bits 20:16 + SendCommand(stopTrb); + + // Reset the enqueue pointer to the start and set the dequeue pointer + // to match so the ring is back in sync. + uint64_t newDeq = dev.EP0RingPhys + + (uint64_t)dev.EP0RingEnqueue * sizeof(TRB); + if (dev.EP0RingCCS) { + newDeq |= 1; // DCS bit + } + + TRB deqTrb = {}; + deqTrb.Parameter0 = (uint32_t)(newDeq & 0xFFFFFFFF); + deqTrb.Parameter1 = (uint32_t)(newDeq >> 32); + deqTrb.Control = (TRB_SET_TR_DEQUEUE << TRB_TYPE_SHIFT) + | ((uint32_t)slotId << 24) + | (1 << 16); // DCI=1 (EP0) + SendCommand(deqTrb); + return 0xFF; } @@ -534,6 +621,81 @@ namespace Drivers::USB::Xhci { WriteDoorbell(slotId, target); } + // ------------------------------------------------------------------------- + // QueueBulkInTransfer + // ------------------------------------------------------------------------- + + void QueueBulkInTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length) { + if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return; + + UsbDeviceInfo& dev = g_devices[slotId]; + if (!dev.BulkInRing || dev.BulkInEpNum == 0) return; + + // If caller provides nullptr, use the per-slot bulk IN DMA buffer + if (data == nullptr) { + if (g_bulkInDataBuf[slotId] == nullptr) { + g_bulkInDataBuf[slotId] = AllocateDmaBuffer(g_bulkInDataBufPhys[slotId]); + } + data = g_bulkInDataBuf[slotId]; + dataPhys = g_bulkInDataBufPhys[slotId]; + } + + TRB& trb = dev.BulkInRing[dev.BulkInRingEnqueue]; + trb.Parameter0 = (uint32_t)(dataPhys & 0xFFFFFFFF); + trb.Parameter1 = (uint32_t)(dataPhys >> 32); + trb.Status = length; + + uint32_t control = (TRB_NORMAL << TRB_TYPE_SHIFT) | TRB_IOC | TRB_ISP; + if (dev.BulkInRingCCS) { + control |= TRB_CYCLE_BIT; + } + trb.Control = control; + + AdvanceBulkInRing(dev); + + // Ring doorbell: DCI for bulk IN = EpNum * 2 + 1 + uint8_t target = dev.BulkInEpNum * 2 + 1; + WriteDoorbell(slotId, target); + } + + // ------------------------------------------------------------------------- + // QueueBulkOutTransfer + // ------------------------------------------------------------------------- + + void QueueBulkOutTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length) { + if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return; + + UsbDeviceInfo& dev = g_devices[slotId]; + if (!dev.BulkOutRing || dev.BulkOutEpNum == 0) return; + + TRB& trb = dev.BulkOutRing[dev.BulkOutRingEnqueue]; + trb.Parameter0 = (uint32_t)(dataPhys & 0xFFFFFFFF); + trb.Parameter1 = (uint32_t)(dataPhys >> 32); + trb.Status = length; + + uint32_t control = (TRB_NORMAL << TRB_TYPE_SHIFT) | TRB_IOC; + if (dev.BulkOutRingCCS) { + control |= TRB_CYCLE_BIT; + } + trb.Control = control; + + AdvanceBulkOutRing(dev); + + // Ring doorbell: DCI for bulk OUT = EpNum * 2 + uint8_t target = dev.BulkOutEpNum * 2; + WriteDoorbell(slotId, target); + } + + // ------------------------------------------------------------------------- + // RegisterTransferCallback + // ------------------------------------------------------------------------- + + void RegisterTransferCallback(uint8_t slotId, TransferCallback cb) { + if (slotId > 0 && slotId <= MAX_SLOTS) { + g_transferCallbacks[slotId] = cb; + } + } + // ------------------------------------------------------------------------- // RingDoorbell // ------------------------------------------------------------------------- diff --git a/kernel/src/Drivers/USB/Xhci.hpp b/kernel/src/Drivers/USB/Xhci.hpp index 2319f07..5d89a01 100644 --- a/kernel/src/Drivers/USB/Xhci.hpp +++ b/kernel/src/Drivers/USB/Xhci.hpp @@ -138,6 +138,8 @@ namespace Drivers::USB::Xhci { constexpr uint32_t TRB_CONFIGURE_ENDPOINT = 12; constexpr uint32_t TRB_EVALUATE_CONTEXT = 13; constexpr uint32_t TRB_RESET_ENDPOINT = 14; + constexpr uint32_t TRB_STOP_ENDPOINT = 15; + constexpr uint32_t TRB_SET_TR_DEQUEUE = 16; constexpr uint32_t TRB_NOOP_CMD = 23; constexpr uint32_t TRB_TRANSFER_EVENT = 32; constexpr uint32_t TRB_COMMAND_COMPLETION = 33; @@ -235,6 +237,7 @@ namespace Drivers::USB::Xhci { uint8_t InterfaceClass; uint8_t InterfaceSubClass; uint8_t InterfaceProtocol; + uint8_t DeviceClass; // bDeviceClass from device descriptor // Interrupt IN endpoint uint8_t InterruptEpNum; // Endpoint number (1-15) @@ -247,6 +250,22 @@ namespace Drivers::USB::Xhci { uint32_t InterruptRingEnqueue; bool InterruptRingCCS; // Current Cycle State + // Bulk IN endpoint + uint8_t BulkInEpNum; + uint16_t BulkInMaxPacket; + TRB* BulkInRing; + uint64_t BulkInRingPhys; + uint32_t BulkInRingEnqueue; + bool BulkInRingCCS; + + // Bulk OUT endpoint + uint8_t BulkOutEpNum; + uint16_t BulkOutMaxPacket; + TRB* BulkOutRing; + uint64_t BulkOutRingPhys; + uint32_t BulkOutRingEnqueue; + bool BulkOutRingCCS; + // EP0 transfer ring TRB* EP0Ring; uint64_t EP0RingPhys; @@ -258,6 +277,14 @@ namespace Drivers::USB::Xhci { uint64_t OutputContextPhys; }; + // --------------------------------------------------------------------------- + // Transfer callback for non-HID class drivers (Bluetooth, etc.) + // --------------------------------------------------------------------------- + + using TransferCallback = void (*)(uint8_t slotId, uint8_t epDci, + const uint8_t* data, uint32_t length, + uint32_t completionCode); + // --------------------------------------------------------------------------- // Public API // --------------------------------------------------------------------------- @@ -285,6 +312,13 @@ namespace Drivers::USB::Xhci { // Queue an interrupt IN transfer on a device's interrupt endpoint void QueueInterruptTransfer(uint8_t slotId); + // Queue a bulk transfer on a device's bulk IN or OUT endpoint + void QueueBulkInTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length); + void QueueBulkOutTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length); + + // Register a transfer callback for a specific slot (used by non-HID class drivers) + void RegisterTransferCallback(uint8_t slotId, TransferCallback cb); + // Ring a doorbell void RingDoorbell(uint8_t slotId, uint8_t target); diff --git a/kernel/src/Fs/Ext2.cpp b/kernel/src/Fs/Ext2.cpp index 5fcadc2..87101e9 100644 --- a/kernel/src/Fs/Ext2.cpp +++ b/kernel/src/Fs/Ext2.cpp @@ -1103,10 +1103,14 @@ namespace Fs::Ext2 { self.dirNameCount = count; for (int i = 0; i < count; i++) { int j = 0; - while (entries[i].name[j] && j < MaxNameLen - 1) { + while (entries[i].name[j] && j < MaxNameLen - 2) { self.dirNames[i][j] = entries[i].name[j]; j++; } + // Append trailing '/' for directories so userspace can distinguish them + if (entries[i].fileType == EXT2_FT_DIR && j < MaxNameLen - 1) { + self.dirNames[i][j++] = '/'; + } self.dirNames[i][j] = '\0'; outNames[i] = self.dirNames[i]; } @@ -1279,15 +1283,27 @@ namespace Fs::Ext2 { Inode targetInode; if (!ReadInode(self, existing.inodeNum, &targetInode)) return -1; - // Don't delete directories through this call - if ((targetInode.i_mode & IMODE_TYPE_MASK) == IMODE_DIR) return -1; + bool isDir = (targetInode.i_mode & IMODE_TYPE_MASK) == IMODE_DIR; + if (isDir) { + // Only allow deleting empty directories + ParsedEntry children[1]; + int childCount = ReadDirectoryEntries(self, targetInode, children, 1); + if (childCount > 0) return -1; + } // Remove directory entry if (!RemoveDirEntry(self, parentInode, fileName)) return -1; + // If we deleted a subdirectory, decrement parent's link count (for "..") + if (isDir) { + parentInode.i_links_count--; + WriteInode(self, parentInodeNum, &parentInode); + } + // Decrement link count targetInode.i_links_count--; - if (targetInode.i_links_count == 0) { + if (isDir) targetInode.i_links_count--; // account for "." self-link + if (targetInode.i_links_count <= 0) { // Free all blocks and the inode FreeInodeBlocks(self, targetInode); targetInode.i_mode = 0; diff --git a/kernel/src/Fs/Fat32.cpp b/kernel/src/Fs/Fat32.cpp index 584cebf..cf2ec1c 100644 --- a/kernel/src/Fs/Fat32.cpp +++ b/kernel/src/Fs/Fat32.cpp @@ -940,10 +940,14 @@ namespace Fs::Fat32 { self.dirNameCount = count; for (int i = 0; i < count; i++) { int j = 0; - while (entries[i].name[j] && j < MaxNameLen - 1) { + while (entries[i].name[j] && j < MaxNameLen - 2) { self.dirNames[i][j] = entries[i].name[j]; j++; } + // Append trailing '/' for directories so userspace can distinguish them + if ((entries[i].attributes & ATTR_DIRECTORY) && j < MaxNameLen - 1) { + self.dirNames[i][j++] = '/'; + } self.dirNames[i][j] = '\0'; outNames[i] = self.dirNames[i]; } @@ -1221,10 +1225,15 @@ namespace Fs::Fat32 { uint32_t parentCluster = parentEntry.firstCluster; - // Find the file + // Find the entry ParsedEntry existing; if (!FindInDirectory(inst, parentCluster, fileName, &existing)) return -1; - if (existing.attributes & ATTR_DIRECTORY) return -1; // don't delete directories + if (existing.attributes & ATTR_DIRECTORY) { + // Only allow deleting empty directories + ParsedEntry children[1]; + int childCount = ReadDirectory(inst, existing.firstCluster, children, 1); + if (childCount > 0) return -1; + } // Free the cluster chain uint32_t cl = existing.firstCluster; diff --git a/kernel/src/Main.cpp b/kernel/src/Main.cpp index 7fb1b31..5489b20 100644 --- a/kernel/src/Main.cpp +++ b/kernel/src/Main.cpp @@ -158,6 +158,7 @@ extern "C" void kmain() { Drivers::ProbeNormal(); Drivers::InitializeNetwork(); Drivers::InitializeStorage(); + Drivers::InitializeAudio(); } #endif diff --git a/programs/GNUmakefile b/programs/GNUmakefile index a33c8c0..a073261 100644 --- a/programs/GNUmakefile +++ b/programs/GNUmakefile @@ -59,7 +59,7 @@ BINDIR := bin PROGRAMS := $(notdir $(wildcard src/*)) # Programs with custom Makefiles (built separately). -CUSTOM_BUILDS := doom fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer desktop +CUSTOM_BUILDS := doom fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer volume music bluetooth desktop SYSTEM_PROGRAMS := $(filter-out $(CUSTOM_BUILDS),$(PROGRAMS)) # Build targets: system programs go to bin/os/, apps go to bin/apps//. @@ -81,9 +81,9 @@ CA_CERTS := $(BINDIR)/etc/ca-certificates.crt # Home directory placeholder. HOMEKEEP := $(BINDIR)/home/.keep -.PHONY: all clean doom fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer desktop icons fonts bearssl libc tls libjpeg install-apps +.PHONY: all clean doom fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer volume music bluetooth desktop icons fonts bearssl libc tls libjpeg install-apps -all: bearssl libc libjpeg tls $(TARGETS) fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer doom desktop icons fonts install-apps $(MANDST) $(WWWDST) $(CA_CERTS) $(HOMEKEEP) +all: bearssl libc libjpeg tls $(TARGETS) fetch wiki wikipedia weather imageviewer fontpreview spreadsheet pdfviewer disks devexplorer installer volume music bluetooth doom desktop icons fonts install-apps $(MANDST) $(WWWDST) $(CA_CERTS) $(HOMEKEEP) # Build BearSSL static library (cross-compiled for freestanding x86_64). BEARSSL_INCLUDES := -isystem $(shell cd .. && pwd)/kernel/freestnd-c-hdrs/x86_64/include -isystem $(abspath include/libc) @@ -152,6 +152,18 @@ devexplorer: libc installer: libc $(MAKE) -C src/installer +# Build volume control standalone GUI tool (depends on libc). +volume: libc + $(MAKE) -C src/volume + +# Build music player standalone GUI tool (depends on libc). +music: libc + $(MAKE) -C src/music + +# Build bluetooth manager standalone GUI tool (depends on libc). +bluetooth: libc + $(MAKE) -C src/bluetooth + # Build desktop via its own Makefile (depends on libc and libjpeg for wallpaper). desktop: libc libjpeg $(MAKE) -C src/desktop @@ -169,7 +181,7 @@ doom: $(MAKE) -C src/doom # Install app bundles (manifests, icons, data files) into bin/apps//. -install-apps: doom spreadsheet weather wikipedia imageviewer fontpreview pdfviewer disks devexplorer installer +install-apps: doom spreadsheet weather wikipedia imageviewer fontpreview pdfviewer disks devexplorer installer volume music bluetooth ../scripts/install_apps.sh # Copy man pages into bin/man/ so mkramdisk.sh picks them up. @@ -216,3 +228,6 @@ clean: $(MAKE) -C src/disks clean $(MAKE) -C src/devexplorer clean $(MAKE) -C src/installer clean + $(MAKE) -C src/volume clean + $(MAKE) -C src/music clean + $(MAKE) -C src/bluetooth clean diff --git a/programs/include/Api/Syscall.hpp b/programs/include/Api/Syscall.hpp index e1c0dfd..7dc1435 100644 --- a/programs/include/Api/Syscall.hpp +++ b/programs/include/Api/Syscall.hpp @@ -100,6 +100,28 @@ namespace Montauk { static constexpr uint64_t SYS_FSMOUNT = 75; static constexpr uint64_t SYS_FSFORMAT = 76; + // Audio syscalls + static constexpr uint64_t SYS_AUDIOOPEN = 80; + static constexpr uint64_t SYS_AUDIOCLOSE = 81; + static constexpr uint64_t SYS_AUDIOWRITE = 82; + static constexpr uint64_t SYS_AUDIOCTL = 83; + + // Bluetooth syscalls + static constexpr uint64_t SYS_BTSCAN = 84; + static constexpr uint64_t SYS_BTCONNECT = 85; + static constexpr uint64_t SYS_BTDISCONNECT = 86; + static constexpr uint64_t SYS_BTLIST = 87; + static constexpr uint64_t SYS_BTINFO = 88; + + // Audio control commands (for SYS_AUDIOCTL) + static constexpr int AUDIO_CTL_SET_VOLUME = 0; + static constexpr int AUDIO_CTL_GET_VOLUME = 1; + static constexpr int AUDIO_CTL_GET_POS = 2; + static constexpr int AUDIO_CTL_PAUSE = 3; + static constexpr int AUDIO_CTL_GET_OUTPUT = 4; // 0=HDA, 1=Bluetooth + static constexpr int AUDIO_CTL_SET_OUTPUT = 5; // Switch audio output + static constexpr int AUDIO_CTL_BT_STATUS = 6; // Get Bluetooth connection status + static constexpr int SOCK_TCP = 1; static constexpr int SOCK_UDP = 2; @@ -188,8 +210,8 @@ namespace Montauk { }; struct DiskInfo { - uint8_t port; // AHCI port index - uint8_t type; // 0=none, 1=SATA, 2=SATAPI + uint8_t port; // block device index + uint8_t type; // 0=none, 1=SATA, 2=SATAPI, 3=NVMe uint8_t sataGen; // SATA gen (1/2/3) uint8_t _pad0; uint64_t sectorCount; // Total user-addressable sectors @@ -249,6 +271,34 @@ namespace Montauk { char label[32]; // volume label }; + // Bluetooth scan result (returned by SYS_BTSCAN) + struct BtScanResult { + uint8_t bdAddr[6]; + uint8_t _pad[2]; + uint32_t classOfDevice; + int8_t rssi; + uint8_t _pad2[3]; + char name[64]; + }; + + // Bluetooth connected device info (returned by SYS_BTLIST) + struct BtDevInfo { + uint8_t bdAddr[6]; + uint8_t connected; + uint8_t encrypted; + uint16_t handle; + uint8_t linkType; + uint8_t _pad; + }; + + // Bluetooth adapter info (returned by SYS_BTINFO) + struct BtAdapterInfo { + uint8_t bdAddr[6]; + uint8_t initialized; + uint8_t scanning; + char name[64]; + }; + struct ProcInfo { int32_t pid; int32_t parentPid; diff --git a/programs/include/gui/desktop.hpp b/programs/include/gui/desktop.hpp index 9388647..4af2abf 100644 --- a/programs/include/gui/desktop.hpp +++ b/programs/include/gui/desktop.hpp @@ -111,6 +111,13 @@ struct DesktopState { int screen_w, screen_h; + // IDs of external windows we've sent a close event to but that haven't + // been destroyed yet by their owning process. Prevents the poll loop + // from re-creating them at the default position (visible flicker). + static constexpr int MAX_CLOSING = 8; + int closing_ext_ids[MAX_CLOSING]; + int closing_ext_count; + DesktopSettings settings; }; diff --git a/programs/include/libc/string.h b/programs/include/libc/string.h index 180247f..77542ee 100644 --- a/programs/include/libc/string.h +++ b/programs/include/libc/string.h @@ -13,6 +13,7 @@ void *memcpy(void *dest, const void *src, size_t n); void *memset(void *s, int c, size_t n); void *memmove(void *dest, const void *src, size_t n); int memcmp(const void *s1, const void *s2, size_t n); +void *memchr(const void *s, int c, size_t n); size_t strlen(const char *s); int strcmp(const char *s1, const char *s2); diff --git a/programs/include/montauk/syscall.h b/programs/include/montauk/syscall.h index bd61000..148a92b 100644 --- a/programs/include/montauk/syscall.h +++ b/programs/include/montauk/syscall.h @@ -336,6 +336,61 @@ namespace montauk { return (int)syscall1(Montauk::SYS_FSFORMAT, (uint64_t)params); } + // Audio + inline int audio_open(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { + return (int)syscall3(Montauk::SYS_AUDIOOPEN, (uint64_t)sampleRate, + (uint64_t)channels, (uint64_t)bitsPerSample); + } + inline void audio_close(int handle) { + syscall1(Montauk::SYS_AUDIOCLOSE, (uint64_t)handle); + } + inline int audio_write(int handle, const void* data, uint32_t size) { + return (int)syscall3(Montauk::SYS_AUDIOWRITE, (uint64_t)handle, + (uint64_t)data, (uint64_t)size); + } + inline int audio_ctl(int handle, int cmd, int value) { + return (int)syscall3(Montauk::SYS_AUDIOCTL, (uint64_t)handle, + (uint64_t)cmd, (uint64_t)value); + } + inline int audio_set_volume(int handle, int percent) { + return audio_ctl(handle, Montauk::AUDIO_CTL_SET_VOLUME, percent); + } + inline int audio_get_volume(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_GET_VOLUME, 0); + } + inline int audio_get_pos(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_GET_POS, 0); + } + inline int audio_pause(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_PAUSE, 1); + } + inline int audio_resume(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_PAUSE, 0); + } + inline int audio_get_output(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_GET_OUTPUT, 0); + } + inline int audio_bt_status(int handle) { + return audio_ctl(handle, Montauk::AUDIO_CTL_BT_STATUS, 0); + } + + // Bluetooth + inline int bt_scan(Montauk::BtScanResult* buf, int maxCount, uint32_t timeoutMs) { + return (int)syscall3(Montauk::SYS_BTSCAN, (uint64_t)buf, (uint64_t)maxCount, (uint64_t)timeoutMs); + } + inline int bt_connect(const uint8_t* bdAddr) { + return (int)syscall1(Montauk::SYS_BTCONNECT, (uint64_t)bdAddr); + } + inline int bt_disconnect(const uint8_t* bdAddr) { + return (int)syscall1(Montauk::SYS_BTDISCONNECT, (uint64_t)bdAddr); + } + inline int bt_list(Montauk::BtDevInfo* buf, int maxCount) { + return (int)syscall2(Montauk::SYS_BTLIST, (uint64_t)buf, (uint64_t)maxCount); + } + inline int bt_info(Montauk::BtAdapterInfo* buf) { + return (int)syscall1(Montauk::SYS_BTINFO, (uint64_t)buf); + } + // Kernel introspection inline void memstats(Montauk::MemStats* out) { syscall1(Montauk::SYS_MEMSTATS, (uint64_t)out); } diff --git a/programs/lib/libc/libc.c b/programs/lib/libc/libc.c index c57f1e8..9df0744 100644 --- a/programs/lib/libc/libc.c +++ b/programs/lib/libc/libc.c @@ -135,6 +135,16 @@ int memcmp(const void *s1, const void *s2, size_t n) { return 0; } +void *memchr(const void *s, int c, size_t n) { + const unsigned char *p = (const unsigned char *)s; + unsigned char uc = (unsigned char)c; + for (size_t i = 0; i < n; i++) { + if (p[i] == uc) + return (void *)(p + i); + } + return (void *)0; +} + size_t strlen(const char *s) { size_t len = 0; while (s[len]) len++; diff --git a/programs/lib/libc/obj/libc.o b/programs/lib/libc/obj/libc.o index b8aef845cee5c3e5c841deded606fcb891fd3d70..2747fc517e37f7c367a524ac394bb04bb60fea0a 100644 GIT binary patch literal 103296 zcmeFad0r3<0Cm(b4CVl@}4X**ZOpyL=kPFtW=YoR8KYN5!8Q2z0{m7%pe z18c4XAkcFSV}&BSLQUg*fQKz$)P5ei>N{!(PYyLDmNlD+W%YEh1F8ueF=fui9md zLpPnrbb(vYA$QLDzyTJOMvi#TU-fWrj zLzFHt^f1UWhkr(at@Sz(+8^X3(5PV*!P?V*)P8H0V+c}Ky$>v`TZCG1-a(~z8@05g z5)MqiCJ}|!e*m2|_1wQ5a6b>lPN)&NiNI^UFKS;N@@5+HN%U*I-lehzHA3$ueXs54 z&k}-T|CVj2Kv(TtC6RAKYY)~3TB|W6=J6e&#={~!D-=22_;x})=Kg__$iYzKJI4zf z-%SWTb=U|Osw!Py)@1G}iFnJ(!CwwS5w%s{B|DY)o!^+(Dg zdw1vUYkkdSr6h7Za;z+}lbGn&vIAg+Jk2kNFmBxnh;kUdxe4ueC)6J>=En!CAlZ>t?YtG!?_%7V&sc?3vIfR&&Tb6$ytj?qLp-3KhOAAFVJ&v~e zBa_y?5grZpeUrp%WnwS-h6WyJd`E<{#;qF_BB#vX748yqVhPp9t-B27*hTW~++7R- z|LO=>68S!qe=M;2AE<|#3$jY`zYjD%i%LnenL$DiZ+QSUMQEi6`2$OzWO73Iv(7sk_X?1g>sQ{+-7YD< z!!e0uc1V2Jn(X)o=0P}N+{oPeFLtkw$Isi@Fjfn9j?O0TVCp(p|}#* zlCWlH{h-!y3e!q)ePGMku+6g~)O@a3voj1wIuzKFHF8(@!-4xT0_=CL$lFQfEm=^* zVPgJt6fzzCt=ShIW}WG_I>1MeReBYG5;-EXTqy^a)u|+_^HK^w-G zi;W+OaK;{i`6zL-9sVUq-&1%3YXs)^Zk0J`5ZVU|;oMO3c_PqroK_OqPrNl;c0#NF zGVdVtmz-4?WB3T9Y#_xWF0{UlyVHMRsQDbH|2|@&VI}ux$p*F#Q$>r(M&lK)GyN{l z88P`v#J)nXzI%O%IKTzh-w+R5?OUhoMX!V+jyl`B@i7thK@%!@k*5_i~uRpB>1}LLAJRz{dG@4gD z35?Fmo?f$)vY6IgtT$)AcxMaz=}h*=88C;Fe;_l0KNe)c9~(w$;gO2Izz-HKD4>HJ?ZPd^cx#O60r7ci(C}T-f+s=jDF*nvAzm{(dWmaq7dDXNI<( zFlc9Sr4|a@emwB2oyjug^PHZc#`lhd8ov()uI@(3YUB6g0#_dd zozPX^H{cey`V50Bw6zLcJ<_QlBM)3X-Km&Vsc|YksvJP2a7Th9qoomubX3-qf=`V% zHdspQ2enK&`CegtS>!ovjRI>10jDJLR7q1_NmC)#;eXbCK%1vf^MoMwRDspQFj&d6 zqOdr0eo1p-+N8)LgypBRos7M3E^>DE*TNl?R8MY7{+gKJrM#r6#8(=T0P5?IX@>;- zxj63|rE~r6$xVHk(&HpxSyM-%T9W@}Nnpa8$m9ZRwxce0bA6SG5sP1C$PP)q8>wGp zS4q>*k|v1NM!v<6KNNYDa{WUk$G*xt=*0Q52;%uJQ5HF+B!X;nvahsx*eS@QYCoim zrzG%~rzbTh>n+`QXHleSTaz%*;~ESZio6xDXzaDL1$>sb!K9m(RwTR8^}ek6@e={C zcGZa!W66SLKqxYXQ8e22zJwx57tp?<`5N#yC0$R1|8 zEON}@n0(#wOSnW}%^}pK)p8V;)uKD!UH`lTJ>vYc1#SAGlHbh6et#q`9E8RNv$Oit1IuyGVE=bJ2i-FrjT4k1#zbzrc9Z z&hci(bWS_I4^Pe8sVI%s0Xk3N6w<(YQPw<9FN=J{gOo&e(-}x=S>)YN1eh3DxzMe1} ze;Bvy7on?uXh0=!H7)q4$cRQ6H&t|M`GZq|0RmT(U8%*VY0EBnj3)V38PR^A^0F?W z$SW;CiJlidHV&1o#A7IO&XG`4@-rx+oax-pIgX{GNYfq*0*p2LHOG%1qY<`}Zd><| zo*rvJaVz-ITDNeW7~M*~(|V_@qiri#)LL(nb(FV~FSK5bkLWcM+qxfU4VY09`PX#T zbVzDC1)9$L##J<83u5J*-E6;^v`|F8#3>LCzD@+zBw%cGr$eAM87P-EEjeD&Tnq)j z4Fw;Eg8zg8Q5qMx`gtmiX~1XTYC0DRjgcz;QkGf64jdc@uKphIZKO-=LPCKF`+-lI zppQ`C>|LSe=@{nSBeYkaiTI1C#X>mlQd*Xe-PB>}>7l?`f9FQmr?3L};6S_x%#nKE z8YDhqL~9=?iOlYMd{X4Sc;q!e1S8|{(fVR+zJO#Xa!itB%cH4C(|FwCyf<36%KFv@ z>UY!ntt5Tx_0j%Y$;VoL8I4n0HE5ybV$^|;D{}nUtAWt7yJZcmV!w9m)sPBuoA@4j z?8Iv)PGIt#2t|H?ega$Ug5a`Qfi3$B1_#!V-xr(~zNqnVZr+K;@BD#lehWz6zS#QH zoEZu1*crlcTXWeIBu811{XholvJWDAkG%@(7w^T9LzI8yE>8t^?60+{OY4T_%f4J7xPZEHYx2-LM8B)JyE2<)r%lN-aZTzihLV+r6h7B zutPLYIo9x<8MvBGsiNZ-1OsbsCaEA@5Ok327MvDXW1$FM*U~I|GLhPI18cGYT!Xds zOx&?>R=(8QZGGd%x%tPh$j8EXAapZtY{V{%g*`^U=B4|`HT>XT_Ga_a%_wwUUfKBZ zdHKgL|GN^x)=xr=GD;%wf&H)x4kWQ@*)KO64Hy>rdm;8WPr1xrQwG7}VIu)+JtY04 zc`1@_jCEJz$0J(i5pqwa(|95+uy!o!5M=ELf^V5BkZ?A9R~}e9gz7sAbqu7pbU`I% zPj{@A&)^f}58y{0BobFGuQC+=O7O@oNwB^H5qlDI5LN-jy+9)DCou=LmTjoz9c(c$ z3{~+@z2eUcX6Efgw3!~N#Hj}Ii;8(mYQqBt1}iHTFRq#o!_4HEMJSIG8Ai(6`8X^j z%UdzbiQNNg>K88_sO7HEkhXroDQrpPB|f62$n{hPXxRIfi+_*gC=t#8EiUle(p^1iiiLZVP}m5e=w z7g7SsaRU8L4Xm99%@#IwYW!|U;F>PLj2vp*lav)3~G&^m7@|2T+<1aaX92VA#+q(Cz{XQ zU!3=81BHSHtQPAVKg*raJmY>C9wP;Id{fjs=v;^kG~g_Ihq(Z;oWR;E!4ENp8YJio z(yjzIV{}!5rYzvWId|*8T^v|DnR+_=BZmUyn1)`CeEq`Z4$sRM#PM6wv>ZwPxxSLf zdA^o+*n1lwHsTy}0xg`a&MJyk`r>GOoZoH;e{Gs_Q9N6p@c?Yr8 zhtfxamdYRW-<9FO&HHet z+4$J&)x!Lj1K0mA3Pq96$|CO!JX9t=IR$TJO`?Yl@Tj(nWAZyPOJE%!ic z%y;uyC*XT)2Yrv|yR&t=JAcSiWH{;}*IWM?t9#~8Wh}oh6CdqK zh<96e+O|B1Q77SPT1oR#%)I5kmNKwa(sT}{;#4^m@5QJdKn(yqpIUx}QO2*E(O19A8`de1{%e!HL%LEmSrQYKR<-@gsBEreaL6D|Vhwv#bnH z_Atw4O>R!^UpDZ=!u+rC$S-ichSB2uzoa=ilYH1J++(K0iFXOO`l^04AaBx1c5XZrlu#3V$_4<>H&lxbuDHqv1 zOak_Q??|(pkqXb}VKAH`ENw(iO5Y=2QT}J4z&SDb20-0K`G>EVTp0N~LP+tz_f(c+ zbH|MznExZ7x;>(}Ir;SB{FciP7dF@9@#VgOhoH=oz~rMcmK+S+umGxS`OUYn^-gsT zo~j;~Z}|j1;6Vw+1DeSf)zH~;GaxwDhV!0<(Tdl88o0g>MnsM_h!)x{8IvMUw-fvlM)@j0;iY0KY_(G%BCTk?;)TMhEJ|9AaA4R|y_`+sEDQ$o$fcosM5<52UYtyt}` zf>G2dK2jE$bo|-kt=Z@V@Lry;`*z1jc>B+M8*wT0>>l_f_NOPMMrlR6)ZFtVoO|J# z&P)+=k@~$m_{<@_)z6>TAZEDN~meh@u%V-a2AxCfo`IOpEr-04_?FDw~+(SUT3uIM70;F>i^(=1Ie^h^SD zr;qloEFT?=X<0rba+=ngKJ-JKPQ`7kX)US=yDeF1JXDsT!!9jLkD(hm*>wbxqYdD) zE{U3aW1_~0&O~%TGd>t#4(NIh%H!})dj$XbkUP@9frN)7gFdn?jkZe+;RCX0$>b6> z`l6*YZM;v?UXZ;BEferh69rZgZhnI*bmh@UT9%jqU7kkeP9l}Kl9g;(@i}N+#YHY! zH<1}b>n^gW!KbAYnLf9m`zjQe_XKUg5|q0V&EBFD!DFNMp>FrYP#NnK(fL*n7DM}s zY$6`(%g_NLxgB}3N9#dd(}vEguht6mBr0n|moBN(#^}i-QT7eJq+*43rmUoC>HUD< z*m3}s`_RXD{3{}D;tZb0>SUzk0T$x`ZUqHKRXLNYxWvZO;2p_S%rk;ib}t7uv>jLh ziQ!%j-eeUo^=7uLL;)MPh8mWI(Z!#Y7V(HuVejnl1Vb{^{!yNzQ{{BSn)su%B$<;? zNtJAA!whS~e^14owr3dDEB`$emzUv&)g8J&xpt&Pr#q}`?^Co$r)PA`#{h$8ird=F zDTs}K0MV36ZGe%kn#`}NSKQb%E~Xl{10bf+*p96O zUKw0Y#KuQS28tp9BZH%eBOS{XF#5KUE6tVbZYfs>uUzqbNmgd){u7d~8O+z+E?)^= zzJ^D2OJXr!2}Y*M7YB|6BS=Bcy_nYo!xMDa2EI666Mq)3U>RJJ5~q5Zt=ByhW~N^< zAU3{>XXVUnz2316v-NtfHq6%Rr>lxhfxp!2%=>OHj5|Cbd4KvR zd4KxpIlOomnA0 z?Z@0m>h)d(8KX{nTkot+Qr7CbUahjC4nP1X>I1!t8&+w=lQ06G;p+o^*pD&7gtLf` zMcPMtmP)vt$?=gs$U`hsPL6zMEN!S$r~|?7%e5BC=}&Oj^5Y!(?>faL@6OS8_3mPe z0%e~wpHv{|!IB*($ttza^bQoq+^ZeQK8LM;WZGx^xV5j)Q}SW?AEA)imcW;KO3jaV z@{Ahe-%TvO(Nq5N;{-f{O6iaNxS(44Zs3g~+=H)sy(3R(4a z3tzHSE7KDzz=N@2>nb?!B;Awt@Fbe7r?;6{v)aKsza7|xUKo4Rnb15Y;C#IU@!?(` zCCeyMrKe<)Hwo|J!9*G*kp(|VWZ{nx*-J#0YBhT5NjZ_$TcZ2hnV!v) z>@mQldD0E7vun~5)SS}Y$cT-;4$dgO^)94y=!litLl3Cao=p2NxQm3$f7OX8K;_(9le4*7d$Ar~7j%5d2DL3kEe9`Xlq zN*gwQ5JO3e?uDiKgz!WVPJ%Q&Q5}zSR?$whD8}#{hSIzlu;xD&bcb;7hMp|!UT%%< zEZP^svk}M1?Q0aXc99yh1x7(nx3UoZ^akb(;9xD;Nt)?Hg+){^vq&H5^ad zFA^wYbFa%JFF-#d3wl@@uGRL7jw%M5c)##$h>V5vYqfufZ0h4)#*)SJwU?c9ItR!$ zd({i;zG(gm?KP=cZhzC;&aihylF9?aJ`j9N=8l(I)59%7S}wjO0ylR=-5LQ7I|e_mxD?W_^I_Tj)Q4hX4KV4 zv+Qbr5$UR#*{vS-x)rmy)$1Y#xzOX@$r(+Yvn5a3N({oCpA=o_KsS1|<2a9tz%xMi zc!IyJ?4oJ-U(;@uO6X1#_jjTjO|=uL{K6tyFrv=PPr2k)2D@JT*qHGX>&qxwAWd5v)lA&_%=0o`Im?QchMMeQa zvPT;8r2e6K+)V1?Pa^#<@f805bhCCG<` zu#AUXu2;&-W&C6Kl@dO<`yxEw%V5#RG|L!UP_INJ~3DX6=+7WQi+je!=Qs9dK3mhc7}qbWvg2Xrpg~~ za5coBxb1~@3Lzv2@`2_cW?LmH@&N%4WP4Q+v(YXf!$!7O6;glJ$vPdAovgS{%!A21 zkXpcl>0oe+!7U^iS`4*C=N8fvT6DEVaLZiTBAgcX-)}tI@P&|@F0rv!Z%s?8)4t(7 zxCCliPkP{SjQ1GUeC@b=K<_k$)RR+bA91Y1#~AV_PftDBGkveZ$@rtIJ?$q5CifC` zD9iMjCHUAQL50JP*tj2ra;WyPM}{E`%H^Yl8M#zB%mS6D{A2IJmpGiU5o9vy{MUQr zTRNDH0qH(T$t*xQvzR_6I?pvNscBMPe#`F6?;T76j-PJRUj`Os) z21!;s)@W}HrDs#}q#>~F#VlQ-6$#$&NUg%9! z#f*uBDPHuPiAajicqqE(BVyw;N%0wv!9nqvUXr5hz1`EBwj*?jjUS^od&hQu+$m;u ztgEfmw)ZFY#0=eXXC&I0si7+L~2pJPo@>b8+ATSYb^@uA+zR^ddpQn-Qv>Vx_ zIa>Zk3eMwP>p54#EaOXToZMg2I@W2|cA)~?u2Q?!aagGG+SAlYZY|ZCy37EYds+3B z+I1P6&pIHR?ofHNCpzF@WIDQ)rC)|t=BB|T^#rH4*!UiElczOgJM7dA)fxtHnI=Jl zGYPB;?J6%P^R(3-z$AyHHqI%4sm3^_n3KgGy?hv2_gQAe*IAa+!V$K3Eawc@*&)z;ua?)DMgCd6~y0gg=uiwez}C zTyrld;k!)%RGKOhC9wuLWIi zTi^vtdB%I>S>X&JHquv^IF^j}N<@)u{3&sU9nX{26%wItf>)+Gt;l0ovS%qv5baYp zQk&SZjXld^vNeDq$TrCaXNm#+k+4kW*(Wkku51P-@EFq1Ti70l&9atm!{_0^J+&P*~=u_19AnL&Jy zq&;HTv3k&S(CFe(D!z~GsS`#w*CbMQK)QP%NXV0*WwKCDw}Dy2Y;}io>b-J+sM@D@ z7AZSvwA0>4h>{Evxi2~@M!=Z;q~56Ul$e68{9YDmIM_>2OMmt`U=KJ&Bh3!%h1(FA z{L~btXN@MnNB~Zg0LKvtGRQ;K?%!GEb8wPCM0J^x29T~j#0$jOxvmBsq4F|>k(!hw z9k-W{_#BGSW7B{}KRu*Lw-^Qr+8H_M;&e#q-`SCFI7!zH$RlFS>|aj%NS77I2l8>E z0Z*a&4D8&&SmorIgcCt+=Z+tO=!-?D?@%$v96Hm)7i~AUhcUzK8xph45Mj)a7#<2! z&A!w$wVkGEvT538Y~G=KdG*zYdFzRW+q4-+8&NzT;i+{oN3EvDHK+b;GP8d+8OfI- zZ6@@U*#+9@(oO11ktwF3xs=q9F{-3GQH)PW?!6(ePd znNhW9V!FzIfJRlc6wpVtGu){d?$oJbsyTI$9IlOImY8B%>0l_mqnM(aD>2SoiK?E7 zjG~@G3{zNik`cP#nN62M@W2@CNl~G6j2TDkbkI6|c#M`Z%;})SO-#hY!ResSP3(?s z=*NvQ7GsS4%FlzCU-{Q1qP{Ni z?n0?eEyveXy>^FG-N(W>W5*hGt2UcsCmD5OvpK08pz;L_T5zX8<$3Wn-M$v9b;ek& z-=?E}kNyeWKGDAd82!J}uQyP?-q>O=+7@F63i@Vqhq0F$_8RZXhIfsRss1lxgNfS) z^B$A&?=c^x`W9=i#r3_`KP=SWwhq(nuyqt*40F_Kv{7fpPB(8%yfYECI};yG8~Ov&a?;TK}A zW|%lt_|Gq|rT^3Zp5BUFnl|gt) zpk>kwr847Aq15ga+l5bJ0&zQ(=8W{bc)?WgUNDcT)noD14VHqu!Fs@o5q$uPcZhDW zKD8CRPwh1cN_9=b9}^PmwJ6(vOt?ECfoovx?u7diVl?kdcu=W5m~f|0p>wD2AXjHd z=@0r|SF5l4-trM4+4z?4Bem;Cz6O7+vB4kl6WwVNH{x&hlg4WW)J=MA_TQm|iHV>_RU&qE;=06SrygEK23>~N?Dhx_Fk8|AvLP`4XHP!(zxa9bT_3w zluA8OBd#AxJvBAV%lrz#{>L&Le?0xlz)2Gw`K{&X<9I z#pwJiusSV9XLZ`9G=~ClRd*xd#2y2w%?k5 zZ#oSmY2BOtPzUPl(0ZuDcCI>lpzsuH)tm*vnf}T|K2^`%Y>p^IB6gLyQBdVKVz)r` zA$^Zd)f&TdF zl!SFZDcuZfJ;7L%83lof`O;~q^fN@cd9CqV1GnEAzccjZ*yw|xf_(gE5HV3$_ua54zrJKV-KZ zy%R>i-d>%+qkD!Mf<8mev{z`CYu%7Tx5LYT|B9DU;sW!31Qy5(K$0#+#0)#E`A&4% zY-aSsDteoEP&k12@%w@8+eD-8Roc*#C~fG^V^oi+KSfPE;|w;6b#9Ithm#RW+0O+64O+s ztGk~@bv-`TIhAR7dUj-b=C^iZLc5eZ8oqv;xtR)ZuBcD}iLXK!CC-j1a)1}?S8UlK zz8&$68XXUaYt4sxHW+_GFfex-Z_({7qs4HxCF~fgU*v;U{sse?;ndBt+f?&UgnxnA zV%%dhTz0~zVv2lBA=wWZC=WLc{kQ`*$woA$IeP^rp>)qBOXlWNe9QkxfYwLi6Ja3n zFh3FN@Rp#tPG6^UeVx9+K%Iu5K0A%)4dXJTwpU5oIECGREk8k*O)S+C>9!Hl%7FqDecD_<4(d==}Nf$YnCQ+$uQI!$^S zNY$t}@9vb|6VY$tIo#`*2Wh zZ@R0qL^7g~@8*q#-3;Gg3ODqhNAJKh&zXgfD{mF}+$`?GtzAT-093vKneT+ol$Gh( z-M0ZY?ClG6Qx6g{;|>xs)p#DamL>Y@-Ln%jG2m?Q6z!N`)TDizb7bXdnjxFo9u823 z!@ZodDvug)#$sBX;HjN{PxCr@7DisBke>@H%*CHDDK{Szhsie%i>vSwsCkwCdpMYM zLHCoUbG%z1GMcF&j1`(9qsNqTRG8LG5P1*ZcroA8ztLsmH~Kn5AGg_DXRLFA%{t>I zLm#=>yve|LGh-vsSnhc7V-ih$CPbTEgO8d^jas|$Vw}_88G5kpU zK-C|_trV8zq@(T}_>}%5ssG3=0&_jq6%3va24CQa@PhufPI2&U9ekY6EGYdmahf$a zkE1vFyV8>EtJQ84oX|Xhj3-t*Al{1Aq)5zF8w;jDn<=HReCcGKn3HcNH@I2+nZn+m z={5tmClD|Ym!H7aSw;oE8OeDR=*AfHQRbO=0?W3M03RR|FEmSHv51 z-ZFEu`G)w8;NOWg$PmpnSg0g?oqiYj=Uw{44&Y&ZI{~)qy9jZY{vB%ZqoJ-0h=4W2 zyi2@HM_(_?0dYL{1`fdhSS?2aU>$ZN0NkxVL@UBWazMZzrXi-E%$2V+>X&XduY6b- zm*f7h*dlDmGY%`%7L3S^?};yD;rIH>IzgiBDoj2|+qs9u0lK{FyJadj+C#!QVYMWno2D#3;*jmJ` zJN@uxP=`M5TR{zYEhsBfsSbAXjD7?A-7aSQG@{3TUR9n{iT7Y@4DS9f>yMaGSm=Zi zJ9Txe!i*}7yb9&=%!qI50H^IhP3}pvN|gKc!hGq~#|QTWQV-V)73%32Iz0<>Agha# z*(wN)G({W5HNvpGVV$^mFT=;L#aEJqXNYcSnX@#0MfS4M z8^ZQRIGTfSbgizh^qJS{&r_L+au@74V*pPSJ4BFE!K*B`Yr( zAM*AK8<=R?4)6`uZOXPqq0k@mv;S$vQX~$i4NI=m?mqQ${OfwRMv1$}y{9r;`-mSS zzAa5jtj&IT7%*=b!m`O}AqlRG*blA!^7PAmz|k-DnZu?6u*Cx5^BrR;$my6<-O8rK)@yR2_@HaT;3)2H(a(0B*PIe}SeI}MP4mAbJVm$7zK1ZK zS(}Qrf@paqp#=zVr06UwK@~s32xJA-BW%tnOxsao6k!$2{%GreItA3zhm)wo$9pI7 zx$Wt)ld4PVVU90upZpNC4H-@3N1sXL&pgL*B!vZ1;C6d29k0gOv%5L2pzmh$MQlm9 z1gm<)>`oglYQ?>+VaS`di5DnsctQM~cFK0ldoszJ)VdrX&cEb|io)wi*x&#L#O+%B zRv3_tJg~1t6=8QR< zdYE%))n3v_Sv_*+MsXV@-?xeT1m5vA?-LI=4Rrp4hDXFUZrFw=12T1iHR5-Bv$3?! zsm4b&@`kCPxt&>#dc4Ak`YjJLt2+ejUWzW$SK{Li@i4l}kknR#rNEtjDLyV;K|Zr` zGim1zaX;PeM^6B0KxgJEL-wJ7KIjbkJYyPdIj2=od2y4#jmJ@t5nfCq*{JW;eoIfI zzHOgA=+kEqx9Dbc3euMvamDs**mrsw<8fn{PFHpAO1{kccd(Su)rMuW{wb#^t~n3f zI~6?M$5(>qHuEusm`a&}Q=JPc zou>jyzX4#TxXj6)6?g{>He^KGarm$Yp6T7hDjmx(Ps*l~7hgwH<~lq9MCC?0Sfe9^ zyU496MUD%{$>5_aXAA>e%_&3;km%aO(UAhrfj)D}zzju2kEZzvTyP}%6_m9@FclqB zbEDe^g(N-4F#Tg7{`iRyKR(=|-@ZPzb2%HS!Z&yL*=9nS~R8+$LSu%ya z(<>C3lln+`mE;|fPR%mEX-_^wiGQ*hTM~@=6}^s9FBqR^9e*gjn2YXscGUgXv>Lrex!eWjQTi^|6wMGgz2i&i(H*Q1@=Q4b z@%4$rD0h>4b)~}s(4~Dn2*%CN1t7vLGszaJFe$XyQ4{XmMwU`&rcwLgGcfPZ$mb*F ze8-DbBtDpU4d>L95O^~Pugy0ZFgMJz zC!&9x)iFs3&hyV}b>4Sp^o75iJ&}Z#6I28 zSzzUSguTqsWlIzrTr-Kov&9+bWRixkPI4{p{LgJUUG zG{~^~x<6OI6?~lP%uKhw!Yw#=_hl{@{Iy$f?i1qbed6l9;_B{OiY~a=EjagJT-|+< z(*?UPn7Z|Px8U6K;_78_^@ro?m2vfoxVroHkqdU;%W>;Lx8U46#?|BTf2uwxu7A(C zdWX1rJiT~+sJm-Qb zV5?W0JIeR*6ojBsrebtfJ8<`Bpk1`#Zo#=1#nl(Z)$y{R=j!IE;cs2U)!ph#P=x!A zLR2P1Up$rEq2m$IbGlrA2p@smG{%`lGtAfVbpSlRzt2(|)6C3lvujTkWSYHm@lAE>+*P9Oj*~mm z961wTqTXeg+s&OG{A}~e7X)_s?}(qEVwed@=8(xMR$qi-wQiW5yPN0z)#>o=F)l<1S;j`FDg|~KQkv} zPN8!p=Q6ckCf=G<>bjYbqSjN)tX^t;q4^=@Id|fe3uj#4*yi;Kn-a_|INvhw#Oaob z8Z`fjQ!N#ej;~<+$ugg@57=ICJY;}5<~xD!%s*$C*IIwH%$KbXE%Q6;THE}iz0C%J zLmmPT3Y@+*7_j%NrrBg|w9HqnBbNESb)5$-!AzZNK5xC^aj|rGRm1~c!D+uV<*3o6b~hTv((&2bYdA<;}3Z06;ezgVc^|H52) z8J=u^CR|{9Ks7#bZOYEz!*AWOnLeE?sG^TJND^;1tvhFE?)y z8$Iyucx1khCdvmMa2K=JS>{bRT~evzCsGW2JtPifn~9k!D8Y4bKX?Ni7W#M~O zuj8o_UY#&+v2U@>jrL==LDTHqU+skF z?AwhO4AB0AZQg5dvCUiks}s%F62J7&&gL%xsvQ%|#1ecR@gBblw9G`0f_!EY9wCW? z9{n5UzxAVrc@&4^csJxm(|pl<&N83CH|fpm{Wp2&C7V$2gW^8jJYpU9Xi%8GbTf0V zIyePvwLaB+Mm!_Tz3>V1PUBh=Pnu3x<|BAEq+(^7-|GMCnE=pQ2eV_Tie#F>9JOwn zlgrh*g@XcbJtf6V%}_y_X(yYV`pVy(qENP7BsdXGyT^_eS42y%#oE7)6(>a7`$S8< zqNOR(QgO5tjFxJnr7_V`eYBJpEtN$}561?si~=j7r9iZl94*C$>llT_TH7lPisJQ* zmO4aBu^zF}Vx>>yA9Er_E{t}$QT{O|0_H}6-^GBjK1tDL{E#c*)Af5JTC}327%Vn; zOw^eDUXF1-JleS^T3Qq>4UCq$MN2ca9`wTr*guX3ANa2N_o*p#qb)4{q0m$AFHqM} z4#Z_*gvaN7@%u|*e_m62g(*&VI4OD)Sj6P0XXAF}**V}_M#7g*VL$g*zF zuzdLOs-7FH&IML?O*cgekmoY6^DL{Qg!t_XtxTZ#?TLIBWhff8r4DdGhJ`;=Lv++G z=W}b9F;->?u*X;(9Rk+8I8966%j$w|3H4UT5E0L`l2IK9Y{TeN zY9$+U3QA?wHu^yML8Sd@EE7k80TUPuG+?_ z!eXk-wp|YEceD!%a;%QVRA0erqMl3C&uL5jac~b}wpfi$#DlQQ)&pi-d2G`8u$gSZTIT_G23i&oE~H#E^~+f~22c#B30Z+2BGZoqAD88 zNtS+40u&)R`?F;|;idJ8N(x%yL5u-zhtDnl4iy8VOE*d;t+ATf4$?nhQ`Q&{3~!de}MzH^Z*BPUE|S`A|oN6{&0AdW&_8uI(t9jaBYg zWz$J8Wpj)LkImv(h}vvrsr26As?A!Z_hwkb5$JtmtOfE_+9roX-)bulPyZ3 z&S0+dGI=tr98%1-k_SiW_fYBM2?x=)lBtWZISC*Wi*hRyH{D*(Yo89wGYVY9dYm*O zc|BF7bqmvyUge?-S>>f3lN%%rQi-G`V>j#~7g{%%px^+zoV}b#Wty+lvQpeBP*MQb+Z%^4pW5fQjPj) zRM+uMFp6P{f$*aMl(?McghR}u97nqQSc5RLoElg(Nw2I4;(cOFiyn8nj|pZtDF^&9 z%}PE~*4S67VOHNnxR_wInRbb6Eam*ufQYojVb*YLwa=$P?egf*JbnRaG%^WkU z_J7$f6!VP9q+998U{TaDDuXDcxgaQ=!K0V|k$UkNb5sX|o{V6D)C5upB3kVbbo7Hv ziig@upDb|D(WlW{w5w5$rBRJKdBN<}?S#R{U7pVK@HE^uPH}lUzg?arM^ewWF_7~G z#Ux=e$p)l&KV_1{Sup$B1@W;ScY53@=xSABPN6JRaP(XPP3O@8ztxMr;_^{lfR+?RJr4ORy(^tlw z$Z`Gl0+q5gnP()mh=o?Vv66Do(-oMEj-CK^NOByIItvu6x5nD~Fbs(aGj5_)go#!R zdI308CRpG&JK7pfTq923FH86xF;h&m0>8(2=kroTJdi4BS(4?=puwRbaIS^@hh(|ke_ z=|=sTN?;-Uo}f@*bzA5JdaF0HU1<%H?B__^j8qxIm(_?NN{L5~wtQodbKkcrC&!sB z3)ua1c#ad6nNYQDyo}X|**5-X6|!Us6I^X<6Y?6_VUtT~`)Is?ZiJg`pKZ@UfGw3A zbyPWOz^1qpvOyd2PPwlg1iYq22>l6R7+wt_C3`3UMhI4#Lr1Rcxz>Plxdf-O?DL`| zXvZfqtdaPol{S>%&4f1yEC<(H7gSq|%G((a-#m{}YnLH>7yQQ&u9gTa^YHdWx<(>c z>7YZVVxI9~b0EoqK+|aiugGEzp_N<3ozQMvhBemFnj>;CsJ0Oqub&bg_MX;-zD?~Q zL?pr~#74DsdbL&9o=&cnSZt&-+LH)xsM0tP+3Hxpi4>N!3G3S#@VXzv?5esBz8g%= z+!b@m2UoW2H|T5y?nV<8VavE>Rn*%p>{-d{WA}Yd-Fr`l=Jr}v=V&8k5o7~jHNdv< z4qnPa*hw`WW!iE;3nwLMSm=>MrCqRzJU`JF&al$(T>yI3)JjJBm_{c*yr?7hV^)k4S8gMgXvH>KFu_>j)mD$$ z0F+3oetQj($+R*NxIxJ|CIVeze!WBJhbz|;Aqq3b;V2>7xDeYtI*lSOV>oxxf%}R=e`zq(P2zKgGyy#EziNAFUYm&K761m04zW z84bvo3|A7_lUo1uAa5V0r1;g03Y8%xdAPz^jIpaKrKWY!c z_YwXO!k39mcQqj3i&tbUpYeM~C|@I*Gw7URe z(pM8Vx2-BA6(R&!o!B4l=O`i9N=%wpDEW30OXGK|zK{t)mdfD+GGxg}bbh8>iG|T& zlhw_r4O-d8N{U^R(a;Se1l&OO`IS;I9@PzG3!*VflKas>wn3^9iM+3zRoetz`McN! zdma6LbS0jJGqxNu*6E18SFr$n}ObR*fhqgSiII^XO!m1AjIomPZxSP`(pEVYe$Nj0M|97tlrWZBoyd38aI zY2=bJ54DixR$10WdsZW{zd9!;Oj!60HI)GErs=ec?iBb17Kzn4+sf~=vPhA(u@Eqt z2=YOIv9J^<*#$wjTJGT8FVkyz0EP9cV0C#x`F;3LPLmF*(msRIGI4^0;Z6(76*xm} z{~YZ&4#Q$@4RTU~&T9KnR(b~VBx61F+1dCEXL52DEOguq2ByXWTJw=kksljJ;c@t; zxoO(Q4d~_rfVL=&SRE)ugffjLz|n(7_H#2+9V(N}>q#NBGW>xx))k${SSj=*0+vm) z`k)g=X}S`7RY`(78((7-%bxA*v!KllIo1Wnx+^zXgG#LnaN-+TY3SX0}7mP)4{EzHkPP;0X8}@8+>wv zn8inn=uVMAp0&e{`&`Q{xG(B;Y(QtzPp3){n>~tB`U|k8IJQ1PHgpF4@h-4C94ql+5nV}R?^wd^BCJ$`99pfqjaLPiAbsAA6Joo zq}W-~InI*KQ7tjpm;Pw=mo-*IIUi^0&`~j|yR${-cRDR2L{&%Dn2N~4k3Znv!cniC?ctx2_hc^QFacOEk(02ihMX3n*e`39#LWyD6vxJTB(E4=F8&fP6x-x+=tPP zb^!|*I?6tq??4;E1>VTB&E-TrPRR>w>oZmsSmNfra42K#D(o<5>e16UT0zizGv3?9 zv*I*k1uf&#WIJ$EH_2iuP-#Ez-u-x8cas&gjcVV0_WNk+#e3hLNE&BZgKz{`3fM#| zn8sN#j@bP6I^>(QZqsSzW+;rvr4ia@@)z83WCc|qo6p2fhZyx^V?cbe^V$#yUWiAV ztOI~gCtZ}vGYd-9CnFwY7?nuBdr^yRbU_ghX@L?>E@lSTmo$5>HjZ*+ckV{_`0&h<^>9b(@C;^Tn!J_jHex!yRavXAHGQcuWC;Iqb|E2|L<(B&r7 zUB56skzM4{!1#NuG~DSZKE+rW)m%&+(ybXai^$z5)*rH~vHw7Vei1n@?LN|tvS4W` zoF5f<(II3>?1=X}{=5M$`>->O<=FP`owFtx$UTn$>d~1azVSRJWQwzGPO-maY81nC zdo6#$;fS*l@)1lS%0@MN2%ZCwK(9wNBoEmP<4hG$5ddd=$x}a%V}F6qUZsHYV() z-7uXaaA3BLOADgQ5iGn0(nl|>S;iVq4baoSzC&Y^I(rAoM+rNhF2AaoC>|tFF^Cz=!NGPATM=S zfb`s0fv7{LEu2#O$haJRjSG>A!w$XhFvZ4$0|kfA5IA0>NgUV;IJNEINRREq;-UxJ zNIuy6o3@FD!j=^;x_nX24RcId&RKE~=|9D!G68#f@VrNim7oEF-*7Dyzv zM78vG1nLyyGu81nI4(U4G)I6Y{P_m>Gm|GlC_6|o`r~;k?X6eujd3z4o|EQP#K{-h z&*Nk-$Tsm-+gTIa;p7YWn8L{>@Q{o{0ttgM+OTi}2{3msu7X0cOMaG4@XZb5-#?*~ z2Cq(dj)zG-q|o8+EH^IZ2B?e;n=dE-Si*B>K8Kh$)zR9S`hUP^>eMF!d+EcO61ts9 zHJiR(gMu)M-3gOzWC%E{gyPRwX8SUH9q`Q^nF}D(fteAI|3=5y%pQ zGYrA7b4)kR!`g>seg|yj&vw#=Dx=_Si${jPE;M)iUgInQETEUv@;&8kSF!Jp0){@NihWu!Ay= zuM8sCt)pk{nYQr_5-wS0i$(*blUMLr!qVDBHQ{QlE?iqvSy5M2 zdC5{L^KH>(Rdd5yJ=%v4Lt(+vxl5|6>+q+Qmey7+)0S1tUA(9+EPF0psV%BoF@Mp5 zMGl~X@1UgB(I2i{zNB`3omRPI>B{M#s?}7j2y2U%)GP?{2S(897A>f$njfrOSW&Ct zuiaKItko7*%&U`Fj1aC3FIlY3ojb37(cRMo*`wF|1|!xr!@_KNvS>gNG{QC;2A zic08z?t+^7%Ba;BIAh^&%YkgweB!m1yk>qCB&lBp{ua!2)e9mOi zJe*<}Rw4U$sK32rpR_Gc3DT4LJ$MuiFsaUY6QiEPdh>FEzuuI@PODeV6ss(fFDwb7AlUJ>P!-QZn{F@I0kDobVV)2Z* z(@V}To~~6ctgpEkpYY;p_$J$8QQe{?HB#@3YM9}upcV6AWo=$n^^)2uo&}iCY@l$( z#Z@tVuomjHYIwNoPjy^?-NTT@)egI{oU)iw^N1n^0LY3dg&73wddzDn6&f!ABig)` zVc3)6#z=&X>S)-&NSi{R%3G>)3&z?LpI3X`Dm^UnEXijc$@Fl~C;iCek9J6BN z@WEWGSu%KJ?&%{(paI28E?FT9uvN4$RF-P1<`2GXNe#g(FM(ASkEtV-2L~?*4n7As zb>aDA;6@m4a5V`J0cz$iLL3@g4a)5FgO}DW30GBym(&6%A(acODlZ;WvqWM|DVadM zDNw)+gAp_7UI*Cxif~1=4!>MJ8Lmn9rEo1LT+IzvEI>e+SD90(4OzJ4lByx|D{2;1 zEgrnE;u1`OA@i%24Vkp0CR|Z}@j26nfEUc2OX`N82mSlM)hE1U$zu4}qM9La&x*x7 z+=9x=Auz$*;ll=3Ev_CS4K!p?P37Wxj)^RSBMJ06zp6S1p(YGVC@mHB9Oc)g$0J9HtY))UY*E0DN-?qODzh=53O>|98@ZnV@{`-yY`-|>-zr%p* zzSGI?{uZXDU9E>w?4s*&BsJ+e{Jj(GC=z#%|J(Q%#=ls&2c#y_JDq9x=e{fIz9USq z%khsc_dQ`6Z#_dajo%g4-=P7{O0i3b7SnmqX?NcpCOS_$F!%jog1zd%dO3H3ed3g- zC@|*@1Fcwrx$m(C6&QYwkuUe1Vxm{zz})wW2{y%nx$hPeY`z0?-!CTE3I|q(GF=3_ z&Vjk_858U_2j;$OOt42CSOv;-5p1smbKlt|7+!zm%YDz6U`HHSP&RR2(|NbTegBy7 zvK^S(cThX9aqYlnIIuy=7z-Vk`%W^Aak&F?-%BRg`gZ8u;lNbcw#LCi==(PZ#_uV! zZ%#^*FSNcbNox}o@Xc3#X*YTumeCV+j*Io{nn*5;X0$y)Jqa~ z7ypgBmnO0PiX2?mZ~CHtxdY2ZS>yg^r`Y4KGm09`3o${sJMrA$;PJcB?l{HG#&!7d zKK*Z!IYvG!X`WImNh()rm}R89zd5vgG$t;Bz3sr^&PNVo{*Gr6d_?^qM8 zn*(!SQ6*Te19M1d*?<)}Ft!iNP@G~HHX79S3#ToJveX0O1=@(#At8jK5Ml@j)~r~-f`|>Qh>8VuS<9-6Vo=wzD&nra;w~zzWl?d({+(y$d2^q< z$$Wgj@BA`%X8!Y@=Y7waGv`dpy(m*%;rd+?nW$lvoi1dOA0k#1FK23eb3kH}iFT@C z3y_JzuXEOtGBv(2ATi3sM@Y>KnP}IVFpx}SJzDk6nP|7_fNPnmQMmf%Ok_Q>=iwJ1 zYe|_X{MutJDH9*1H8*6UmNj7@nP|@xzE=v5TW^hRnFy_=X39kS)`Wp%A{!m5Z_dO= zQ+0;5Ow_t2jCq?B-Zq8fNUWJMQTv)OkWADeh3}Wb(b}u01Hx*ZXk-<%BQTt(1)lqE zw|rU*jrnn0iz4FynJE0IaxEzn*$7;HVgq78Wvb5Zs&C3fy{ZGQWg?WKX39i3 zQ>>XXQSX{CkWA#iX(KVqL;W>-@Dx5Gg&&o|<6r%*u`MI*RySs3T&T`-?YTP7 zwdd+Qe63wADHEMg3q^TPOyRjNHdBL4bgHgaEoY+fRm-}Ni3)TzUWR3O-d@G$aA{J^ z^QyRA+f0wSorUrGCa_sC&y7>oV$O|uZk)27YhKLlOgO6Q0WXVru5s81eRa%psp)n1As<9o+EH%@h7&UP`ky&ZnOo4jkxbK{il zx!y6)jZ>z7Xv}ltl+{CcJQCu`jZ?;tj(KjJvhR8t6Z72oGKKM+9CM30+(!uONCh$P zmaK%&9$cCnb6d-O7gTA?dsW#t;L?nk=f)R1a>`?#8(-Gbes0WrSLxVwOhwFd=`p_>v*ND(1QI#p>tUnCHfkEbVWMxg8C;@07Yd<^yOIzWa$?#qyXBuHqKi%9z{H z8Lp{lac9hPHeC{T3wR@_$8~%Jwz}uR30~0jdH^nU6P!2acajQPe_;4h-&70gBkMH`1 zTjbc9Jo5;TQQ{k<%o^X5j<;4lT+s(DVM~ph({{7i?tJ?P0CsoXo>o*nd9eo@cEjBs zd)R$_-hB=}amzct`O90kc272bdZ&fGvn0Yr>?xgdMdU8*n zPhQ+D_4s)JcCxi|+$uq9v;A#H6ppi8D~zHW!a5R0H08I)v?hlx66|-3my!=Vj*Q!g zw23x|V`P|q`8`QCazGEqg7NFZGLnz~#st@hbI;Jekx;H9;-%zc{}g^=3b%WB(8(Q* zQH1^r$;Wdsxv#g*tRO4bHkS7s;ksWc$i2>ba$l~^qJu@Nx31jrV5)xzx!3{Tbj z1_P$NV+zkt;YBIjdT?yUIg`adA}k|;YdcvU)0+GDz7&2DxsTu4r}x{A_TV_DvUu(k zu5qpvu5rF3e1_;8%yt5O<=upJaNgnE~7N>S&we4Y)o++5<+% z`Va}}Hfay_!n)PN7gz}iesiN~IQE$l^{n=nr0^{%9NI_-52Ny2`m^%{<743{q$bI% z%HFu0S$g|yihaivZf7uF$I9+`eu~b}6pl8AY6h0?~ zSETUyj+@^_j+@^lDSTN9zbA#SO5u;E@Jh$Kv)*2G+~QoH!ryUx4Rtm+ZaSMBe~kNO zi{oa$ErrKBJZ!V}P}Tkcgtw)B6UR+|j})Fw;T=WiwLa6sqiFjXL9NOpe!++qf|^J?QZzT2G?aGYyB%Kk*_A$Ro{MO`5llKHiEZ`TpV zm&I1ezEgYiXR(?6*|a~NbK}=>f1l#`e6ANcek}XvDUP4S`kC!`AKG8&_+~V)kQOAf zs`8#nzR>a3T))lndl~;q$89`&&hg3YuWUTBdgx5M_ndtp%eB$*^BL#Yj?ZFwe|8+l z7*c)aGry-Ye!I_u%~n^i-^ba{r+z2LPbTl<_(rY|aeObXk8<4XZ9iC?I44HZ>n|J^ z*U;}`>R@&x44H8M1<7Jt8rQKi4|@_i2y&DM*;%I?K}A&i*K_TRE^g#EirI zD`($``?n+gSez?ZuECCHn12*GRtJmzNzT48b;gVRAhDnA?DwSoAA}DN_SWxQ?6}>R zyi7RuOCfda_(r*)WBtfdu?O!KCZt=1L#G|fYsZ=CjESwI=mD{Z&M5MSghOW~<9SLr z>~U>|wAS%65*&r^Xpa-Z7(|BI8)A=sbvgUhjl!XC{n#eQFM&u%-#ES`!O{1QUr+v% zaP^zV@>!hiXx}V__aL|WwEpTq#~-B5p^jIQ4|E*Q0gy(KTRe70d0Yyg?Q~vCWTLYj zf0cZm;~$e>>NuV!AYD&x@$^e@v@C^(?~S1fefAgoe8!I1DPoWI zKZt${9JhDuraGOSsWnsV(ax{p@pXZ4=oe6bp5v#HU*UKuxxJ5M^;trGo!G12<-$=9 z=kqvxfE?L#PX(grRk25$ms9_B$89`&&*|(G@*r&#d&F}A>tU;K=-*5IZI0hfUWex$ zR^HX*J39Utd7g0fyC*r!P+l7s+NJRRqJuc!qJF;PZ;%gj{4?^S9k=oIB;nXEmL`%T zd+sSt6kQ_ri03EjU*`B$^6oLVdPsLvVPkFN_yJbkJ2 zrsKWIKXUv?@_#yhIQi$o)$e!Y7UywEF{6DL7s`wJwDZW0oB<-(wuN z^UTv6x8rDn<8!EUq2o7`U*h6MoP!vvUH4eIcIR5&=lG}OA3AQw=O>PjYm=`hC+BOfdr_0Wv%`$XZ; zsighMjz39$hU1@*&v5*0@^ayb|1GxL3xp33e3wSyBcyqbHzJ=e9P#&K{8tKx{+q1l zCBmWKi8?ns-k$t^#|M(Ha(n>!qryi7{i7Jq3&P>|INHDL_%Y;fkYl&v_^o8S{7CGf zKc4#k77qO>)cM-+N#s8`em42fj?W>l%XVk+T*i3XlII1VBf@0oNqvPQo-3(yh~o>$ z^Myla2Og)xgzJ7BBOLm7Q)jH>caTqUd@cDj$DblUQ}~FW-<f{ zZuORB{aQa^_11~{E1b?&>V)6Ji4*MM_gnIZ9k0g=`^OyLfqV_Q#j}UobCH?;B_!{!Mje=1~*bMvk$m==&7P*ZZ7JmWbX-Rv}dpe!(sMFi=Z^#dGJbaaQ zoCZ0bArJqW6eq+#B1{Lfotz>Z_(@7Yjc+*cXNgsZ=<0deOez@h;?Zg^vjK zH!z;dg+u2!+F#}PDDoxbR-YB@-NIlvHu4tarazhb{T#RFM?+Hh zDNg@k>K8cvC-SL||CM~Y^b#$DSWBZd4f8(IQ|&<3ddh2 zzt8cP$R8oMcur^itPy+E&*iM2zX?bEd_o6f5C>;7HQ0E}W$B+*Y z4xP3T3F%nj(3wvA;~g(1KaJettbj;JQ^X$n`G6tK6%PHYsdJv=SCL;N96FanB&6$w zL+5_l-{|-<@;k^a&Ps@c^nlny|DKqJ{|gch{g0{hjN>1Yzv}ey|0PIoi#_x?oJF4t zhyE7oeBt=l*vH!HN;}RV4H0J}7tk{F^Oy0@y z0pwjB?@Qj7+~R4#`WYzph-Y<}kd6}$zo%2@1jnb4mkG!5YtJ38ar_yGhxCZ!XK*}v znjGQvjV+Upx19Z%jQ>-|Ut@pxh3NDVo$#w!aYAif_K33! z?Vk~j`niwgdfxFBHY0~w zuP|BK-`Uq^`{^Sb<@y`f`#JtR*X{T-zlSjX;m-b3t{>z0CtM#b9P!)v%xS_AkG(iJ z!SP?XKACeXFE5WGYmXM^z0eBjCb93Cp!oA4(MLR1KTioqJp1rP#`(|JJd;`nLgtsK9P zyshImlXn!3wfkD4<^CZJg zj_*L;OgQTQro=pIMGn8+$uJu!yqoZuPN$6R;e4_0D)!679&t`)Ja;;NEcv~{5vM)x zendEQ-l6^Djz3TSqT{{!FXXQ{-iiDJ$Im0*;P@=^PlY2+`>v}k!V%{h+HZAyIeGX^ zwK(O4Pw0$eN8Cg>bcXYV?`Dn`7UgaRu8?f5K>3S?fLj25#mb%fY=fdJE~&ffm7 za*A-&=N&B9G{^tj9 zg(IHFsZ;LwpUKa6`q_{NX};K_AGnhBzeG6nU!~5?j=w;Dm(!mJ@sR!~_RwEW{nf&u z|0Q*vc6<~0^G<(0#6x;t>`^}@)c;KEZ48I~ug>0%uLkVT@`4TGX|flr!+7p^M82zV z_6noUeyR6Tx!cpFrsk6=T7s!7V zt~&MDALj)P=zLE5JjXvF-_7ye_!T-W9B)XT6%M~&GyYD(;WwZ5T^;X7-izGo$Hv*C z9k>6(ndEdPQRj5WPa~fx9C1eMAI=qyI4`Gth2xi!Urugu+I{Bh#2)%hsDGDm=-)-1 zdmX=x{6XQW^O$hxyh!`ijz2^Goa6r`f6?(R-!Ex@ zt>c@>my%ok*!}899B)GXHKGH*_Prl za(oE+H;x}h{u6l}XotzlwFA#zJkOHn1zW^ZM4fhy7m{~%d?tBM$Im41=lI3shdDlv zdiJamZ)Xcfxt37>e8(4)&l9dXc0OzMvzqpc#2)&Ok}q|9 zGx;r!|BHMDc^#Iw1>;|p!e4PZ4cdf^A+2}39{ER(??L`g$D5OHBDZ+z^S)pBLEJc@ z9&jE$sBW^}+VQh#-_`MDEZ4z~+x5laj_-~DLORuPyFQ)fxc&dvS&olpJo6m4>x-Km zxA)5KcigVO9(Vk}B+Te_$L+fQW5@r(diaza#bi~+N_jkQvU;}b$K8day&cT`)zb0y z!wb^p|gbcT^(OU-b*-ith|Q`ht9LKAL#fKUbCO8N#8{iuHei!9aJ(`7-awB1)jn|D!P(LKS;g^|W1Rgk+Mgu$t;sN( zEB1)zJo-J)@ww#lgd?7v5I{%^ML!#pxzmNNUDLGczUKP?;2s_N%w z>ObZ959H4Zhu`JYUq_BrcCle8+&{?1vnu`OJSe|)yeauM;n2UGKSW-~`F&FC z(ay_R-dBVpo^tA}cf5rBP2sBZk#Oi-L;HU^ekHk`Pt{?Z2ZuRQoi?0@8R~6hvKqA^ z&kO6=U*pKTh&^v{#ajkD`(|~M3`Yw`oR2V`V;z5x`~>0fdj@ri9LM()BbAY374b9x z4CxxNhu_zze}m(%lHV>I^=#i4^JEJDt8nOlMxB=&|A72;;V2jX9VdF99P#rOcvuSW z=ZHP@cjn;vo8x)pEqQ$B1zW`b0_&%PaOiZTeP_qpkargjo%g77sBq{ULHmJ@4PL)kUeo^Bi)Nmyb+i{-xNXygTsW$ri_}eoBMwE-%=|?Tz}4 zghOXv>g?k99^|_Vht7-KFRjS2%18HMDg2+Z*hBwN>I`!HVDizzp?@4iLONCS?O7$b z^_Nzz*IBOnLhZP-$CBX@z2ToIQ|Lw z0CFGCFtJBH;CtMWMhizgyYCm{=tReNB|k;D>J$ryPFLDbb-WY#nZi*&0~5E=Iphd$ zS2D~NJA13on}x&gk?8g0{7i>@u4^h8`-bpm zA&xI0AL{r*^5Y%<8~I6&SCXG59PMft+f}J>)WgrTFL(S0@(Uf`y>n7;mpHyF`Q^e< z|C18)=z8IZ2fvqsWbHaHEF+%o)LAL^;QN#RNjP*)q0SoNupdeLXB=A!AwznU|zF};cd^Bf0L1TEB%BjDn=} zIlhkkXvd!?ALsbDVN6@2y#3A z@`5ewCzJo~?2E`7cTW79{YB(W96z6YAII+|Z|(SPpd;}WX_ZQAv{xKizboQmriH`3{Zs+yT9~3JjAHBKk`SB^dG=(on;Y(Baox%rl z8M8kLN1VJokJhH}^^UXn(S{WMg>d+_df6r%e*1B~0V*aW%#^oq-0F0{6mIY1WAnf; z**)LEVxJ$lrNR_$|8I)T`C+naGy9Ep@Bv}6WY0Z3e^T^ezY{aqeJQiIc5d${dj7f7 zG5eo|A8Nsbxm_P)Gx!*8JE(55;JLk5<@so_A50C*CJ65@{DKsIi{pcs=Rx6zh>qP~ zL_Eqjrr7`LxIMpa%Ik0F*fRvU8JxoHxxKgliRZ1J+wXgLep5quN!l~~z8tSb(Pqc% zB4bD=q5})bbnttUNNXMcddGzSGle&4lx#O0v+wM9qk4(mAjj?dG)JfKsgB$C1fG+^ z7drk7>+v4P*O5QtxP9N{$0_`K$Fn@CwC_i=oe$1($Riy)PW?&Fr^2Uch!=j~$WKkMS0c+j}wxIQ|=T zMmye`?YqeF3htL#DSW=;&DkE7JDyMOzxSqs{CQ_@{9VVbT%S7Lg7I&2-0WL&Ah3F~ z?`!Ij!iS{rlO4CQWO@oe-|++4-`$$R|Kj*q+P{**KX%-{N9gwyz7J2DQ4c&+;sanr z3ZIw4?fX5vPFvp0HO|{p(Lut|723UT`(6v!vx|)5nIiUx)5d_?QusHHkBU{o|1#5& z#WN2JA)V-WKI^%}ano7sctxxdo^Lq*0Qm;TE6Klcd@|#)6K0Fkcnh8|7`N{y$xq>D zIBw@>e-Pe>v0!#}3cp#nodd&urEnXo!Cw~M3xMgf6kdlXmME9n?<>5giG=y#Dg5FT z{-fjU_M$VIC8F>Ped8-r_!o{BCD|kUe__+%a26fOhpFH_Xp7kuDg5CS{-NV6PPCK! zUnKN9(8s=xk0$T#_>JWGjz3Lq|F4SpyOLq{uGlN@BmZXy&c`>A{=Xr3Cy0b(|KDf& zwvR{fgx~n0#5_97@s;FPIlhW~hnC5L>6redj?bd~ox;12VfJ(i|G@E`n5Ql;^5GXc ztgfi7@cn6v*$IwY-*=hgEM|0%;}z6;!too)_5U*9H%tDNv!6)5D{n3!err?MbgtuV zXg}7yhrF2l2FJf7e<6i$aC`~^2&olc4zqad|KDc?U7UyclGuhc&Jof&o+1vX+E1bRkPQ*)&+wVdA;P?!VU(H)3L7To^ z?{#q8?E5-ChdQHEc!6-Op9_See(ZYke#ci+{~gCWCt*jsXRF6wM&8wNYj61}{Al4C z=jp-`=h84CU6GAv@pT-y{h!WlDg1uns{fL3=-BtGeC7E2VH;BY*2&!VS0mQXekuHL z$6L{UqT{Ahp29D2+;o;Y{w?+GJ;OR7T$Jl~@_#$~)*Mh9wy9n&`@e~nj+=cy;gpUA zmZtFM9Oq?qWbeNrPTeoRrP%jvn~M{EZM>K$oQtvH`NCQBm@g5YjY;ycDuu6g+~U{w zvY=!A{|71dUpQ{|zX)en5eL+wT@n}Shh1F^`UtmU5gSf+ynk#R-b-?vIJ!24KPuc} zM4t7IH=)qyj^kVhX_xlN9P#UWjGa>Wk&fHAJl^pxiEMP1aI?jxB`N$h$1kAHSB@_r z-w_8-NQj@Oc=39x6n>a+o{GlyV};|~(DvnQ$7AF0u`q={D7>i|hxsPQv)tC47j%eU z?d^RJ&+R=8@Lj2aS%K&%KPQD>pTZv&Zfytp|8_hB7}7o+lR4tyC2|Z7P2pzAC1D3K}0JpqA zHZECx(k#m70=Tx%QNr`Mj9CGfVc&?$n3ZxFyfK$CtKc%Y*8d{m+P^ImuKQ(`@FrZw ztdh&n*Zz6EaI0gW4Z`hS1<)4ZI)23iVAuwI9S55LgoK%nzggir?)MO`=N0+FwY`lJ zuI;TrxVE=a;k;Chb7!dnYpFPx{e@zxE(dFmJQ zEy8(9m8h|CV>3@xV%|hJPXS_{70xa*<~@Y7%Za(&PlY}Y*_e+KdmfrGFA&}>Cdo&s z@b1DZg!d4>NO(`-%Y^q5zDoE3!YhRzD15!}KEgK$?<;(Z@PmchjYrhaA;RtZW5DfP z8=JEnu)q%mVCo@!fWa`&7oIPCl<+~qtq*}d`ZtT!+Klm0%rlU4;}u*V$u&||BvJOtap1(i#RdfwV|E;|GIH&i|qkI!VLYk)x*J#+xft7 z;pop>h6(9Z$BPmil{#+cMHdR!etxlV?YEZ;*M9g>;o1-DdkflszAyIRO&I6b!qKnx zApcFc_LDnHJI@AN8{763j{d~@j$w|MQU7Ge-zT^4SB87&7{AEb+wUen?05m=e$Vmg z*bvah&G(Hu71*tu9eT3x%T}&H{#X zhvO3y96c-?ZQuGz`(Acbi|un{*(~xx#UK9(t?e8J1_2+|BEpW|z&U+B0!*PiLPJ%69?xYh3x$H&s|1CFm?Ip1-7NWCQfZyn#2 z<=TU#}`wl!0~0|rNS{jay6FP5M@Hm1Yges>sL zuM3*7{=jv7vHb>aV?tPrI*Pqs7ufF+!(PXu{$j7=(GkM+x?q%Wy)GChT(1j?h3mLB zL%3cS%oT29HOe(lxLy}rDO|_98-(k1!R^BJx?rVny)JlExLy}LEj)_=nO+j!+F+Rf zLwFnE_Itw@;e?OY6QdI;C+ z0{i`9_|@xzA!4uN*GS=C|s`#N`&k9HAlFPUl$73>w+tUWBjsogYZKP zhWYKn^}1lCaJ??D-xo&x=yk!w+ZllA1nwko?YB?nP6F3{dxdcAx8LW^gT40KKMU_ppP22%oeKLN z!tdoy1@9^RXYM?3?YCQS=Yea#-CMZ!+jE6$zr9Yl_S;_y*M55&D;IvPk3_ikf6d_9 zZ=bs#wkGGDhbD{h=NH1Q55$In9TOwyq=R)CwglD;oJ`a;6D?6C>d(&tDuKiwx zaP9Z570#|DCXWc$e(y!$>_TGlfpG2jwg}gLZwFLBNQhJWy*-5UP>b!l2?^L*jj z?~M_z{hqy-1i#HJ;Jr$ds4Xedv>1| zo3-EDJCisA*M4uPaP9X_5w88-MZ&e;TQ6Muy^X@P-`gf!`@P0I4?&#T?`4H+zjvT; z?e~TXw__hQVCOl|*M6@;?6u#!R=D&(AR$NK;hc&+3!NbUi-a^#6CZ4 zvvj}kK?X7JHgoRu^1=yWUS5{Wr`S??@tpFg+r+~1!l>JXnKPqqWyRA9yMcC_Qe0R% zern0g;BZBFO6TTQ zW)=shIZt8p^xz@4y=Z!=tV}86O7JwTxFoldGYeZvs;%YR#ha>}hqa=@;5;s2yh+!J z)KyuvwW7kA#nn8N)F45Q!c%7!PAi=fsx)>pb>_sWlc$daZC zrNudwP~);zS~j&Lr&2nrq$syGr@D)xl2BDuE+&^1mQLYX`3!YYK7%e~XB1AGHls*) zPbk;KSz(9dY$lZzhmdpYMK$u26%)lZP&gs9taxc!Zn3a@##GYaAw;5zF_~pWm|R?5 zTr!)+#dA=XY=?ytW|WoJ(gb5}>T_q77l#UGmf15);+mXPOSYl0=bol%Sk=yTBVgGU@bX8h=3;|?F4a9R8}~>SUb1>|1!=6aB86Ed{E2BM=gn`5mj3-c7t)as^a-oOVt;A zew^0gyz>8>A7dai)8c)_}{f$q_Uy6e*q{3uY_386ZOUb{1`Sn^JB0m4wDfzcBf8#38$%4;6KPA7tRF~aO z{_9inXX#k4{UPQ3{hGp!S_q?zlb;0rXaHe-!V25%?1?EDY)kFt3G=cf2y-z>>*_Q_h!{_m2M z{2Q2`-MD41vHz}4$!`yv)>q|B7P4;r=9K*F*>T^po%+M~-N3|Of7tJ^@-()}tFQli zL;l*@Uj^%5&tLll6{}Ca@)K;^LsxVnE!a52O=+0HvY_g;Qim)%D;-`ucRr;kNc;-{CNKmv4m=io8(8E*W~LT?@88H{$(E}xy%pz z4-S*}U!3B95&aKQ=V96VFG%tKcrE^ME#>{sN%6md{-*;?e^P~Jn*5d!- z6#t{zB^f(0zs-F47p3H{VE(-=Bj-Lp-jhW8^T*H4%&*U|AOTw>n^&gzuVnt?6Z5K1 z?|*g3zeCuXUuBUj?D26jFCd>7=0}Ce@4q)fey}jBma^@+>KtaPS%u#@-%ftSrpd2= zx1IdXeU0{;34;Gy{Q2rUEN9(%(~uwg!uKCm{(NVjb?f`3_%C4n`SSYpzVz>8Bg=m( zPTC|{yD`)&BKPk_|Hi3zi}RNh5s{i4cq≪Xe1Nr`0> zo&drBNn!H-FG%q}=2CbM?=M=0WVJ^5@woE8~IZP&J;{u*v`ZB@Q zev9xK?%%o&(A!>>;{PCi0srGtel#WTe;NJvh`r@LhCPui&tiVtV*YKuG9~|-HA#-2 zEhFca&*rOB@;~u(l7W{IRVtRx<}We7KmOM<|9Tg{mEQ!P3;Dxejs3TUUx2<18$+5H zrU_xP{I>rPhxZ*&wCu0=48JeT{a+lGM~2Dg$LW=8;(>(!bALUaMQ>O^lhqn+1(&(C Q<}Xh&{)6!|dyV}61A|Q*umAu6 literal 102208 zcmeFad3;pW`9FTo-7*PDCLsYr*oI9&AtZo6FbIJ}Cm|rK#sz5z2~b18WWrX3NI)55 z6k2Q5BG_uJRkUs&if9o*5wt4MitER%ZXu%Jin!$Wex7sh%(;`Ge){=+{`+2D%suBh z&w2LqoaN5VtS&4$M;AhKFQJ{SMQbip({|NGq2n1mUaQn9v|yt}wP1KuF!x05ir~6k z{fn;2{ebHJ=8rI!O(oDZ$2sk|r~uB=?z6CN;+gn-W1}U+9ux z_*`EwywbPF-%AUM+Cj5Q^JDEL#2+ zG_tuQu}8JsRk;Bmw{~-O&amMl&Kx;vbZ-8Hi3Nq{Xi-4f-12!9l|&M{hK~d5_7^l(q%_XXDDfA56kPX}f87F@aBtzh>_^Fz!A252~8vV<_~(dAa}37X`-%am6chp8QU+CeR2=f`h$&K_L_RYB^hkkB^ve$ zGGl)3vDI%Bu6xhFZa4@Qt~(SOmh+WDY=hDthLIvE{R?sr*S*ouEQ00xTe>U#DqX=y zg&-k7jugn@-xf;RH-7yHorysZl49Ndx|f3uPX#W{za)Ql{-v|`L>0@g|0WxH_s;j< zxI6!=r-Xm)tuRL6+C%=e8&O#Ix<4F-xfC&nsA3Xzve$Kw0J(WaYip~&S3QbaN%?E} zJIKx@O?A(xf=5})xCtN2IaCrpSoHPh`TlJ)J82Wwr*{s9KPzh34bk_OM)TAk zB6LZhhd`Dk{0j=~t=E9iK9f^GBaT%RYtQ}>|E*mH7G$hCA4FKc5VgXbLrU*9W@%0) z9JqdM0tzkv0yz6~7BZ$q-XumtI?nn`d_0RGCZC?c+ z70wm(|Mhqg3;J&<=<<@L^aBm=B8;94Wdxgo$Z6Rnxd%h)INA^l=RmfUVEFPAXsbIm zdEM)wvmm~2vUs&b>_gw+po0zXh*0P8>&FBsD06p*GNVB(q5AmsE8&jaq~ESRg;4Ns zj)FzuZ-cpC``7#n^`==t<@R3;W zlUBHrXoa6_cqgvz?O^yYj1g@301kRb>V0MK>(_!$zH6{QOBxIOCrZKvz7xn2K61g) z$?Nuq#swf;C+Djc@{)T&#n4$fyIKMYX{ETqiJK4-SE4;SF)s8;*If3p$vR|tV{ z*uSmw=-r_Y1|0xJ^gmzZ?4t6v&QRkB=`$RKbVvWS`$L0W9l8}D+T%!({L%YD?<1e> z8nkbZ90$;Y{Q+A0R2ebQatwN9tA5Je+BDJL&TnK%egSiK5M+C+0pm-7*1qZ#*@PP=W?G+eL5;wz5QZc^h}9|AJuCc~1ZR zBtrcP?$4SHY8j!b7S)aT6|OhqU76FO`W1=&`4D~2h9Ys03vRq1pS9dg0yvj{TNlW` z_E4y2_;^clCFGq7fA122B#y>-cje7FwC<~*zi@Yq`Z9h(x#8m;FGXb>{bbNz@Uk04 zF#RV)r{Lvan8IhgCh5w$E0cQn@1*!FY5qNc?OUOJsH?Akrzd{LQ~d7iT+lT2iGrqJ zAV0h(zy4%qLDPgM3jGsaZ7L4r*PqDrPkc5%_qoth1>sky3aEd=K@Ruvntvz7J9Ti8 zWz$YXKS5ibSEVOEe@jDRY9Yo%4#rEvt^lT}3I3glcKF4DrlLT@r}^XSTQl?h6ZYrs ztINi`U3W$i98f}ebfSN!X*4Z=0vKJFKDBlitwLIMv)!EW;vFrBr?WX8XTlxM`T;8& z#A9A(#AE$vEi_uO7v#ZYilI7Q!#E8Dn~IUXUk-H(hMz^;ri@AK9Qu$G$Y`zZFpp3z z*8dG3zLm2JIyAI)_1}bC)bMFG>aFoyfhoc#c$L%e_FD}f!h=Hba zziv2T0=uC#0OjHjAStLkK$_@D;-J^?X<1A?K=n&qeoYsUJSS%_OA4QCc=xS_Bl!&{ zyDp1ItjT;Ck?(hsjZ+`FEIqir)u5fhDlO>0h^?Vd3qE7*%AKd zZ3&-fy5Q}GZ!-N8Fj|_)9hZs zhJPOmHhdfKU)`M+n+@NN_g{Sobb?oXTaTOn>N5?l(ALO*^=PMp6}A8B8BWEdO0`q* zQRN^i`8(qz87+lCWTTSCBz$VTF~L+=H@JD~sn_}&O2W@#YvW%#7&t}YCyN?$iW>7V zkG@&+K5c%2O%nsy6ZzMS0JEZJM1EoLqN1k!l*!?RNXt)UI4kb_IaqUZyyowuMe&rz z#4kw*o{Ebai+sgl3820XnRZAZo(prnQaab|nbO#gDLqCCmNa%EszteP6!|B?mU_|p*308Nc-;LBSyt}AzSWzR?YNKBj^bdw#p|$$qqOZTmIppN|k}&f5 zZc!3GttgDu;uK$T(}>fs5~}%tRxCySzdkj&DN%3k&O3twP21i9j2_eA$YA&_zeT*) z(H8C*-o}z`np?0~jm+;QO^>zuA=*`~t>egpB|s^<>O<{VYimjP%6%o_R;E}KMzz(U zR|_1+6poh<)t%P722jNW5&5P5^N8+`=zjy+UBfqjjbjh*wMLo`8x?A|8Yh; zJG#%y-${;ZJ_C5c#yYH*9ZOsRN+sbBBK4l!1@qyq<^jNs_%}G{&7!8fuF`yueHXE- zgl9^414~g4Mi|gG@gqzR$}hl=+Bx6MoWaYEZ$r~^b}2@qd4SGCc*$sBz9?y$tCxg7 zWF|%7J#>bVToQga7=F{0`(mtV^Dp7SOSywAfI!e%_HzXv!F8>ngy2=HkI~2vi9OgH zpTJt_Sg>gcvf2m8YCi=0S6@$<4L^)u`itOIKh&e*znUg|RAfe@6*EQBHU6=bgY* zQJ`s$1^~vK{i+kkj}nLNWZRa#WT!{#QQQuBw3cmLCqcJU?6lk^>uB2!5w(_^WF6)0 z6bmg^<0Ep-!nW)OngeDQg}gT95wjQ7UtLa=PI98hYb6I8! zJ8($szxrFicaSZyn-2OX9soXBfhDQOkoD_A%J)im@VzTJwS3qiPqd-6uzY2iOJ!A$0Dx!Qd=DgmrUDmhPQ@@)wY$xqou8;KJPBGT}t4N;OuE7e;m!S@PT;UU6 zzv2%*vq#n-YWPI(>sNv*$Zg_#@X^*+TU#;sT7%&qU?2ZBJ1?;GLjSe{c|-hbDem*m z2>r6*NOn$Z!^wF6HNOKSXMc45X-W_KckT+}xU8vUDi%kb!v}y2)TQr-_kH~eykEEv zM+Oo3v37aVzwlmGRp=9!!P8twlpg3PrxLI zmc0=q|J4hjK+b+J3WmQ4zg!eP=HDrrrhZ+2()3?Vr%Vz4c>(|0n@KBZ7XTeBcJt2g zueDHwtm|l$J&{1|+5WW|0ItQ{dN%HuILlvb>As=iqwL%hSLR}3JQ%#0H#TB7cwy_= zziG*V@%2B%FMXqF$rcp4E-P>N=z`o6SNu~+VH+kRM;S$7#J~ag1qX`Qv>cF|je4-f z`aK`}n*{sb{W@c?<`K_E%h{0c)6uLKY8mIUiM60s*R z2H_P@+y^AWegb1qYuJ@uFFe_&lvdxTOIZh?8z9^f!xF$4kP@uf5 zs;Xih95ah^7NI;wWEd%D*JJRIEN=&!);$BO>#C{-Y1zv)EL%V0C2UdnMLuGt%=AxW zrh)!#9r;xG5C%}rE;=y14n&HuAi$Dfpg?52*V1W2QSMW<=7ja>1Bx1OxVkM3`_4;( z4SPiW$^Lc!&e@;OdAOx#B);H`A$1-2XhyaXvVYrfz_b0=5b+}a9beUsrh46>{716^ zX?Z|MIZvS9>Hc+dVcGn~E)6G#`mf0ZX83T!ZeNkH zEG0kOrJym(zpYm6AVUyy^vLqBT?0NSRr%M_x&);K{+fHi2x<+V zl%f*wU(*GZ@i?Swm1|U*Cz{SbP?&SHo>DFDL? z*FRM1$h>TRjJ!pS%dp5l-&YjAz}Nf^M{hmUMxJ9yV1;uPI{tMUc&XzR{`aq$2NcKu zEm?BhQCwIP_L>9w7d4giqsR#6KIvamATjkwJWyN>LmDwSH4N8NUF12z%+f&GA5Y&D;cT z;e#9~=TV@LY7PCNZ(}M2I41%Zm$pOir11s??m+TjE@QaZRx#nKyV}& zzH*n0?O@ZDyK?rIgl9c~6NVj>B1*y)SY{R;_+{%a<*avUu<^=WC5?bK7GefTD`}kd zKz?cSZWeu1!w-n@2Pj&?#~SwfCL8BM_ED1?vG+o1wqfsl92@x8o{nhD`Dz1A3;FeV z*;?I~`QhUjN$U?nmE6z#P5W`DwfO{$_jQL>O)e>a7SsOZL2pkM?WJ|Il+0eBe*sl8l z`b>vp`+unwhCj;Lzk{Z&=6hf@mb>YkR>a=A!QUeL?rNFgjvtB?nU4DV*FA$(cylKB zN8=6cddr*9x@Y{9N7wfy;={dhv0>{D+vW$rbuylY6*Vov$Xn)XE`eA@jptz~PLo6N zUU2mQssZ5f)ck93nXrE50MYyqN**#tm6&)J`g>vmwnB^{r>}?)e*I>?vxsirc(Y;X z>G@MVLH0;2!O4voIbYSERjPVmy#Gc#FOd;UUf2MmPY)l52ROKYiST~2xxeDl#j#Wi z8#7w&b{u~7T(qhh7L{i=(`@awC&#qsH)iwxx*+^)^H6Z%{My|2BzkOzBwF(~FxhmN zA$&Y4k6hC>7Gi*1x$7btWhHnrhf#Lnl%~W1C4)Z5&;1gQ?EKej;1(1AMNNt66vI~j zUNaRzyjvjDS2q`gkL2th^cjW&o+(gwf5|}%h!jeM%|3X4x)YH3bh79XApc@Vf{RFm z7ztLvFNy?}8&1KWSHPsFoMdw_2sr+|oGx@Yq;Sx`Rgt4($ITxY|D#~Ky`r!wad=^F^A$((o9giRasQyhFlLc| z%5j-X4*74G57RaO_M7N@rv?WPRZqw_e~cLLpaf$9O%#i2>TJFl5FBeGc+Ua1!gWXe z*Y^cStkDM3M7u3*a`>s{0zhcq4s~dF7NQ<){^!^9#Pn!$?g@9RLGkwguK#BNj|FJ| zkJa_mU{fKU#ZCSw*fe=NW_!$FlywS^m4qjsc&2cB208(}kH_o&Jy8QK7p-8xJ-M*cCW)Pl*&DY5%s1@{IfwXsn&;vlkd|UW7 zr2Xib!n=LkT)3U34e#MC2+jy__}B2%ROjG>NZ^X@Pl9-|LR%OEV#Z@w~;^z6YyVq82|fHIMV+? zgr{i}=_A9^XuHG^J|K&fOfFHQFH%a;3Vf3G{0ur9EX03J!sAmY)!4DBfr+tDW@M-S+lHf(NPr8Z3OKxJ*%lEt;! za6NG}%D!QX%9d**WF)fbPQ484)XY`aNQN0+hJzf= zVpA!yi@#ROFgl+y9wcs8!|DXwpT{&)6*w9Xc;3WU=8p;QgcRG!?-hHY(_d`rj@;<4 zTols$MusZ*h#qB2v=k%hBruJQH?KBoDneREBVM&;JHeiASfBlD<($N?X=SjpE@tUh z;B%&tprz!t^S+YW8NJ&PV<`wobuduV?r2t~nyLeX4!d|9#Y9s7rdha&EUZxLM4eLj|T&9mF zGVW#6hO{cZ^T(JF+=~ZPk0oXEwB>p-mJ znYLB$p`s`JIub_Ei-p*#pCxWnp?52l$T~>VK7TM$G%e4JJsowtop?c%JPAS5W?mD2gjHF%fMcOxNwZnQ>b&#@EU-4R%4RsI-z)-L2 znQmGoj;9cWd!YGseZ-Fm!f2gIj)mHP>z!4??F^3p)(3ltrOClDhUB1Yn7nDVAlP%6 z_Lh|NCnS99$0ba-@id1jZqiAw=sm<83Y25c+^Rr;%9@=Z&C0d+^p2Fr+^ZeU-iNP$ zWY~B9ac|e^NxAU+k5EW%OQ2a#s{Zj#9I_zJ59t#VkJ!sC+$7&CWb^=Ie!OFgdXER9HXB8G^!WB9X(t>n(Br=c)&E%xm+A4tNY7Ib9glUSObgnVDV5eBykDfW%sbvL z+L`2lu{w>O{)oG=`ap`i{3Uyv06MU2YIiRO=GnC8xs5jf5ql12ZLh4CvArZ&S&T1) zt*#+&b0%`J>6dAa7+(m_1j{VH5T~^f;|np2tms~tn!gmD48m*HuS9~%kJqeUi?-RK z5bQb7Z0`zKOZYJ0vT*O;h(z&~2g<5_Bs{y80}i^_l$MT-K&|$X@N8sRlxD9eEON8( zWUS=20vr*EpFm!(5D8s-y8@z#i$@{oKn3df-~+D@*uuaUnwwCZZ(`p+biN|#pYfsBQ8F(GwiS=RHN+`om31KV6X6Oh>QjE zYP4rX2K8|-V{z3y?K$V1&H=K|Uhu+tE}XYqdr?}J+Yfo$8TN|kpvu6o*99Mwx%`r= zd%8u}qAv>n&7DwLrL<(KXweScoMHe@P|-E*G#BZ$E}HfL$k9%{R)L>3NYlP@;J-ra zOy0*&>sPB?snu$I>)lISsa6K#i!+_mqSW7o>5}nZo&#t>mFoZrr$HLsLr@1TmA*yJ z@%)6>kJ(OtY;1wYK6^rR8tqcW-GTL0VQkl3Ub|v0e9~)uk41JzWk$Z2*ZBioE5RBf z8+$pJM3dS7PfmboT6!eZ$g&)2e-f#xn>noR_J$RUc)vG93<{wKy@NBdaL#aqS^*~9 z`M*Ra9q2}`b^_-?;eQ(Fo=8Y&8#rx)C6Zp>#OXb`vJPz6KdZd{sY0@+OKdWM$-Y?S z^$*LswjD*U@hZxAH+wzAhTGyb9Iuf6n2Gak;(NU`LQfjF8%1{-xCtbQ_i7Xa)6xNz zyMTbwZVpx}qt!}*Qc3>3QjtbqB+`Oiq4>Q@i=_WI1!C?|D!pEGD&_$N8VNVm*7*9- zR0Sv3(dTM#G^Z&Iy)Jf1UZOxT1TRyna*|&x;IjY|*((;g!J=&P3j1SEdX3`eF7pZ@ZrSSh}n6(oq^&)-_PV@fbK;e7mB{LbmX-4OPLS3e3?>SB=aA`LlQou=R&Pcq&1=I5QZ;}=%`XIn z;AmM45E^5Z$=*E|RV>oVovg}Ym5AgMK%!*Bol3;%kxrB4qs5-(bv4=)S(ZGfi_V1P zZ2N~qtoAqq;n__zt8}4d)(|m}=!;oA84M94Wk-p~p^+siJ6+{umg9vPB*8E*jQfuE zP8!nEn7_!fjSzuV3Wx#BlqW31yN+~2Qf%5m5|?X+;Im|R$D*GKy;-8`UUcC_XO5)( zrk=g97AV(*M0a(SbExL+XePFR`f}MPIqr>RGtU|{HNkpl;IrW>EJ%z?A-S1h?t3WKhT>D;X#WwrSV}Phz9sw$B_MOK7(q`YD zqRWS}%eUSx%>Jb80yV(?q(tC`Z@7UpLc=#o!wGI6%LdluW6zd* zfhI4pDX|YUsntH_{k8+u0`zd>DDNk%dD)`4Jh+fa+LTq&~xZ+a9cb7+Z8k3oSfs=et^VJL&L`4nMhHdWqceTr4_ zu}$Giyq$Ip$fVIxulIC!Ql=xdcsD6+5Xx!8=;JJW^eS+g%tEK&P-Agll0J1n7>s<0 zO@)17?=tPB9_n#L*SXqDgQciltF@O#Qu-0ocz(7PYKK!f9XTWn$nD-Vb?t)6Rn~ zvFTsv&C&4?KiL$sy4Kc|YYz<|@x)Bn|Doa1{(*Vg!yNkX!6O_zk_5gMt_G1D0}prp z$@rIIZS{yX)*&f2&F=4rwUwU^i&;JrYwKyM)i4keY}f&p;3! zP&o9!`M>CmLJqiQ80_Z;MrVvqxo_v46`! zJrxAf%e3F}pYkfp|J8`bnuH~TSblE5sXFl~FJ`*E+CJfNDuC+N@eFl{%@`h>k1G!9rSMLO# zRi<5)83cxVS#z~D9>8R`WxP{>S21DT__#;&_7n zUFA90Vt-c+AmQ8#{;u-+d!Du^o%*|%RjXBd1dXSbVZgBF;cqRZm3VMRYfHOMQy`qC zm!*(>xnCyrTc|Df9Ms6&H^9I0g556;;wUOIjlc-B+D~nwk6ZA;hPc3Eh*troOKhTk zNK_Z3t9HUHe?d1&EA9m&T+p9HbT4a(R_ZmvQtiSt-eou_*3!}19Qh4B2NGB^S}XID zHCX4l96PLYGK7F~NTEg>fK4@mFU6 zQC%L7Q#s3d=$oD;g!R8!6ZihmWeLuaCDNwNpgZ6zG+~c3b%BBs)x^8J$NogiNz=Rh zZ35PNdXc7Qtjd5ZM`(s`OmA_PmD{1nZ8b+kF)bINgMwfKm>koqi9IlQvy!pJ2`EQrcTwXkCCEWe0R=LoP8(jO!W!59nHymMS{Njn7!wmFOfz zTfrx`HMuPp>g0DtQ<&h< zrge_+A=w;#h*KB0kK_bHXLwA8iO0o|W&%cLd&D%YTN_OSy#?{;?xCoILJ!wh5w1Nw z5Ui3r!2IN;fZO0G)jJCfs)74-CZBXT%W3Z`L{S=X?FYFPUx90XR}kik*Ct=!)BfUc@^Lplg#Q&$veLK)dZ zy+H8JcJ1V-l9MKk2#`sR^M*NCnZr(+{z2!n#I z!v(#25`nYZYZou!Osz^atB^VqQ&jHp7?iYOpm0~C{-tW*c+#M~wk z2vRcuZ=I@Kom@6qS`e!%%nRCiY($oeI0u-@R5iMR^aPAzMZUNVfIP3Ff~obx%+zm~ zm~9T5W#W6TTL_BFmN$tp)9e=%mzY7qm?<$l?bFPD)HJP~rs=Y2`W9@cVP<*t(?@vg z35MIW1&3J?JRjkybunA5CdV`<|7KRQdhHRk4y^AJY!I)4}fa zX=0i=ZJ}h>Ml(xHHLX+#l-fy5Ro#^k7o37aCLWn&=0gU?;7E!n zrGsY-t>K_Gd}NfC!e%%qaT62qNH84qxrxKE4gDA%)LWy<4oh~dy816!;O)baPRJ-t7b?Z(B#` zcEmakFxVWo8f?_rurtgX6Yfet?XHAJ61dYN3EQZ?E#aW7A5A#w7QRFQ(tMe4k}$qW zd^riXx00Iab};qd6sBLK{;nf#k9T|tH)-dXPbFZ+iRtFJ{X)z!_lxHQygPjhO!bHb zz$4Z+O9Hl8U&I5z(oTn0TM|%ffl#vfDB?oQ(F_y&{Qvpo_4NO=zo)mNmuAiSjqf)p zrgmC93fWsw@tLP(cW^2MFtf)gZQ~|W<&xj{rlMu?Oruc_77d~f-PkZgR*_o3SLLtlM- zw6Q)u98YwoOWbgLQ#@I`M!?);*QWTp)b_jLpXI7!^=IS1h*#D5BK|vWcD(mp{M88w zphi_9c6Gw~ghZzvT1^gJj{&2obui)egs26MCR~%KG+vWLbc&HqUJ#?$;w z;ulFu?=OaTo z2%TU1Z}vy&-0XkYuh4ne{{}ZoI&b(t_t5#=|6P>Mcm6dgQ95fVqArvqS5_j*oNIu>+;2 zXshN-2+r_V2J-28_7-zgJ{DqEi5mq~ek=9}R3Fs$>QuckadRTwkEdLliaRJprWvMY zsMAG)5#PGSyz)B1_v^$XO!0BCS707_#5iOSrH_qI4d3NjPQQ@WXN&o%ag|Apz88Je zxFhACDb$%I=E;jnN*YpRPp1w&$J6ISNE>v2T)ZLloGs=${ce{2ySVi}s^99n)w$o} z(}!&_-|>CI?2aZp*Ma&y?SI)%_mBLIDY&y{3a1X3_dBGTA*~k#iwL72Ffm^$u}VEt zl$zHXzcXyF%-Z zHFP_&4E&E|86z$>|4U$kyci_uQdCT{Lz=HObBmeQAG7G~;sN0RV)^%n*td%Y-D|XA zr!d;EpC_muHGh)2@QiWT7`C}NW;{-agwn-vg#rQF7zGm$A&(@yfE%~ys`4o?_^Z}C z7H+pBJe(k7GU8W`QN_>kZ9E^}p698X`P=wE#uGV3+Kz3zw}mi4Xs<`4fZ&lb-Z7vk z7n-hkki<5VGN8jFSbIqwJTCU@HJa&9CyzuemG0W^r%~OAk9JOGTArSrn4bBK-4NF< zy#2B*JH)mlzfq$T0Wqz)FwaKgPe=ylF5@k_ zy=62T&bEXDLye1E(8}FtU}ZRMi|jVd{1f3{Y&IMB*bJAQ@Ts^W7eh$)!wQtyO-DcO zfK9Ry$!X4Bfk`Ocv&oa$xs=~>KNO(#q4-!BSa_Hpi}iR(&s?vs*SWr4-)NvtEU3>e z<2l1vi6!}S#v6vX$b17l#dJqPg+Zf^Ij2#-K@UXs@2<nSCKAbtd^AFrGH) z{(*Vi1o*hQ*`n&-u+zZZ5uuGBl$}qAC+WL-w-`Ifr#p=MO}hr^6I0%c<~1~~Z?|5< zjguK!T@ff5t*c%U_XN#}8|^Jb&`qRD)e z5r(eLwBWj1>CXD4Wuar%FeWl=6v{2;F}A}A^Lx6hqeKd#knirzh20I`5K1@n&trGs znde=Ik5#t{d~Okc#;sjMq5xFB0h#ZF&Xkqm*~7OHKJ4QQc2^G)(qj%1($#z(GnXaA z*STjWW`f_@;3?KIA*f0FHs{F7(=<~ywLKi56%O}u&Z<0Sz!{5abwQ+dxsAqk;L|lb;FwLv<+Yn$f1l>;>&aq*E%xESD!7DgbW{;_*s4%Tr zAo6c~3&s4m{*^8pztYzm`uHv8dSktlY}Ok$8T#lg=1m6p&5Cj&UM{~_onyVih$&)>AO+|D zg^ltT;6Rm#H59awgf@O22+ylDmtfs%&ObnXqu^ zYEFv+xSYHHG{9jUdsBqIgVM+y;$?va;mhK6I&Yb^#e7|yB=||O7Ar(^EhZ`nU$6g} z;`7h?Lk{2}{c!?3uJ0zq-TFz?V!5HNFhs(dY5rNfL`PpQNrpI{dmV>h0IZRm09cRR z2mp8M57LbApkxU6L&RdnsX}>`QMY7^x#}TdT!H&TVwL( zbb>_0RfK$iwsQ}OgLHdKd_uQd^n;jhBgpL)6ap6`rZ1jjU)d}E7*^wRmZEwOt=Xr- z$nVkZJ%Lx0oaT-CCLN$n`exk)ZPxE3(4G2S5zt-wBLsRxe>4JmRDX&%NpzkGFC3?!ZpZO>KUOgTzbujMLpCgRt^k%~y*uTX(*exHhzM&)cZ|n_mE@(sC{y2d4 z$GsK-y%u+bKu6-fjDWt3yTJ$04Zc77+>U?tJxZWQeb2feMOj6+<$TQaw)u(0g$M0t zY%UzJPs+l&xGiz))o27M-TmT1^J?=(Uhuwb9p(E5`*xeAzFqdywzyD@hMuUZmqG~h z&L!rH;y6tk51Y@LxHVWeTDa|}E&L_s=hj-=IaF~;DBU-ZZypzKl6h}8|3(iuj+s}{ z1C4Fg3-a*>wlJ~J8WaNKuspe}UMIQh^(I=Zyl8yD+b?WjBFlDwZ?JAxwk-;U0T`bH z&M=l>;c&)?#9Hm{)33n)Zg*?6aQB4wG!|<=$z#N}r6>z)vwsd4=8QmEHhEb{iYpxZ zqqTpIzS0LA{c@ivBdTYI<;|qQ~v_V+GVgtmdAP=Nu z8kb|GnMvGZ`1(M@*Ogrx(~=L(-qXJ&FFayAji*oC)gyGewOY%=VL^H~ySB9#mHwr- z=n~jg>UbFSJN+8$<;`o1CWG==^k^TGx;lW9GV?ls8N(d-*zu>}zD zZ5#yPc8C5KGTdSNIAwwLaktPg|7+Znblc*45W|_Rsdy`ZmY3t2fq+1Ytp18n#os^# zvV!VSHfs!q?U=EOvI^z^v<(Wl{cCCIZ9LP@c2-Oka^`t=WnBj)}Q{put zf$ag5IgmyySVd%Lj0?vdnXU^uOhdG;O?ZpkWs>j;7LEKJ@@7u*z zfmdwJt>S*CfzE%>@UYmy4Lk5;KrUV2jo97ZC74=gSK^}*>xOBd`8bOliFid4^;;e$ zzLGZ8>`{y^Ggjc^PVo@B%aqjCg2lj{aXCINUrsTzVhdU4PH`XI?n6%iiJ>!c6_$M| zp$|TjKF^#^Th8efRIX|ixG^6E8R^9|5{e}|{i#~k^bBk_9h9G^}5m$82 zhJB}}F%~z9>2y`cu9VBHdk0eqU2Q}*>z;I)Vw!UxUCva5=F|)1lIQ~Dn^+nhVZ@Hz zu*N6vdZ=^v9CYVy^4Ynf4UtPB$uq6V z^G^mg&KbQC8aNA(r=1EO@8ioMbDQy)L_#Ia#Hr53<<3(9rQbjZQ&{4xpA~pVFdI6m zEgwGYL1cP2u}a4hjFXZX6vfxkkhu;|08zP-4%X-h;m;J-v_y_c$BB@m8?P7!x|UOj z8X(cFr(+`po&|eml|UHEiXKbz6S%-=^vf%0hhQ=~CTB;s4GKwmj$y`+h58dFLH*ct zi+&60z%VAw^5e_}bq=St!>$)h5eVQ})8%wn8yJjdm3UIw7X}%eV{>_nnKPCQ!+jNb zG&~^VPFkXmOVlYb)q%#!*9-E2PW$rZjBUpfL(pSLP1^t{gv5n4GRwu-b{woaj+$`t zNa3bP%VzH|LB>sNW~8N7N{VZQlx|TYbPG5}AWpH}==>xp$*z-TNXkIBI%&I%9_WU0 zn=CL8T?S5t8PrP=b1)p1m-FL)5}hti9s=z&K$J$^%z!>hEky4XBUU#*&|83KoCd?`J8KomlrB{`5>A3K6_ zH@S6II?M-M+Gc}b%y5|xBHS`7)@n&K_fQk=+(vF!Xr@s6kTWq1&&=f$-drcBR3ttp zcooOju_}y*3gaE?whbseRyGF?W4;)Z_akTa=*=6L-b35^_bf0u>tB=# zW0C>w<5ywwpUF#5MX8RIThKzo(2TB6Gus)KJX0)pwucId1kg@cu11N{m8KG4=A8dRa`}eIv*PyRXZ*^?+M&?ww-lvG_k#9~{%aS4_QQOg)xf zth}*x_iy#MWEQvu=kER?BNxnXxVaVF-M^{jg1ph>{PiEV5%&X1nTqb8U30-St*KW` z_-V272UKK~Bgw;oyMF}PMH}fBoO?k`ePK);FS&Ux{1p(-<(=H4P5%W$xUUFAbV9bp zL&2Rop1?e-%lU?SAaR?I!{kb2d+VRr3dUhq2)r7<)~sYuxF(zgaw z5R=ZXQ9=EALRTT9&GCh5-7xKdIeKgisE4_vP6fdyrRFcnRZxF3D`-xoV;tu)wcaM) zTvY118JDEilg!S&)%pVS16s%2g+nDADSc&|*T-#+Gq>S5$-E1PNh)f1PYnazsf3(b(tPd>nq;;)r{?XoHgTP@Afd>Q*yXp=2 z_Z8D@v^H7hE7mc~{MNe8gBE8d&oQ5~UiO4kDxxUtfxsuJs(7REK>37j-fKK=sNk7= z0;ob#aXj~+2PeZkfWv$hXBb29aN*{d0Th>DCJixja?D>WQ1O3ZE?J2O$zOPgbT{XP zRHQlPU-8rmj}Si<=I`*7PHjst)4G^lGR-9`%+(t_R16$CnxSRpEn<@g-UCml_tU_5 z-viDxd!J+8gu@(_x@HOk-^Pdm*=9nz3Q985mzWy_oKtUYk2ep*-<)9Hm+;$;UYwK+ z^Qir`$74zS?JN~-uz8DhkH=l{_?nuEvDkdWJn9+fmT7n7uR-B!Ot0Y~4&GESZ?SK& z%}w^Bwz<>yP`vqY!qW*Vk#u|y^LkJ0>SlbJ+1Xu`io$y#tn7&jqeU8eU60};MWu6w>E<{3e|rW1tk%)&RIDPIW*|$g+vb!~wQgZw?yV;! znaOD?NHgt3vr9ku@;#-p#|6K)O~rfcNO5_j^lG&I%V=?8q`h~fG&NExjFbYAQca{Z zHd3mKlyV}al1S;HD7*3quq;yYM@os2Qj}e%2rSy#UTJUyuUDkhF;a^5h;oaTK9)aN zM5bF1>2jm|!65=>M}Q}zz-XTik>&$YO>Z+IP%Bc3!lKNhsz$x`QdIJhkn+0LLpAxKS$SG~q=T^) ze@M$pOS3xLovf70Ez3R|l?-7Yw&LxEG%MN8voh?UW!;fx`D|hJ+GusnvwCQ{DT;tR zhk;!$v^q&hynU&a4zzfC65mA$ibhSb1Du~`;m25rj@soSZp|EPr56EvtkuaOV9kv| z^_8N1X$1QQ!cGfD>fAdG$7J|pUs$CmO71dQ$uxj%zm-a|_CO~wn+r~0{Lm9J4 zd>-94SV^|tFU`1+WN`>zXl0^XT%FY^NW{~vL{tX>+c5eTTZzW(ykc3kjlM8GkwNc^ zxpx<;**EfMDMXnm$Y;h7VI>*Ue~L$6=5adlI3GN?Y8$5ui>b2Mb}78y$l ze0gh#dNxr%uPyb*AU%lLVhuWx48kti0GM&vVfZKAibn{+oD>_rVyhbo?aN}*B^keH zOU`4EHAJVEqkbCe3mScmC3%}+Qr~98?p74Gfch9Km6(jdPe7&kI4Pp$Z!Dh9aAFiIq=v6A$=<4WlSQ zSeb+_<)c0#%NmHAVehks@m*M1bT790akW@>U{pk>Wp`>rW`lJ)qgF;J43!k5*m)|; z4VLI6>ySvtLvI20O4c~cmaqoesbF=sl?Intc8+V8bURrV$Af{`?AYb7qk)k2DYmj5 z$)z<3Rhp*~=1C})Mj2}*8_N-8J^x$QCpkv4ItlAE_C_0PIcyF~`5~FdP#!f)9o!uf z2e{IjE|P3^oDoaOF?lqGIw_(^Tl+RxY-4n7$H?rgQpYQsPk||$qat{G79&E$XDf%ZT*cqJ&DO4TKl#bJpm(xJdsM!h&C0p_R ztdXOw`0*5QNu}J#J&cv5azJ>D+v*xOGIU|d(qbzBCc5pbbcUxDe<9CfNtieL6Mvg+ zOc5$}Sc#BfsE)5IlUbC}76yP8B8e=q&66~_K$KePxasx=o||-dhEd=`>oJm8(mGwG zbpz9qUU$)jtnyTh!3UBCi7aW!+zPwNiO@|rP9Lz!U=c!0L#J7OlGPKEh@sZdBAU68 z?VM^aEGfa>lT-1eljeMn*`%@VbBtG{*e)5AI+t3BST$XUd94^32qDaNt3>?_s_XbV z6y+GjfSV+fGn{mVIL~sl+sYP%8)WEWQX{*vB}n#3Q7d}f{YNI4<}3*iFDX{y*|NsI ztVAIBCLsg_o6WR~WMeU}4-I5!nt~A3ocrvHh^bu~Va@#WK%;?4NGG4=rq}q-+l68r zFqu><6)Q{>b#P@6Ejca@$YAi;(*+)xM%u<{u1puTE0dH++SxV+@qDhhBt$OR zh$Yxhxg_C2IDV{^07NGMEPF7Dc^;5ldCb@#HRz{C+YYHgB4!i&EGu!Wl`@I)>rjmR z6f2W2Wy&*e|UV3+ug15g)Za0&|=oeOsU1y~Zrp2MJgga5-#@q8% z%GMMfk<=mCH703`SUhd<7|BdSYp78pJWwapcR6i9|4sP z5jYQ?WsM}Mkw@>7CH#>sQ%th_w}Ib9JXMekWJ;PgWO)l{aBA?MZ()C6*%fK+w)VtU z3g2ddSh=d4cyQ=V7P%0RS4v~8)KX6HqJa%Ut7*#jSe5mXnIFGoOXCTPJU1)PQW6W{ z_X34HtNQ{c5nFv&>~d?c6hBM)X0$5Me@lUA$vXNh%QqHl;;pN*vYgQ}pTkc_?Tmb2|vF65%uwqtY5) zY2~+PlX@IIxa`hsPb0i>MhI}T)hUk`I+)TXZfJ)gdY>Azn;JUb0~1qmH;gGCLfNuk zr?U@)8x2&XE#sEej6l(DVNdD3?|16ndm=2i&$7Bk8fg_lKJZlnY#Z<3%_O9qWaAO0 zEg4!k$wwjgI&=_OX?S%8d9b2X7o z$xaXUkldV5QBK{F68obAopt>ULGQ6)7sqVfyDspZ7lLZEjkFe zoG`5(46Wm+7`LD75Q>s{A(jkgzGQhWaq9R8{7=mV@wyA!muKVq8uq@e5WyIM$&_R~ zr>%^SRnp-eEdnCi-WeETlgx!H-L>AWHMoXq_hefUE3$wxG?^{r&$ zaTpS%kYwuyNH#!xh6%zb7B;4EH14Gd!`QKU2i2h<4lj2&d>+LQtAp@X!XHZbav{@0 zO$hjc6*~L?4$KJHVHVp4b|7gWnMx$FVY2LN=sY~s2c4(7%d$qJYaW5NY21%YWw?W$8i;w zOV(g#Nzheo|B#KIW(~HD4X|fd;}e{j$x*Pt2{U+w8uMr-$8w6|*f@@e!`H1%(>88E zHy;4BMQOn7Kubgz(`W=-E+edXZf2^(BKe_)GsL+7zp5bgxS3Fb z_KPM?pu6D?S9}J%zL=VMwnFTJdxUjXTh@$rffb+YIO={2lO7HLs1L89gS>{ux~(rC zs5Y)@hX`%Hb-WigR4!UQOZv~*8`fyVBt7xLa|e~dAa8Bp36AW*%& zhKCFJARZ@3NpyC!mO-@hNW%gm@q9asHlt>Zm1cCxa?XIt_)LmcMe+7{Sb*-6*eRLC zR>Dx}l(7I~>?}vFre>Ty5Fif|{(Meusd;=(Cvg5u2dk3WIHL9$#OT6e@W~N!79ahg zJ7orW77jn|cRe?MYb5H}fzGC%PL-fGM-(mTFUFkW_?i~A?!xZJyTI>w2yz3~2$Yc9 z%1n~KC-vm5A_vI^Ua`Zv%#PMa8-dcv>Ts^}yu>zEyoa#wB!lQ&B2w(c$5j*`Np@!$ z9Oua3sFWBSOMkQm$Qm1>l#d&A*r<@q-PIb0MxCY+qN-!nn2gNgJZ-`m!|Bu)n$Wp1 zcqZN{^Ya9CeE)_Yv0+_}@7%&r0k!RS^6?vGG#QIs)YeV1XS#9nS8x{AxLs_cfndh> zzY17gP{!V6lJVWI^6X1#Ds44*JLkzxe{gAh>CotC8|#_I3DD?>GSaAV;#b8sFKE`H zDc$%&;_f1%>r5uvTNUijOf-$fwn?c1we^T>g}enA28q6#=&Ag!DMA|KSy>Kq|kjAmmD88#7{fcQE*Lc}akWF^h9l82zp*O{|B9qeM|K7wwv z3z*N)G4{E92ij0B@J60}E(hufTD-uwK4V3mC2rn_fHKyt#twsq9z89i83etz;N?|3 z%S|zs(=FQGAlIq8)R{ux8ZGMD9kh;jmqa{RbB4za%@+?jzMG2^1G2_)$R=9mXn& z1Mxm5o;M<7A99AV-ZA^N0N<_ zk4W-SHYzzn@Em{?dM#ohdB|oMey)zKFNyM z{{Wkwh3R$!f5HG1x^yI-WV{mL1LBc{fFANuA#>%1{sZC7ShqFu4)A++`I^yeuFPEUo(?~HE zd1Fd37p5621#aDqbh^QyFTnyZD2-gFAWtd2#Wz$S9*`UrtD`e6RJ@;4M~anlwv!i1 zEj&plhLl6;Arw|^NjA=dJ(h5uLEB)j8VqYH^h&)L&F7*Qo@0Q#v|S#S=f-km9Xf5{ zCAE*7D+l_eSc=0Bz40)`#)AU|htCi=U!+JJ_zE~R?cm6W?aS(-2m44qc>9O8nOI@V ziq}g%E9D1;I-J`R5|TLd;7}iHWg5p2l^Mnjm_p2{0vS~!F0WAzCZ6Rke8 z_;a;Gw@;KV?Xic_8QSx-E0228RV8kdRMVO2cPj!U5`-~M(gQ9S7}VKV z2HGU8dh~#ZmGYcprb_HChnR6pvbSd1Miaq|&sS7hGf{?hP~K|uVPqIh>BgrLcRekN zuVu(5j_{Q>0~;B5R6)CrAs>6uh*2sTlg|>C)GVwHRcf`Nn(Feh+KTc;OQ_7Zg)1xO zgtR)ej~s!*{3UZ1S60^IM~9ZwR4mn&md&YJSR0Z(m#okh)-Ip7aQ;FEP{wyq(rW1k z6_+ipnOCcoFJ7`@2B>P)70W|f)#B>;0sa67t#;x3>WX=R@&#ozS{40#eY7Q16IxuQ z&6zW|ZedkuVf7rB^PDA>bC%SF$~ERYad92U;Ky3aAq;_uQfP7Ak|h;2n*3Z?TV5Ti zsnTj4rRVT{Ze1mWUs6|H9)g5Khdcqo&H9cNdS!H2Lfb68NeqBc|p`3Zr{SW{6xwQQcYxN6=6)+@9) zR92-`RL`T|sby|86_pFCs^FXQ%Mcl`BgK_2CIbzOPH>+>C6__;NZ zt(Zr$)|A5n^D3Z8-BQRme~xQj5Gku#vY-ri8JtUNtD_M=uWpG}O`6P^Q&wABQ4^X| zS+)@BEnng&C$*ZhuzFs_a;+@17{4RzI+nw$nt}xUt9iVzok{yeW%C!7Yp@F`QC39` zTZG_QT&~qr%%4+RwzNXJyka>5CJ3M5|2#N&!mNpt3TMulQFKw^46S@YUG-)7L=;yd zHrW>oYZorAmUdrQ%>qXhEt?B3YjZ0q7uQtqD8N8w2ZhQmtBBfztx&5~BEsExs^tRw z9)d2ebvTUWSjCW{zqmeHvBl!Y@CxjnS zMwGJ==5kn`J7rd2VopvVXGGSptn9#$MI%SxqYSPbyL|M>AzZ6oJY;nC@X@2tfa0P> z%Vhz+iWG*)QccCYAuAVG6TEy8yizr`mP{TPGCwfnJmA!Z=8Z)df!~lyQXUFa&s&HD zG^7%gIp~KhsaYJVC=V^J0Z>B97gUsAHnw`P#F|<(k$O|2fE$J&S<<~0uz6*nvPd0q zxoiqTlkQ6pT28u}6DpgJgfh20t6UqpVDX}gq4UbB7gkgaSx~kJLtyBSPV`owAQ1qbx|5trNix*cR#uio&MR=A~F}wNY+PDjhU* zVRd;`9p^+=!O;YEomWwrg;W!QIYUFxS5}bMP{`w)J!E0^+4!NNi4(^J2Ey%w9G}y- z-|0%1H%+X>pYvRvBDy8U`S9`(|Ei(;2BG_=?FityZ&mW!w)x2^SL?wfyWo1G$((#0 ze((dkiG)29{xRYC2`}XD1*u8+0VSs8zOCuLm74yab!)z)F~cw8wG>=DyWSc-J{F_sw2{-R{83l)jHRF!v2p!rSM- z+_!28h8Ga|3dkGV@0bH~-}WUu(|PYf?VHgKY;ZfU@eXXT!egcbbKd|a9t#|p`xY?4 zu4sqe1_!3#JGxc;#lh!bAqP&~IoZ49IU z!HIWB@cJY^aB+;fv?gAMBUZgAVNc;dxO;I2wqLe`>&8ew^eb>+*+himLt&Dg-(XN%iPIK9 zS=s@)0c}KUfz#%`g>wmDS2!?!^O{KdFs8bW1zrIE zgAOjgh0VB9_e=Ngq12`I15TBLL%bvQk#SPwk|Vqu9Xx(xJHq#RBiLjXUS}0FxHcp* zu@?<*Q#;-o3#Yn4?{CLk6hFEGf81&BfxD2ug~|}}p?fJmH{yc$ZfKFWvm$V^qprZ! zztUjHpWBV#x&=c^aEqS1p}CRm0YfkTeO)gQHABNMSGIE*ZQjk7NCuCXwjB&DwJi`3 zLvyp4x7pCrya1IcH{mv-Z2z z-m_=#J#!}K99PZlObc_bz=u@xJgyxb^X!+H8OQFN$bXjuDER)#ihIwAx;T<-<|Myz zUSnTQWV4g4cjZI}Yz?@T6QOi9Q%-~#Va=2i^{5F0$%%TU@ZKr>;1u2`h4)S2Hf!5@ zcTR+(Q#0j6c{O1mIZ^)>!RBkj2whLxNs+n%@P$yWv{!+K5>k8mCbKD-7?>_?{XQ&ae;6kbq+ zgM`n{tNC=Ujg5JBJhE$*i80TPN1JIsHRjuMEBvM`*UpW3c095kZFbCUwi~V=Xn$$U zvyI~j@&z%^jz<~tMKRBgM-}9^#XLJ6*_m)p%>GzCzc096rI4tJb@yPg*G0%=i_AN=r#ymTYlrqj?G0%=8b{+G#n0KizS1#8I zW8STr+qWo^HcV#{7V4dpkoW#ymTYtf56k%(LT&U6ag+d5>zH`dqsp=Gk#1 zhy0S5XU7q%pUY#O9WOGpzdGjG@uC6w4KdG-3-+x{H^w~M|M#T*oiWe$|F(v5G>@|V z|9ILz5c6z5Z%63im=B^=xc1=MlQGZs=O>ZB82C=niPiS?Y>oGZ;@gk@KBe57mHXLp z13i0xcdOf>*?aZ8#m_sEcJC21`RoF>*Kz+k*-CEBCbzTWJGJ4qHufeTP=wDs;+vYd ze;jChA35Gz{Yi*^DiZe8xS?z}cW@k6!w9=cZXZ?Hy?EX+4nA?4 zI=*4c+pTsVHU5|+zAGJ!@DWM82zO_Bk3GJh&AZ^qeRzIa;%%tfOt&akeuC=a;p)uo_*Dw~#PmDZeeIH92&VU~lIbw(a~eZX?bH>k-$}-IOdbXKGSIeU8AJx&M@qwx&-Ynb40?UV2ka1@fwa6NC8 z?5<&-N#Pw*xXoj{zReUo&r8v<5!>4jNwFW{cn*(8q2t%Heo7s;`YBK0(^Gh53ctj0 z^E=;h^SdyGFHYh2r0}IF{D~A^<#;#N+bYK`&NV6gJ;zs2XPx7wv%&FYJT4m@H~TFq zJZ`XIpS6?f_75PuJ@uP8Zu+fKcqWB+Na49Dymt!EOX1cpVR!HFvN(sN*pEozg^t^K zQR?`coQ&9bjos>Zy0f=+x#H{@%E?|6GWL1lbwkW#g1xfqC8# z#vx(+>L0HqZ~JoHbByUo|1X`q*dlrB%5~`Eg_qqq8R&Q`ZlAy<^v?<}i(lhBnl@N$ z&vob@6J9frF(kV$0Df#tk~f=I8@KV@?v#M%hu2-~PqZEei%u7=!ybO;^7z@@!#M6E zA=%u+xcRgCG=6qsUVSIe_!T_fr*gmXOSoO^cs~2*QpZnZ{Y-P*+W#!aKV!M(IzF8I z3dh@W`&P$qXZ-g&elNG5cf64Om5oPM-cGdpz}b&vxz;-#z8f!2-#cE$`uW{)J5Jj% zpZOim`0f4hVvXNeYp?zLP zzYA!O#gVkd;(n*|Fw3j`s$M^?boOu2{x#7bDEjX@`|wC{oYsr|0I~nh+1KOo?La?P z&o(a~;P@ZZ8bS`a{-XakXJ4Q7Qy}*J#D1Ey-vt}4-0nwSDjdgUBz3M8 z4jt=97CCPH=`F&cW9{!=;m{ctUPuo*J|w}>Bf_C`C*%32aOg~;{YuBrAP?Va9w*eZ z)|<5p9IyQFTEKpFz0eE`ESD2Z)29v;@q3|EmF8WpMv;N z4@+r(5bcdGAwSgdD)N4guOJ`d{B|a{c42WEpXPL4r~TQEuO^@E_$TCZ9LMJcNY{~@ z-#!VB7N_v=&E9dcdiaI*RgP~af5~y{XIDFp&k>M5BDZ+@Cph{dh1X?2Ve#xjod%9u zzq+I2_MXjlEIU+B0!6F1)J>_Dx_ zVxJ$_`8>bQ7Y_YG>d$t36!~S2my^$Tyo~%>;p+Ex;i!kRcpg4Tj_fuU4Xe>=u}7Tq zsQ-rJHlBUpbZq{&UhEOiB-XJFNXYOeU*h;KeH#UAxpz~lIaaKzJ#I&VAPgM6*yN0EQ#_z~n^3Rk~B zkz1Vh2~iuyh4P|4H-#5c2XfdS5|iXDU+fWo5&a(L_-W*$9G^)(+VSb+vmC#P{9?y% zAfHEWarS4dkBdFZwFA$SDzUdQ0a-p1dz8!O8DBVV^Af#Z3VoZ${_N~mG0xu{f1bQC zFWjuX{hNFT$3G$O>i8ez2RLr?hF;{TAC%YTW240$@&AW96U83a>6We(d(_WP9Jj7^ zyb<|A;aWd;2!~F8+W+10KIF?Bf0F!Z$Il>t+3{a_++TD2bMlWJxAXH;$B*av^M!Dn zU;U}`t8kR}AE@w<@Qf#tonLnS&|Em`;f6-BS+tAemyz!-9QyaNy!#4=&Ia0da{L|g zZo;8s*Kd7Sa(o~12ORH5zSQx<$?
C1Rt6b`>9(*704k0XDJ9L*2sZx!3+TCs=z@znoXIP^=Y^S$F^$$xSDZ1Ue7 zpH5!4Q4kHu;<=RZv?sUnuH<>tOE}`WoH~a%K9@XCILc-7q(Q>4+?xO?dLfD0r`A#tG5j6*ZK*ox7O6Z+v#kgPWVNc zIKh5Uc>PHJsN)TqCU(mluSdRu+~V29csdA&zMby}3WxqE>hyAaB>7Q}Pa_}f z_!ROX!t;avr;KN`aQK}=`>~GCCZ9xZ$0d))Wro=22c1pSzeqUrZ>G*%$8RKG;P^x2 z*E#+m`OV}OPb0?jgxKc?zYp>_j^B1;hRMV@mJCBua2)Eugmt1-LU_Fyn*BI zklVOn@fWf^x2C=4-JQ-))al{)59EhA9=`TEPW>IvArJpv6eq-=A6^}K98VRFa_vI< zLdTnvmk2*5*cXKtQn_&G96bpg`HPNEA%Dg33i3CFqgd;P`CvAIPm-d2Hvui#_zuqW*Sl z7mWjh{%zE0;`puPyONv!IO_Ltypnui3P089KT7>V#~&si@A$vSCp!Kd`E+uN^WQv= zW{ExOVF>Ht3gM`S_o%bL@wdpY7p^*sg+u2T+TZE;59ALBhtB1!H|sy_xU}Mps%OO> z`Yp&`A-D4G&hox3_RznT`kxAiPB-d&?symS?;RgN{-fiElmAI>@#F;`Na42~g?zE2=ZLVCy*cH`03<_kyrcWy!~jgNBxXt{frQf`ZSrx= zUUU2d@^>BoFZqX#|3K3Dsm9C7jzBCLhaZJ5v6&mMdbudU-P$oCZv{WDnJ z?!r;7eA@SLJdeDOaOl|l@Cf11nMM0Sj-O3_oN(yab-J}LlndOhyKfVF=&z*CU5?*R zexGpY{D=L+;Gfn zu)l@&pE`a8`4_^W)0H|u2#3x_+HZDzJ-M|z#GfBt?}ZmqQ`VoAs~aCUZsz!Y3FKTJ5{98R6yjt?V0TsU;vQ|AQX(3wd4lN>K0A4P6)R#K-_?4h4W zK0`S4ucFR*j$cWBp>XKTqt120q4NOkZ*Y7u`5ojIXBBlG6np63L%u>d^gp4_vyOjE zzS`-JgQJk%6?^D6jcF8pDIEG6sq>BF-;-~0{4eq?j{iwsm+jHYYxk?0k~a=M5zkP@ zvxjiRvkN~U$vD0Pc}K?&C-35TFY;dG7EeRgPd~9oJWn&86NMw5)2TDu@lx^%;W&SP zXTNl{kr&4xR7|S8+mi<;}?LxO8JY z8R4jhN|twT$7hmvaeO{`cgL?F?VqGQHObM3O`G9Q2*z$9?lc{?0-jv z?*MT2Jz3tnQ|yuMh_Rk7O{oKcLz2NxW(M{)^*V$bWZyEqN0z zP5*SBkUKg)ihM7}?;~&T_)X*;gy)Aiw`05QE*#~}#Q_beuW*#>YwF}V{snnHIkMyU z@-{*=Ozh!zBinzWaOl@-8Qww~F>*W_*3km z{~qhV5$m;aV9;;6OOj!G$Lo=|5RUr4CNYopB8T6uWLO+8yo>P3PUj4^!ne4c#UAbXcDCmN;n2@%9o|AJcKj>q zPjLEE6Z2?V3cteXTul81j?W@rJ8<+A-s@E#2)e3bGqfi5ziCUnd10A$Y(nJOvrjT);~U6daQc@(JfsiB9`!Sh`d^6sKCxx;_NTMA z^Q$5IGiz7Pc2De^Iv$blBpiO9=D4$maFpvD+GiXeN}lWZ$^62HgB(AW{9xhmYyV#{ zP&oWvNBg54zl?l@<1dh(>iF~IMZ$3$C$qe#3)l8}F*({Z>ffH@oiFy#vH!2RIfdUX z9Oe3i@!aS5|ByfG_?~S-rjV97-kiM3@dEPa9Y2A5wd1#uzv1|eft5U=O*DO?<>^V;`oc?e+pNf2JDX;2My?aN&Ci*e@ecy<2&yeGKI9OB*hAm0yYCVX{ky1huj99pFA=Uf%Y;K`744sP{8{qn9sikpmE#-9-ypYg zjb{DWyukBcoK8!A!I6#sjYEuxb7%5K?7xlgL%zM^?a6m0H^0BIKkwnVJwJT7=%C(q zrM->k=C>#Hj~9Eyc>wt^;i_|*aM+(n`y$7WAwS*mndD`TPa!``IQ({G{FTDt_kP-6 z==fdaHXpHiv*!eC-eJ5`&_a4xbl`V;9`}{P;rCtY|I6{0$X_Ej{R)VO^q$y5zc=;Q z3x|G-On3{)#`(r!9lTj4yoL0W(_fI_=+6}1n&YbJbj~Ch+Bn{cd|$^8C-3C=!Q=-z zKAyan<3;5C$gO_te#>C7NB!ipeohgNdYDI@0>|f)j}fjq6NN+P7TTZb_(Jlt9bZL0 z)A1_uIl|%B{x4v@aQOX}_SZPRfqW6U)lX;k|BpG|jPk_@=Naq&IKGI-;d63?&8m!* z8gt%c^=#LVErp}K9n9m^+VOqJ_Y|(zO&x{D=LPM%I6j~J0O8QF@*XN2I?vI*pW{!G zpWyhP})1Nlhd@H?w+cnhgSIQ;f(ALD4e0@%r?8Jvolo z-igcj{b^^vmit~1dxjG4`P$i^O8w1Z&nk%RZT@ThPcg+>bADvJfP63EDDSt7rxQ8i zZ%c;7an2rfiZnv(Gcift&Jugro=3muI6i~?0^x|K0RjkVp6GKm=J$y`^slGR634G5 zUzVb?GKGKWbRMG4CyqZz{*}`~eIxxO_W5D^PnPSy!V!NJb?RoqMo4zNmXkNh#5n$c z7GyR3mMOd&x#@gJodX?zpZrj#!_DZZ6n>i1`IfIk{>a3O>iYSe`VTq&3;E;1p^xuPMS3O^FXH;-P!@x=J=#?d#yP|B zF637^K9c-e$A^>OLSD!Dy+`cx!@e;r?=sQ@7CwhS#@$;jVuogw1i#_yr;LY-{9B)iskLNdz7vf*W`q@c1bUM(!rQ_|$cM}es zm#Nc1ICPGrz0DWk7rZ}tA7?*`{4mEyk{>A?^|P6Nhmd2FA0dXd@O=zo55Lo>GuiP9 z@=C|AC%@3~dE^Tme~tV)$5)Zx>G-$g_c*?Re2H+B_u0BARrtJ!9OZ2hljQAPu}699 z@c{@M$E^PCCo#Sed+6I_`DfwK*^@fIIo`@1uweUzK6I+Tr->YK+GmMjHR>q#&_9$q z*1uYu2a_Kr_Rv2OA|V|u`ppv*zZW=r`<|d{g~RX3)L-QI3FNn@=szI#`9bGSw!>w@ zp+A*6Pdk1ld6jV0c}X~QE~mZqLsma?$=?=x)DOOA9_bTuZ1PdQnE&kTt$wy=yEFX< zslSuscaZPt_?P6?Pngc9oad-{g1!`C`XUA%D>E zv&kQJd>Z)*$FC)S*72*zt>3bGuvt z65985{6g~MgrmIn`QQlAM;Ra=wHDe~QgL+4cLbPzr$@U^t>?D#w6 z2a+Q@e198WNGFRu;@_`cjH4p4ZXpw)7r0<>o5Af%GvKh`)eIE zeaP>2{2=m29Y2kHnd2vsKkxVj$9F3 z2Zs85lsb(Ze~5fX;n1<~yW2xJbY7)>#_^ZQ?fh#TY!T<{;f2(l_6V>wMF;lQ z&zvZHP~dB+Kiu&T$V(jmnS8wCKax*#d}ls{c(&u)lh1LyC;2?byOJ+-d=&Xjjt?il z&+%F0OB|m`zTEK}$g3Q`j{IN3wf(FSo*(MD1>4W3!UqNZ7$2R4 zhadIFI&H`shb@$ABJJBcK90Ooiq1h|pC5GWdwT~6hyLZ%Im+>Q_NyM{a^g`eqk zZlcaq$8R976dita9HI*MYlZWpiI_j-bRMM63dipyxA`pUxnHc1y!GI^=O?A`@)SNd zg)d6scM8wrIu;Le-QwZpXS6bfuW_8kjn<{`Z-nQCb*q;x!cjkcxNZNJY4I4}6(A(z z{gcgTzZ7oIp<{Pmc-gr(KCw8Bu@McYt{ib8~og8n{AhEOWO)`7?9?7B3-uQUO ztF}vY&Pm}{IQ}f_@gB!tBY)O$`<};7Qut=aGn`b~_X1hDjPKTT>-ssI_0ZGtVdMoV z{4B@!VLe=u!f$arkM>Jb_)5p^d%a#2-YXP^GSP>^(az0&ljDVK-woIiS^XH_)p7gY zp#vTNFLj1G-j?mV*zrmpm#HcI632I8d$`?k`#vcDeJ}QXO)ofm#A%>+3mGFJZjz37g&haYp9~>XYc8KTzLz5}g`eTL&Cf0nZgX0MbX5w!Nw|&4u)kmUfdEXe2=8t%EWb$MbvUs^xzv77 z;klx7L<+wsh5za}Pxt5ye#i&E&^La63jfCOV#Z|uCu=$!&Z49Ep(%Jb+G2583V$?( zf9yDm6Kya54+#Bz>0?jFhmv=5{08zo$Dbj$|IaXS=f@V2{+}Iq zM~H-E|JP>vcI*dp!f$+jVji94`2FNpI=+;=9xt{{$MokoK9%-&3g4d$i)T{!M~-v2 ziR$i_WQJepu)3o5!aF4<@%wPct?#?kaTYVW$MH&PJ?Z!jz@S7q3&e@M4-^o3X zO&p!;csuSG!H1SmUL5lUC_pT!V86K{hTix$IGrKA8_2BPkzsFJ2zTpwvN9d*^Ih4 zZtX2Eg&!+i<2+qB;#?G7NS9^eMSL9xZvUThYYKlrxaz+o96H@OzvE!z*$nhVkzufV^$iH^HEz8@e-PYx@{}*WOxY_p+ zPU%=+Q3`+Fab8wOZwS}p@?YU~gRCf;adz`#;2jX_W0HdbTDDUSC|vPV-L zCyuU3;g1W~=NQ*G-i$(DI*z#-(vCQ(LPB|MjKy}x6n>QBmZ!k+&JYRdEa9Aj#e883 zf8Fu(W9x7q-SN5P^*Lcf{G3|Gd-h7)jw!Ys<7tk^;^FOz6uw0GPG%gI z8ywGYpFRJ8_|?8EFZ4V=TDTosNEN2=b5i(qDg064yi|=N{o3)In1*`lkgOn1o4bJ= zn!?8mZ<1t<->0YWxhdQ}zlL8scOmqQ*em}rg?He|>2*e?aQl4I?91Zl!~bDBd*jcf z@J}4SnmYA5B`eTxM21D16n;nwx6k`b$Bx%5XJ4N_ZghM&`C}=3wd3G)5!kcj&iz==| zU;E)T!mW;h)(O{fWTS8$kK&36`=GDmU^9S_u&}Wd+Zo|HzUKjM9l5~q`}!G$2=qU?7Cu}E1X?I%Q_ zz28+SJXh@J3-2y`vG4IO*glm7Rzq_sdJN6@?n~HxBe;Q%@hQ%J7shdi%+O+kG?pooKVK z?lI+DX~sB?b9@VViE!<2X9`ChXTl5VYRAVUIJ!eP z`djP!o)ND7=lhQDOPwEtqm9^Hq+z|p0ObWAnQTUFglqqqD_r}}0m9Kw=26GKM+f>b zXZ@V(?CtZ%D;&>Z+>0Hz&taE3-i`M5+=2PMg?zKKH@ofXCjp!NAKN9{`g^{J-+rI6 zle1qz{SzFw&ykBAx6iw$JD$h*E^&MWFB)!k{BGK>a(o~sv-Z2UR<4~`uFcMVPxAWA zXZH5I%l?l4#r%glzJNMI9A8XcC>-M;H=}al7zg1WsZuz`LE~2o?-yGpZ+8o4Q;T;z zES#$`UoKp)%U%$Uam3Oa!maPbzK?|4xdm>&hl{v%p1)D-ZS01<{q8O7bsVY3`UBT_ zzWwekxQz*6HEJdHI_|U+uH#N;;X2RnDO~6I_B;#x>O6n2*z5Rpl5m~p7YJ{Kz?sGi zw=o&JCJWbb?L6T+&z~b)=lNF&*LnVp!gZd1mvEiuKO{WE*s*w0cw6Dm32!HSwea@B z-xa>MaQnSo)c-!hzZH8OcYYRbV>`0dVWWqR&hwiH*LlAE-Y)EI&IKKN-U?jj`S!cI z;5ttA5Ph4oKqpVQjdkGp!gZcMOt_9yqlD`?RVutEgqbRYqo22Qo^YM#&k?Tk{HuiP zJpV@FI?umLxX$w*65h{(2}^4;=zES0FY~=GZH!Oiw$-ii=g6&2jK557Wi-%9~!mBhSE#xL!kwSPlQmhAVAp`-ot zM^ew)KX;%g?6rS>Lb&$NZRia4+CP^G*Z#Q&gMhvE&o2qr{&^mQgT408_WPXR+CLx4 zpux3&*5}`}f1bv|z+U_3hAa%Y_Rk%KYyWJ&KL&g4pRW;n?Vq0#uKn|e!nJ??L%8-Tx7;{c~#`JowfAd4O>3pNH{az`i4dnSK`D z$zWLi&VvMd?Vpe2K?2wQ`5Yc3aP6OO6t4aA)55!jJeCIW;6W$XU|8PBg9P4P_%h+z zKff(p`{&j?sL-*#2y$h@wST@$cyIB$kR2a%tnb3^cZFLY3BJdEcu(e$eUsJreV%ab zpFb6DeIPP)?T}=K-+swz{GQ=D`Zn#~4ic{Y+XUg-zf}s?{_Ps!+P^&}T>H0G!nJ?< zNVxWI8-;8CRu2UY31#9b7;m={&Qm1jorP=vmM2{Mw_(Dyf3xR$;8**%bH!f!H@i;@ zd+p!s`ys%!f3y27;5-$=TKIoQ(bxWMop9~nHVN1Mts#U%!h)wlyuF8T?cZ{RYyUP- zxb|-&glA&qKq-^zt+ z|F&GX_HS#1YyY-hxb|;bglqrSv|cuT?cXxOwSPNExb|;DglqpcLAdsBmBO`uTPIxm zH+$$A^{o9{hVvqD?ca_OuKioNaP8j~3h$RxN%G!x@{Ea7ibh9WCr_zJmZi2frDXb) zsOy-bDMeA&(UT`fT`Ni^6m@N;`h?)bjF@tV*b4*($FXUzSxVpITO&-I~6&i{i3SRn;!W zRTPz%a%;*YbuncUT}b^GO_(sLSdU03*O;l{7-Vh6R+NO0v)jcr@>G-%#WhegIy9zu zZ9;anXv(DVq`^aoL=|H))rv5#WJ*cdG#ZyoM_sb%6^))$F{PGP7jsjeF?mW!sBmVP zHn}XW$+5L$tKP?0e3q?3q|8<e8jMVp@T*q zF*M0jQc+PhiQ$ha89jAeIO)d*kgOMt9$irqjzfIp6BOFYgz=MOO>`Ett0*cPR{}p- zR#7&trs0I~WuYM@nc~xjq5&7g+r^V6PAoyIkCjSF3&vIyO)Sy=?EkoqF&;y{&gCGL zhqqc1rw7%0FeZa>7^>!Z)oWX?`23ibV!rr)&5yASnrZPb5BoG_bR(=|OvccZ7hal{ zzL9Jd5*CLg>}+mpbF5r;vc=`Cx4Xif*l^n09Cort^9c`za1x(AJ-GU{PzY{Tm0+Tpaz8fnlic(*8dS+wdK!6Wrx%@SyaF4 zwFTgF;ZR{lp* z@@M`(^RGI?K;~Ekg*jm@jZ&QjejFJ{%t{UA)z1GFTDKt z*CoZj{cd?FGE4IH*CECKkhhXdxy%dy9mC7}KRm^M)lNx9`#vYi)Hwh8r1-C-|D%}^ z{`U_r@BjD||LfTBEdDhA`6>Qa)lz=k=kfmWdpWhWpJu#ycs7q;nt%K*L2bu>3;o;t z4dusuBkv#o2UnZ_`7IKUW}oJNMoRw0%+GGrve!6%7pCN|V*WMNIg^!)+rBa-|9p1b zH*cfe>=&d^UoduS$!IRD8+vs{pVDBPFB2s+`p{t z_}TA>Zma)!k@*jBehTQ{`?Kn_<)kq#>R9pGy zGk$+v=>7keQvU0hUq72bQLNmy#PfHx`CrWZCvhHV)sV3jE_V+3Yb*a^mcNR2IDc`! z)0e*s{fA;~m9}J(O?$r2c3OVh?vUcY@SVgWmlX*AdEsUGt^E$9{~Gn&KFP25hCzJ& zAD-fW{zplE^8^3=!^`_0k>cOJ$9-TTQ~mD!4^8oZPc8nT<^7+L;(u}b#O3&E&&i7S zKQ_gG75yL0{Ka8;WO#Z1(^LF6+dIjKF;|oKKRYFV9`o;^nZvrze--ol{^L~U*Uy$9 zW#^48A5HOJ$^0j6!~c?yzh1D(b9N0sPCy}faaaxxFJFHzhWub*u~k|}|GvPPt*oNx zoo(btY?}P`7u(41>)UVtyp8;M>O8Dx+;;sC5RQfKKdk(D&OYO|cMJJztG~=XiJ>pA z-|j^JPByaq&tuXi$=Z#feL{X{<{99!l*fPZsR<|!i=pA=kAKe;|Lcb&`I@*1fcHNr zCI5feaG&d(=-9i@kMFUrE&hgm5=;9XMZ`ZmynOtlQv5$MEAdzu+J`3Z|8FV&7oL<@ zZb5J%!T*Tx^8PDQ{5QWCxx({b&c=l~DgIx% zBJuES2lJFHefejl_@6#Dv0TjK5C3RN-v8C~pS!i2=<;O=u;;sMxB0i_Eh+hbU!LUn z-7<1%`E0o)CI8R=Ofu**2q2cvmdlynpa0EPB>A6k?cd6u>Dpfqrs@iS|+ Q<=?`8ka>aeo4(Kgzs*Ov@&Et; diff --git a/programs/src/bluetooth/Makefile b/programs/src/bluetooth/Makefile new file mode 100644 index 0000000..33ce747 --- /dev/null +++ b/programs/src/bluetooth/Makefile @@ -0,0 +1,86 @@ +# Makefile for bluetooth (standalone Bluetooth Manager) on MontaukOS +# Copyright (c) 2026 Daniel Hammer + +MAKEFLAGS += -rR +.SUFFIXES: + +# ---- Toolchain ---- + +TOOLCHAIN_PREFIX := $(shell cd ../../.. && pwd)/toolchain/local/bin/x86_64-elf- +ifneq ($(wildcard $(TOOLCHAIN_PREFIX)gcc),) + CXX := $(TOOLCHAIN_PREFIX)g++ +else + CXX := g++ +endif + +# ---- Paths ---- + +PROG_INC := ../../include +LINK_LD := ../../link.ld +BINDIR := ../../bin +OBJDIR := obj +LIBDIR := ../../lib + +# ---- Compiler flags ---- + +CXXFLAGS := \ + -std=gnu++20 \ + -g -O2 -pipe \ + -Wall \ + -Wextra \ + -Wno-unused-parameter \ + -Wno-unused-function \ + -nostdinc \ + -ffreestanding \ + -fno-stack-protector \ + -fno-stack-check \ + -fno-PIC \ + -fno-rtti \ + -fno-exceptions \ + -ffunction-sections \ + -fdata-sections \ + -m64 \ + -march=x86-64 \ + -msse \ + -msse2 \ + -mno-red-zone \ + -mcmodel=small \ + -I $(PROG_INC) \ + -isystem $(PROG_INC)/libc \ + -isystem ../../../kernel/freestnd-c-hdrs/x86_64/include \ + -isystem ../../../kernel/freestnd-cxx-hdrs/x86_64/include + +# ---- Linker flags ---- + +LDFLAGS := \ + -nostdlib \ + -static \ + -Wl,--build-id=none \ + -Wl,--gc-sections \ + -Wl,-m,elf_x86_64 \ + -z max-page-size=0x1000 \ + -T $(LINK_LD) + +# ---- Source files ---- + +SRCS := main.cpp stb_truetype_impl.cpp +OBJS := $(addprefix $(OBJDIR)/,$(SRCS:.cpp=.o)) + +# ---- Target ---- + +TARGET := $(BINDIR)/apps/bluetooth/bluetooth.elf + +.PHONY: all clean + +all: $(TARGET) + +$(TARGET): $(OBJS) $(LINK_LD) Makefile + mkdir -p $(BINDIR)/apps/bluetooth + $(CXX) $(CXXFLAGS) $(LDFLAGS) $(OBJS) $(LIBDIR)/libc/liblibc.a -o $@ + +$(OBJDIR)/%.o: %.cpp Makefile + mkdir -p $(OBJDIR) + $(CXX) $(CXXFLAGS) -c $< -o $@ + +clean: + rm -rf $(OBJDIR) $(TARGET) diff --git a/programs/src/bluetooth/main.cpp b/programs/src/bluetooth/main.cpp new file mode 100644 index 0000000..ad14c7d --- /dev/null +++ b/programs/src/bluetooth/main.cpp @@ -0,0 +1,670 @@ +/* + * main.cpp + * MontaukOS Bluetooth Manager + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include + +extern "C" { +#include +#include +} + +using namespace gui; + +// ============================================================================ +// Constants +// ============================================================================ + +static constexpr int WIN_W = 360; +static constexpr int WIN_H = 420; +static constexpr int FONT_SIZE = 18; +static constexpr int FONT_SIZE_SM = 14; + +static constexpr int ROW_H = 56; +static constexpr int TAB_H = 36; +static constexpr int PAD = 16; +static constexpr int BTN_W = 90; +static constexpr int BTN_H = 28; +static constexpr int BTN_RAD = 6; +static constexpr int STATUS_H = 28; + +static constexpr int MAX_SCAN = 16; +static constexpr int MAX_CONNECTED = 8; + +static constexpr Color BG_COLOR = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color TEXT_COLOR = Color::from_rgb(0x33, 0x33, 0x33); +static constexpr Color DIM_TEXT = Color::from_rgb(0x66, 0x66, 0x66); +static constexpr Color ACCENT = Color::from_rgb(0x36, 0x7B, 0xF0); +static constexpr Color ROW_HOVER = Color::from_rgb(0xF0, 0xF4, 0xFF); +static constexpr Color BORDER = Color::from_rgb(0xCC, 0xCC, 0xCC); +static constexpr Color WHITE = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color GREEN = Color::from_rgb(0x2E, 0xA0, 0x43); +static constexpr Color RED = Color::from_rgb(0xCC, 0x33, 0x33); +static constexpr Color TAB_BG = Color::from_rgb(0xF5, 0xF5, 0xF5); +static constexpr Color TAB_ACTIVE = Color::from_rgb(0x36, 0x7B, 0xF0); +static constexpr Color TAB_INACTIVE = Color::from_rgb(0x66, 0x66, 0x66); + +// ============================================================================ +// State +// ============================================================================ + +enum Tab { TAB_DEVICES = 0, TAB_SCAN = 1 }; + +static TrueTypeFont* g_font = nullptr; +static int g_win_w = WIN_W; +static int g_win_h = WIN_H; + +static Tab g_tab = TAB_DEVICES; +static int g_hover_row = -1; + +// Adapter info +static Montauk::BtAdapterInfo g_adapter; +static bool g_adapter_ok = false; + +// Connected/paired devices +static Montauk::BtDevInfo g_devices[MAX_CONNECTED]; +static char g_device_names[MAX_CONNECTED][64]; +static int g_device_count = 0; + +// Scan results +static Montauk::BtScanResult g_scan[MAX_SCAN]; +static int g_scan_count = 0; +static bool g_scanning = false; + +// Scroll +static int g_scroll = 0; + +// Status message +static char g_status[80]; +static uint64_t g_status_time = 0; + +// ============================================================================ +// Pixel helpers +// ============================================================================ + +static void px_fill(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, Color c) { + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x, y0 = y < 0 ? 0 : y; + int x1 = x + w > bw ? bw : x + w; + int y1 = y + h > bh ? bh : y + h; + for (int row = y0; row < y1; row++) + for (int col = x0; col < x1; col++) + px[row * bw + col] = v; +} + +static void px_fill_rounded(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int row = 0; row < h; row++) { + int dy = y + row; + if (dy < 0 || dy >= bh) continue; + for (int col = 0; col < w; col++) { + int dx = x + col; + if (dx < 0 || dx >= bw) continue; + bool skip = false; + int cx2, cy2; + if (col < r && row < r) { cx2 = r - col - 1; cy2 = r - row - 1; if (cx2*cx2 + cy2*cy2 >= r*r) skip = true; } + else if (col >= w - r && row < r) { cx2 = col - (w - r); cy2 = r - row - 1; if (cx2*cx2 + cy2*cy2 >= r*r) skip = true; } + else if (col < r && row >= h - r) { cx2 = r - col - 1; cy2 = row - (h - r); if (cx2*cx2 + cy2*cy2 >= r*r) skip = true; } + else if (col >= w - r && row >= h - r) { cx2 = col - (w - r); cy2 = row - (h - r); if (cx2*cx2 + cy2*cy2 >= r*r) skip = true; } + if (!skip) px[dy * bw + dx] = v; + } + } +} + +static void px_hline(uint32_t* px, int bw, int bh, int x, int y, int w, Color c) { + if (y < 0 || y >= bh) return; + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x; + int x1 = x + w > bw ? bw : x + w; + for (int col = x0; col < x1; col++) + px[y * bw + col] = v; +} + +static void px_circle(uint32_t* px, int bw, int bh, + int cx, int cy, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int dy = -r; dy <= r; dy++) { + int py = cy + dy; + if (py < 0 || py >= bh) continue; + for (int dx = -r; dx <= r; dx++) { + int ppx = cx + dx; + if (ppx < 0 || ppx >= bw) continue; + if (dx * dx + dy * dy <= r * r) + px[py * bw + ppx] = v; + } + } +} + +static void px_text(uint32_t* px, int bw, int bh, + int x, int y, const char* text, Color c, int size = FONT_SIZE) { + if (g_font) + g_font->draw_to_buffer(px, bw, bh, x, y, text, c, size); +} + +static int text_w(const char* text, int size = FONT_SIZE) { + return g_font ? g_font->measure_text(text, size) : 0; +} + +static int font_h(int size = FONT_SIZE) { + if (!g_font) return 14; + auto* cache = g_font->get_cache(size); + return cache->ascent - cache->descent; +} + +static void px_button(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, + const char* label, Color bg, Color fg, int r = BTN_RAD) { + px_fill_rounded(px, bw, bh, x, y, w, h, r, bg); + int tw = text_w(label, FONT_SIZE_SM); + int fh = font_h(FONT_SIZE_SM); + px_text(px, bw, bh, x + (w - tw) / 2, y + (h - fh) / 2 - 1, label, fg, FONT_SIZE_SM); +} + +// ============================================================================ +// Bluetooth helpers +// ============================================================================ + +static void set_status(const char* msg) { + snprintf(g_status, sizeof(g_status), "%s", msg); + g_status_time = montauk::get_milliseconds(); +} + +static void refresh_adapter() { + montauk::memset(&g_adapter, 0, sizeof(g_adapter)); + int r = montauk::bt_info(&g_adapter); + g_adapter_ok = (r >= 0 && g_adapter.initialized); +} + +static void refresh_devices() { + g_device_count = montauk::bt_list(g_devices, MAX_CONNECTED); + if (g_device_count < 0) g_device_count = 0; + + // Resolve names from scan results or use address string + for (int i = 0; i < g_device_count; i++) { + bool found = false; + for (int j = 0; j < g_scan_count; j++) { + if (memcmp(g_devices[i].bdAddr, g_scan[j].bdAddr, 6) == 0 && g_scan[j].name[0]) { + montauk::memcpy(g_device_names[i], g_scan[j].name, 64); + found = true; + break; + } + } + if (!found) { + snprintf(g_device_names[i], 64, "%02X:%02X:%02X:%02X:%02X:%02X", + g_devices[i].bdAddr[0], g_devices[i].bdAddr[1], + g_devices[i].bdAddr[2], g_devices[i].bdAddr[3], + g_devices[i].bdAddr[4], g_devices[i].bdAddr[5]); + } + } +} + +static void do_scan() { + g_scanning = true; + g_scan_count = 0; + set_status("Scanning..."); +} + +static void finish_scan() { + int r = montauk::bt_scan(g_scan, MAX_SCAN, 3000); + g_scan_count = r > 0 ? r : 0; + g_scanning = false; + char msg[64]; + snprintf(msg, sizeof(msg), "Found %d device%s", g_scan_count, g_scan_count == 1 ? "" : "s"); + set_status(msg); +} + +static void do_connect(const uint8_t* addr) { + int r = montauk::bt_connect(addr); + if (r >= 0) { + set_status("Connected"); + } else { + set_status("Connection failed"); + } + refresh_devices(); +} + +static void do_disconnect(const uint8_t* addr) { + int r = montauk::bt_disconnect(addr); + if (r >= 0) { + set_status("Disconnected"); + } else { + set_status("Disconnect failed"); + } + refresh_devices(); +} + +static bool is_connected(const uint8_t* addr) { + for (int i = 0; i < g_device_count; i++) { + if (memcmp(g_devices[i].bdAddr, addr, 6) == 0 && g_devices[i].connected) + return true; + } + return false; +} + +// ============================================================================ +// Format helpers +// ============================================================================ + +static const char* device_class_str(uint32_t cod) { + uint32_t major = (cod >> 8) & 0x1F; + switch (major) { + case 1: return "Computer"; + case 2: return "Phone"; + case 3: return "Network"; + case 4: return "Audio/Video"; + case 5: return "Peripheral"; + case 6: return "Imaging"; + case 7: return "Wearable"; + default: return "Device"; + } +} + +static void format_addr(char* buf, int len, const uint8_t* addr) { + snprintf(buf, len, "%02X:%02X:%02X:%02X:%02X:%02X", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); +} + +static const char* rssi_bar(int8_t rssi) { + if (rssi >= -50) return "Strong"; + if (rssi >= -70) return "Good"; + if (rssi >= -85) return "Weak"; + return "Very weak"; +} + +// ============================================================================ +// Render +// ============================================================================ + +static void render(uint32_t* pixels) { + int W = g_win_w; + int H = g_win_h; + + // Background + px_fill(pixels, W, H, 0, 0, W, H, BG_COLOR); + + // Tab bar + int tab_y = 0; + px_fill(pixels, W, H, 0, tab_y, W, TAB_H, TAB_BG); + px_hline(pixels, W, H, 0, tab_y + TAB_H - 1, W, BORDER); + + int tab_w = W / 2; + const char* tab_labels[] = { "Devices", "Scan" }; + for (int i = 0; i < 2; i++) { + // Active tab gets white background to merge with content area + if (g_tab == i) + px_fill(pixels, W, H, i * tab_w, tab_y, tab_w, TAB_H, BG_COLOR); + + Color tc = (g_tab == i) ? TAB_ACTIVE : TAB_INACTIVE; + int tw = text_w(tab_labels[i], FONT_SIZE); + int tx = i * tab_w + (tab_w - tw) / 2; + px_text(pixels, W, H, tx, tab_y + (TAB_H - font_h(FONT_SIZE)) / 2, + tab_labels[i], tc, FONT_SIZE); + // Active indicator underline + if (g_tab == i) { + px_fill(pixels, W, H, i * tab_w + 4, tab_y + TAB_H - 3, tab_w - 8, 3, TAB_ACTIVE); + } + } + + int content_y = tab_y + TAB_H; + int content_h = H - content_y; + + // Status bar at bottom + int list_h = content_h - STATUS_H; + + if (g_tab == TAB_DEVICES) { + // Connected devices list + if (g_device_count == 0) { + const char* msg = "No connected devices"; + int mw = text_w(msg, FONT_SIZE); + px_text(pixels, W, H, (W - mw) / 2, content_y + list_h / 2 - font_h(FONT_SIZE) / 2, + msg, DIM_TEXT, FONT_SIZE); + } else { + for (int i = 0; i < g_device_count; i++) { + int ry = content_y + i * ROW_H - g_scroll; + if (ry + ROW_H < content_y || ry >= content_y + list_h) continue; + + // Hover highlight + if (g_hover_row == i) + px_fill(pixels, W, H, 0, ry, W, ROW_H, ROW_HOVER); + + // Connected indicator dot + Color dot_c = g_devices[i].connected ? GREEN : DIM_TEXT; + px_circle(pixels, W, H, PAD + 6, ry + ROW_H / 2, 5, dot_c); + + // Device name + px_text(pixels, W, H, PAD + 20, ry + 8, + g_device_names[i], TEXT_COLOR, FONT_SIZE); + + // Address below name + char addr_str[24]; + format_addr(addr_str, sizeof(addr_str), g_devices[i].bdAddr); + px_text(pixels, W, H, PAD + 20, ry + 8 + font_h(FONT_SIZE) + 2, + addr_str, DIM_TEXT, FONT_SIZE_SM); + + // Disconnect button + if (g_devices[i].connected) { + int bx = W - PAD - BTN_W; + int by = ry + (ROW_H - BTN_H) / 2; + px_button(pixels, W, H, bx, by, BTN_W, BTN_H, + "Disconnect", RED, WHITE); + } + + // Divider + px_hline(pixels, W, H, PAD, ry + ROW_H - 1, W - 2 * PAD, BORDER); + } + } + } else { + // Scan tab + // Scan button at top of content area + int scan_btn_y = content_y + 12; + int scan_btn_w = 100; + int scan_btn_x = (W - scan_btn_w) / 2; + if (g_scanning) { + px_button(pixels, W, H, scan_btn_x, scan_btn_y, scan_btn_w, BTN_H, + "Scanning...", BORDER, DIM_TEXT); + } else { + px_button(pixels, W, H, scan_btn_x, scan_btn_y, scan_btn_w, BTN_H, + "Scan", ACCENT, WHITE); + } + + int list_top = scan_btn_y + BTN_H + 12; + int scan_list_h = content_y + list_h - list_top; + + if (g_scan_count == 0 && !g_scanning) { + const char* msg = "Press Scan to find devices"; + int mw = text_w(msg, FONT_SIZE); + px_text(pixels, W, H, (W - mw) / 2, list_top + scan_list_h / 2 - font_h(FONT_SIZE) / 2, + msg, DIM_TEXT, FONT_SIZE); + } else { + for (int i = 0; i < g_scan_count; i++) { + int ry = list_top + i * ROW_H - g_scroll; + if (ry + ROW_H < list_top || ry >= list_top + scan_list_h) continue; + + // Hover highlight + if (g_hover_row == i) + px_fill(pixels, W, H, 0, ry, W, ROW_H, ROW_HOVER); + + // Device type indicator + const char* type_str = device_class_str(g_scan[i].classOfDevice); + px_circle(pixels, W, H, PAD + 6, ry + ROW_H / 2, 5, ACCENT); + + // Device name (or address if unnamed) + const char* display_name = g_scan[i].name[0] ? g_scan[i].name : "Unknown Device"; + px_text(pixels, W, H, PAD + 20, ry + 4, + display_name, TEXT_COLOR, FONT_SIZE); + + // Type + signal below name + char detail[80]; + char addr_str[24]; + format_addr(addr_str, sizeof(addr_str), g_scan[i].bdAddr); + snprintf(detail, sizeof(detail), "%s | %s | %s", + type_str, addr_str, rssi_bar(g_scan[i].rssi)); + px_text(pixels, W, H, PAD + 20, ry + 4 + font_h(FONT_SIZE) + 2, + detail, DIM_TEXT, FONT_SIZE_SM); + + // Connect/Disconnect button + bool conn = is_connected(g_scan[i].bdAddr); + int bx = W - PAD - BTN_W; + int by = ry + (ROW_H - BTN_H) / 2; + if (conn) { + px_button(pixels, W, H, bx, by, BTN_W, BTN_H, + "Disconnect", RED, WHITE); + } else { + px_button(pixels, W, H, bx, by, BTN_W, BTN_H, + "Connect", ACCENT, WHITE); + } + + // Divider + px_hline(pixels, W, H, PAD, ry + ROW_H - 1, W - 2 * PAD, BORDER); + } + } + } + + // Status bar + int status_y = H - STATUS_H; + px_fill(pixels, W, H, 0, status_y, W, STATUS_H, TAB_BG); + px_hline(pixels, W, H, 0, status_y, W, BORDER); + + // Status bar content: adapter info on the left, status message on the right + uint64_t now = montauk::get_milliseconds(); + int sy = status_y + (STATUS_H - font_h(FONT_SIZE_SM)) / 2; + + if (g_adapter_ok) { + char adapter_info[96]; + char addr_str[24]; + format_addr(addr_str, sizeof(addr_str), g_adapter.bdAddr); + const char* name = g_adapter.name[0] ? g_adapter.name : "Adapter"; + snprintf(adapter_info, sizeof(adapter_info), "%s | %s | %d connected", + name, addr_str, g_device_count); + px_text(pixels, W, H, PAD, sy, adapter_info, DIM_TEXT, FONT_SIZE_SM); + } else { + px_text(pixels, W, H, PAD, sy, "No adapter found", RED, FONT_SIZE_SM); + } + + // Temporary status message (right-aligned, fades after 5s) + if (g_status[0] && (now - g_status_time) < 5000) { + int sw = text_w(g_status, FONT_SIZE_SM); + px_text(pixels, W, H, W - PAD - sw, sy, g_status, DIM_TEXT, FONT_SIZE_SM); + } +} + +// ============================================================================ +// Hit testing +// ============================================================================ + +static bool handle_click(int mx, int my) { + int W = g_win_w; + + // Tab bar + int tab_y = 0; + if (my >= tab_y && my < tab_y + TAB_H) { + Tab new_tab = (mx < W / 2) ? TAB_DEVICES : TAB_SCAN; + if (new_tab != g_tab) { + g_tab = new_tab; + g_scroll = 0; + g_hover_row = -1; + return true; + } + return false; + } + + int content_y = tab_y + TAB_H; + + if (g_tab == TAB_SCAN) { + // Scan button + int scan_btn_y = content_y + 8; + int scan_btn_w = 100; + int scan_btn_x = (W - scan_btn_w) / 2; + if (mx >= scan_btn_x && mx < scan_btn_x + scan_btn_w && + my >= scan_btn_y && my < scan_btn_y + BTN_H && !g_scanning) { + do_scan(); + return true; + } + + // Scan result rows + int list_top = scan_btn_y + BTN_H + 12; + for (int i = 0; i < g_scan_count; i++) { + int ry = list_top + i * ROW_H - g_scroll; + if (my >= ry && my < ry + ROW_H) { + // Check if disconnect/connect button clicked + int bx = W - PAD - BTN_W; + int by = ry + (ROW_H - BTN_H) / 2; + if (mx >= bx && mx < bx + BTN_W && my >= by && my < by + BTN_H) { + if (is_connected(g_scan[i].bdAddr)) { + do_disconnect(g_scan[i].bdAddr); + } else { + do_connect(g_scan[i].bdAddr); + } + return true; + } + } + } + } else { + // Device rows + for (int i = 0; i < g_device_count; i++) { + int ry = content_y + i * ROW_H - g_scroll; + if (my >= ry && my < ry + ROW_H) { + // Disconnect button + if (g_devices[i].connected) { + int bx = W - PAD - BTN_W; + int by = ry + (ROW_H - BTN_H) / 2; + if (mx >= bx && mx < bx + BTN_W && my >= by && my < by + BTN_H) { + do_disconnect(g_devices[i].bdAddr); + return true; + } + } + } + } + } + + return false; +} + +static int get_hover_row(int mx, int my) { + int content_y = TAB_H; + + if (g_tab == TAB_DEVICES) { + int count = g_device_count; + for (int i = 0; i < count; i++) { + int ry = content_y + i * ROW_H - g_scroll; + if (my >= ry && my < ry + ROW_H) + return i; + } + } else { + int list_top = content_y + 8 + BTN_H + 12; + for (int i = 0; i < g_scan_count; i++) { + int ry = list_top + i * ROW_H - g_scroll; + if (my >= ry && my < ry + ROW_H) + return i; + } + } + return -1; +} + +// ============================================================================ +// Entry point +// ============================================================================ + +extern "C" void _start() { + // Load font + { + TrueTypeFont* f = (TrueTypeFont*)montauk::malloc(sizeof(TrueTypeFont)); + if (f) { + montauk::memset(f, 0, sizeof(TrueTypeFont)); + if (!f->init("0:/fonts/Roboto-Medium.ttf")) { montauk::mfree(f); f = nullptr; } + } + g_font = f; + } + + // Query adapter + refresh_adapter(); + refresh_devices(); + g_status[0] = 0; + + // Create window + Montauk::WinCreateResult wres; + if (montauk::win_create("Bluetooth", WIN_W, WIN_H, &wres) < 0 || wres.id < 0) + montauk::exit(1); + + int win_id = wres.id; + uint32_t* pixels = (uint32_t*)(uintptr_t)wres.pixelVa; + + render(pixels); + montauk::win_present(win_id); + + // Periodic refresh timer + uint64_t last_refresh = montauk::get_milliseconds(); + + while (true) { + Montauk::WinEvent ev; + int r = montauk::win_poll(win_id, &ev); + + if (r < 0) break; + + // Handle pending scan (non-blocking: kick it off after render) + if (g_scanning) { + render(pixels); + montauk::win_present(win_id); + finish_scan(); + refresh_devices(); + render(pixels); + montauk::win_present(win_id); + continue; + } + + if (r == 0) { + // Periodic refresh every 5 seconds + uint64_t now = montauk::get_milliseconds(); + if (now - last_refresh >= 5000) { + refresh_adapter(); + refresh_devices(); + last_refresh = now; + render(pixels); + montauk::win_present(win_id); + } + montauk::sleep_ms(16); + continue; + } + + bool redraw = false; + + if (ev.type == 3) break; // close + + // Resize + if (ev.type == 2) { + g_win_w = ev.resize.w; + g_win_h = ev.resize.h; + pixels = (uint32_t*)(uintptr_t)montauk::win_resize(win_id, g_win_w, g_win_h); + redraw = true; + } + + // Keyboard + if (ev.type == 0 && ev.key.pressed) { + if (ev.key.scancode == 0x01) break; // Escape + if (ev.key.ascii == 's' || ev.key.ascii == 'S') { + if (!g_scanning) { do_scan(); redraw = true; } + } + if (ev.key.ascii == '1') { g_tab = TAB_DEVICES; g_scroll = 0; redraw = true; } + if (ev.key.ascii == '2') { g_tab = TAB_SCAN; g_scroll = 0; redraw = true; } + } + + // Mouse + if (ev.type == 1) { + bool clicked = (ev.mouse.buttons & 1) && !(ev.mouse.prev_buttons & 1); + + int new_hover = get_hover_row(ev.mouse.x, ev.mouse.y); + if (new_hover != g_hover_row) { + g_hover_row = new_hover; + redraw = true; + } + + if (clicked) { + if (handle_click(ev.mouse.x, ev.mouse.y)) + redraw = true; + } + + // Scroll + if (ev.mouse.scroll) { + g_scroll -= ev.mouse.scroll * 20; + if (g_scroll < 0) g_scroll = 0; + redraw = true; + } + } + + if (redraw) { + render(pixels); + montauk::win_present(win_id); + } + } + + montauk::win_destroy(win_id); + montauk::exit(0); +} diff --git a/programs/src/bluetooth/manifest.toml b/programs/src/bluetooth/manifest.toml new file mode 100644 index 0000000..402d0ca --- /dev/null +++ b/programs/src/bluetooth/manifest.toml @@ -0,0 +1,8 @@ +[app] +name = "Bluetooth" +binary = "bluetooth.elf" +icon = "bluetooth.svg" + +[menu] +category = "System" +visible = true diff --git a/programs/src/bluetooth/stb_truetype_impl.cpp b/programs/src/bluetooth/stb_truetype_impl.cpp new file mode 100644 index 0000000..9ce2266 --- /dev/null +++ b/programs/src/bluetooth/stb_truetype_impl.cpp @@ -0,0 +1,35 @@ +/* + * stb_truetype_impl.cpp + * Single compilation unit for stb_truetype in MontaukOS freestanding environment + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include + +// Override all stb_truetype dependencies before including the implementation + +#define STBTT_ifloor(x) ((int) stb_floor(x)) +#define STBTT_iceil(x) ((int) stb_ceil(x)) +#define STBTT_sqrt(x) stb_sqrt(x) +#define STBTT_pow(x,y) stb_pow(x,y) +#define STBTT_fmod(x,y) stb_fmod(x,y) +#define STBTT_cos(x) stb_cos(x) +#define STBTT_acos(x) stb_acos(x) +#define STBTT_fabs(x) stb_fabs(x) + +#define STBTT_malloc(x,u) ((void)(u), montauk::malloc(x)) +#define STBTT_free(x,u) ((void)(u), montauk::mfree(x)) + +#define STBTT_memcpy(d,s,n) montauk::memcpy(d,s,n) +#define STBTT_memset(d,v,n) montauk::memset(d,v,n) + +#define STBTT_strlen(x) montauk::slen(x) + +#define STBTT_assert(x) ((void)(x)) + +#define STB_TRUETYPE_IMPLEMENTATION +#include diff --git a/programs/src/desktop/apps/app_filemanager.cpp b/programs/src/desktop/apps/app_filemanager.cpp index 7b3d158..fed2759 100644 --- a/programs/src/desktop/apps/app_filemanager.cpp +++ b/programs/src/desktop/apps/app_filemanager.cpp @@ -77,6 +77,14 @@ static bool is_spreadsheet_file(const char* name) { return str_ends_with(name, ".mss"); } +static bool is_video_file(const char* name) { + return str_ends_with(name, ".mp4") || str_ends_with(name, ".m4v"); +} + +static bool is_audio_file(const char* name) { + return str_ends_with(name, ".mp3") || str_ends_with(name, ".wav"); +} + static int detect_file_type(const char* name, bool is_dir) { if (is_dir) return 1; if (str_ends_with(name, ".elf")) return 2; @@ -174,11 +182,7 @@ static void filemanager_read_dir(FileManagerState* fm) { fm->is_dir[i] = true; fm->entry_names[i][len - 1] = '\0'; } else { - bool has_dot = false; - for (int j = 0; j < len; j++) { - if (fm->entry_names[i][j] == '.') { has_dot = true; break; } - } - fm->is_dir[i] = !has_dot; + fm->is_dir[i] = false; } fm->entry_types[i] = detect_file_type(fm->entry_names[i], fm->is_dir[i]); @@ -240,6 +244,66 @@ static void filemanager_read_dir(FileManagerState* fm) { fm->last_click_time = 0; } +// ============================================================================ +// Recursive directory deletion +// ============================================================================ + +static bool filemanager_delete_recursive(const char* path) { + // Try deleting as a file (or empty directory) first + if (montauk::fdelete(path) == 0) return true; + + // If that failed, it may be a non-empty directory — enumerate and delete children + const char* names[64]; + int count = montauk::readdir(path, names, 64); + if (count < 0) return false; + + // Compute the prefix to strip (readdir returns paths relative to drive root) + const char* after_drive = path; + for (int k = 0; after_drive[k]; k++) { + if (after_drive[k] == ':' && after_drive[k + 1] == '/') { + after_drive += k + 2; + break; + } + } + char prefix[256] = {0}; + int prefix_len = 0; + if (after_drive[0] != '\0') { + montauk::strcpy(prefix, after_drive); + prefix_len = montauk::slen(prefix); + if (prefix_len > 0 && prefix[prefix_len - 1] != '/') { + prefix[prefix_len++] = '/'; + prefix[prefix_len] = '\0'; + } + } + + for (int i = 0; i < count; i++) { + const char* raw = names[i]; + if (prefix_len > 0) { + bool match = true; + for (int k = 0; k < prefix_len; k++) { + if (raw[k] != prefix[k]) { match = false; break; } + } + if (match) raw += prefix_len; + } + + char child[512]; + montauk::strcpy(child, path); + int plen = montauk::slen(child); + if (plen > 0 && child[plen - 1] != '/') + str_append(child, "/", 512); + str_append(child, raw, 512); + + // Strip trailing slash if present (directory marker) + int clen = montauk::slen(child); + if (clen > 0 && child[clen - 1] == '/') child[clen - 1] = '\0'; + + filemanager_delete_recursive(child); + } + + // Now the directory should be empty — delete it + return montauk::fdelete(path) == 0; +} + // ============================================================================ // History management // ============================================================================ @@ -373,11 +437,12 @@ static void filemanager_draw_header(Canvas& c, FileManagerState* fm, } } - // Delete button (6th toolbar button) — only active when a file is selected + // Delete button (6th toolbar button) — active when a file or directory is selected { int bx = 148, by = 4; bool has_sel = fm->selected >= 0 && fm->selected < fm->entry_count - && !fm->at_drives_root && !fm->is_dir[fm->selected]; + && !fm->at_drives_root + && fm->entry_types[fm->selected] != 3; Color del_bg = has_sel ? btn_bg : Color::from_rgb(0xF0, 0xF0, 0xF0); c.fill_rect(bx, by, 24, 24, del_bg); if (ds && ds->icon_delete.pixels) { @@ -634,16 +699,19 @@ static void filemanager_on_mouse(Window* win, MouseEvent& ev) { fm->scrollbar.scroll_offset = 0; } else if (local_x >= 148 && local_x < 172) { - // Delete selected file + // Delete selected file or directory if (fm->selected >= 0 && fm->selected < fm->entry_count - && !fm->at_drives_root && !fm->is_dir[fm->selected]) { + && !fm->at_drives_root && fm->entry_types[fm->selected] != 3) { char fullpath[512]; montauk::strcpy(fullpath, fm->current_path); int plen = montauk::slen(fullpath); if (plen > 0 && fullpath[plen - 1] != '/') str_append(fullpath, "/", 512); str_append(fullpath, fm->entry_names[fm->selected], 512); - montauk::fdelete(fullpath); + if (fm->is_dir[fm->selected]) + filemanager_delete_recursive(fullpath); + else + montauk::fdelete(fullpath); filemanager_read_dir(fm); } } @@ -695,6 +763,10 @@ static void filemanager_on_mouse(Window* win, MouseEvent& ev) { montauk::spawn("0:/apps/pdfviewer/pdfviewer.elf", fullpath); } else if (is_spreadsheet_file(fm->entry_names[clicked_idx])) { montauk::spawn("0:/apps/spreadsheet/spreadsheet.elf", fullpath); + } else if (is_video_file(fm->entry_names[clicked_idx])) { + montauk::spawn("0:/apps/video/video.elf", fullpath); + } else if (is_audio_file(fm->entry_names[clicked_idx])) { + montauk::spawn("0:/apps/music/music.elf", fullpath); } else if (str_ends_with(fm->entry_names[clicked_idx], ".elf")) { montauk::spawn(fullpath); } else if (fm->desktop) { @@ -753,6 +825,10 @@ static void filemanager_on_mouse(Window* win, MouseEvent& ev) { montauk::spawn("0:/apps/pdfviewer/pdfviewer.elf", fullpath); } else if (is_spreadsheet_file(fm->entry_names[clicked_idx])) { montauk::spawn("0:/apps/spreadsheet/spreadsheet.elf", fullpath); + } else if (is_video_file(fm->entry_names[clicked_idx])) { + montauk::spawn("0:/apps/video/video.elf", fullpath); + } else if (is_audio_file(fm->entry_names[clicked_idx])) { + montauk::spawn("0:/apps/music/music.elf", fullpath); } else if (str_ends_with(fm->entry_names[clicked_idx], ".elf")) { montauk::spawn(fullpath); } else if (fm->desktop) { @@ -842,14 +918,17 @@ static void filemanager_on_key(Window* win, const Montauk::KeyEvent& key) { } else if (key.scancode == 0x53) { // Delete key if (fm->selected >= 0 && fm->selected < fm->entry_count - && !fm->at_drives_root && !fm->is_dir[fm->selected]) { + && !fm->at_drives_root && fm->entry_types[fm->selected] != 3) { char fullpath[512]; montauk::strcpy(fullpath, fm->current_path); int plen = montauk::slen(fullpath); if (plen > 0 && fullpath[plen - 1] != '/') str_append(fullpath, "/", 512); str_append(fullpath, fm->entry_names[fm->selected], 512); - montauk::fdelete(fullpath); + if (fm->is_dir[fm->selected]) + filemanager_delete_recursive(fullpath); + else + montauk::fdelete(fullpath); filemanager_read_dir(fm); } } else if (key.alt && key.scancode == 0x4B) { diff --git a/programs/src/desktop/main.cpp b/programs/src/desktop/main.cpp index 6ee27ec..a1c271e 100644 --- a/programs/src/desktop/main.cpp +++ b/programs/src/desktop/main.cpp @@ -262,6 +262,8 @@ void gui::desktop_init(DesktopState* ds) { ds->net_cfg_last_poll = montauk::get_milliseconds(); ds->net_icon_rect = {0, 0, 0, 0}; + ds->closing_ext_count = 0; + } // ============================================================================ @@ -302,6 +304,16 @@ void desktop_poll_external_windows(DesktopState* ds) { } if (!found && ds->window_count < MAX_WINDOWS) { + // Skip windows we've already sent a close event to — the owning + // process hasn't destroyed them yet, so they still appear in + // win_enumerate. Re-creating them would place them at the + // default position instead of where the user dragged them. + bool closing = false; + for (int c = 0; c < ds->closing_ext_count; c++) { + if (ds->closing_ext_ids[c] == extId) { closing = true; break; } + } + if (closing) continue; + // Map the pixel buffer into our address space uint64_t va = montauk::win_map(extId); if (va == 0) continue; @@ -367,6 +379,17 @@ void desktop_poll_external_windows(DesktopState* ds) { gui::desktop_close_window(ds, i); } } + + // Purge closing IDs that the kernel no longer reports (fully destroyed) + for (int c = ds->closing_ext_count - 1; c >= 0; c--) { + bool alive = false; + for (int e = 0; e < extCount; e++) { + if (extWins[e].id == ds->closing_ext_ids[c]) { alive = true; break; } + } + if (!alive) { + ds->closing_ext_ids[c] = ds->closing_ext_ids[--ds->closing_ext_count]; + } + } } // ============================================================================ diff --git a/programs/src/desktop/window.cpp b/programs/src/desktop/window.cpp index 8161c8a..6d04548 100644 --- a/programs/src/desktop/window.cpp +++ b/programs/src/desktop/window.cpp @@ -55,8 +55,13 @@ void gui::desktop_close_window(DesktopState* ds, int idx) { Window* win = &ds->windows[idx]; - // For external windows, send a close event instead of freeing the buffer + // For external windows, send a close event instead of freeing the buffer. + // Track the ID so the poll loop won't re-create it at the default position + // before the owning process has finished destroying it. if (win->external) { + if (ds->closing_ext_count < DesktopState::MAX_CLOSING) { + ds->closing_ext_ids[ds->closing_ext_count++] = win->ext_win_id; + } Montauk::WinEvent ev; montauk::memset(&ev, 0, sizeof(ev)); ev.type = 3; // close diff --git a/programs/src/disks/actions.cpp b/programs/src/disks/actions.cpp index 8489845..8075d52 100644 --- a/programs/src/disks/actions.cpp +++ b/programs/src/disks/actions.cpp @@ -29,20 +29,70 @@ void disktool_refresh() { } // ============================================================================ -// Create partition +// Create partition — confirmation dialog // ============================================================================ -void do_create_partition() { +void open_newpart_dialog() { auto& dt = g_state; if (dt.selected_disk < 0 || dt.selected_disk >= dt.disk_count) return; + if (dt.np_dlg.open) return; int part_indices[MAX_PARTS]; int nparts = get_disk_parts(part_indices, MAX_PARTS); - if (nparts == 0) { + auto& dlg = dt.np_dlg; + dlg.will_init_gpt = (nparts == 0); + dlg.hover_confirm = false; + dlg.hover_cancel = false; + + Montauk::DiskInfo& disk = dt.disks[dt.selected_disk]; + char sz[24]; + format_disk_size(sz, sizeof(sz), disk.sectorCount, disk.sectorSizeLog); + snprintf(dlg.disk_desc, sizeof(dlg.disk_desc), "Disk %d: %s (%s)", + dt.selected_disk, disk.model, sz); + + if (dlg.will_init_gpt) { + snprintf(dlg.warn_line1, sizeof(dlg.warn_line1), + "This will initialize a new GPT on this disk."); + snprintf(dlg.warn_line2, sizeof(dlg.warn_line2), + "ALL existing partitions will be destroyed!"); + } else { + snprintf(dlg.warn_line1, sizeof(dlg.warn_line1), + "This will add a new partition in the largest"); + snprintf(dlg.warn_line2, sizeof(dlg.warn_line2), + "free region on this disk."); + } + + Montauk::WinCreateResult wres; + const char* title = dlg.will_init_gpt ? "Initialize Disk" : "New Partition"; + if (montauk::win_create(title, NP_DLG_W, NP_DLG_H, &wres) < 0 || wres.id < 0) { + set_status("Failed to open dialog"); + return; + } + dlg.win_id = wres.id; + dlg.pixels = (uint32_t*)(uintptr_t)wres.pixelVa; + dlg.open = true; + + render_newpart_window(); + montauk::win_present(dlg.win_id); +} + +void close_newpart_dialog() { + auto& dlg = g_state.np_dlg; + if (!dlg.open) return; + montauk::win_destroy(dlg.win_id); + dlg.open = false; +} + +void newpart_dialog_confirm() { + auto& dt = g_state; + auto& dlg = dt.np_dlg; + + if (dlg.will_init_gpt) { int r = montauk::gpt_init(dt.selected_disk); if (r < 0) { set_status("Failed to initialize GPT on disk"); + close_newpart_dialog(); return; } set_status("Initialized GPT"); @@ -69,13 +119,18 @@ void do_create_partition() { int r = montauk::gpt_add(¶ms); if (r < 0) { set_status("Failed to create partition"); - return; + } else { + set_status("Partition created successfully"); } - set_status("Partition created successfully"); + close_newpart_dialog(); disktool_refresh(); } +void do_create_partition() { + open_newpart_dialog(); +} + // ============================================================================ // Mount partition // ============================================================================ diff --git a/programs/src/disks/disks.h b/programs/src/disks/disks.h index 051c2b2..1cdeb2e 100644 --- a/programs/src/disks/disks.h +++ b/programs/src/disks/disks.h @@ -72,6 +72,8 @@ static constexpr int NUM_FS_TYPES = 1; static constexpr int FMT_DLG_W = 280; static constexpr int FMT_DLG_H = 220; +static constexpr int NP_DLG_W = 340; +static constexpr int NP_DLG_H = 200; struct FormatDialog { bool open; @@ -84,6 +86,18 @@ struct FormatDialog { uint32_t* pixels; }; +struct NewPartDialog { + bool open; + bool will_init_gpt; // true if GPT init is needed (most destructive) + bool hover_confirm; + bool hover_cancel; + char disk_desc[80]; + char warn_line1[96]; + char warn_line2[96]; + int win_id; + uint32_t* pixels; +}; + struct DiskToolState { Montauk::DiskInfo disks[MAX_DISKS]; int disk_count; @@ -95,6 +109,7 @@ struct DiskToolState { char status[80]; uint64_t status_time; FormatDialog fmt_dlg; + NewPartDialog np_dlg; }; // ============================================================================ @@ -125,6 +140,7 @@ void format_disk_size(char* buf, int bufsize, uint64_t sectors, uint16_t sectorS void render(uint32_t* pixels); void render_format_window(); +void render_newpart_window(); // ============================================================================ // Function declarations — actions.cpp @@ -136,3 +152,6 @@ void do_mount_partition(); void open_format_dialog(); void close_format_dialog(); void format_dialog_do_format(); +void open_newpart_dialog(); +void close_newpart_dialog(); +void newpart_dialog_confirm(); diff --git a/programs/src/disks/main.cpp b/programs/src/disks/main.cpp index 3719036..53775c0 100644 --- a/programs/src/disks/main.cpp +++ b/programs/src/disks/main.cpp @@ -180,6 +180,40 @@ static bool handle_content_click(int mx, int my) { return false; } +// ============================================================================ +// New partition dialog mouse handling +// ============================================================================ + +static bool handle_newpart_dialog_click(int mx, int my, bool clicked) { + auto& dlg = g_state.np_dlg; + if (!dlg.open) return false; + + int dw = NP_DLG_W, dh = NP_DLG_H; + + int btn_w = 90, btn_h = 30; + int btn_y = dh - btn_h - 16; + int gap = 16; + int total_w = btn_w * 2 + gap; + int bx = (dw - total_w) / 2; + + dlg.hover_confirm = (mx >= bx && mx < bx + btn_w && my >= btn_y && my < btn_y + btn_h); + dlg.hover_cancel = (mx >= bx + btn_w + gap && mx < bx + btn_w * 2 + gap && my >= btn_y && my < btn_y + btn_h); + + if (!clicked) return true; + + if (dlg.hover_confirm) { + newpart_dialog_confirm(); + return true; + } + + if (dlg.hover_cancel) { + close_newpart_dialog(); + return true; + } + + return true; +} + // ============================================================================ // Format dialog mouse handling // ============================================================================ @@ -236,6 +270,16 @@ static bool handle_key(const Montauk::KeyEvent& key) { auto& dt = g_state; + // New partition dialog keys + if (dt.np_dlg.open) { + if (key.scancode == 0x01) { // Escape + close_newpart_dialog(); + } else if (key.ascii == '\n' || key.ascii == '\r') { + newpart_dialog_confirm(); + } + return true; + } + // Format dialog keys if (dt.fmt_dlg.open) { if (key.scancode == 0x01) { // Escape @@ -313,6 +357,28 @@ extern "C" void _start() { bool redraw_main = false; bool redraw_dlg = false; + bool redraw_np = false; + + // Poll new partition dialog window + if (g_state.np_dlg.open) { + int dr = montauk::win_poll(g_state.np_dlg.win_id, &ev); + if (dr > 0) { + if (ev.type == 3) { // close + close_newpart_dialog(); + redraw_main = true; + } else if (ev.type == 0 && ev.key.pressed) { + handle_key(ev.key); + redraw_np = g_state.np_dlg.open; + redraw_main = true; + } else if (ev.type == 1) { + bool clicked = (ev.mouse.buttons & 1) && !(ev.mouse.prev_buttons & 1); + handle_newpart_dialog_click(ev.mouse.x, ev.mouse.y, clicked); + redraw_np = g_state.np_dlg.open; + redraw_main = true; + } + } + } + // Poll format dialog window if (g_state.fmt_dlg.open) { int dr = montauk::win_poll(g_state.fmt_dlg.win_id, &ev); @@ -341,7 +407,8 @@ extern "C" void _start() { // Even with no main event, dialog may have triggered redraws if (redraw_main) { render(pixels); montauk::win_present(win_id); } if (redraw_dlg) { render_format_window(); montauk::win_present(g_state.fmt_dlg.win_id); } - if (!redraw_main && !redraw_dlg) montauk::sleep_ms(16); + if (redraw_np) { render_newpart_window(); montauk::win_present(g_state.np_dlg.win_id); } + if (!redraw_main && !redraw_dlg && !redraw_np) montauk::sleep_ms(16); continue; } @@ -383,8 +450,10 @@ extern "C" void _start() { if (redraw_main) { render(pixels); montauk::win_present(win_id); } if (redraw_dlg) { render_format_window(); montauk::win_present(g_state.fmt_dlg.win_id); } + if (redraw_np) { render_newpart_window(); montauk::win_present(g_state.np_dlg.win_id); } } + close_newpart_dialog(); close_format_dialog(); montauk::win_destroy(win_id); montauk::exit(0); diff --git a/programs/src/disks/render.cpp b/programs/src/disks/render.cpp index 8ae4835..60933c4 100644 --- a/programs/src/disks/render.cpp +++ b/programs/src/disks/render.cpp @@ -345,6 +345,61 @@ void render_format_window() { px_button(px, dw, dh, bx + btn_w + gap, btn_y, btn_w, btn_h, "Cancel", can_bg, WHITE, 6); } +// ============================================================================ +// Render: new partition confirmation dialog +// ============================================================================ + +void render_newpart_window() { + auto& dlg = g_state.np_dlg; + if (!dlg.open) return; + + uint32_t* px = dlg.pixels; + int dw = NP_DLG_W, dh = NP_DLG_H; + int fh = font_h(); + + px_fill(px, dw, dh, 0, 0, dw, dh, BG_COLOR); + + // Title + const char* title = dlg.will_init_gpt ? "Initialize Disk" : "New Partition"; + int tw = text_w(title); + px_text(px, dw, dh, (dw - tw) / 2, 12, title, TEXT_COLOR); + + // Disk description + int ddw = text_w(dlg.disk_desc); + px_text(px, dw, dh, (dw - ddw) / 2, 12 + fh + 6, dlg.disk_desc, DIM_TEXT); + + // Warning lines + Color warn_color = dlg.will_init_gpt + ? Color::from_rgb(0xCC, 0x22, 0x22) + : TEXT_COLOR; + + int wy = 12 + fh * 2 + 20; + int w1w = text_w(dlg.warn_line1); + px_text(px, dw, dh, (dw - w1w) / 2, wy, dlg.warn_line1, warn_color); + int w2w = text_w(dlg.warn_line2); + px_text(px, dw, dh, (dw - w2w) / 2, wy + fh + 4, dlg.warn_line2, warn_color); + + // Buttons + int btn_w = 90, btn_h = 30; + int btn_y = dh - btn_h - 16; + int gap = 16; + int total_w = btn_w * 2 + gap; + int bx = (dw - total_w) / 2; + + Color confirm_bg = dlg.will_init_gpt + ? (dlg.hover_confirm ? Color::from_rgb(0xDD, 0x44, 0x44) + : Color::from_rgb(0xCC, 0x33, 0x33)) + : (dlg.hover_confirm ? Color::from_rgb(0x4A, 0x88, 0xC8) + : Color::from_rgb(0x42, 0x7A, 0xB5)); + const char* confirm_label = dlg.will_init_gpt ? "Initialize" : "Create"; + px_button(px, dw, dh, bx, btn_y, btn_w, btn_h, confirm_label, confirm_bg, WHITE, 6); + + Color can_bg = dlg.hover_cancel + ? Color::from_rgb(0x99, 0x99, 0x99) + : Color::from_rgb(0x88, 0x88, 0x88); + px_button(px, dw, dh, bx + btn_w + gap, btn_y, btn_w, btn_h, "Cancel", can_bg, WHITE, 6); +} + // ============================================================================ // Top-level render // ============================================================================ diff --git a/programs/src/music/Makefile b/programs/src/music/Makefile new file mode 100644 index 0000000..e3a156b --- /dev/null +++ b/programs/src/music/Makefile @@ -0,0 +1,87 @@ +# Makefile for music (standalone Music Player) on MontaukOS +# Copyright (c) 2026 Daniel Hammer + +MAKEFLAGS += -rR +.SUFFIXES: + +# ---- Toolchain ---- + +TOOLCHAIN_PREFIX := $(shell cd ../../.. && pwd)/toolchain/local/bin/x86_64-elf- +ifneq ($(wildcard $(TOOLCHAIN_PREFIX)gcc),) + CXX := $(TOOLCHAIN_PREFIX)g++ +else + CXX := g++ +endif + +# ---- Paths ---- + +PROG_INC := ../../include +LINK_LD := ../../link.ld +BINDIR := ../../bin +OBJDIR := obj +LIBDIR := ../../lib + +# ---- Compiler flags ---- + +CXXFLAGS := \ + -std=gnu++20 \ + -g -O2 -pipe \ + -Wall \ + -Wextra \ + -Wno-unused-parameter \ + -Wno-unused-function \ + -nostdinc \ + -ffreestanding \ + -fno-stack-protector \ + -fno-stack-check \ + -fno-PIC \ + -fno-rtti \ + -fno-exceptions \ + -ffunction-sections \ + -fdata-sections \ + -m64 \ + -march=x86-64 \ + -msse \ + -msse2 \ + -mno-red-zone \ + -mcmodel=small \ + -I $(PROG_INC) \ + -I . \ + -isystem $(PROG_INC)/libc \ + -isystem ../../../kernel/freestnd-c-hdrs/x86_64/include \ + -isystem ../../../kernel/freestnd-cxx-hdrs/x86_64/include + +# ---- Linker flags ---- + +LDFLAGS := \ + -nostdlib \ + -static \ + -Wl,--build-id=none \ + -Wl,--gc-sections \ + -Wl,-m,elf_x86_64 \ + -z max-page-size=0x1000 \ + -T $(LINK_LD) + +# ---- Source files ---- + +SRCS := main.cpp minimp3_impl.cpp stb_truetype_impl.cpp +OBJS := $(addprefix $(OBJDIR)/,$(SRCS:.cpp=.o)) + +# ---- Target ---- + +TARGET := $(BINDIR)/apps/music/music.elf + +.PHONY: all clean + +all: $(TARGET) + +$(TARGET): $(OBJS) $(LINK_LD) Makefile + mkdir -p $(BINDIR)/apps/music + $(CXX) $(CXXFLAGS) $(LDFLAGS) $(OBJS) $(LIBDIR)/libc/liblibc.a -o $@ + +$(OBJDIR)/%.o: %.cpp Makefile + mkdir -p $(OBJDIR) + $(CXX) $(CXXFLAGS) -c $< -o $@ + +clean: + rm -rf $(OBJDIR) $(TARGET) diff --git a/programs/src/music/main.cpp b/programs/src/music/main.cpp new file mode 100644 index 0000000..8b7b60d --- /dev/null +++ b/programs/src/music/main.cpp @@ -0,0 +1,1147 @@ +/* + * main.cpp + * MontaukOS Music Player — MP3 and WAV playback + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include +#include + +extern "C" { +#include +#include +} + +#include "minimp3.h" + +using namespace gui; + +// ============================================================================ +// Constants +// ============================================================================ + +static constexpr int INIT_W = 380; +static constexpr int INIT_H = 460; +static constexpr int TOOLBAR_H = 36; +static constexpr int FONT_SIZE = 18; +static constexpr int FONT_SIZE_SM = 16; +static constexpr int FONT_SIZE_LG = 20; + +static constexpr int LIST_TOP = TOOLBAR_H; +static constexpr int LIST_ITEM_H = 32; +static constexpr int INFO_H = 64; +static constexpr int PROGRESS_H = 24; +static constexpr int TRANSPORT_H = 44; + +static constexpr int MAX_FILES = 128; +static constexpr int MAX_PATH = 256; + +// Decode buffer: one MP3 frame = up to 1152 samples * 2 channels +static constexpr int PCM_BUF_SAMPLES = 1152 * 2; +// Audio write chunk size (bytes) — feed in small chunks for responsiveness +static constexpr int AUDIO_CHUNK = 4096; + +static constexpr Color BG_COLOR = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color TOOLBAR_BG = Color::from_rgb(0xF5, 0xF5, 0xF5); +static constexpr Color BORDER_COLOR = Color::from_rgb(0xCC, 0xCC, 0xCC); +static constexpr Color TEXT_COLOR = Color::from_rgb(0x22, 0x22, 0x22); +static constexpr Color DIM_TEXT = Color::from_rgb(0x88, 0x88, 0x88); +static constexpr Color ACCENT = Color::from_rgb(0x36, 0x7B, 0xF0); +static constexpr Color ACCENT_DARK = Color::from_rgb(0x2A, 0x62, 0xC8); +static constexpr Color TRACK_BG = Color::from_rgb(0xDD, 0xDD, 0xDD); +static constexpr Color WHITE = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color LIST_HOVER = Color::from_rgb(0xE8, 0xF0, 0xFD); +static constexpr Color LIST_PLAYING = Color::from_rgb(0xD0, 0xE2, 0xFB); +static constexpr Color BTN_BG = Color::from_rgb(0xE8, 0xE8, 0xE8); +static constexpr Color BTN_HOVER = Color::from_rgb(0xD8, 0xD8, 0xD8); +static constexpr Color STOP_COLOR = Color::from_rgb(0xCC, 0x33, 0x33); + +// ============================================================================ +// State +// ============================================================================ + +enum class PlayState { Stopped, Playing, Paused }; + +struct FileEntry { + char name[128]; + bool is_mp3; // true = mp3, false = wav +}; + +struct PlayerState { + // Window + int win_w, win_h; + + // Font + TrueTypeFont* font; + + // Directory + char dir_path[MAX_PATH]; + FileEntry files[MAX_FILES]; + int file_count; + + // List UI + int scroll_y; + int hovered_item; + + // Playback + PlayState play_state; + int current_track; // index into files[], -1 if none + + // Audio handle + int audio_handle; + + // File data (entire file loaded into memory) + uint8_t* file_data; + uint64_t file_size; + + // MP3 decoder + mp3dec_t mp3dec; + uint64_t mp3_offset; // current read position in file_data + int sample_rate; + int channels; + + // WAV state + uint64_t wav_data_offset; // offset to PCM data in file + uint64_t wav_data_size; // size of PCM data + uint64_t wav_pos; // current position in PCM data + + // Timing + uint64_t total_samples; // total decoded samples (per channel) + uint64_t played_samples; // samples fed to audio device + + // PCM buffer for decoded audio + int16_t pcm_buf[PCM_BUF_SAMPLES]; + int pcm_buf_len; // samples remaining in pcm_buf + int pcm_buf_pos; // current read position in pcm_buf + + // Progress dragging + bool dragging_progress; + + // Transport icons (dark for normal, white for active/colored backgrounds) + SvgIcon ico_rewind; + SvgIcon ico_play; + SvgIcon ico_pause; + SvgIcon ico_stop; + SvgIcon ico_forward; + SvgIcon ico_play_w; + SvgIcon ico_pause_w; + SvgIcon ico_stop_w; +}; + +static PlayerState g; + +// ============================================================================ +// Pixel helpers +// ============================================================================ + +static void px_fill(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, Color c) { + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x, y0 = y < 0 ? 0 : y; + int x1 = x + w > bw ? bw : x + w; + int y1 = y + h > bh ? bh : y + h; + for (int row = y0; row < y1; row++) + for (int col = x0; col < x1; col++) + px[row * bw + col] = v; +} + +static void px_hline(uint32_t* px, int bw, int bh, int x, int y, int w, Color c) { + if (y < 0 || y >= bh) return; + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x; + int x1 = x + w > bw ? bw : x + w; + for (int col = x0; col < x1; col++) + px[y * bw + col] = v; +} + +static void px_fill_rounded(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int row = 0; row < h; row++) { + int dy = y + row; + if (dy < 0 || dy >= bh) continue; + for (int col = 0; col < w; col++) { + int dx = x + col; + if (dx < 0 || dx >= bw) continue; + bool skip = false; + int cx, cy; + if (col < r && row < r) { cx = r - col - 1; cy = r - row - 1; if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col >= w - r && row < r) { cx = col - (w - r); cy = r - row - 1; if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col < r && row >= h - r) { cx = r - col - 1; cy = row - (h - r); if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col >= w - r && row >= h - r){ cx = col - (w - r); cy = row - (h - r); if (cx*cx + cy*cy >= r*r) skip = true; } + if (!skip) px[dy * bw + dx] = v; + } + } +} + +static void px_circle(uint32_t* px, int bw, int bh, + int cx, int cy, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int dy = -r; dy <= r; dy++) { + int py = cy + dy; + if (py < 0 || py >= bh) continue; + for (int dx = -r; dx <= r; dx++) { + int ppx = cx + dx; + if (ppx < 0 || ppx >= bw) continue; + if (dx * dx + dy * dy <= r * r) + px[py * bw + ppx] = v; + } + } +} + +// Draw a filled triangle (play button shape) +static void px_triangle_right(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, Color c) { + uint32_t v = c.to_pixel(); + for (int row = 0; row < h; row++) { + int dy = y + row; + if (dy < 0 || dy >= bh) continue; + // For a right-pointing triangle: width = w * min(row, h-1-row) / (h/2) + int half = h / 2; + int span; + if (half == 0) span = w; + else span = w * (row <= half ? row : h - 1 - row) / half; + for (int col = 0; col < span; col++) { + int dx = x + col; + if (dx >= 0 && dx < bw) + px[dy * bw + dx] = v; + } + } +} + +static void px_icon(uint32_t* px, int bw, int bh, + int x, int y, const SvgIcon& ic) { + if (!ic.pixels) return; + for (int row = 0; row < ic.height; row++) { + int dy = y + row; + if (dy < 0 || dy >= bh) continue; + for (int col = 0; col < ic.width; col++) { + int dx = x + col; + if (dx < 0 || dx >= bw) continue; + uint32_t src = ic.pixels[row * ic.width + col]; + uint8_t sa = (src >> 24) & 0xFF; + if (sa == 0) continue; + if (sa == 255) { + px[dy * bw + dx] = src; + } else { + uint32_t dst = px[dy * bw + dx]; + uint32_t a = sa, inv_a = 255 - sa; + uint32_t rr = (a * ((src >> 16) & 0xFF) + inv_a * ((dst >> 16) & 0xFF) + 128) / 255; + uint32_t gg = (a * ((src >> 8) & 0xFF) + inv_a * ((dst >> 8) & 0xFF) + 128) / 255; + uint32_t bb = (a * (src & 0xFF) + inv_a * (dst & 0xFF) + 128) / 255; + px[dy * bw + dx] = 0xFF000000 | (rr << 16) | (gg << 8) | bb; + } + } + } +} + +static void px_text(uint32_t* px, int bw, int bh, + int x, int y, const char* text, Color c, int size = FONT_SIZE) { + if (g.font) + g.font->draw_to_buffer(px, bw, bh, x, y, text, c, size); +} + +static int text_w(const char* text, int size = FONT_SIZE) { + return g.font ? g.font->measure_text(text, size) : 0; +} + +static int font_h(int size = FONT_SIZE) { + if (!g.font) return 16; + auto* cache = g.font->get_cache(size); + return cache->ascent - cache->descent; +} + +static void px_button(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, + const char* label, Color bg, Color fg, int r) { + px_fill_rounded(px, bw, bh, x, y, w, h, r, bg); + int tw = text_w(label); + int fh = font_h(); + px_text(px, bw, bh, x + (w - tw) / 2, y + (h - fh) / 2, label, fg); +} + +static void px_icon_button(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, + const SvgIcon& ic, Color bg, int r) { + px_fill_rounded(px, bw, bh, x, y, w, h, r, bg); + int ix = x + (w - ic.width) / 2; + int iy = y + (h - ic.height) / 2; + px_icon(px, bw, bh, ix, iy, ic); +} + +// ============================================================================ +// String helpers +// ============================================================================ + +static bool str_ends_with(const char* s, const char* suffix) { + int sl = montauk::slen(s); + int xl = montauk::slen(suffix); + if (xl > sl) return false; + for (int i = 0; i < xl; i++) { + char a = s[sl - xl + i]; + char b = suffix[i]; + // case-insensitive + if (a >= 'A' && a <= 'Z') a += 32; + if (b >= 'A' && b <= 'Z') b += 32; + if (a != b) return false; + } + return true; +} + +// ============================================================================ +// Directory scanning +// ============================================================================ + +static void scan_directory() { + g.file_count = 0; + + const char* names[256]; + int count = montauk::readdir(g.dir_path, names, 256); + + for (int i = 0; i < count && g.file_count < MAX_FILES; i++) { + bool is_mp3 = str_ends_with(names[i], ".mp3"); + bool is_wav = str_ends_with(names[i], ".wav"); + if (!is_mp3 && !is_wav) continue; + + FileEntry& f = g.files[g.file_count]; + int len = montauk::slen(names[i]); + if (len >= (int)sizeof(f.name)) len = sizeof(f.name) - 1; + montauk::memcpy(f.name, names[i], len); + f.name[len] = 0; + f.is_mp3 = is_mp3; + g.file_count++; + } +} + +// ============================================================================ +// WAV parser +// ============================================================================ + +struct WavHeader { + uint32_t riff_id; + uint32_t file_size; + uint32_t wave_id; +}; + +struct WavChunk { + uint32_t id; + uint32_t size; +}; + +struct WavFmt { + uint16_t audio_format; + uint16_t channels; + uint32_t sample_rate; + uint32_t byte_rate; + uint16_t block_align; + uint16_t bits_per_sample; +}; + +static bool parse_wav(const uint8_t* data, uint64_t size) { + if (size < 44) return false; + + auto* hdr = (const WavHeader*)data; + if (hdr->riff_id != 0x46464952) return false; // "RIFF" + if (hdr->wave_id != 0x45564157) return false; // "WAVE" + + uint64_t pos = 12; + const WavFmt* fmt = nullptr; + + while (pos + 8 <= size) { + auto* chunk = (const WavChunk*)(data + pos); + uint64_t chunk_data = pos + 8; + + if (chunk->id == 0x20746D66) { // "fmt " + if (chunk->size < sizeof(WavFmt)) return false; + fmt = (const WavFmt*)(data + chunk_data); + } else if (chunk->id == 0x61746164) { // "data" + if (!fmt) return false; + if (fmt->audio_format != 1) return false; // PCM only + g.sample_rate = fmt->sample_rate; + g.channels = fmt->channels; + g.wav_data_offset = chunk_data; + g.wav_data_size = chunk->size; + g.wav_pos = 0; + int bytes_per_sample = (fmt->bits_per_sample / 8) * fmt->channels; + if (bytes_per_sample > 0) + g.total_samples = chunk->size / bytes_per_sample; + return true; + } + + pos = chunk_data + ((chunk->size + 1) & ~1u); // align to 2 bytes + } + + return false; +} + +// ============================================================================ +// MP3 helpers +// ============================================================================ + +static bool scan_mp3_info() { + // Scan through the file to estimate total duration + mp3dec_t tmp; + mp3dec_init(&tmp); + + uint64_t offset = 0; + g.total_samples = 0; + g.sample_rate = 0; + g.channels = 0; + + // Skip ID3v2 tag if present + if (g.file_size >= 10 && g.file_data[0] == 'I' && g.file_data[1] == 'D' && g.file_data[2] == '3') { + uint64_t tag_size = ((uint64_t)(g.file_data[6] & 0x7F) << 21) | + ((uint64_t)(g.file_data[7] & 0x7F) << 14) | + ((uint64_t)(g.file_data[8] & 0x7F) << 7) | + ((uint64_t)(g.file_data[9] & 0x7F)); + offset = tag_size + 10; + } + + // Quick scan: decode first frame for format, then estimate from bitrate + mp3dec_frame_info_t info; + int16_t pcm_tmp[MINIMP3_MAX_SAMPLES_PER_FRAME]; + int samples = mp3dec_decode_frame(&tmp, g.file_data + offset, + (int)(g.file_size - offset), pcm_tmp, &info); + if (samples <= 0 || info.hz == 0) return false; + + g.sample_rate = info.hz; + g.channels = info.channels; + + // Estimate total samples from file size and bitrate + if (info.bitrate_kbps > 0) { + uint64_t audio_bytes = g.file_size - offset; + // duration_sec = audio_bytes * 8 / (bitrate_kbps * 1000) + // total_samples = duration_sec * sample_rate + g.total_samples = (audio_bytes * 8 * (uint64_t)g.sample_rate) / + ((uint64_t)info.bitrate_kbps * 1000); + } else { + // VBR with no bitrate info — rough estimate + g.total_samples = g.file_size / 4 * (uint64_t)g.sample_rate / 11025; + } + + return true; +} + +// ============================================================================ +// Playback control +// ============================================================================ + +static void stop_playback() { + if (g.audio_handle >= 0) { + montauk::audio_close(g.audio_handle); + g.audio_handle = -1; + } + if (g.file_data) { + montauk::mfree(g.file_data); + g.file_data = nullptr; + } + g.play_state = PlayState::Stopped; + g.played_samples = 0; + g.pcm_buf_len = 0; + g.pcm_buf_pos = 0; + g.mp3_offset = 0; + g.wav_pos = 0; +} + +static bool start_track(int index) { + stop_playback(); + + if (index < 0 || index >= g.file_count) return false; + + // Build full path + char path[MAX_PATH * 2]; + snprintf(path, sizeof(path), "%s/%s", g.dir_path, g.files[index].name); + + // Open and read entire file + int fh = montauk::open(path); + if (fh < 0) return false; + + g.file_size = montauk::getsize(fh); + if (g.file_size == 0 || g.file_size > 64 * 1024 * 1024) { // 64 MiB limit + montauk::close(fh); + return false; + } + + g.file_data = (uint8_t*)montauk::malloc(g.file_size); + if (!g.file_data) { + montauk::close(fh); + return false; + } + + // Read in chunks (syscall may have size limits) + uint64_t off = 0; + while (off < g.file_size) { + uint64_t chunk = g.file_size - off; + if (chunk > 32768) chunk = 32768; + int rd = montauk::read(fh, g.file_data + off, off, chunk); + if (rd <= 0) break; + off += rd; + } + montauk::close(fh); + + if (off < g.file_size) { + montauk::mfree(g.file_data); + g.file_data = nullptr; + return false; + } + + // Parse format + if (g.files[index].is_mp3) { + mp3dec_init(&g.mp3dec); + g.mp3_offset = 0; + + // Skip ID3v2 + if (g.file_size >= 10 && g.file_data[0] == 'I' && g.file_data[1] == 'D' && g.file_data[2] == '3') { + g.mp3_offset = 10 + (((uint64_t)(g.file_data[6] & 0x7F) << 21) | + ((uint64_t)(g.file_data[7] & 0x7F) << 14) | + ((uint64_t)(g.file_data[8] & 0x7F) << 7) | + ((uint64_t)(g.file_data[9] & 0x7F))); + } + + if (!scan_mp3_info()) { + montauk::mfree(g.file_data); + g.file_data = nullptr; + return false; + } + } else { + if (!parse_wav(g.file_data, g.file_size)) { + montauk::mfree(g.file_data); + g.file_data = nullptr; + return false; + } + } + + // Open audio device + g.audio_handle = montauk::audio_open(g.sample_rate, g.channels, 16); + if (g.audio_handle < 0) { + montauk::mfree(g.file_data); + g.file_data = nullptr; + return false; + } + + g.current_track = index; + g.play_state = PlayState::Playing; + g.played_samples = 0; + g.pcm_buf_len = 0; + g.pcm_buf_pos = 0; + + return true; +} + +static void toggle_pause() { + if (g.play_state == PlayState::Playing) { + montauk::audio_pause(g.audio_handle); + g.play_state = PlayState::Paused; + } else if (g.play_state == PlayState::Paused) { + montauk::audio_resume(g.audio_handle); + g.play_state = PlayState::Playing; + } +} + +static void next_track() { + if (g.file_count == 0) return; + int next = (g.current_track + 1) % g.file_count; + start_track(next); +} + +static void prev_track() { + if (g.file_count == 0) return; + // If we're more than 3 seconds in, restart current track + if (g.sample_rate > 0 && g.played_samples > (uint64_t)g.sample_rate * 3) { + start_track(g.current_track); + return; + } + int prev = g.current_track - 1; + if (prev < 0) prev = g.file_count - 1; + start_track(prev); +} + +// ============================================================================ +// Audio feeding — call from the main loop +// ============================================================================ + +static void feed_audio() { + if (g.play_state != PlayState::Playing) return; + if (g.audio_handle < 0) return; + + // Try to feed a chunk of audio data + for (int iter = 0; iter < 4; iter++) { + // If we have leftover PCM in the buffer, write it + if (g.pcm_buf_len > 0) { + int bytes_avail = g.pcm_buf_len * 2; // 16-bit samples + int to_write = bytes_avail > AUDIO_CHUNK ? AUDIO_CHUNK : bytes_avail; + int written = montauk::audio_write(g.audio_handle, + (const uint8_t*)(g.pcm_buf + g.pcm_buf_pos), + to_write); + if (written <= 0) return; // device buffer full, try later + int samples_written = written / 2; + g.pcm_buf_pos += samples_written; + g.pcm_buf_len -= samples_written; + g.played_samples += samples_written / g.channels; + continue; + } + + // Decode more audio + g.pcm_buf_pos = 0; + g.pcm_buf_len = 0; + + if (g.current_track >= 0 && g.files[g.current_track].is_mp3) { + // MP3 decode + if (g.mp3_offset >= g.file_size) { + // End of file — advance to next track + next_track(); + return; + } + + mp3dec_frame_info_t info; + int samples = mp3dec_decode_frame(&g.mp3dec, + g.file_data + g.mp3_offset, + (int)(g.file_size - g.mp3_offset), + g.pcm_buf, &info); + if (info.frame_bytes > 0) { + g.mp3_offset += info.frame_bytes; + } else { + // Can't decode — end + next_track(); + return; + } + + if (samples > 0) { + g.pcm_buf_len = samples * info.channels; + } + } else { + // WAV decode — just copy PCM data + if (g.wav_pos >= g.wav_data_size) { + next_track(); + return; + } + + uint64_t remaining = g.wav_data_size - g.wav_pos; + uint64_t to_copy = remaining; + if (to_copy > sizeof(g.pcm_buf)) to_copy = sizeof(g.pcm_buf); + + memcpy(g.pcm_buf, g.file_data + g.wav_data_offset + g.wav_pos, to_copy); + g.wav_pos += to_copy; + g.pcm_buf_len = (int)(to_copy / 2); // 16-bit samples + } + } +} + +// ============================================================================ +// Time formatting +// ============================================================================ + +static void format_time(char* buf, int buf_size, uint64_t samples, int rate) { + if (rate <= 0) { snprintf(buf, buf_size, "--:--"); return; } + uint64_t secs = samples / rate; + int m = (int)(secs / 60); + int s = (int)(secs % 60); + snprintf(buf, buf_size, "%d:%02d", m, s); +} + +// ============================================================================ +// Render +// ============================================================================ + +static void render(uint32_t* pixels) { + int W = g.win_w, H = g.win_h; + int fh = font_h(); + int fh_sm = font_h(FONT_SIZE_SM); + + // Background + px_fill(pixels, W, H, 0, 0, W, H, BG_COLOR); + + // Toolbar + px_fill(pixels, W, H, 0, 0, W, TOOLBAR_H, TOOLBAR_BG); + px_hline(pixels, W, H, 0, TOOLBAR_H - 1, W, BORDER_COLOR); + // Directory path in toolbar + { + const char* display_dir = g.dir_path; + int dw = text_w(display_dir, FONT_SIZE_SM); + int max_dir_w = W - 24; + if (dw > max_dir_w) { + display_dir = g.dir_path + montauk::slen(g.dir_path) - 20; + } + px_text(pixels, W, H, 12, + (TOOLBAR_H - fh) / 2, display_dir, TEXT_COLOR); + } + + // ---- File list ---- + int list_bottom = H - INFO_H - PROGRESS_H - TRANSPORT_H; + int visible_items = (list_bottom - LIST_TOP) / LIST_ITEM_H; + + px_hline(pixels, W, H, 0, list_bottom, W, BORDER_COLOR); + + for (int i = 0; i < visible_items && (i + g.scroll_y) < g.file_count; i++) { + int idx = i + g.scroll_y; + int iy = LIST_TOP + i * LIST_ITEM_H; + + // Highlight + Color bg = BG_COLOR; + if (idx == g.current_track && g.play_state != PlayState::Stopped) + bg = LIST_PLAYING; + else if (idx == g.hovered_item) + bg = LIST_HOVER; + + if (bg.r != BG_COLOR.r || bg.g != BG_COLOR.g || bg.b != BG_COLOR.b) + px_fill(pixels, W, H, 0, iy, W, LIST_ITEM_H, bg); + + // Playing indicator + if (idx == g.current_track && g.play_state == PlayState::Playing) { + px_triangle_right(pixels, W, H, 8, iy + 8, 8, 12, ACCENT); + } else if (idx == g.current_track && g.play_state == PlayState::Paused) { + // Pause bars + px_fill(pixels, W, H, 8, iy + 8, 3, 12, ACCENT); + px_fill(pixels, W, H, 13, iy + 8, 3, 12, ACCENT); + } + + // File name + Color name_color = (idx == g.current_track && g.play_state != PlayState::Stopped) + ? ACCENT_DARK : TEXT_COLOR; + px_text(pixels, W, H, 24, iy + (LIST_ITEM_H - fh_sm) / 2, + g.files[idx].name, name_color, FONT_SIZE_SM); + + // Format tag + const char* tag = g.files[idx].is_mp3 ? "MP3" : "WAV"; + int tw = text_w(tag, FONT_SIZE_SM); + px_text(pixels, W, H, W - tw - 12, iy + (LIST_ITEM_H - fh_sm) / 2, + tag, DIM_TEXT, FONT_SIZE_SM); + } + + // Scrollbar + if (g.file_count > visible_items && visible_items > 0) { + int list_h = list_bottom - LIST_TOP; + int sb_h = (visible_items * list_h) / g.file_count; + if (sb_h < 20) sb_h = 20; + int sb_y = LIST_TOP + (g.scroll_y * (list_h - sb_h)) / (g.file_count - visible_items); + px_fill_rounded(pixels, W, H, W - 6, sb_y, 4, sb_h, 2, TRACK_BG); + } + + // Empty state + if (g.file_count == 0) { + const char* msg = "No audio files found"; + int mw = text_w(msg); + int cy = LIST_TOP + (list_bottom - LIST_TOP) / 2 - fh / 2; + px_text(pixels, W, H, (W - mw) / 2, cy, msg, DIM_TEXT); + } + + // ---- Now Playing info ---- + int info_y = list_bottom + 1; + px_fill(pixels, W, H, 0, info_y, W, INFO_H, TOOLBAR_BG); + px_hline(pixels, W, H, 0, info_y + INFO_H - 1, W, BORDER_COLOR); + + if (g.current_track >= 0 && g.play_state != PlayState::Stopped) { + // Track name + px_text(pixels, W, H, 12, info_y + 8, + g.files[g.current_track].name, TEXT_COLOR, FONT_SIZE); + + // Time display + char time_cur[16], time_total[16], time_str[40]; + format_time(time_cur, sizeof(time_cur), g.played_samples, g.sample_rate); + format_time(time_total, sizeof(time_total), g.total_samples, g.sample_rate); + snprintf(time_str, sizeof(time_str), "%s / %s", time_cur, time_total); + px_text(pixels, W, H, 12, info_y + 8 + fh + 4, time_str, DIM_TEXT, FONT_SIZE_SM); + + // State indicator on right + const char* state_str = (g.play_state == PlayState::Playing) ? "Playing" : "Paused"; + int sw = text_w(state_str, FONT_SIZE_SM); + px_text(pixels, W, H, W - sw - 12, info_y + 8 + fh + 4, + state_str, ACCENT, FONT_SIZE_SM); + } else { + const char* msg = "No track selected"; + px_text(pixels, W, H, 12, info_y + (INFO_H - fh) / 2, msg, DIM_TEXT); + } + + // ---- Progress bar ---- + int prog_y = info_y + INFO_H; + px_fill(pixels, W, H, 0, prog_y, W, PROGRESS_H, Color::from_rgb(0xF0, 0xF0, 0xF0)); + + int bar_x = 16, bar_w = W - 32, bar_h = 6; + int bar_y = prog_y + (PROGRESS_H - bar_h) / 2; + px_fill_rounded(pixels, W, H, bar_x, bar_y, bar_w, bar_h, 3, TRACK_BG); + + if (g.total_samples > 0 && g.play_state != PlayState::Stopped) { + int fill = (int)((g.played_samples * bar_w) / g.total_samples); + if (fill > bar_w) fill = bar_w; + if (fill > 0) + px_fill_rounded(pixels, W, H, bar_x, bar_y, fill, bar_h, 3, ACCENT); + // Knob + px_circle(pixels, W, H, bar_x + fill, bar_y + bar_h / 2, 5, ACCENT); + px_circle(pixels, W, H, bar_x + fill, bar_y + bar_h / 2, 2, WHITE); + } + + // ---- Transport controls ---- + int trans_y = prog_y + PROGRESS_H; + px_fill(pixels, W, H, 0, trans_y, W, TRANSPORT_H, BG_COLOR); + + int btn_w = 48, btn_h = 32, btn_r = 6, gap = 8; + int total_w = btn_w * 4 + gap * 3; // prev, play, stop, next + int bx = (W - total_w) / 2; + int by = trans_y + (TRANSPORT_H - btn_h) / 2; + + // [<<] Prev + px_icon_button(pixels, W, H, bx, by, btn_w, btn_h, g.ico_rewind, BTN_BG, btn_r); + bx += btn_w + gap; + + // [Play/Pause] + { + bool active = (g.play_state == PlayState::Playing); + Color bg = active ? ACCENT : BTN_BG; + const SvgIcon& ic = active ? g.ico_pause_w : g.ico_play; + px_icon_button(pixels, W, H, bx, by, btn_w, btn_h, ic, bg, btn_r); + } + bx += btn_w + gap; + + // [Stop] + { + bool active = (g.play_state != PlayState::Stopped); + Color bg = active ? STOP_COLOR : BTN_BG; + const SvgIcon& ic = active ? g.ico_stop_w : g.ico_stop; + px_icon_button(pixels, W, H, bx, by, btn_w, btn_h, ic, bg, btn_r); + } + bx += btn_w + gap; + + // [>>] Next + px_icon_button(pixels, W, H, bx, by, btn_w, btn_h, g.ico_forward, BTN_BG, btn_r); +} + +// ============================================================================ +// Hit testing +// ============================================================================ + +static int list_bottom() { + return g.win_h - INFO_H - PROGRESS_H - TRANSPORT_H; +} + +static int visible_items() { + return (list_bottom() - LIST_TOP) / LIST_ITEM_H; +} + +static bool handle_click(int mx, int my) { + int W = g.win_w; + int lb = list_bottom(); + int info_y = lb + 1; + int prog_y = info_y + INFO_H; + int trans_y = prog_y + PROGRESS_H; + + // File list click + if (my >= LIST_TOP && my < lb && mx >= 0 && mx < W) { + int idx = (my - LIST_TOP) / LIST_ITEM_H + g.scroll_y; + if (idx >= 0 && idx < g.file_count) { + start_track(idx); + return true; + } + } + + // Progress bar click + if (my >= prog_y && my < prog_y + PROGRESS_H) { + if (g.play_state != PlayState::Stopped && g.total_samples > 0) { + int bar_x = 16, bar_w = W - 32; + int rel = mx - bar_x; + if (rel < 0) rel = 0; + if (rel > bar_w) rel = bar_w; + // Seek: restart track and skip ahead + // For simplicity, we estimate the file offset from the progress ratio + uint64_t target_samples = (uint64_t)rel * g.total_samples / bar_w; + + if (g.current_track >= 0 && g.files[g.current_track].is_mp3) { + // MP3 seek: estimate byte offset from ratio + uint64_t target_offset = (g.file_size * rel) / bar_w; + g.mp3_offset = target_offset; + g.played_samples = target_samples; + g.pcm_buf_len = 0; + g.pcm_buf_pos = 0; + mp3dec_init(&g.mp3dec); // reset decoder for clean seek + } else { + // WAV seek: direct offset + int bytes_per_sample = 2 * g.channels; + g.wav_pos = target_samples * bytes_per_sample; + if (g.wav_pos > g.wav_data_size) g.wav_pos = g.wav_data_size; + g.played_samples = target_samples; + g.pcm_buf_len = 0; + g.pcm_buf_pos = 0; + } + g.dragging_progress = true; + return true; + } + } + + // Transport buttons + if (my >= trans_y && my < trans_y + TRANSPORT_H) { + int btn_w = 48, btn_h = 32, gap = 8; + int total_w = btn_w * 4 + gap * 3; + int bx = (W - total_w) / 2; + int by = trans_y + (TRANSPORT_H - btn_h) / 2; + + if (my >= by && my < by + btn_h) { + // Prev + if (mx >= bx && mx < bx + btn_w) { prev_track(); return true; } + bx += btn_w + gap; + + // Play/Pause + if (mx >= bx && mx < bx + btn_w) { + if (g.play_state == PlayState::Stopped && g.current_track >= 0) { + start_track(g.current_track); + } else if (g.play_state == PlayState::Stopped && g.file_count > 0) { + start_track(0); + } else { + toggle_pause(); + } + return true; + } + bx += btn_w + gap; + + // Stop + if (mx >= bx && mx < bx + btn_w) { stop_playback(); return true; } + bx += btn_w + gap; + + // Next + if (mx >= bx && mx < bx + btn_w) { next_track(); return true; } + } + } + + return false; +} + +// ============================================================================ +// Entry point +// ============================================================================ + +extern "C" void _start() { + // Init state + memset(&g, 0, sizeof(g)); + g.win_w = INIT_W; + g.win_h = INIT_H; + g.audio_handle = -1; + g.current_track = -1; + g.play_state = PlayState::Stopped; + g.hovered_item = -1; + + // Parse arguments: accept a directory path or a file path + char arg_file[128] = {}; + { + char args[256]; + int arglen = montauk::getargs(args, sizeof(args)); + if (arglen > 0 && args[0]) { + if (str_ends_with(args, ".mp3") || str_ends_with(args, ".wav")) { + // File path: extract directory and filename + int last_slash = -1; + for (int i = 0; args[i]; i++) { + if (args[i] == '/') last_slash = i; + } + if (last_slash > 0) { + memcpy(g.dir_path, args, last_slash); + g.dir_path[last_slash] = '\0'; + const char* fname = args + last_slash + 1; + int flen = montauk::slen(fname); + if (flen > 0 && flen < (int)sizeof(arg_file)) { + memcpy(arg_file, fname, flen); + arg_file[flen] = '\0'; + } + } else { + memcpy(g.dir_path, "0:/home", 8); + } + } else { + int len = montauk::slen(args); + if (len >= (int)sizeof(g.dir_path)) len = sizeof(g.dir_path) - 1; + memcpy(g.dir_path, args, len); + g.dir_path[len] = '\0'; + } + } else { + memcpy(g.dir_path, "0:/home", 8); + } + } + + // Load font + { + TrueTypeFont* f = (TrueTypeFont*)montauk::malloc(sizeof(TrueTypeFont)); + if (f) { + montauk::memset(f, 0, sizeof(TrueTypeFont)); + if (!f->init("0:/fonts/Roboto-Medium.ttf")) { montauk::mfree(f); f = nullptr; } + } + g.font = f; + } + + // Load transport icons (dark + white variants for colored backgrounds) + { + Color ic_color = TEXT_COLOR; + g.ico_rewind = svg_load("0:/icons/media-rewind.svg", 16, 16, ic_color); + g.ico_play = svg_load("0:/icons/media-play.svg", 16, 16, ic_color); + g.ico_pause = svg_load("0:/icons/media-pause.svg", 16, 16, ic_color); + g.ico_stop = svg_load("0:/icons/media-stop.svg", 16, 16, ic_color); + g.ico_forward = svg_load("0:/icons/media-forward.svg", 16, 16, ic_color); + g.ico_play_w = svg_load("0:/icons/media-play.svg", 16, 16, WHITE); + g.ico_pause_w = svg_load("0:/icons/media-pause.svg", 16, 16, WHITE); + g.ico_stop_w = svg_load("0:/icons/media-stop.svg", 16, 16, WHITE); + } + + // Scan for audio files + scan_directory(); + + // If a specific file was requested, find it and auto-play + int auto_play_idx = -1; + if (arg_file[0]) { + for (int i = 0; i < g.file_count; i++) { + if (str_ends_with(g.files[i].name, arg_file) && + montauk::slen(g.files[i].name) == montauk::slen(arg_file)) { + auto_play_idx = i; + break; + } + } + } + + // Create window + Montauk::WinCreateResult wres; + if (montauk::win_create("Music", g.win_w, g.win_h, &wres) < 0 || wres.id < 0) + montauk::exit(1); + + int win_id = wres.id; + uint32_t* pixels = (uint32_t*)(uintptr_t)wres.pixelVa; + + // Auto-play the requested file + if (auto_play_idx >= 0) { + start_track(auto_play_idx); + } + + render(pixels); + montauk::win_present(win_id); + + uint64_t last_render = montauk::get_milliseconds(); + + while (true) { + Montauk::WinEvent ev; + int r = montauk::win_poll(win_id, &ev); + + if (r < 0) break; + + bool redraw = false; + + if (r > 0) { + if (ev.type == 3) break; // close + + // Keyboard + if (ev.type == 0 && ev.key.pressed) { + if (ev.key.scancode == 0x01) break; // Escape + if (ev.key.ascii == ' ') { + if (g.play_state == PlayState::Stopped && g.file_count > 0) + start_track(g.current_track >= 0 ? g.current_track : 0); + else + toggle_pause(); + redraw = true; + } + if (ev.key.ascii == 's' || ev.key.ascii == 'S') { + stop_playback(); + redraw = true; + } + if (ev.key.scancode == 0x4D) { // Right arrow + next_track(); + redraw = true; + } + if (ev.key.scancode == 0x4B) { // Left arrow + prev_track(); + redraw = true; + } + } + + // Mouse click + if (ev.type == 1) { + bool clicked = (ev.mouse.buttons & 1) && !(ev.mouse.prev_buttons & 1); + bool released = !(ev.mouse.buttons & 1) && (ev.mouse.prev_buttons & 1); + + if (clicked) { + if (handle_click(ev.mouse.x, ev.mouse.y)) + redraw = true; + } + + if (released) { + g.dragging_progress = false; + } + + // Progress bar drag + if (g.dragging_progress && (ev.mouse.buttons & 1)) { + int bar_x = 16, bar_w = g.win_w - 32; + int rel = ev.mouse.x - bar_x; + if (rel < 0) rel = 0; + if (rel > bar_w) rel = bar_w; + uint64_t target_samples = (uint64_t)rel * g.total_samples / bar_w; + + if (g.current_track >= 0 && g.files[g.current_track].is_mp3) { + uint64_t target_offset = (g.file_size * rel) / bar_w; + g.mp3_offset = target_offset; + g.played_samples = target_samples; + g.pcm_buf_len = 0; + mp3dec_init(&g.mp3dec); + } else { + int bytes_per_sample = 2 * g.channels; + g.wav_pos = target_samples * bytes_per_sample; + if (g.wav_pos > g.wav_data_size) g.wav_pos = g.wav_data_size; + g.played_samples = target_samples; + g.pcm_buf_len = 0; + } + redraw = true; + } + + // Hover tracking for file list + int lb = list_bottom(); + if (ev.mouse.y >= LIST_TOP && ev.mouse.y < lb) { + int new_hover = (ev.mouse.y - LIST_TOP) / LIST_ITEM_H + g.scroll_y; + if (new_hover >= g.file_count) new_hover = -1; + if (new_hover != g.hovered_item) { + g.hovered_item = new_hover; + redraw = true; + } + } else if (g.hovered_item != -1) { + g.hovered_item = -1; + redraw = true; + } + + // Scroll + if (ev.mouse.scroll != 0) { + g.scroll_y -= ev.mouse.scroll * 2; + int max_scroll = g.file_count - visible_items(); + if (max_scroll < 0) max_scroll = 0; + if (g.scroll_y < 0) g.scroll_y = 0; + if (g.scroll_y > max_scroll) g.scroll_y = max_scroll; + redraw = true; + } + } + + // Resize + if (ev.type == 2) { + g.win_w = ev.resize.w; + g.win_h = ev.resize.h; + pixels = (uint32_t*)(uintptr_t)montauk::win_resize(win_id, g.win_w, g.win_h); + redraw = true; + } + } + + // Feed audio data to the device + feed_audio(); + + // Periodic redraw while playing (update time display) + uint64_t now = montauk::get_milliseconds(); + if (g.play_state == PlayState::Playing && now - last_render >= 250) { + redraw = true; + } + + if (redraw) { + render(pixels); + montauk::win_present(win_id); + last_render = now; + } + + // Sleep less when playing to keep audio fed + if (g.play_state == PlayState::Playing) + montauk::sleep_ms(4); + else + montauk::sleep_ms(16); + } + + stop_playback(); + montauk::win_destroy(win_id); + montauk::exit(0); +} diff --git a/programs/src/music/manifest.toml b/programs/src/music/manifest.toml new file mode 100644 index 0000000..e4e7888 --- /dev/null +++ b/programs/src/music/manifest.toml @@ -0,0 +1,8 @@ +[app] +name = "Music" +binary = "music.elf" +icon = "audio-player.svg" + +[menu] +category = "Applications" +visible = true diff --git a/programs/src/music/minimp3.h b/programs/src/music/minimp3.h new file mode 100644 index 0000000..3220ae1 --- /dev/null +++ b/programs/src/music/minimp3.h @@ -0,0 +1,1865 @@ +#ifndef MINIMP3_H +#define MINIMP3_H +/* + https://github.com/lieff/minimp3 + To the extent possible under law, the author(s) have dedicated all copyright and related and neighboring rights to this software to the public domain worldwide. + This software is distributed without any warranty. + See . +*/ +#include + +#define MINIMP3_MAX_SAMPLES_PER_FRAME (1152*2) + +typedef struct +{ + int frame_bytes, frame_offset, channels, hz, layer, bitrate_kbps; +} mp3dec_frame_info_t; + +typedef struct +{ + float mdct_overlap[2][9*32], qmf_state[15*2*32]; + int reserv, free_format_bytes; + unsigned char header[4], reserv_buf[511]; +} mp3dec_t; + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +void mp3dec_init(mp3dec_t *dec); +#ifndef MINIMP3_FLOAT_OUTPUT +typedef int16_t mp3d_sample_t; +#else /* MINIMP3_FLOAT_OUTPUT */ +typedef float mp3d_sample_t; +void mp3dec_f32_to_s16(const float *in, int16_t *out, int num_samples); +#endif /* MINIMP3_FLOAT_OUTPUT */ +int mp3dec_decode_frame(mp3dec_t *dec, const uint8_t *mp3, int mp3_bytes, mp3d_sample_t *pcm, mp3dec_frame_info_t *info); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ + +#endif /* MINIMP3_H */ +#if defined(MINIMP3_IMPLEMENTATION) && !defined(_MINIMP3_IMPLEMENTATION_GUARD) +#define _MINIMP3_IMPLEMENTATION_GUARD + +#include +#include + +#define MAX_FREE_FORMAT_FRAME_SIZE 2304 /* more than ISO spec's */ +#ifndef MAX_FRAME_SYNC_MATCHES +#define MAX_FRAME_SYNC_MATCHES 10 +#endif /* MAX_FRAME_SYNC_MATCHES */ + +#define MAX_L3_FRAME_PAYLOAD_BYTES MAX_FREE_FORMAT_FRAME_SIZE /* MUST be >= 320000/8/32000*1152 = 1440 */ + +#define MAX_BITRESERVOIR_BYTES 511 +#define SHORT_BLOCK_TYPE 2 +#define STOP_BLOCK_TYPE 3 +#define MODE_MONO 3 +#define MODE_JOINT_STEREO 1 +#define HDR_SIZE 4 +#define HDR_IS_MONO(h) (((h[3]) & 0xC0) == 0xC0) +#define HDR_IS_MS_STEREO(h) (((h[3]) & 0xE0) == 0x60) +#define HDR_IS_FREE_FORMAT(h) (((h[2]) & 0xF0) == 0) +#define HDR_IS_CRC(h) (!((h[1]) & 1)) +#define HDR_TEST_PADDING(h) ((h[2]) & 0x2) +#define HDR_TEST_MPEG1(h) ((h[1]) & 0x8) +#define HDR_TEST_NOT_MPEG25(h) ((h[1]) & 0x10) +#define HDR_TEST_I_STEREO(h) ((h[3]) & 0x10) +#define HDR_TEST_MS_STEREO(h) ((h[3]) & 0x20) +#define HDR_GET_STEREO_MODE(h) (((h[3]) >> 6) & 3) +#define HDR_GET_STEREO_MODE_EXT(h) (((h[3]) >> 4) & 3) +#define HDR_GET_LAYER(h) (((h[1]) >> 1) & 3) +#define HDR_GET_BITRATE(h) ((h[2]) >> 4) +#define HDR_GET_SAMPLE_RATE(h) (((h[2]) >> 2) & 3) +#define HDR_GET_MY_SAMPLE_RATE(h) (HDR_GET_SAMPLE_RATE(h) + (((h[1] >> 3) & 1) + ((h[1] >> 4) & 1))*3) +#define HDR_IS_FRAME_576(h) ((h[1] & 14) == 2) +#define HDR_IS_LAYER_1(h) ((h[1] & 6) == 6) + +#define BITS_DEQUANTIZER_OUT -1 +#define MAX_SCF (255 + BITS_DEQUANTIZER_OUT*4 - 210) +#define MAX_SCFI ((MAX_SCF + 3) & ~3) + +#define MINIMP3_MIN(a, b) ((a) > (b) ? (b) : (a)) +#define MINIMP3_MAX(a, b) ((a) < (b) ? (b) : (a)) + +#if !defined(MINIMP3_NO_SIMD) + +#if !defined(MINIMP3_ONLY_SIMD) && (defined(_M_X64) || defined(__x86_64__) || defined(__aarch64__) || defined(_M_ARM64)) +/* x64 always have SSE2, arm64 always have neon, no need for generic code */ +#define MINIMP3_ONLY_SIMD +#endif /* SIMD checks... */ + +#if (defined(_MSC_VER) && (defined(_M_IX86) || defined(_M_X64))) || ((defined(__i386__) || defined(__x86_64__)) && defined(__SSE2__)) +#if defined(_MSC_VER) +#include +#endif /* defined(_MSC_VER) */ +#include +#define HAVE_SSE 1 +#define HAVE_SIMD 1 +#define VSTORE _mm_storeu_ps +#define VLD _mm_loadu_ps +#define VSET _mm_set1_ps +#define VADD _mm_add_ps +#define VSUB _mm_sub_ps +#define VMUL _mm_mul_ps +#define VMAC(a, x, y) _mm_add_ps(a, _mm_mul_ps(x, y)) +#define VMSB(a, x, y) _mm_sub_ps(a, _mm_mul_ps(x, y)) +#define VMUL_S(x, s) _mm_mul_ps(x, _mm_set1_ps(s)) +#define VREV(x) _mm_shuffle_ps(x, x, _MM_SHUFFLE(0, 1, 2, 3)) +typedef __m128 f4; +#if defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) +#define minimp3_cpuid __cpuid +#else /* defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) */ +static __inline__ __attribute__((always_inline)) void minimp3_cpuid(int CPUInfo[], const int InfoType) +{ +#if defined(__PIC__) + __asm__ __volatile__( +#if defined(__x86_64__) + "push %%rbx\n" + "cpuid\n" + "xchgl %%ebx, %1\n" + "pop %%rbx\n" +#else /* defined(__x86_64__) */ + "xchgl %%ebx, %1\n" + "cpuid\n" + "xchgl %%ebx, %1\n" +#endif /* defined(__x86_64__) */ + : "=a" (CPUInfo[0]), "=r" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "a" (InfoType)); +#else /* defined(__PIC__) */ + __asm__ __volatile__( + "cpuid" + : "=a" (CPUInfo[0]), "=b" (CPUInfo[1]), "=c" (CPUInfo[2]), "=d" (CPUInfo[3]) + : "a" (InfoType)); +#endif /* defined(__PIC__)*/ +} +#endif /* defined(_MSC_VER) || defined(MINIMP3_ONLY_SIMD) */ +static int have_simd(void) +{ +#ifdef MINIMP3_ONLY_SIMD + return 1; +#else /* MINIMP3_ONLY_SIMD */ + static int g_have_simd; + int CPUInfo[4]; +#ifdef MINIMP3_TEST + static int g_counter; + if (g_counter++ > 100) + return 0; +#endif /* MINIMP3_TEST */ + if (g_have_simd) + goto end; + minimp3_cpuid(CPUInfo, 0); + g_have_simd = 1; + if (CPUInfo[0] > 0) + { + minimp3_cpuid(CPUInfo, 1); + g_have_simd = (CPUInfo[3] & (1 << 26)) + 1; /* SSE2 */ + } +end: + return g_have_simd - 1; +#endif /* MINIMP3_ONLY_SIMD */ +} +#elif defined(__ARM_NEON) || defined(__aarch64__) || defined(_M_ARM64) +#include +#define HAVE_SSE 0 +#define HAVE_SIMD 1 +#define VSTORE vst1q_f32 +#define VLD vld1q_f32 +#define VSET vmovq_n_f32 +#define VADD vaddq_f32 +#define VSUB vsubq_f32 +#define VMUL vmulq_f32 +#define VMAC(a, x, y) vmlaq_f32(a, x, y) +#define VMSB(a, x, y) vmlsq_f32(a, x, y) +#define VMUL_S(x, s) vmulq_f32(x, vmovq_n_f32(s)) +#define VREV(x) vcombine_f32(vget_high_f32(vrev64q_f32(x)), vget_low_f32(vrev64q_f32(x))) +typedef float32x4_t f4; +static int have_simd() +{ /* TODO: detect neon for !MINIMP3_ONLY_SIMD */ + return 1; +} +#else /* SIMD checks... */ +#define HAVE_SSE 0 +#define HAVE_SIMD 0 +#ifdef MINIMP3_ONLY_SIMD +#error MINIMP3_ONLY_SIMD used, but SSE/NEON not enabled +#endif /* MINIMP3_ONLY_SIMD */ +#endif /* SIMD checks... */ +#else /* !defined(MINIMP3_NO_SIMD) */ +#define HAVE_SIMD 0 +#endif /* !defined(MINIMP3_NO_SIMD) */ + +#if defined(__ARM_ARCH) && (__ARM_ARCH >= 6) && !defined(__aarch64__) && !defined(_M_ARM64) +#define HAVE_ARMV6 1 +static __inline__ __attribute__((always_inline)) int32_t minimp3_clip_int16_arm(int32_t a) +{ + int32_t x = 0; + __asm__ ("ssat %0, #16, %1" : "=r"(x) : "r"(a)); + return x; +} +#else +#define HAVE_ARMV6 0 +#endif + +typedef struct +{ + const uint8_t *buf; + int pos, limit; +} bs_t; + +typedef struct +{ + float scf[3*64]; + uint8_t total_bands, stereo_bands, bitalloc[64], scfcod[64]; +} L12_scale_info; + +typedef struct +{ + uint8_t tab_offset, code_tab_width, band_count; +} L12_subband_alloc_t; + +typedef struct +{ + const uint8_t *sfbtab; + uint16_t part_23_length, big_values, scalefac_compress; + uint8_t global_gain, block_type, mixed_block_flag, n_long_sfb, n_short_sfb; + uint8_t table_select[3], region_count[3], subblock_gain[3]; + uint8_t preflag, scalefac_scale, count1_table, scfsi; +} L3_gr_info_t; + +typedef struct +{ + bs_t bs; + uint8_t maindata[MAX_BITRESERVOIR_BYTES + MAX_L3_FRAME_PAYLOAD_BYTES]; + L3_gr_info_t gr_info[4]; + float grbuf[2][576], scf[40], syn[18 + 15][2*32]; + uint8_t ist_pos[2][39]; +} mp3dec_scratch_t; + +static void bs_init(bs_t *bs, const uint8_t *data, int bytes) +{ + bs->buf = data; + bs->pos = 0; + bs->limit = bytes*8; +} + +static uint32_t get_bits(bs_t *bs, int n) +{ + uint32_t next, cache = 0, s = bs->pos & 7; + int shl = n + s; + const uint8_t *p = bs->buf + (bs->pos >> 3); + if ((bs->pos += n) > bs->limit) + return 0; + next = *p++ & (255 >> s); + while ((shl -= 8) > 0) + { + cache |= next << shl; + next = *p++; + } + return cache | (next >> -shl); +} + +static int hdr_valid(const uint8_t *h) +{ + return h[0] == 0xff && + ((h[1] & 0xF0) == 0xf0 || (h[1] & 0xFE) == 0xe2) && + (HDR_GET_LAYER(h) != 0) && + (HDR_GET_BITRATE(h) != 15) && + (HDR_GET_SAMPLE_RATE(h) != 3); +} + +static int hdr_compare(const uint8_t *h1, const uint8_t *h2) +{ + return hdr_valid(h2) && + ((h1[1] ^ h2[1]) & 0xFE) == 0 && + ((h1[2] ^ h2[2]) & 0x0C) == 0 && + !(HDR_IS_FREE_FORMAT(h1) ^ HDR_IS_FREE_FORMAT(h2)); +} + +static unsigned hdr_bitrate_kbps(const uint8_t *h) +{ + static const uint8_t halfrate[2][3][15] = { + { { 0,4,8,12,16,20,24,28,32,40,48,56,64,72,80 }, { 0,4,8,12,16,20,24,28,32,40,48,56,64,72,80 }, { 0,16,24,28,32,40,48,56,64,72,80,88,96,112,128 } }, + { { 0,16,20,24,28,32,40,48,56,64,80,96,112,128,160 }, { 0,16,24,28,32,40,48,56,64,80,96,112,128,160,192 }, { 0,16,32,48,64,80,96,112,128,144,160,176,192,208,224 } }, + }; + return 2*halfrate[!!HDR_TEST_MPEG1(h)][HDR_GET_LAYER(h) - 1][HDR_GET_BITRATE(h)]; +} + +static unsigned hdr_sample_rate_hz(const uint8_t *h) +{ + static const unsigned g_hz[3] = { 44100, 48000, 32000 }; + return g_hz[HDR_GET_SAMPLE_RATE(h)] >> (int)!HDR_TEST_MPEG1(h) >> (int)!HDR_TEST_NOT_MPEG25(h); +} + +static unsigned hdr_frame_samples(const uint8_t *h) +{ + return HDR_IS_LAYER_1(h) ? 384 : (1152 >> (int)HDR_IS_FRAME_576(h)); +} + +static int hdr_frame_bytes(const uint8_t *h, int free_format_size) +{ + int frame_bytes = hdr_frame_samples(h)*hdr_bitrate_kbps(h)*125/hdr_sample_rate_hz(h); + if (HDR_IS_LAYER_1(h)) + { + frame_bytes &= ~3; /* slot align */ + } + return frame_bytes ? frame_bytes : free_format_size; +} + +static int hdr_padding(const uint8_t *h) +{ + return HDR_TEST_PADDING(h) ? (HDR_IS_LAYER_1(h) ? 4 : 1) : 0; +} + +#ifndef MINIMP3_ONLY_MP3 +static const L12_subband_alloc_t *L12_subband_alloc_table(const uint8_t *hdr, L12_scale_info *sci) +{ + const L12_subband_alloc_t *alloc; + int mode = HDR_GET_STEREO_MODE(hdr); + int nbands, stereo_bands = (mode == MODE_MONO) ? 0 : (mode == MODE_JOINT_STEREO) ? (HDR_GET_STEREO_MODE_EXT(hdr) << 2) + 4 : 32; + + if (HDR_IS_LAYER_1(hdr)) + { + static const L12_subband_alloc_t g_alloc_L1[] = { { 76, 4, 32 } }; + alloc = g_alloc_L1; + nbands = 32; + } else if (!HDR_TEST_MPEG1(hdr)) + { + static const L12_subband_alloc_t g_alloc_L2M2[] = { { 60, 4, 4 }, { 44, 3, 7 }, { 44, 2, 19 } }; + alloc = g_alloc_L2M2; + nbands = 30; + } else + { + static const L12_subband_alloc_t g_alloc_L2M1[] = { { 0, 4, 3 }, { 16, 4, 8 }, { 32, 3, 12 }, { 40, 2, 7 } }; + int sample_rate_idx = HDR_GET_SAMPLE_RATE(hdr); + unsigned kbps = hdr_bitrate_kbps(hdr) >> (int)(mode != MODE_MONO); + if (!kbps) /* free-format */ + { + kbps = 192; + } + + alloc = g_alloc_L2M1; + nbands = 27; + if (kbps < 56) + { + static const L12_subband_alloc_t g_alloc_L2M1_lowrate[] = { { 44, 4, 2 }, { 44, 3, 10 } }; + alloc = g_alloc_L2M1_lowrate; + nbands = sample_rate_idx == 2 ? 12 : 8; + } else if (kbps >= 96 && sample_rate_idx != 1) + { + nbands = 30; + } + } + + sci->total_bands = (uint8_t)nbands; + sci->stereo_bands = (uint8_t)MINIMP3_MIN(stereo_bands, nbands); + + return alloc; +} + +static void L12_read_scalefactors(bs_t *bs, uint8_t *pba, uint8_t *scfcod, int bands, float *scf) +{ + static const float g_deq_L12[18*3] = { +#define DQ(x) 9.53674316e-07f/x, 7.56931807e-07f/x, 6.00777173e-07f/x + DQ(3),DQ(7),DQ(15),DQ(31),DQ(63),DQ(127),DQ(255),DQ(511),DQ(1023),DQ(2047),DQ(4095),DQ(8191),DQ(16383),DQ(32767),DQ(65535),DQ(3),DQ(5),DQ(9) + }; + int i, m; + for (i = 0; i < bands; i++) + { + float s = 0; + int ba = *pba++; + int mask = ba ? 4 + ((19 >> scfcod[i]) & 3) : 0; + for (m = 4; m; m >>= 1) + { + if (mask & m) + { + int b = get_bits(bs, 6); + s = g_deq_L12[ba*3 - 6 + b % 3]*(1 << 21 >> b/3); + } + *scf++ = s; + } + } +} + +static void L12_read_scale_info(const uint8_t *hdr, bs_t *bs, L12_scale_info *sci) +{ + static const uint8_t g_bitalloc_code_tab[] = { + 0,17, 3, 4, 5,6,7, 8,9,10,11,12,13,14,15,16, + 0,17,18, 3,19,4,5, 6,7, 8, 9,10,11,12,13,16, + 0,17,18, 3,19,4,5,16, + 0,17,18,16, + 0,17,18,19, 4,5,6, 7,8, 9,10,11,12,13,14,15, + 0,17,18, 3,19,4,5, 6,7, 8, 9,10,11,12,13,14, + 0, 2, 3, 4, 5,6,7, 8,9,10,11,12,13,14,15,16 + }; + const L12_subband_alloc_t *subband_alloc = L12_subband_alloc_table(hdr, sci); + + int i, k = 0, ba_bits = 0; + const uint8_t *ba_code_tab = g_bitalloc_code_tab; + + for (i = 0; i < sci->total_bands; i++) + { + uint8_t ba; + if (i == k) + { + k += subband_alloc->band_count; + ba_bits = subband_alloc->code_tab_width; + ba_code_tab = g_bitalloc_code_tab + subband_alloc->tab_offset; + subband_alloc++; + } + ba = ba_code_tab[get_bits(bs, ba_bits)]; + sci->bitalloc[2*i] = ba; + if (i < sci->stereo_bands) + { + ba = ba_code_tab[get_bits(bs, ba_bits)]; + } + sci->bitalloc[2*i + 1] = sci->stereo_bands ? ba : 0; + } + + for (i = 0; i < 2*sci->total_bands; i++) + { + sci->scfcod[i] = sci->bitalloc[i] ? HDR_IS_LAYER_1(hdr) ? 2 : get_bits(bs, 2) : 6; + } + + L12_read_scalefactors(bs, sci->bitalloc, sci->scfcod, sci->total_bands*2, sci->scf); + + for (i = sci->stereo_bands; i < sci->total_bands; i++) + { + sci->bitalloc[2*i + 1] = 0; + } +} + +static int L12_dequantize_granule(float *grbuf, bs_t *bs, L12_scale_info *sci, int group_size) +{ + int i, j, k, choff = 576; + for (j = 0; j < 4; j++) + { + float *dst = grbuf + group_size*j; + for (i = 0; i < 2*sci->total_bands; i++) + { + int ba = sci->bitalloc[i]; + if (ba != 0) + { + if (ba < 17) + { + int half = (1 << (ba - 1)) - 1; + for (k = 0; k < group_size; k++) + { + dst[k] = (float)((int)get_bits(bs, ba) - half); + } + } else + { + unsigned mod = (2 << (ba - 17)) + 1; /* 3, 5, 9 */ + unsigned code = get_bits(bs, mod + 2 - (mod >> 3)); /* 5, 7, 10 */ + for (k = 0; k < group_size; k++, code /= mod) + { + dst[k] = (float)((int)(code % mod - mod/2)); + } + } + } + dst += choff; + choff = 18 - choff; + } + } + return group_size*4; +} + +static void L12_apply_scf_384(L12_scale_info *sci, const float *scf, float *dst) +{ + int i, k; + memcpy(dst + 576 + sci->stereo_bands*18, dst + sci->stereo_bands*18, (sci->total_bands - sci->stereo_bands)*18*sizeof(float)); + for (i = 0; i < sci->total_bands; i++, dst += 18, scf += 6) + { + for (k = 0; k < 12; k++) + { + dst[k + 0] *= scf[0]; + dst[k + 576] *= scf[3]; + } + } +} +#endif /* MINIMP3_ONLY_MP3 */ + +static int L3_read_side_info(bs_t *bs, L3_gr_info_t *gr, const uint8_t *hdr) +{ + static const uint8_t g_scf_long[8][23] = { + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 12,12,12,12,12,12,16,20,24,28,32,40,48,56,64,76,90,2,2,2,2,2,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,18,22,26,32,38,46,54,62,70,76,36,0 }, + { 6,6,6,6,6,6,8,10,12,14,16,20,24,28,32,38,46,52,60,68,58,54,0 }, + { 4,4,4,4,4,4,6,6,8,8,10,12,16,20,24,28,34,42,50,54,76,158,0 }, + { 4,4,4,4,4,4,6,6,6,8,10,12,16,18,22,28,34,40,46,54,54,192,0 }, + { 4,4,4,4,4,4,6,6,8,10,12,16,20,24,30,38,46,56,68,84,102,26,0 } + }; + static const uint8_t g_scf_short[8][40] = { + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 8,8,8,8,8,8,8,8,8,12,12,12,16,16,16,20,20,20,24,24,24,28,28,28,36,36,36,2,2,2,2,2,2,2,2,2,26,26,26,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,6,6,6,8,8,8,10,10,10,14,14,14,18,18,18,26,26,26,32,32,32,42,42,42,18,18,18,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,32,32,32,44,44,44,12,12,12,0 }, + { 4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,22,22,22,30,30,30,56,56,56,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,6,6,6,10,10,10,12,12,12,14,14,14,16,16,16,20,20,20,26,26,26,66,66,66,0 }, + { 4,4,4,4,4,4,4,4,4,4,4,4,6,6,6,8,8,8,12,12,12,16,16,16,20,20,20,26,26,26,34,34,34,42,42,42,12,12,12,0 } + }; + static const uint8_t g_scf_mixed[8][40] = { + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 12,12,12,4,4,4,8,8,8,12,12,12,16,16,16,20,20,20,24,24,24,28,28,28,36,36,36,2,2,2,2,2,2,2,2,2,26,26,26,0 }, + { 6,6,6,6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,14,14,14,18,18,18,26,26,26,32,32,32,42,42,42,18,18,18,0 }, + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,32,32,32,44,44,44,12,12,12,0 }, + { 6,6,6,6,6,6,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,24,24,24,30,30,30,40,40,40,18,18,18,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,8,8,8,10,10,10,12,12,12,14,14,14,18,18,18,22,22,22,30,30,30,56,56,56,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,6,6,6,10,10,10,12,12,12,14,14,14,16,16,16,20,20,20,26,26,26,66,66,66,0 }, + { 4,4,4,4,4,4,6,6,4,4,4,6,6,6,8,8,8,12,12,12,16,16,16,20,20,20,26,26,26,34,34,34,42,42,42,12,12,12,0 } + }; + + unsigned tables, scfsi = 0; + int main_data_begin, part_23_sum = 0; + int sr_idx = HDR_GET_MY_SAMPLE_RATE(hdr); sr_idx -= (sr_idx != 0); + int gr_count = HDR_IS_MONO(hdr) ? 1 : 2; + + if (HDR_TEST_MPEG1(hdr)) + { + gr_count *= 2; + main_data_begin = get_bits(bs, 9); + scfsi = get_bits(bs, 7 + gr_count); + } else + { + main_data_begin = get_bits(bs, 8 + gr_count) >> gr_count; + } + + do + { + if (HDR_IS_MONO(hdr)) + { + scfsi <<= 4; + } + gr->part_23_length = (uint16_t)get_bits(bs, 12); + part_23_sum += gr->part_23_length; + gr->big_values = (uint16_t)get_bits(bs, 9); + if (gr->big_values > 288) + { + return -1; + } + gr->global_gain = (uint8_t)get_bits(bs, 8); + gr->scalefac_compress = (uint16_t)get_bits(bs, HDR_TEST_MPEG1(hdr) ? 4 : 9); + gr->sfbtab = g_scf_long[sr_idx]; + gr->n_long_sfb = 22; + gr->n_short_sfb = 0; + if (get_bits(bs, 1)) + { + gr->block_type = (uint8_t)get_bits(bs, 2); + if (!gr->block_type) + { + return -1; + } + gr->mixed_block_flag = (uint8_t)get_bits(bs, 1); + gr->region_count[0] = 7; + gr->region_count[1] = 255; + if (gr->block_type == SHORT_BLOCK_TYPE) + { + scfsi &= 0x0F0F; + if (!gr->mixed_block_flag) + { + gr->region_count[0] = 8; + gr->sfbtab = g_scf_short[sr_idx]; + gr->n_long_sfb = 0; + gr->n_short_sfb = 39; + } else + { + gr->sfbtab = g_scf_mixed[sr_idx]; + gr->n_long_sfb = HDR_TEST_MPEG1(hdr) ? 8 : 6; + gr->n_short_sfb = 30; + } + } + tables = get_bits(bs, 10); + tables <<= 5; + gr->subblock_gain[0] = (uint8_t)get_bits(bs, 3); + gr->subblock_gain[1] = (uint8_t)get_bits(bs, 3); + gr->subblock_gain[2] = (uint8_t)get_bits(bs, 3); + } else + { + gr->block_type = 0; + gr->mixed_block_flag = 0; + tables = get_bits(bs, 15); + gr->region_count[0] = (uint8_t)get_bits(bs, 4); + gr->region_count[1] = (uint8_t)get_bits(bs, 3); + gr->region_count[2] = 255; + } + gr->table_select[0] = (uint8_t)(tables >> 10); + gr->table_select[1] = (uint8_t)((tables >> 5) & 31); + gr->table_select[2] = (uint8_t)((tables) & 31); + gr->preflag = HDR_TEST_MPEG1(hdr) ? get_bits(bs, 1) : (gr->scalefac_compress >= 500); + gr->scalefac_scale = (uint8_t)get_bits(bs, 1); + gr->count1_table = (uint8_t)get_bits(bs, 1); + gr->scfsi = (uint8_t)((scfsi >> 12) & 15); + scfsi <<= 4; + gr++; + } while(--gr_count); + + if (part_23_sum + bs->pos > bs->limit + main_data_begin*8) + { + return -1; + } + + return main_data_begin; +} + +static void L3_read_scalefactors(uint8_t *scf, uint8_t *ist_pos, const uint8_t *scf_size, const uint8_t *scf_count, bs_t *bitbuf, int scfsi) +{ + int i, k; + for (i = 0; i < 4 && scf_count[i]; i++, scfsi *= 2) + { + int cnt = scf_count[i]; + if (scfsi & 8) + { + memcpy(scf, ist_pos, cnt); + } else + { + int bits = scf_size[i]; + if (!bits) + { + memset(scf, 0, cnt); + memset(ist_pos, 0, cnt); + } else + { + int max_scf = (scfsi < 0) ? (1 << bits) - 1 : -1; + for (k = 0; k < cnt; k++) + { + int s = get_bits(bitbuf, bits); + ist_pos[k] = (s == max_scf ? -1 : s); + scf[k] = s; + } + } + } + ist_pos += cnt; + scf += cnt; + } + scf[0] = scf[1] = scf[2] = 0; +} + +static float L3_ldexp_q2(float y, int exp_q2) +{ + static const float g_expfrac[4] = { 9.31322575e-10f,7.83145814e-10f,6.58544508e-10f,5.53767716e-10f }; + int e; + do + { + e = MINIMP3_MIN(30*4, exp_q2); + y *= g_expfrac[e & 3]*(1 << 30 >> (e >> 2)); + } while ((exp_q2 -= e) > 0); + return y; +} + +static void L3_decode_scalefactors(const uint8_t *hdr, uint8_t *ist_pos, bs_t *bs, const L3_gr_info_t *gr, float *scf, int ch) +{ + static const uint8_t g_scf_partitions[3][28] = { + { 6,5,5, 5,6,5,5,5,6,5, 7,3,11,10,0,0, 7, 7, 7,0, 6, 6,6,3, 8, 8,5,0 }, + { 8,9,6,12,6,9,9,9,6,9,12,6,15,18,0,0, 6,15,12,0, 6,12,9,6, 6,18,9,0 }, + { 9,9,6,12,9,9,9,9,9,9,12,6,18,18,0,0,12,12,12,0,12, 9,9,6,15,12,9,0 } + }; + const uint8_t *scf_partition = g_scf_partitions[!!gr->n_short_sfb + !gr->n_long_sfb]; + uint8_t scf_size[4], iscf[40]; + int i, scf_shift = gr->scalefac_scale + 1, gain_exp, scfsi = gr->scfsi; + float gain; + + if (HDR_TEST_MPEG1(hdr)) + { + static const uint8_t g_scfc_decode[16] = { 0,1,2,3, 12,5,6,7, 9,10,11,13, 14,15,18,19 }; + int part = g_scfc_decode[gr->scalefac_compress]; + scf_size[1] = scf_size[0] = (uint8_t)(part >> 2); + scf_size[3] = scf_size[2] = (uint8_t)(part & 3); + } else + { + static const uint8_t g_mod[6*4] = { 5,5,4,4,5,5,4,1,4,3,1,1,5,6,6,1,4,4,4,1,4,3,1,1 }; + int k, modprod, sfc, ist = HDR_TEST_I_STEREO(hdr) && ch; + sfc = gr->scalefac_compress >> ist; + for (k = ist*3*4; sfc >= 0; sfc -= modprod, k += 4) + { + for (modprod = 1, i = 3; i >= 0; i--) + { + scf_size[i] = (uint8_t)(sfc / modprod % g_mod[k + i]); + modprod *= g_mod[k + i]; + } + } + scf_partition += k; + scfsi = -16; + } + L3_read_scalefactors(iscf, ist_pos, scf_size, scf_partition, bs, scfsi); + + if (gr->n_short_sfb) + { + int sh = 3 - scf_shift; + for (i = 0; i < gr->n_short_sfb; i += 3) + { + iscf[gr->n_long_sfb + i + 0] += gr->subblock_gain[0] << sh; + iscf[gr->n_long_sfb + i + 1] += gr->subblock_gain[1] << sh; + iscf[gr->n_long_sfb + i + 2] += gr->subblock_gain[2] << sh; + } + } else if (gr->preflag) + { + static const uint8_t g_preamp[10] = { 1,1,1,1,2,2,3,3,3,2 }; + for (i = 0; i < 10; i++) + { + iscf[11 + i] += g_preamp[i]; + } + } + + gain_exp = gr->global_gain + BITS_DEQUANTIZER_OUT*4 - 210 - (HDR_IS_MS_STEREO(hdr) ? 2 : 0); + gain = L3_ldexp_q2(1 << (MAX_SCFI/4), MAX_SCFI - gain_exp); + for (i = 0; i < (int)(gr->n_long_sfb + gr->n_short_sfb); i++) + { + scf[i] = L3_ldexp_q2(gain, iscf[i] << scf_shift); + } +} + +static const float g_pow43[129 + 16] = { + 0,-1,-2.519842f,-4.326749f,-6.349604f,-8.549880f,-10.902724f,-13.390518f,-16.000000f,-18.720754f,-21.544347f,-24.463781f,-27.473142f,-30.567351f,-33.741992f,-36.993181f, + 0,1,2.519842f,4.326749f,6.349604f,8.549880f,10.902724f,13.390518f,16.000000f,18.720754f,21.544347f,24.463781f,27.473142f,30.567351f,33.741992f,36.993181f,40.317474f,43.711787f,47.173345f,50.699631f,54.288352f,57.937408f,61.644865f,65.408941f,69.227979f,73.100443f,77.024898f,81.000000f,85.024491f,89.097188f,93.216975f,97.382800f,101.593667f,105.848633f,110.146801f,114.487321f,118.869381f,123.292209f,127.755065f,132.257246f,136.798076f,141.376907f,145.993119f,150.646117f,155.335327f,160.060199f,164.820202f,169.614826f,174.443577f,179.305980f,184.201575f,189.129918f,194.090580f,199.083145f,204.107210f,209.162385f,214.248292f,219.364564f,224.510845f,229.686789f,234.892058f,240.126328f,245.389280f,250.680604f,256.000000f,261.347174f,266.721841f,272.123723f,277.552547f,283.008049f,288.489971f,293.998060f,299.532071f,305.091761f,310.676898f,316.287249f,321.922592f,327.582707f,333.267377f,338.976394f,344.709550f,350.466646f,356.247482f,362.051866f,367.879608f,373.730522f,379.604427f,385.501143f,391.420496f,397.362314f,403.326427f,409.312672f,415.320884f,421.350905f,427.402579f,433.475750f,439.570269f,445.685987f,451.822757f,457.980436f,464.158883f,470.357960f,476.577530f,482.817459f,489.077615f,495.357868f,501.658090f,507.978156f,514.317941f,520.677324f,527.056184f,533.454404f,539.871867f,546.308458f,552.764065f,559.238575f,565.731879f,572.243870f,578.774440f,585.323483f,591.890898f,598.476581f,605.080431f,611.702349f,618.342238f,625.000000f,631.675540f,638.368763f,645.079578f +}; + +static float L3_pow_43(int x) +{ + float frac; + int sign, mult = 256; + + if (x < 129) + { + return g_pow43[16 + x]; + } + + if (x < 1024) + { + mult = 16; + x <<= 3; + } + + sign = 2*x & 64; + frac = (float)((x & 63) - sign) / ((x & ~63) + sign); + return g_pow43[16 + ((x + sign) >> 6)]*(1.f + frac*((4.f/3) + frac*(2.f/9)))*mult; +} + +static void L3_huffman(float *dst, bs_t *bs, const L3_gr_info_t *gr_info, const float *scf, int layer3gr_limit) +{ + static const int16_t tabs[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 785,785,785,785,784,784,784,784,513,513,513,513,513,513,513,513,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256, + -255,1313,1298,1282,785,785,785,785,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,290,288, + -255,1313,1298,1282,769,769,769,769,529,529,529,529,529,529,529,529,528,528,528,528,528,528,528,528,512,512,512,512,512,512,512,512,290,288, + -253,-318,-351,-367,785,785,785,785,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,819,818,547,547,275,275,275,275,561,560,515,546,289,274,288,258, + -254,-287,1329,1299,1314,1312,1057,1057,1042,1042,1026,1026,784,784,784,784,529,529,529,529,529,529,529,529,769,769,769,769,768,768,768,768,563,560,306,306,291,259, + -252,-413,-477,-542,1298,-575,1041,1041,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-383,-399,1107,1092,1106,1061,849,849,789,789,1104,1091,773,773,1076,1075,341,340,325,309,834,804,577,577,532,532,516,516,832,818,803,816,561,561,531,531,515,546,289,289,288,258, + -252,-429,-493,-559,1057,1057,1042,1042,529,529,529,529,529,529,529,529,784,784,784,784,769,769,769,769,512,512,512,512,512,512,512,512,-382,1077,-415,1106,1061,1104,849,849,789,789,1091,1076,1029,1075,834,834,597,581,340,340,339,324,804,833,532,532,832,772,818,803,817,787,816,771,290,290,290,290,288,258, + -253,-349,-414,-447,-463,1329,1299,-479,1314,1312,1057,1057,1042,1042,1026,1026,785,785,785,785,784,784,784,784,769,769,769,769,768,768,768,768,-319,851,821,-335,836,850,805,849,341,340,325,336,533,533,579,579,564,564,773,832,578,548,563,516,321,276,306,291,304,259, + -251,-572,-733,-830,-863,-879,1041,1041,784,784,784,784,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-511,-527,-543,1396,1351,1381,1366,1395,1335,1380,-559,1334,1138,1138,1063,1063,1350,1392,1031,1031,1062,1062,1364,1363,1120,1120,1333,1348,881,881,881,881,375,374,359,373,343,358,341,325,791,791,1123,1122,-703,1105,1045,-719,865,865,790,790,774,774,1104,1029,338,293,323,308,-799,-815,833,788,772,818,803,816,322,292,307,320,561,531,515,546,289,274,288,258, + -251,-525,-605,-685,-765,-831,-846,1298,1057,1057,1312,1282,785,785,785,785,784,784,784,784,769,769,769,769,512,512,512,512,512,512,512,512,1399,1398,1383,1367,1382,1396,1351,-511,1381,1366,1139,1139,1079,1079,1124,1124,1364,1349,1363,1333,882,882,882,882,807,807,807,807,1094,1094,1136,1136,373,341,535,535,881,775,867,822,774,-591,324,338,-671,849,550,550,866,864,609,609,293,336,534,534,789,835,773,-751,834,804,308,307,833,788,832,772,562,562,547,547,305,275,560,515,290,290, + -252,-397,-477,-557,-622,-653,-719,-735,-750,1329,1299,1314,1057,1057,1042,1042,1312,1282,1024,1024,785,785,785,785,784,784,784,784,769,769,769,769,-383,1127,1141,1111,1126,1140,1095,1110,869,869,883,883,1079,1109,882,882,375,374,807,868,838,881,791,-463,867,822,368,263,852,837,836,-543,610,610,550,550,352,336,534,534,865,774,851,821,850,805,593,533,579,564,773,832,578,578,548,548,577,577,307,276,306,291,516,560,259,259, + -250,-2107,-2507,-2764,-2909,-2974,-3007,-3023,1041,1041,1040,1040,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-767,-1052,-1213,-1277,-1358,-1405,-1469,-1535,-1550,-1582,-1614,-1647,-1662,-1694,-1726,-1759,-1774,-1807,-1822,-1854,-1886,1565,-1919,-1935,-1951,-1967,1731,1730,1580,1717,-1983,1729,1564,-1999,1548,-2015,-2031,1715,1595,-2047,1714,-2063,1610,-2079,1609,-2095,1323,1323,1457,1457,1307,1307,1712,1547,1641,1700,1699,1594,1685,1625,1442,1442,1322,1322,-780,-973,-910,1279,1278,1277,1262,1276,1261,1275,1215,1260,1229,-959,974,974,989,989,-943,735,478,478,495,463,506,414,-1039,1003,958,1017,927,942,987,957,431,476,1272,1167,1228,-1183,1256,-1199,895,895,941,941,1242,1227,1212,1135,1014,1014,490,489,503,487,910,1013,985,925,863,894,970,955,1012,847,-1343,831,755,755,984,909,428,366,754,559,-1391,752,486,457,924,997,698,698,983,893,740,740,908,877,739,739,667,667,953,938,497,287,271,271,683,606,590,712,726,574,302,302,738,736,481,286,526,725,605,711,636,724,696,651,589,681,666,710,364,467,573,695,466,466,301,465,379,379,709,604,665,679,316,316,634,633,436,436,464,269,424,394,452,332,438,363,347,408,393,448,331,422,362,407,392,421,346,406,391,376,375,359,1441,1306,-2367,1290,-2383,1337,-2399,-2415,1426,1321,-2431,1411,1336,-2447,-2463,-2479,1169,1169,1049,1049,1424,1289,1412,1352,1319,-2495,1154,1154,1064,1064,1153,1153,416,390,360,404,403,389,344,374,373,343,358,372,327,357,342,311,356,326,1395,1394,1137,1137,1047,1047,1365,1392,1287,1379,1334,1364,1349,1378,1318,1363,792,792,792,792,1152,1152,1032,1032,1121,1121,1046,1046,1120,1120,1030,1030,-2895,1106,1061,1104,849,849,789,789,1091,1076,1029,1090,1060,1075,833,833,309,324,532,532,832,772,818,803,561,561,531,560,515,546,289,274,288,258, + -250,-1179,-1579,-1836,-1996,-2124,-2253,-2333,-2413,-2477,-2542,-2574,-2607,-2622,-2655,1314,1313,1298,1312,1282,785,785,785,785,1040,1040,1025,1025,768,768,768,768,-766,-798,-830,-862,-895,-911,-927,-943,-959,-975,-991,-1007,-1023,-1039,-1055,-1070,1724,1647,-1103,-1119,1631,1767,1662,1738,1708,1723,-1135,1780,1615,1779,1599,1677,1646,1778,1583,-1151,1777,1567,1737,1692,1765,1722,1707,1630,1751,1661,1764,1614,1736,1676,1763,1750,1645,1598,1721,1691,1762,1706,1582,1761,1566,-1167,1749,1629,767,766,751,765,494,494,735,764,719,749,734,763,447,447,748,718,477,506,431,491,446,476,461,505,415,430,475,445,504,399,460,489,414,503,383,474,429,459,502,502,746,752,488,398,501,473,413,472,486,271,480,270,-1439,-1455,1357,-1471,-1487,-1503,1341,1325,-1519,1489,1463,1403,1309,-1535,1372,1448,1418,1476,1356,1462,1387,-1551,1475,1340,1447,1402,1386,-1567,1068,1068,1474,1461,455,380,468,440,395,425,410,454,364,467,466,464,453,269,409,448,268,432,1371,1473,1432,1417,1308,1460,1355,1446,1459,1431,1083,1083,1401,1416,1458,1445,1067,1067,1370,1457,1051,1051,1291,1430,1385,1444,1354,1415,1400,1443,1082,1082,1173,1113,1186,1066,1185,1050,-1967,1158,1128,1172,1097,1171,1081,-1983,1157,1112,416,266,375,400,1170,1142,1127,1065,793,793,1169,1033,1156,1096,1141,1111,1155,1080,1126,1140,898,898,808,808,897,897,792,792,1095,1152,1032,1125,1110,1139,1079,1124,882,807,838,881,853,791,-2319,867,368,263,822,852,837,866,806,865,-2399,851,352,262,534,534,821,836,594,594,549,549,593,593,533,533,848,773,579,579,564,578,548,563,276,276,577,576,306,291,516,560,305,305,275,259, + -251,-892,-2058,-2620,-2828,-2957,-3023,-3039,1041,1041,1040,1040,769,769,769,769,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,256,-511,-527,-543,-559,1530,-575,-591,1528,1527,1407,1526,1391,1023,1023,1023,1023,1525,1375,1268,1268,1103,1103,1087,1087,1039,1039,1523,-604,815,815,815,815,510,495,509,479,508,463,507,447,431,505,415,399,-734,-782,1262,-815,1259,1244,-831,1258,1228,-847,-863,1196,-879,1253,987,987,748,-767,493,493,462,477,414,414,686,669,478,446,461,445,474,429,487,458,412,471,1266,1264,1009,1009,799,799,-1019,-1276,-1452,-1581,-1677,-1757,-1821,-1886,-1933,-1997,1257,1257,1483,1468,1512,1422,1497,1406,1467,1496,1421,1510,1134,1134,1225,1225,1466,1451,1374,1405,1252,1252,1358,1480,1164,1164,1251,1251,1238,1238,1389,1465,-1407,1054,1101,-1423,1207,-1439,830,830,1248,1038,1237,1117,1223,1148,1236,1208,411,426,395,410,379,269,1193,1222,1132,1235,1221,1116,976,976,1192,1162,1177,1220,1131,1191,963,963,-1647,961,780,-1663,558,558,994,993,437,408,393,407,829,978,813,797,947,-1743,721,721,377,392,844,950,828,890,706,706,812,859,796,960,948,843,934,874,571,571,-1919,690,555,689,421,346,539,539,944,779,918,873,932,842,903,888,570,570,931,917,674,674,-2575,1562,-2591,1609,-2607,1654,1322,1322,1441,1441,1696,1546,1683,1593,1669,1624,1426,1426,1321,1321,1639,1680,1425,1425,1305,1305,1545,1668,1608,1623,1667,1592,1638,1666,1320,1320,1652,1607,1409,1409,1304,1304,1288,1288,1664,1637,1395,1395,1335,1335,1622,1636,1394,1394,1319,1319,1606,1621,1392,1392,1137,1137,1137,1137,345,390,360,375,404,373,1047,-2751,-2767,-2783,1062,1121,1046,-2799,1077,-2815,1106,1061,789,789,1105,1104,263,355,310,340,325,354,352,262,339,324,1091,1076,1029,1090,1060,1075,833,833,788,788,1088,1028,818,818,803,803,561,561,531,531,816,771,546,546,289,274,288,258, + -253,-317,-381,-446,-478,-509,1279,1279,-811,-1179,-1451,-1756,-1900,-2028,-2189,-2253,-2333,-2414,-2445,-2511,-2526,1313,1298,-2559,1041,1041,1040,1040,1025,1025,1024,1024,1022,1007,1021,991,1020,975,1019,959,687,687,1018,1017,671,671,655,655,1016,1015,639,639,758,758,623,623,757,607,756,591,755,575,754,559,543,543,1009,783,-575,-621,-685,-749,496,-590,750,749,734,748,974,989,1003,958,988,973,1002,942,987,957,972,1001,926,986,941,971,956,1000,910,985,925,999,894,970,-1071,-1087,-1102,1390,-1135,1436,1509,1451,1374,-1151,1405,1358,1480,1420,-1167,1507,1494,1389,1342,1465,1435,1450,1326,1505,1310,1493,1373,1479,1404,1492,1464,1419,428,443,472,397,736,526,464,464,486,457,442,471,484,482,1357,1449,1434,1478,1388,1491,1341,1490,1325,1489,1463,1403,1309,1477,1372,1448,1418,1433,1476,1356,1462,1387,-1439,1475,1340,1447,1402,1474,1324,1461,1371,1473,269,448,1432,1417,1308,1460,-1711,1459,-1727,1441,1099,1099,1446,1386,1431,1401,-1743,1289,1083,1083,1160,1160,1458,1445,1067,1067,1370,1457,1307,1430,1129,1129,1098,1098,268,432,267,416,266,400,-1887,1144,1187,1082,1173,1113,1186,1066,1050,1158,1128,1143,1172,1097,1171,1081,420,391,1157,1112,1170,1142,1127,1065,1169,1049,1156,1096,1141,1111,1155,1080,1126,1154,1064,1153,1140,1095,1048,-2159,1125,1110,1137,-2175,823,823,1139,1138,807,807,384,264,368,263,868,838,853,791,867,822,852,837,866,806,865,790,-2319,851,821,836,352,262,850,805,849,-2399,533,533,835,820,336,261,578,548,563,577,532,532,832,772,562,562,547,547,305,275,560,515,290,290,288,258 }; + static const uint8_t tab32[] = { 130,162,193,209,44,28,76,140,9,9,9,9,9,9,9,9,190,254,222,238,126,94,157,157,109,61,173,205 }; + static const uint8_t tab33[] = { 252,236,220,204,188,172,156,140,124,108,92,76,60,44,28,12 }; + static const int16_t tabindex[2*16] = { 0,32,64,98,0,132,180,218,292,364,426,538,648,746,0,1126,1460,1460,1460,1460,1460,1460,1460,1460,1842,1842,1842,1842,1842,1842,1842,1842 }; + static const uint8_t g_linbits[] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,2,3,4,6,8,10,13,4,5,6,7,8,9,11,13 }; + +#define PEEK_BITS(n) (bs_cache >> (32 - n)) +#define FLUSH_BITS(n) { bs_cache <<= (n); bs_sh += (n); } +#define CHECK_BITS while (bs_sh >= 0) { bs_cache |= (uint32_t)*bs_next_ptr++ << bs_sh; bs_sh -= 8; } +#define BSPOS ((bs_next_ptr - bs->buf)*8 - 24 + bs_sh) + + float one = 0.0f; + int ireg = 0, big_val_cnt = gr_info->big_values; + const uint8_t *sfb = gr_info->sfbtab; + const uint8_t *bs_next_ptr = bs->buf + bs->pos/8; + uint32_t bs_cache = (((bs_next_ptr[0]*256u + bs_next_ptr[1])*256u + bs_next_ptr[2])*256u + bs_next_ptr[3]) << (bs->pos & 7); + int pairs_to_decode, np, bs_sh = (bs->pos & 7) - 8; + bs_next_ptr += 4; + + while (big_val_cnt > 0) + { + int tab_num = gr_info->table_select[ireg]; + int sfb_cnt = gr_info->region_count[ireg++]; + const int16_t *codebook = tabs + tabindex[tab_num]; + int linbits = g_linbits[tab_num]; + if (linbits) + { + do + { + np = *sfb++ / 2; + pairs_to_decode = MINIMP3_MIN(big_val_cnt, np); + one = *scf++; + do + { + int j, w = 5; + int leaf = codebook[PEEK_BITS(w)]; + while (leaf < 0) + { + FLUSH_BITS(w); + w = leaf & 7; + leaf = codebook[PEEK_BITS(w) - (leaf >> 3)]; + } + FLUSH_BITS(leaf >> 8); + + for (j = 0; j < 2; j++, dst++, leaf >>= 4) + { + int lsb = leaf & 0x0F; + if (lsb == 15) + { + lsb += PEEK_BITS(linbits); + FLUSH_BITS(linbits); + CHECK_BITS; + *dst = one*L3_pow_43(lsb)*((int32_t)bs_cache < 0 ? -1: 1); + } else + { + *dst = g_pow43[16 + lsb - 16*(bs_cache >> 31)]*one; + } + FLUSH_BITS(lsb ? 1 : 0); + } + CHECK_BITS; + } while (--pairs_to_decode); + } while ((big_val_cnt -= np) > 0 && --sfb_cnt >= 0); + } else + { + do + { + np = *sfb++ / 2; + pairs_to_decode = MINIMP3_MIN(big_val_cnt, np); + one = *scf++; + do + { + int j, w = 5; + int leaf = codebook[PEEK_BITS(w)]; + while (leaf < 0) + { + FLUSH_BITS(w); + w = leaf & 7; + leaf = codebook[PEEK_BITS(w) - (leaf >> 3)]; + } + FLUSH_BITS(leaf >> 8); + + for (j = 0; j < 2; j++, dst++, leaf >>= 4) + { + int lsb = leaf & 0x0F; + *dst = g_pow43[16 + lsb - 16*(bs_cache >> 31)]*one; + FLUSH_BITS(lsb ? 1 : 0); + } + CHECK_BITS; + } while (--pairs_to_decode); + } while ((big_val_cnt -= np) > 0 && --sfb_cnt >= 0); + } + } + + for (np = 1 - big_val_cnt;; dst += 4) + { + const uint8_t *codebook_count1 = (gr_info->count1_table) ? tab33 : tab32; + int leaf = codebook_count1[PEEK_BITS(4)]; + if (!(leaf & 8)) + { + leaf = codebook_count1[(leaf >> 3) + (bs_cache << 4 >> (32 - (leaf & 3)))]; + } + FLUSH_BITS(leaf & 7); + if (BSPOS > layer3gr_limit) + { + break; + } +#define RELOAD_SCALEFACTOR if (!--np) { np = *sfb++/2; if (!np) break; one = *scf++; } +#define DEQ_COUNT1(s) if (leaf & (128 >> s)) { dst[s] = ((int32_t)bs_cache < 0) ? -one : one; FLUSH_BITS(1) } + RELOAD_SCALEFACTOR; + DEQ_COUNT1(0); + DEQ_COUNT1(1); + RELOAD_SCALEFACTOR; + DEQ_COUNT1(2); + DEQ_COUNT1(3); + CHECK_BITS; + } + + bs->pos = layer3gr_limit; +} + +static void L3_midside_stereo(float *left, int n) +{ + int i = 0; + float *right = left + 576; +#if HAVE_SIMD + if (have_simd()) + { + for (; i < n - 3; i += 4) + { + f4 vl = VLD(left + i); + f4 vr = VLD(right + i); + VSTORE(left + i, VADD(vl, vr)); + VSTORE(right + i, VSUB(vl, vr)); + } +#ifdef __GNUC__ + /* Workaround for spurious -Waggressive-loop-optimizations warning from gcc. + * For more info see: https://github.com/lieff/minimp3/issues/88 + */ + if (__builtin_constant_p(n % 4 == 0) && n % 4 == 0) + return; +#endif + } +#endif /* HAVE_SIMD */ + for (; i < n; i++) + { + float a = left[i]; + float b = right[i]; + left[i] = a + b; + right[i] = a - b; + } +} + +static void L3_intensity_stereo_band(float *left, int n, float kl, float kr) +{ + int i; + for (i = 0; i < n; i++) + { + left[i + 576] = left[i]*kr; + left[i] = left[i]*kl; + } +} + +static void L3_stereo_top_band(const float *right, const uint8_t *sfb, int nbands, int max_band[3]) +{ + int i, k; + + max_band[0] = max_band[1] = max_band[2] = -1; + + for (i = 0; i < nbands; i++) + { + for (k = 0; k < sfb[i]; k += 2) + { + if (right[k] != 0 || right[k + 1] != 0) + { + max_band[i % 3] = i; + break; + } + } + right += sfb[i]; + } +} + +static void L3_stereo_process(float *left, const uint8_t *ist_pos, const uint8_t *sfb, const uint8_t *hdr, int max_band[3], int mpeg2_sh) +{ + static const float g_pan[7*2] = { 0,1,0.21132487f,0.78867513f,0.36602540f,0.63397460f,0.5f,0.5f,0.63397460f,0.36602540f,0.78867513f,0.21132487f,1,0 }; + unsigned i, max_pos = HDR_TEST_MPEG1(hdr) ? 7 : 64; + + for (i = 0; sfb[i]; i++) + { + unsigned ipos = ist_pos[i]; + if ((int)i > max_band[i % 3] && ipos < max_pos) + { + float kl, kr, s = HDR_TEST_MS_STEREO(hdr) ? 1.41421356f : 1; + if (HDR_TEST_MPEG1(hdr)) + { + kl = g_pan[2*ipos]; + kr = g_pan[2*ipos + 1]; + } else + { + kl = 1; + kr = L3_ldexp_q2(1, (ipos + 1) >> 1 << mpeg2_sh); + if (ipos & 1) + { + kl = kr; + kr = 1; + } + } + L3_intensity_stereo_band(left, sfb[i], kl*s, kr*s); + } else if (HDR_TEST_MS_STEREO(hdr)) + { + L3_midside_stereo(left, sfb[i]); + } + left += sfb[i]; + } +} + +static void L3_intensity_stereo(float *left, uint8_t *ist_pos, const L3_gr_info_t *gr, const uint8_t *hdr) +{ + int max_band[3], n_sfb = gr->n_long_sfb + gr->n_short_sfb; + int i, max_blocks = gr->n_short_sfb ? 3 : 1; + + L3_stereo_top_band(left + 576, gr->sfbtab, n_sfb, max_band); + if (gr->n_long_sfb) + { + max_band[0] = max_band[1] = max_band[2] = MINIMP3_MAX(MINIMP3_MAX(max_band[0], max_band[1]), max_band[2]); + } + for (i = 0; i < max_blocks; i++) + { + int default_pos = HDR_TEST_MPEG1(hdr) ? 3 : 0; + int itop = n_sfb - max_blocks + i; + int prev = itop - max_blocks; + ist_pos[itop] = max_band[i] >= prev ? default_pos : ist_pos[prev]; + } + L3_stereo_process(left, ist_pos, gr->sfbtab, hdr, max_band, gr[1].scalefac_compress & 1); +} + +static void L3_reorder(float *grbuf, float *scratch, const uint8_t *sfb) +{ + int i, len; + float *src = grbuf, *dst = scratch; + + for (;0 != (len = *sfb); sfb += 3, src += 2*len) + { + for (i = 0; i < len; i++, src++) + { + *dst++ = src[0*len]; + *dst++ = src[1*len]; + *dst++ = src[2*len]; + } + } + memcpy(grbuf, scratch, (dst - scratch)*sizeof(float)); +} + +static void L3_antialias(float *grbuf, int nbands) +{ + static const float g_aa[2][8] = { + {0.85749293f,0.88174200f,0.94962865f,0.98331459f,0.99551782f,0.99916056f,0.99989920f,0.99999316f}, + {0.51449576f,0.47173197f,0.31337745f,0.18191320f,0.09457419f,0.04096558f,0.01419856f,0.00369997f} + }; + + for (; nbands > 0; nbands--, grbuf += 18) + { + int i = 0; +#if HAVE_SIMD + if (have_simd()) for (; i < 8; i += 4) + { + f4 vu = VLD(grbuf + 18 + i); + f4 vd = VLD(grbuf + 14 - i); + f4 vc0 = VLD(g_aa[0] + i); + f4 vc1 = VLD(g_aa[1] + i); + vd = VREV(vd); + VSTORE(grbuf + 18 + i, VSUB(VMUL(vu, vc0), VMUL(vd, vc1))); + vd = VADD(VMUL(vu, vc1), VMUL(vd, vc0)); + VSTORE(grbuf + 14 - i, VREV(vd)); + } +#endif /* HAVE_SIMD */ +#ifndef MINIMP3_ONLY_SIMD + for(; i < 8; i++) + { + float u = grbuf[18 + i]; + float d = grbuf[17 - i]; + grbuf[18 + i] = u*g_aa[0][i] - d*g_aa[1][i]; + grbuf[17 - i] = u*g_aa[1][i] + d*g_aa[0][i]; + } +#endif /* MINIMP3_ONLY_SIMD */ + } +} + +static void L3_dct3_9(float *y) +{ + float s0, s1, s2, s3, s4, s5, s6, s7, s8, t0, t2, t4; + + s0 = y[0]; s2 = y[2]; s4 = y[4]; s6 = y[6]; s8 = y[8]; + t0 = s0 + s6*0.5f; + s0 -= s6; + t4 = (s4 + s2)*0.93969262f; + t2 = (s8 + s2)*0.76604444f; + s6 = (s4 - s8)*0.17364818f; + s4 += s8 - s2; + + s2 = s0 - s4*0.5f; + y[4] = s4 + s0; + s8 = t0 - t2 + s6; + s0 = t0 - t4 + t2; + s4 = t0 + t4 - s6; + + s1 = y[1]; s3 = y[3]; s5 = y[5]; s7 = y[7]; + + s3 *= 0.86602540f; + t0 = (s5 + s1)*0.98480775f; + t4 = (s5 - s7)*0.34202014f; + t2 = (s1 + s7)*0.64278761f; + s1 = (s1 - s5 - s7)*0.86602540f; + + s5 = t0 - s3 - t2; + s7 = t4 - s3 - t0; + s3 = t4 + s3 - t2; + + y[0] = s4 - s7; + y[1] = s2 + s1; + y[2] = s0 - s3; + y[3] = s8 + s5; + y[5] = s8 - s5; + y[6] = s0 + s3; + y[7] = s2 - s1; + y[8] = s4 + s7; +} + +static void L3_imdct36(float *grbuf, float *overlap, const float *window, int nbands) +{ + int i, j; + static const float g_twid9[18] = { + 0.73727734f,0.79335334f,0.84339145f,0.88701083f,0.92387953f,0.95371695f,0.97629601f,0.99144486f,0.99904822f,0.67559021f,0.60876143f,0.53729961f,0.46174861f,0.38268343f,0.30070580f,0.21643961f,0.13052619f,0.04361938f + }; + + for (j = 0; j < nbands; j++, grbuf += 18, overlap += 9) + { + float co[9], si[9]; + co[0] = -grbuf[0]; + si[0] = grbuf[17]; + for (i = 0; i < 4; i++) + { + si[8 - 2*i] = grbuf[4*i + 1] - grbuf[4*i + 2]; + co[1 + 2*i] = grbuf[4*i + 1] + grbuf[4*i + 2]; + si[7 - 2*i] = grbuf[4*i + 4] - grbuf[4*i + 3]; + co[2 + 2*i] = -(grbuf[4*i + 3] + grbuf[4*i + 4]); + } + L3_dct3_9(co); + L3_dct3_9(si); + + si[1] = -si[1]; + si[3] = -si[3]; + si[5] = -si[5]; + si[7] = -si[7]; + + i = 0; + +#if HAVE_SIMD + if (have_simd()) for (; i < 8; i += 4) + { + f4 vovl = VLD(overlap + i); + f4 vc = VLD(co + i); + f4 vs = VLD(si + i); + f4 vr0 = VLD(g_twid9 + i); + f4 vr1 = VLD(g_twid9 + 9 + i); + f4 vw0 = VLD(window + i); + f4 vw1 = VLD(window + 9 + i); + f4 vsum = VADD(VMUL(vc, vr1), VMUL(vs, vr0)); + VSTORE(overlap + i, VSUB(VMUL(vc, vr0), VMUL(vs, vr1))); + VSTORE(grbuf + i, VSUB(VMUL(vovl, vw0), VMUL(vsum, vw1))); + vsum = VADD(VMUL(vovl, vw1), VMUL(vsum, vw0)); + VSTORE(grbuf + 14 - i, VREV(vsum)); + } +#endif /* HAVE_SIMD */ + for (; i < 9; i++) + { + float ovl = overlap[i]; + float sum = co[i]*g_twid9[9 + i] + si[i]*g_twid9[0 + i]; + overlap[i] = co[i]*g_twid9[0 + i] - si[i]*g_twid9[9 + i]; + grbuf[i] = ovl*window[0 + i] - sum*window[9 + i]; + grbuf[17 - i] = ovl*window[9 + i] + sum*window[0 + i]; + } + } +} + +static void L3_idct3(float x0, float x1, float x2, float *dst) +{ + float m1 = x1*0.86602540f; + float a1 = x0 - x2*0.5f; + dst[1] = x0 + x2; + dst[0] = a1 + m1; + dst[2] = a1 - m1; +} + +static void L3_imdct12(float *x, float *dst, float *overlap) +{ + static const float g_twid3[6] = { 0.79335334f,0.92387953f,0.99144486f, 0.60876143f,0.38268343f,0.13052619f }; + float co[3], si[3]; + int i; + + L3_idct3(-x[0], x[6] + x[3], x[12] + x[9], co); + L3_idct3(x[15], x[12] - x[9], x[6] - x[3], si); + si[1] = -si[1]; + + for (i = 0; i < 3; i++) + { + float ovl = overlap[i]; + float sum = co[i]*g_twid3[3 + i] + si[i]*g_twid3[0 + i]; + overlap[i] = co[i]*g_twid3[0 + i] - si[i]*g_twid3[3 + i]; + dst[i] = ovl*g_twid3[2 - i] - sum*g_twid3[5 - i]; + dst[5 - i] = ovl*g_twid3[5 - i] + sum*g_twid3[2 - i]; + } +} + +static void L3_imdct_short(float *grbuf, float *overlap, int nbands) +{ + for (;nbands > 0; nbands--, overlap += 9, grbuf += 18) + { + float tmp[18]; + memcpy(tmp, grbuf, sizeof(tmp)); + memcpy(grbuf, overlap, 6*sizeof(float)); + L3_imdct12(tmp, grbuf + 6, overlap + 6); + L3_imdct12(tmp + 1, grbuf + 12, overlap + 6); + L3_imdct12(tmp + 2, overlap, overlap + 6); + } +} + +static void L3_change_sign(float *grbuf) +{ + int b, i; + for (b = 0, grbuf += 18; b < 32; b += 2, grbuf += 36) + for (i = 1; i < 18; i += 2) + grbuf[i] = -grbuf[i]; +} + +static void L3_imdct_gr(float *grbuf, float *overlap, unsigned block_type, unsigned n_long_bands) +{ + static const float g_mdct_window[2][18] = { + { 0.99904822f,0.99144486f,0.97629601f,0.95371695f,0.92387953f,0.88701083f,0.84339145f,0.79335334f,0.73727734f,0.04361938f,0.13052619f,0.21643961f,0.30070580f,0.38268343f,0.46174861f,0.53729961f,0.60876143f,0.67559021f }, + { 1,1,1,1,1,1,0.99144486f,0.92387953f,0.79335334f,0,0,0,0,0,0,0.13052619f,0.38268343f,0.60876143f } + }; + if (n_long_bands) + { + L3_imdct36(grbuf, overlap, g_mdct_window[0], n_long_bands); + grbuf += 18*n_long_bands; + overlap += 9*n_long_bands; + } + if (block_type == SHORT_BLOCK_TYPE) + L3_imdct_short(grbuf, overlap, 32 - n_long_bands); + else + L3_imdct36(grbuf, overlap, g_mdct_window[block_type == STOP_BLOCK_TYPE], 32 - n_long_bands); +} + +static void L3_save_reservoir(mp3dec_t *h, mp3dec_scratch_t *s) +{ + int pos = (s->bs.pos + 7)/8u; + int remains = s->bs.limit/8u - pos; + if (remains > MAX_BITRESERVOIR_BYTES) + { + pos += remains - MAX_BITRESERVOIR_BYTES; + remains = MAX_BITRESERVOIR_BYTES; + } + if (remains > 0) + { + memmove(h->reserv_buf, s->maindata + pos, remains); + } + h->reserv = remains; +} + +static int L3_restore_reservoir(mp3dec_t *h, bs_t *bs, mp3dec_scratch_t *s, int main_data_begin) +{ + int frame_bytes = (bs->limit - bs->pos)/8; + int bytes_have = MINIMP3_MIN(h->reserv, main_data_begin); + memcpy(s->maindata, h->reserv_buf + MINIMP3_MAX(0, h->reserv - main_data_begin), MINIMP3_MIN(h->reserv, main_data_begin)); + memcpy(s->maindata + bytes_have, bs->buf + bs->pos/8, frame_bytes); + bs_init(&s->bs, s->maindata, bytes_have + frame_bytes); + return h->reserv >= main_data_begin; +} + +static void L3_decode(mp3dec_t *h, mp3dec_scratch_t *s, L3_gr_info_t *gr_info, int nch) +{ + int ch; + + for (ch = 0; ch < nch; ch++) + { + int layer3gr_limit = s->bs.pos + gr_info[ch].part_23_length; + L3_decode_scalefactors(h->header, s->ist_pos[ch], &s->bs, gr_info + ch, s->scf, ch); + L3_huffman(s->grbuf[ch], &s->bs, gr_info + ch, s->scf, layer3gr_limit); + } + + if (HDR_TEST_I_STEREO(h->header)) + { + L3_intensity_stereo(s->grbuf[0], s->ist_pos[1], gr_info, h->header); + } else if (HDR_IS_MS_STEREO(h->header)) + { + L3_midside_stereo(s->grbuf[0], 576); + } + + for (ch = 0; ch < nch; ch++, gr_info++) + { + int aa_bands = 31; + int n_long_bands = (gr_info->mixed_block_flag ? 2 : 0) << (int)(HDR_GET_MY_SAMPLE_RATE(h->header) == 2); + + if (gr_info->n_short_sfb) + { + aa_bands = n_long_bands - 1; + L3_reorder(s->grbuf[ch] + n_long_bands*18, s->syn[0], gr_info->sfbtab + gr_info->n_long_sfb); + } + + L3_antialias(s->grbuf[ch], aa_bands); + L3_imdct_gr(s->grbuf[ch], h->mdct_overlap[ch], gr_info->block_type, n_long_bands); + L3_change_sign(s->grbuf[ch]); + } +} + +static void mp3d_DCT_II(float *grbuf, int n) +{ + static const float g_sec[24] = { + 10.19000816f,0.50060302f,0.50241929f,3.40760851f,0.50547093f,0.52249861f,2.05778098f,0.51544732f,0.56694406f,1.48416460f,0.53104258f,0.64682180f,1.16943991f,0.55310392f,0.78815460f,0.97256821f,0.58293498f,1.06067765f,0.83934963f,0.62250412f,1.72244716f,0.74453628f,0.67480832f,5.10114861f + }; + int i, k = 0; +#if HAVE_SIMD + if (have_simd()) for (; k < n; k += 4) + { + f4 t[4][8], *x; + float *y = grbuf + k; + + for (x = t[0], i = 0; i < 8; i++, x++) + { + f4 x0 = VLD(&y[i*18]); + f4 x1 = VLD(&y[(15 - i)*18]); + f4 x2 = VLD(&y[(16 + i)*18]); + f4 x3 = VLD(&y[(31 - i)*18]); + f4 t0 = VADD(x0, x3); + f4 t1 = VADD(x1, x2); + f4 t2 = VMUL_S(VSUB(x1, x2), g_sec[3*i + 0]); + f4 t3 = VMUL_S(VSUB(x0, x3), g_sec[3*i + 1]); + x[0] = VADD(t0, t1); + x[8] = VMUL_S(VSUB(t0, t1), g_sec[3*i + 2]); + x[16] = VADD(t3, t2); + x[24] = VMUL_S(VSUB(t3, t2), g_sec[3*i + 2]); + } + for (x = t[0], i = 0; i < 4; i++, x += 8) + { + f4 x0 = x[0], x1 = x[1], x2 = x[2], x3 = x[3], x4 = x[4], x5 = x[5], x6 = x[6], x7 = x[7], xt; + xt = VSUB(x0, x7); x0 = VADD(x0, x7); + x7 = VSUB(x1, x6); x1 = VADD(x1, x6); + x6 = VSUB(x2, x5); x2 = VADD(x2, x5); + x5 = VSUB(x3, x4); x3 = VADD(x3, x4); + x4 = VSUB(x0, x3); x0 = VADD(x0, x3); + x3 = VSUB(x1, x2); x1 = VADD(x1, x2); + x[0] = VADD(x0, x1); + x[4] = VMUL_S(VSUB(x0, x1), 0.70710677f); + x5 = VADD(x5, x6); + x6 = VMUL_S(VADD(x6, x7), 0.70710677f); + x7 = VADD(x7, xt); + x3 = VMUL_S(VADD(x3, x4), 0.70710677f); + x5 = VSUB(x5, VMUL_S(x7, 0.198912367f)); /* rotate by PI/8 */ + x7 = VADD(x7, VMUL_S(x5, 0.382683432f)); + x5 = VSUB(x5, VMUL_S(x7, 0.198912367f)); + x0 = VSUB(xt, x6); xt = VADD(xt, x6); + x[1] = VMUL_S(VADD(xt, x7), 0.50979561f); + x[2] = VMUL_S(VADD(x4, x3), 0.54119611f); + x[3] = VMUL_S(VSUB(x0, x5), 0.60134488f); + x[5] = VMUL_S(VADD(x0, x5), 0.89997619f); + x[6] = VMUL_S(VSUB(x4, x3), 1.30656302f); + x[7] = VMUL_S(VSUB(xt, x7), 2.56291556f); + } + + if (k > n - 3) + { +#if HAVE_SSE +#define VSAVE2(i, v) _mm_storel_pi((__m64 *)(void*)&y[i*18], v) +#else /* HAVE_SSE */ +#define VSAVE2(i, v) vst1_f32((float32_t *)&y[i*18], vget_low_f32(v)) +#endif /* HAVE_SSE */ + for (i = 0; i < 7; i++, y += 4*18) + { + f4 s = VADD(t[3][i], t[3][i + 1]); + VSAVE2(0, t[0][i]); + VSAVE2(1, VADD(t[2][i], s)); + VSAVE2(2, VADD(t[1][i], t[1][i + 1])); + VSAVE2(3, VADD(t[2][1 + i], s)); + } + VSAVE2(0, t[0][7]); + VSAVE2(1, VADD(t[2][7], t[3][7])); + VSAVE2(2, t[1][7]); + VSAVE2(3, t[3][7]); + } else + { +#define VSAVE4(i, v) VSTORE(&y[i*18], v) + for (i = 0; i < 7; i++, y += 4*18) + { + f4 s = VADD(t[3][i], t[3][i + 1]); + VSAVE4(0, t[0][i]); + VSAVE4(1, VADD(t[2][i], s)); + VSAVE4(2, VADD(t[1][i], t[1][i + 1])); + VSAVE4(3, VADD(t[2][1 + i], s)); + } + VSAVE4(0, t[0][7]); + VSAVE4(1, VADD(t[2][7], t[3][7])); + VSAVE4(2, t[1][7]); + VSAVE4(3, t[3][7]); + } + } else +#endif /* HAVE_SIMD */ +#ifdef MINIMP3_ONLY_SIMD + {} /* for HAVE_SIMD=1, MINIMP3_ONLY_SIMD=1 case we do not need non-intrinsic "else" branch */ +#else /* MINIMP3_ONLY_SIMD */ + for (; k < n; k++) + { + float t[4][8], *x, *y = grbuf + k; + + for (x = t[0], i = 0; i < 8; i++, x++) + { + float x0 = y[i*18]; + float x1 = y[(15 - i)*18]; + float x2 = y[(16 + i)*18]; + float x3 = y[(31 - i)*18]; + float t0 = x0 + x3; + float t1 = x1 + x2; + float t2 = (x1 - x2)*g_sec[3*i + 0]; + float t3 = (x0 - x3)*g_sec[3*i + 1]; + x[0] = t0 + t1; + x[8] = (t0 - t1)*g_sec[3*i + 2]; + x[16] = t3 + t2; + x[24] = (t3 - t2)*g_sec[3*i + 2]; + } + for (x = t[0], i = 0; i < 4; i++, x += 8) + { + float x0 = x[0], x1 = x[1], x2 = x[2], x3 = x[3], x4 = x[4], x5 = x[5], x6 = x[6], x7 = x[7], xt; + xt = x0 - x7; x0 += x7; + x7 = x1 - x6; x1 += x6; + x6 = x2 - x5; x2 += x5; + x5 = x3 - x4; x3 += x4; + x4 = x0 - x3; x0 += x3; + x3 = x1 - x2; x1 += x2; + x[0] = x0 + x1; + x[4] = (x0 - x1)*0.70710677f; + x5 = x5 + x6; + x6 = (x6 + x7)*0.70710677f; + x7 = x7 + xt; + x3 = (x3 + x4)*0.70710677f; + x5 -= x7*0.198912367f; /* rotate by PI/8 */ + x7 += x5*0.382683432f; + x5 -= x7*0.198912367f; + x0 = xt - x6; xt += x6; + x[1] = (xt + x7)*0.50979561f; + x[2] = (x4 + x3)*0.54119611f; + x[3] = (x0 - x5)*0.60134488f; + x[5] = (x0 + x5)*0.89997619f; + x[6] = (x4 - x3)*1.30656302f; + x[7] = (xt - x7)*2.56291556f; + + } + for (i = 0; i < 7; i++, y += 4*18) + { + y[0*18] = t[0][i]; + y[1*18] = t[2][i] + t[3][i] + t[3][i + 1]; + y[2*18] = t[1][i] + t[1][i + 1]; + y[3*18] = t[2][i + 1] + t[3][i] + t[3][i + 1]; + } + y[0*18] = t[0][7]; + y[1*18] = t[2][7] + t[3][7]; + y[2*18] = t[1][7]; + y[3*18] = t[3][7]; + } +#endif /* MINIMP3_ONLY_SIMD */ +} + +#ifndef MINIMP3_FLOAT_OUTPUT +static int16_t mp3d_scale_pcm(float sample) +{ +#if HAVE_ARMV6 + int32_t s32 = (int32_t)(sample + .5f); + s32 -= (s32 < 0); + int16_t s = (int16_t)minimp3_clip_int16_arm(s32); +#else + if (sample >= 32766.5) return (int16_t) 32767; + if (sample <= -32767.5) return (int16_t)-32768; + int16_t s = (int16_t)(sample + .5f); + s -= (s < 0); /* away from zero, to be compliant */ +#endif + return s; +} +#else /* MINIMP3_FLOAT_OUTPUT */ +static float mp3d_scale_pcm(float sample) +{ + return sample*(1.f/32768.f); +} +#endif /* MINIMP3_FLOAT_OUTPUT */ + +static void mp3d_synth_pair(mp3d_sample_t *pcm, int nch, const float *z) +{ + float a; + a = (z[14*64] - z[ 0]) * 29; + a += (z[ 1*64] + z[13*64]) * 213; + a += (z[12*64] - z[ 2*64]) * 459; + a += (z[ 3*64] + z[11*64]) * 2037; + a += (z[10*64] - z[ 4*64]) * 5153; + a += (z[ 5*64] + z[ 9*64]) * 6574; + a += (z[ 8*64] - z[ 6*64]) * 37489; + a += z[ 7*64] * 75038; + pcm[0] = mp3d_scale_pcm(a); + + z += 2; + a = z[14*64] * 104; + a += z[12*64] * 1567; + a += z[10*64] * 9727; + a += z[ 8*64] * 64019; + a += z[ 6*64] * -9975; + a += z[ 4*64] * -45; + a += z[ 2*64] * 146; + a += z[ 0*64] * -5; + pcm[16*nch] = mp3d_scale_pcm(a); +} + +static void mp3d_synth(float *xl, mp3d_sample_t *dstl, int nch, float *lins) +{ + int i; + float *xr = xl + 576*(nch - 1); + mp3d_sample_t *dstr = dstl + (nch - 1); + + static const float g_win[] = { + -1,26,-31,208,218,401,-519,2063,2000,4788,-5517,7134,5959,35640,-39336,74992, + -1,24,-35,202,222,347,-581,2080,1952,4425,-5879,7640,5288,33791,-41176,74856, + -1,21,-38,196,225,294,-645,2087,1893,4063,-6237,8092,4561,31947,-43006,74630, + -1,19,-41,190,227,244,-711,2085,1822,3705,-6589,8492,3776,30112,-44821,74313, + -1,17,-45,183,228,197,-779,2075,1739,3351,-6935,8840,2935,28289,-46617,73908, + -1,16,-49,176,228,153,-848,2057,1644,3004,-7271,9139,2037,26482,-48390,73415, + -2,14,-53,169,227,111,-919,2032,1535,2663,-7597,9389,1082,24694,-50137,72835, + -2,13,-58,161,224,72,-991,2001,1414,2330,-7910,9592,70,22929,-51853,72169, + -2,11,-63,154,221,36,-1064,1962,1280,2006,-8209,9750,-998,21189,-53534,71420, + -2,10,-68,147,215,2,-1137,1919,1131,1692,-8491,9863,-2122,19478,-55178,70590, + -3,9,-73,139,208,-29,-1210,1870,970,1388,-8755,9935,-3300,17799,-56778,69679, + -3,8,-79,132,200,-57,-1283,1817,794,1095,-8998,9966,-4533,16155,-58333,68692, + -4,7,-85,125,189,-83,-1356,1759,605,814,-9219,9959,-5818,14548,-59838,67629, + -4,7,-91,117,177,-106,-1428,1698,402,545,-9416,9916,-7154,12980,-61289,66494, + -5,6,-97,111,163,-127,-1498,1634,185,288,-9585,9838,-8540,11455,-62684,65290 + }; + float *zlin = lins + 15*64; + const float *w = g_win; + + zlin[4*15] = xl[18*16]; + zlin[4*15 + 1] = xr[18*16]; + zlin[4*15 + 2] = xl[0]; + zlin[4*15 + 3] = xr[0]; + + zlin[4*31] = xl[1 + 18*16]; + zlin[4*31 + 1] = xr[1 + 18*16]; + zlin[4*31 + 2] = xl[1]; + zlin[4*31 + 3] = xr[1]; + + mp3d_synth_pair(dstr, nch, lins + 4*15 + 1); + mp3d_synth_pair(dstr + 32*nch, nch, lins + 4*15 + 64 + 1); + mp3d_synth_pair(dstl, nch, lins + 4*15); + mp3d_synth_pair(dstl + 32*nch, nch, lins + 4*15 + 64); + +#if HAVE_SIMD + if (have_simd()) for (i = 14; i >= 0; i--) + { +#define VLOAD(k) f4 w0 = VSET(*w++); f4 w1 = VSET(*w++); f4 vz = VLD(&zlin[4*i - 64*k]); f4 vy = VLD(&zlin[4*i - 64*(15 - k)]); +#define V0(k) { VLOAD(k) b = VADD(VMUL(vz, w1), VMUL(vy, w0)) ; a = VSUB(VMUL(vz, w0), VMUL(vy, w1)); } +#define V1(k) { VLOAD(k) b = VADD(b, VADD(VMUL(vz, w1), VMUL(vy, w0))); a = VADD(a, VSUB(VMUL(vz, w0), VMUL(vy, w1))); } +#define V2(k) { VLOAD(k) b = VADD(b, VADD(VMUL(vz, w1), VMUL(vy, w0))); a = VADD(a, VSUB(VMUL(vy, w1), VMUL(vz, w0))); } + f4 a, b; + zlin[4*i] = xl[18*(31 - i)]; + zlin[4*i + 1] = xr[18*(31 - i)]; + zlin[4*i + 2] = xl[1 + 18*(31 - i)]; + zlin[4*i + 3] = xr[1 + 18*(31 - i)]; + zlin[4*i + 64] = xl[1 + 18*(1 + i)]; + zlin[4*i + 64 + 1] = xr[1 + 18*(1 + i)]; + zlin[4*i - 64 + 2] = xl[18*(1 + i)]; + zlin[4*i - 64 + 3] = xr[18*(1 + i)]; + + V0(0) V2(1) V1(2) V2(3) V1(4) V2(5) V1(6) V2(7) + + { +#ifndef MINIMP3_FLOAT_OUTPUT +#if HAVE_SSE + static const f4 g_max = { 32767.0f, 32767.0f, 32767.0f, 32767.0f }; + static const f4 g_min = { -32768.0f, -32768.0f, -32768.0f, -32768.0f }; + __m128i pcm8 = _mm_packs_epi32(_mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(a, g_max), g_min)), + _mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(b, g_max), g_min))); + dstr[(15 - i)*nch] = _mm_extract_epi16(pcm8, 1); + dstr[(17 + i)*nch] = _mm_extract_epi16(pcm8, 5); + dstl[(15 - i)*nch] = _mm_extract_epi16(pcm8, 0); + dstl[(17 + i)*nch] = _mm_extract_epi16(pcm8, 4); + dstr[(47 - i)*nch] = _mm_extract_epi16(pcm8, 3); + dstr[(49 + i)*nch] = _mm_extract_epi16(pcm8, 7); + dstl[(47 - i)*nch] = _mm_extract_epi16(pcm8, 2); + dstl[(49 + i)*nch] = _mm_extract_epi16(pcm8, 6); +#else /* HAVE_SSE */ + int16x4_t pcma, pcmb; + a = VADD(a, VSET(0.5f)); + b = VADD(b, VSET(0.5f)); + pcma = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(a), vreinterpretq_s32_u32(vcltq_f32(a, VSET(0))))); + pcmb = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(b), vreinterpretq_s32_u32(vcltq_f32(b, VSET(0))))); + vst1_lane_s16(dstr + (15 - i)*nch, pcma, 1); + vst1_lane_s16(dstr + (17 + i)*nch, pcmb, 1); + vst1_lane_s16(dstl + (15 - i)*nch, pcma, 0); + vst1_lane_s16(dstl + (17 + i)*nch, pcmb, 0); + vst1_lane_s16(dstr + (47 - i)*nch, pcma, 3); + vst1_lane_s16(dstr + (49 + i)*nch, pcmb, 3); + vst1_lane_s16(dstl + (47 - i)*nch, pcma, 2); + vst1_lane_s16(dstl + (49 + i)*nch, pcmb, 2); +#endif /* HAVE_SSE */ + +#else /* MINIMP3_FLOAT_OUTPUT */ + + static const f4 g_scale = { 1.0f/32768.0f, 1.0f/32768.0f, 1.0f/32768.0f, 1.0f/32768.0f }; + a = VMUL(a, g_scale); + b = VMUL(b, g_scale); +#if HAVE_SSE + _mm_store_ss(dstr + (15 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(1, 1, 1, 1))); + _mm_store_ss(dstr + (17 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(1, 1, 1, 1))); + _mm_store_ss(dstl + (15 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(0, 0, 0, 0))); + _mm_store_ss(dstl + (17 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(0, 0, 0, 0))); + _mm_store_ss(dstr + (47 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 3, 3, 3))); + _mm_store_ss(dstr + (49 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 3, 3, 3))); + _mm_store_ss(dstl + (47 - i)*nch, _mm_shuffle_ps(a, a, _MM_SHUFFLE(2, 2, 2, 2))); + _mm_store_ss(dstl + (49 + i)*nch, _mm_shuffle_ps(b, b, _MM_SHUFFLE(2, 2, 2, 2))); +#else /* HAVE_SSE */ + vst1q_lane_f32(dstr + (15 - i)*nch, a, 1); + vst1q_lane_f32(dstr + (17 + i)*nch, b, 1); + vst1q_lane_f32(dstl + (15 - i)*nch, a, 0); + vst1q_lane_f32(dstl + (17 + i)*nch, b, 0); + vst1q_lane_f32(dstr + (47 - i)*nch, a, 3); + vst1q_lane_f32(dstr + (49 + i)*nch, b, 3); + vst1q_lane_f32(dstl + (47 - i)*nch, a, 2); + vst1q_lane_f32(dstl + (49 + i)*nch, b, 2); +#endif /* HAVE_SSE */ +#endif /* MINIMP3_FLOAT_OUTPUT */ + } + } else +#endif /* HAVE_SIMD */ +#ifdef MINIMP3_ONLY_SIMD + {} /* for HAVE_SIMD=1, MINIMP3_ONLY_SIMD=1 case we do not need non-intrinsic "else" branch */ +#else /* MINIMP3_ONLY_SIMD */ + for (i = 14; i >= 0; i--) + { +#define LOAD(k) float w0 = *w++; float w1 = *w++; float *vz = &zlin[4*i - k*64]; float *vy = &zlin[4*i - (15 - k)*64]; +#define S0(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] = vz[j]*w1 + vy[j]*w0, a[j] = vz[j]*w0 - vy[j]*w1; } +#define S1(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] += vz[j]*w1 + vy[j]*w0, a[j] += vz[j]*w0 - vy[j]*w1; } +#define S2(k) { int j; LOAD(k); for (j = 0; j < 4; j++) b[j] += vz[j]*w1 + vy[j]*w0, a[j] += vy[j]*w1 - vz[j]*w0; } + float a[4], b[4]; + + zlin[4*i] = xl[18*(31 - i)]; + zlin[4*i + 1] = xr[18*(31 - i)]; + zlin[4*i + 2] = xl[1 + 18*(31 - i)]; + zlin[4*i + 3] = xr[1 + 18*(31 - i)]; + zlin[4*(i + 16)] = xl[1 + 18*(1 + i)]; + zlin[4*(i + 16) + 1] = xr[1 + 18*(1 + i)]; + zlin[4*(i - 16) + 2] = xl[18*(1 + i)]; + zlin[4*(i - 16) + 3] = xr[18*(1 + i)]; + + S0(0) S2(1) S1(2) S2(3) S1(4) S2(5) S1(6) S2(7) + + dstr[(15 - i)*nch] = mp3d_scale_pcm(a[1]); + dstr[(17 + i)*nch] = mp3d_scale_pcm(b[1]); + dstl[(15 - i)*nch] = mp3d_scale_pcm(a[0]); + dstl[(17 + i)*nch] = mp3d_scale_pcm(b[0]); + dstr[(47 - i)*nch] = mp3d_scale_pcm(a[3]); + dstr[(49 + i)*nch] = mp3d_scale_pcm(b[3]); + dstl[(47 - i)*nch] = mp3d_scale_pcm(a[2]); + dstl[(49 + i)*nch] = mp3d_scale_pcm(b[2]); + } +#endif /* MINIMP3_ONLY_SIMD */ +} + +static void mp3d_synth_granule(float *qmf_state, float *grbuf, int nbands, int nch, mp3d_sample_t *pcm, float *lins) +{ + int i; + for (i = 0; i < nch; i++) + { + mp3d_DCT_II(grbuf + 576*i, nbands); + } + + memcpy(lins, qmf_state, sizeof(float)*15*64); + + for (i = 0; i < nbands; i += 2) + { + mp3d_synth(grbuf + i, pcm + 32*nch*i, nch, lins + i*64); + } +#ifndef MINIMP3_NONSTANDARD_BUT_LOGICAL + if (nch == 1) + { + for (i = 0; i < 15*64; i += 2) + { + qmf_state[i] = lins[nbands*64 + i]; + } + } else +#endif /* MINIMP3_NONSTANDARD_BUT_LOGICAL */ + { + memcpy(qmf_state, lins + nbands*64, sizeof(float)*15*64); + } +} + +static int mp3d_match_frame(const uint8_t *hdr, int mp3_bytes, int frame_bytes) +{ + int i, nmatch; + for (i = 0, nmatch = 0; nmatch < MAX_FRAME_SYNC_MATCHES; nmatch++) + { + i += hdr_frame_bytes(hdr + i, frame_bytes) + hdr_padding(hdr + i); + if (i + HDR_SIZE > mp3_bytes) + return nmatch > 0; + if (!hdr_compare(hdr, hdr + i)) + return 0; + } + return 1; +} + +static int mp3d_find_frame(const uint8_t *mp3, int mp3_bytes, int *free_format_bytes, int *ptr_frame_bytes) +{ + int i, k; + for (i = 0; i < mp3_bytes - HDR_SIZE; i++, mp3++) + { + if (hdr_valid(mp3)) + { + int frame_bytes = hdr_frame_bytes(mp3, *free_format_bytes); + int frame_and_padding = frame_bytes + hdr_padding(mp3); + + for (k = HDR_SIZE; !frame_bytes && k < MAX_FREE_FORMAT_FRAME_SIZE && i + 2*k < mp3_bytes - HDR_SIZE; k++) + { + if (hdr_compare(mp3, mp3 + k)) + { + int fb = k - hdr_padding(mp3); + int nextfb = fb + hdr_padding(mp3 + k); + if (i + k + nextfb + HDR_SIZE > mp3_bytes || !hdr_compare(mp3, mp3 + k + nextfb)) + continue; + frame_and_padding = k; + frame_bytes = fb; + *free_format_bytes = fb; + } + } + if ((frame_bytes && i + frame_and_padding <= mp3_bytes && + mp3d_match_frame(mp3, mp3_bytes - i, frame_bytes)) || + (!i && frame_and_padding == mp3_bytes)) + { + *ptr_frame_bytes = frame_and_padding; + return i; + } + *free_format_bytes = 0; + } + } + *ptr_frame_bytes = 0; + return mp3_bytes; +} + +void mp3dec_init(mp3dec_t *dec) +{ + dec->header[0] = 0; +} + +int mp3dec_decode_frame(mp3dec_t *dec, const uint8_t *mp3, int mp3_bytes, mp3d_sample_t *pcm, mp3dec_frame_info_t *info) +{ + int i = 0, igr, frame_size = 0, success = 1; + const uint8_t *hdr; + bs_t bs_frame[1]; + mp3dec_scratch_t scratch; + + if (mp3_bytes > 4 && dec->header[0] == 0xff && hdr_compare(dec->header, mp3)) + { + frame_size = hdr_frame_bytes(mp3, dec->free_format_bytes) + hdr_padding(mp3); + if (frame_size != mp3_bytes && (frame_size + HDR_SIZE > mp3_bytes || !hdr_compare(mp3, mp3 + frame_size))) + { + frame_size = 0; + } + } + if (!frame_size) + { + memset(dec, 0, sizeof(mp3dec_t)); + i = mp3d_find_frame(mp3, mp3_bytes, &dec->free_format_bytes, &frame_size); + if (!frame_size || i + frame_size > mp3_bytes) + { + info->frame_bytes = i; + return 0; + } + } + + hdr = mp3 + i; + memcpy(dec->header, hdr, HDR_SIZE); + info->frame_bytes = i + frame_size; + info->frame_offset = i; + info->channels = HDR_IS_MONO(hdr) ? 1 : 2; + info->hz = hdr_sample_rate_hz(hdr); + info->layer = 4 - HDR_GET_LAYER(hdr); + info->bitrate_kbps = hdr_bitrate_kbps(hdr); + + if (!pcm) + { + return hdr_frame_samples(hdr); + } + + bs_init(bs_frame, hdr + HDR_SIZE, frame_size - HDR_SIZE); + if (HDR_IS_CRC(hdr)) + { + get_bits(bs_frame, 16); + } + + if (info->layer == 3) + { + int main_data_begin = L3_read_side_info(bs_frame, scratch.gr_info, hdr); + if (main_data_begin < 0 || bs_frame->pos > bs_frame->limit) + { + mp3dec_init(dec); + return 0; + } + success = L3_restore_reservoir(dec, bs_frame, &scratch, main_data_begin); + if (success) + { + for (igr = 0; igr < (HDR_TEST_MPEG1(hdr) ? 2 : 1); igr++, pcm += 576*info->channels) + { + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + L3_decode(dec, &scratch, scratch.gr_info + igr*info->channels, info->channels); + mp3d_synth_granule(dec->qmf_state, scratch.grbuf[0], 18, info->channels, pcm, scratch.syn[0]); + } + } + L3_save_reservoir(dec, &scratch); + } else + { +#ifdef MINIMP3_ONLY_MP3 + return 0; +#else /* MINIMP3_ONLY_MP3 */ + L12_scale_info sci[1]; + L12_read_scale_info(hdr, bs_frame, sci); + + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + for (i = 0, igr = 0; igr < 3; igr++) + { + if (12 == (i += L12_dequantize_granule(scratch.grbuf[0] + i, bs_frame, sci, info->layer | 1))) + { + i = 0; + L12_apply_scf_384(sci, sci->scf + igr, scratch.grbuf[0]); + mp3d_synth_granule(dec->qmf_state, scratch.grbuf[0], 12, info->channels, pcm, scratch.syn[0]); + memset(scratch.grbuf[0], 0, 576*2*sizeof(float)); + pcm += 384*info->channels; + } + if (bs_frame->pos > bs_frame->limit) + { + mp3dec_init(dec); + return 0; + } + } +#endif /* MINIMP3_ONLY_MP3 */ + } + return success*hdr_frame_samples(dec->header); +} + +#ifdef MINIMP3_FLOAT_OUTPUT +void mp3dec_f32_to_s16(const float *in, int16_t *out, int num_samples) +{ + int i = 0; +#if HAVE_SIMD + int aligned_count = num_samples & ~7; + for(; i < aligned_count; i += 8) + { + static const f4 g_scale = { 32768.0f, 32768.0f, 32768.0f, 32768.0f }; + f4 a = VMUL(VLD(&in[i ]), g_scale); + f4 b = VMUL(VLD(&in[i+4]), g_scale); +#if HAVE_SSE + static const f4 g_max = { 32767.0f, 32767.0f, 32767.0f, 32767.0f }; + static const f4 g_min = { -32768.0f, -32768.0f, -32768.0f, -32768.0f }; + __m128i pcm8 = _mm_packs_epi32(_mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(a, g_max), g_min)), + _mm_cvtps_epi32(_mm_max_ps(_mm_min_ps(b, g_max), g_min))); + out[i ] = _mm_extract_epi16(pcm8, 0); + out[i+1] = _mm_extract_epi16(pcm8, 1); + out[i+2] = _mm_extract_epi16(pcm8, 2); + out[i+3] = _mm_extract_epi16(pcm8, 3); + out[i+4] = _mm_extract_epi16(pcm8, 4); + out[i+5] = _mm_extract_epi16(pcm8, 5); + out[i+6] = _mm_extract_epi16(pcm8, 6); + out[i+7] = _mm_extract_epi16(pcm8, 7); +#else /* HAVE_SSE */ + int16x4_t pcma, pcmb; + a = VADD(a, VSET(0.5f)); + b = VADD(b, VSET(0.5f)); + pcma = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(a), vreinterpretq_s32_u32(vcltq_f32(a, VSET(0))))); + pcmb = vqmovn_s32(vqaddq_s32(vcvtq_s32_f32(b), vreinterpretq_s32_u32(vcltq_f32(b, VSET(0))))); + vst1_lane_s16(out+i , pcma, 0); + vst1_lane_s16(out+i+1, pcma, 1); + vst1_lane_s16(out+i+2, pcma, 2); + vst1_lane_s16(out+i+3, pcma, 3); + vst1_lane_s16(out+i+4, pcmb, 0); + vst1_lane_s16(out+i+5, pcmb, 1); + vst1_lane_s16(out+i+6, pcmb, 2); + vst1_lane_s16(out+i+7, pcmb, 3); +#endif /* HAVE_SSE */ + } +#endif /* HAVE_SIMD */ + for(; i < num_samples; i++) + { + float sample = in[i] * 32768.0f; + if (sample >= 32766.5) + out[i] = (int16_t) 32767; + else if (sample <= -32767.5) + out[i] = (int16_t)-32768; + else + { + int16_t s = (int16_t)(sample + .5f); + s -= (s < 0); /* away from zero, to be compliant */ + out[i] = s; + } + } +} +#endif /* MINIMP3_FLOAT_OUTPUT */ +#endif /* MINIMP3_IMPLEMENTATION && !_MINIMP3_IMPLEMENTATION_GUARD */ diff --git a/programs/src/music/minimp3_impl.cpp b/programs/src/music/minimp3_impl.cpp new file mode 100644 index 0000000..7eb18f0 --- /dev/null +++ b/programs/src/music/minimp3_impl.cpp @@ -0,0 +1,16 @@ +/* + * minimp3_impl.cpp + * Single compilation unit for minimp3 in MontaukOS freestanding environment + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include + +extern "C" { +#include +#include +} + +#define MINIMP3_IMPLEMENTATION +#include "minimp3.h" diff --git a/programs/src/music/stb_truetype_impl.cpp b/programs/src/music/stb_truetype_impl.cpp new file mode 100644 index 0000000..9ce2266 --- /dev/null +++ b/programs/src/music/stb_truetype_impl.cpp @@ -0,0 +1,35 @@ +/* + * stb_truetype_impl.cpp + * Single compilation unit for stb_truetype in MontaukOS freestanding environment + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include + +// Override all stb_truetype dependencies before including the implementation + +#define STBTT_ifloor(x) ((int) stb_floor(x)) +#define STBTT_iceil(x) ((int) stb_ceil(x)) +#define STBTT_sqrt(x) stb_sqrt(x) +#define STBTT_pow(x,y) stb_pow(x,y) +#define STBTT_fmod(x,y) stb_fmod(x,y) +#define STBTT_cos(x) stb_cos(x) +#define STBTT_acos(x) stb_acos(x) +#define STBTT_fabs(x) stb_fabs(x) + +#define STBTT_malloc(x,u) ((void)(u), montauk::malloc(x)) +#define STBTT_free(x,u) ((void)(u), montauk::mfree(x)) + +#define STBTT_memcpy(d,s,n) montauk::memcpy(d,s,n) +#define STBTT_memset(d,v,n) montauk::memset(d,v,n) + +#define STBTT_strlen(x) montauk::slen(x) + +#define STBTT_assert(x) ((void)(x)) + +#define STB_TRUETYPE_IMPLEMENTATION +#include diff --git a/programs/src/volume/Makefile b/programs/src/volume/Makefile new file mode 100644 index 0000000..a361ab7 --- /dev/null +++ b/programs/src/volume/Makefile @@ -0,0 +1,86 @@ +# Makefile for volume (standalone Volume Control) on MontaukOS +# Copyright (c) 2026 Daniel Hammer + +MAKEFLAGS += -rR +.SUFFIXES: + +# ---- Toolchain ---- + +TOOLCHAIN_PREFIX := $(shell cd ../../.. && pwd)/toolchain/local/bin/x86_64-elf- +ifneq ($(wildcard $(TOOLCHAIN_PREFIX)gcc),) + CXX := $(TOOLCHAIN_PREFIX)g++ +else + CXX := g++ +endif + +# ---- Paths ---- + +PROG_INC := ../../include +LINK_LD := ../../link.ld +BINDIR := ../../bin +OBJDIR := obj +LIBDIR := ../../lib + +# ---- Compiler flags ---- + +CXXFLAGS := \ + -std=gnu++20 \ + -g -O2 -pipe \ + -Wall \ + -Wextra \ + -Wno-unused-parameter \ + -Wno-unused-function \ + -nostdinc \ + -ffreestanding \ + -fno-stack-protector \ + -fno-stack-check \ + -fno-PIC \ + -fno-rtti \ + -fno-exceptions \ + -ffunction-sections \ + -fdata-sections \ + -m64 \ + -march=x86-64 \ + -msse \ + -msse2 \ + -mno-red-zone \ + -mcmodel=small \ + -I $(PROG_INC) \ + -isystem $(PROG_INC)/libc \ + -isystem ../../../kernel/freestnd-c-hdrs/x86_64/include \ + -isystem ../../../kernel/freestnd-cxx-hdrs/x86_64/include + +# ---- Linker flags ---- + +LDFLAGS := \ + -nostdlib \ + -static \ + -Wl,--build-id=none \ + -Wl,--gc-sections \ + -Wl,-m,elf_x86_64 \ + -z max-page-size=0x1000 \ + -T $(LINK_LD) + +# ---- Source files ---- + +SRCS := main.cpp stb_truetype_impl.cpp +OBJS := $(addprefix $(OBJDIR)/,$(SRCS:.cpp=.o)) + +# ---- Target ---- + +TARGET := $(BINDIR)/apps/volume/volume.elf + +.PHONY: all clean + +all: $(TARGET) + +$(TARGET): $(OBJS) $(LINK_LD) Makefile + mkdir -p $(BINDIR)/apps/volume + $(CXX) $(CXXFLAGS) $(LDFLAGS) $(OBJS) $(LIBDIR)/libc/liblibc.a -o $@ + +$(OBJDIR)/%.o: %.cpp Makefile + mkdir -p $(OBJDIR) + $(CXX) $(CXXFLAGS) -c $< -o $@ + +clean: + rm -rf $(OBJDIR) $(TARGET) diff --git a/programs/src/volume/main.cpp b/programs/src/volume/main.cpp new file mode 100644 index 0000000..3df1b73 --- /dev/null +++ b/programs/src/volume/main.cpp @@ -0,0 +1,368 @@ +/* + * main.cpp + * MontaukOS Volume Control + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include + +extern "C" { +#include +#include +} + +using namespace gui; + +// ============================================================================ +// Constants +// ============================================================================ + +static constexpr int WIN_W = 280; +static constexpr int WIN_H = 164; +static constexpr int FONT_SIZE = 16; +static constexpr int FONT_SIZE_LG = 28; + +static constexpr int SLIDER_X = 24; +static constexpr int SLIDER_W = WIN_W - 48; +static constexpr int SLIDER_Y = 78; +static constexpr int SLIDER_H = 8; +static constexpr int KNOB_R = 10; + +static constexpr int BTN_W = 48; +static constexpr int BTN_H = 28; +static constexpr int BTN_Y = 118; +static constexpr int BTN_RAD = 6; + +static constexpr Color BG_COLOR = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color TEXT_COLOR = Color::from_rgb(0x22, 0x22, 0x22); +static constexpr Color DIM_TEXT = Color::from_rgb(0x66, 0x66, 0x66); +static constexpr Color ACCENT = Color::from_rgb(0x36, 0x7B, 0xF0); +static constexpr Color ACCENT_HOVER = Color::from_rgb(0x2A, 0x62, 0xC8); +static constexpr Color TRACK_BG = Color::from_rgb(0xDD, 0xDD, 0xDD); +static constexpr Color WHITE = Color::from_rgb(0xFF, 0xFF, 0xFF); +static constexpr Color MUTE_COLOR = Color::from_rgb(0xCC, 0x33, 0x33); + +// ============================================================================ +// State +// ============================================================================ + +static TrueTypeFont* g_font = nullptr; +static int g_volume = 80; +static bool g_muted = false; +static int g_pre_mute_vol = 80; +static bool g_dragging = false; + +// ============================================================================ +// Pixel helpers (same pattern as disks app) +// ============================================================================ + +static void px_fill(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, Color c) { + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x, y0 = y < 0 ? 0 : y; + int x1 = x + w > bw ? bw : x + w; + int y1 = y + h > bh ? bh : y + h; + for (int row = y0; row < y1; row++) + for (int col = x0; col < x1; col++) + px[row * bw + col] = v; +} + +static void px_hline(uint32_t* px, int bw, int bh, int x, int y, int w, Color c) { + if (y < 0 || y >= bh) return; + uint32_t v = c.to_pixel(); + int x0 = x < 0 ? 0 : x; + int x1 = x + w > bw ? bw : x + w; + for (int col = x0; col < x1; col++) + px[y * bw + col] = v; +} + +static void px_fill_rounded(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int row = 0; row < h; row++) { + int dy = y + row; + if (dy < 0 || dy >= bh) continue; + for (int col = 0; col < w; col++) { + int dx = x + col; + if (dx < 0 || dx >= bw) continue; + bool skip = false; + int cx, cy; + if (col < r && row < r) { cx = r - col - 1; cy = r - row - 1; if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col >= w - r && row < r) { cx = col - (w - r); cy = r - row - 1; if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col < r && row >= h - r) { cx = r - col - 1; cy = row - (h - r); if (cx*cx + cy*cy >= r*r) skip = true; } + else if (col >= w - r && row >= h - r) { cx = col - (w - r); cy = row - (h - r); if (cx*cx + cy*cy >= r*r) skip = true; } + if (!skip) px[dy * bw + dx] = v; + } + } +} + +static void px_circle(uint32_t* px, int bw, int bh, + int cx, int cy, int r, Color c) { + uint32_t v = c.to_pixel(); + for (int dy = -r; dy <= r; dy++) { + int py = cy + dy; + if (py < 0 || py >= bh) continue; + for (int dx = -r; dx <= r; dx++) { + int ppx = cx + dx; + if (ppx < 0 || ppx >= bw) continue; + if (dx * dx + dy * dy <= r * r) + px[py * bw + ppx] = v; + } + } +} + +static void px_text(uint32_t* px, int bw, int bh, + int x, int y, const char* text, Color c, int size = FONT_SIZE) { + if (g_font) + g_font->draw_to_buffer(px, bw, bh, x, y, text, c, size); +} + +static int text_w(const char* text, int size = FONT_SIZE) { + return g_font ? g_font->measure_text(text, size) : 0; +} + +static int font_h(int size = FONT_SIZE) { + if (!g_font) return 16; + auto* cache = g_font->get_cache(size); + return cache->ascent - cache->descent; +} + +static void px_button(uint32_t* px, int bw, int bh, + int x, int y, int w, int h, + const char* label, Color bg, Color fg, int r) { + px_fill_rounded(px, bw, bh, x, y, w, h, r, bg); + int tw = text_w(label); + int fh = font_h(); + px_text(px, bw, bh, x + (w - tw) / 2, y + (h - fh) / 2, label, fg); +} + +// ============================================================================ +// Volume helpers +// ============================================================================ + +static void apply_volume(int vol) { + if (vol < 0) vol = 0; + if (vol > 100) vol = 100; + g_volume = vol; + montauk::audio_set_volume(0, g_volume); +} + +static void refresh_volume() { + int v = montauk::audio_get_volume(0); + if (v >= 0) g_volume = v; +} + +// ============================================================================ +// Render +// ============================================================================ + +static void render(uint32_t* pixels) { + int fh_lg = font_h(FONT_SIZE_LG); + + // Background + px_fill(pixels, WIN_W, WIN_H, 0, 0, WIN_W, WIN_H, BG_COLOR); + + // Volume percentage (large, centered) + char vol_str[8]; + snprintf(vol_str, sizeof(vol_str), "%d%%", g_muted ? 0 : g_volume); + int vw = text_w(vol_str, FONT_SIZE_LG); + Color vol_color = g_muted ? MUTE_COLOR : ACCENT; + px_text(pixels, WIN_W, WIN_H, (WIN_W - vw) / 2, 20, vol_str, vol_color, FONT_SIZE_LG); + + // "Muted" label + if (g_muted) { + const char* muted_label = "Muted"; + int mw = text_w(muted_label); + px_text(pixels, WIN_W, WIN_H, (WIN_W - mw) / 2, + 20 + fh_lg + 4, muted_label, MUTE_COLOR); + } + + // Slider track + px_fill_rounded(pixels, WIN_W, WIN_H, SLIDER_X, SLIDER_Y, SLIDER_W, SLIDER_H, 4, TRACK_BG); + + // Filled portion + int display_vol = g_muted ? 0 : g_volume; + int fill_w = (display_vol * SLIDER_W) / 100; + if (fill_w > 0) + px_fill_rounded(pixels, WIN_W, WIN_H, SLIDER_X, SLIDER_Y, fill_w, SLIDER_H, 4, ACCENT); + + // Knob + int knob_x = SLIDER_X + fill_w; + int knob_y = SLIDER_Y + SLIDER_H / 2; + px_circle(pixels, WIN_W, WIN_H, knob_x, knob_y, KNOB_R, ACCENT); + px_circle(pixels, WIN_W, WIN_H, knob_x, knob_y, KNOB_R - 3, WHITE); + + // Buttons: [-] and [+] and [Mute] + int total_btn_w = BTN_W * 2 + 60 + 12 * 2; // minus, plus, mute + gaps + int bx = (WIN_W - total_btn_w) / 2; + + px_button(pixels, WIN_W, WIN_H, bx, BTN_Y, BTN_W, BTN_H, + "-", Color::from_rgb(0xE0, 0xE0, 0xE0), TEXT_COLOR, BTN_RAD); + bx += BTN_W + 12; + + px_button(pixels, WIN_W, WIN_H, bx, BTN_Y, BTN_W, BTN_H, + "+", Color::from_rgb(0xE0, 0xE0, 0xE0), TEXT_COLOR, BTN_RAD); + bx += BTN_W + 12; + + Color mute_bg = g_muted ? MUTE_COLOR : Color::from_rgb(0xE0, 0xE0, 0xE0); + Color mute_fg = g_muted ? WHITE : TEXT_COLOR; + px_button(pixels, WIN_W, WIN_H, bx, BTN_Y, 60, BTN_H, + "Mute", mute_bg, mute_fg, BTN_RAD); +} + +// ============================================================================ +// Hit testing +// ============================================================================ + +static int vol_from_slider_x(int mx) { + int v = ((mx - SLIDER_X) * 100) / SLIDER_W; + if (v < 0) v = 0; + if (v > 100) v = 100; + return v; +} + +static bool handle_click(int mx, int my) { + // Slider area (generous vertical hit zone) + if (my >= SLIDER_Y - KNOB_R && my <= SLIDER_Y + SLIDER_H + KNOB_R && + mx >= SLIDER_X - KNOB_R && mx <= SLIDER_X + SLIDER_W + KNOB_R) { + g_muted = false; + apply_volume(vol_from_slider_x(mx)); + g_dragging = true; + return true; + } + + // Buttons + int total_btn_w = BTN_W * 2 + 60 + 12 * 2; + int bx = (WIN_W - total_btn_w) / 2; + + if (my >= BTN_Y && my < BTN_Y + BTN_H) { + // [-] button + if (mx >= bx && mx < bx + BTN_W) { + g_muted = false; + apply_volume(g_volume - 5); + return true; + } + bx += BTN_W + 12; + + // [+] button + if (mx >= bx && mx < bx + BTN_W) { + g_muted = false; + apply_volume(g_volume + 5); + return true; + } + bx += BTN_W + 12; + + // [Mute] button + if (mx >= bx && mx < bx + 60) { + if (g_muted) { + g_muted = false; + apply_volume(g_pre_mute_vol); + } else { + g_pre_mute_vol = g_volume; + g_muted = true; + montauk::audio_set_volume(0, 0); + } + return true; + } + } + + return false; +} + +// ============================================================================ +// Entry point +// ============================================================================ + +extern "C" void _start() { + // Load font + { + TrueTypeFont* f = (TrueTypeFont*)montauk::malloc(sizeof(TrueTypeFont)); + if (f) { + montauk::memset(f, 0, sizeof(TrueTypeFont)); + if (!f->init("0:/fonts/Roboto-Medium.ttf")) { montauk::mfree(f); f = nullptr; } + } + g_font = f; + } + + refresh_volume(); + + // Create window + Montauk::WinCreateResult wres; + if (montauk::win_create("Volume", WIN_W, WIN_H, &wres) < 0 || wres.id < 0) + montauk::exit(1); + + int win_id = wres.id; + uint32_t* pixels = (uint32_t*)(uintptr_t)wres.pixelVa; + + render(pixels); + montauk::win_present(win_id); + + while (true) { + Montauk::WinEvent ev; + int r = montauk::win_poll(win_id, &ev); + + if (r < 0) break; + if (r == 0) { montauk::sleep_ms(16); continue; } + + bool redraw = false; + + if (ev.type == 3) break; // close + + // Keyboard + if (ev.type == 0 && ev.key.pressed) { + if (ev.key.scancode == 0x01) break; // Escape + if (ev.key.scancode == 0x4D || ev.key.ascii == '+' || ev.key.ascii == '=') { // Right / + + g_muted = false; + apply_volume(g_volume + 5); + redraw = true; + } else if (ev.key.scancode == 0x4B || ev.key.ascii == '-') { // Left / - + g_muted = false; + apply_volume(g_volume - 5); + redraw = true; + } else if (ev.key.ascii == 'm' || ev.key.ascii == 'M') { + if (g_muted) { + g_muted = false; + apply_volume(g_pre_mute_vol); + } else { + g_pre_mute_vol = g_volume; + g_muted = true; + montauk::audio_set_volume(0, 0); + } + redraw = true; + } + } + + // Mouse + if (ev.type == 1) { + bool clicked = (ev.mouse.buttons & 1) && !(ev.mouse.prev_buttons & 1); + bool released = !(ev.mouse.buttons & 1) && (ev.mouse.prev_buttons & 1); + + if (clicked) { + if (handle_click(ev.mouse.x, ev.mouse.y)) + redraw = true; + } + + if (g_dragging && (ev.mouse.buttons & 1)) { + g_muted = false; + apply_volume(vol_from_slider_x(ev.mouse.x)); + redraw = true; + } + + if (released) { + g_dragging = false; + } + } + + if (redraw) { + render(pixels); + montauk::win_present(win_id); + } + } + + montauk::win_destroy(win_id); + montauk::exit(0); +} diff --git a/programs/src/volume/manifest.toml b/programs/src/volume/manifest.toml new file mode 100644 index 0000000..b2caccd --- /dev/null +++ b/programs/src/volume/manifest.toml @@ -0,0 +1,8 @@ +[app] +name = "Volume" +binary = "volume.elf" +icon = "pavucontrol.svg" + +[menu] +category = "System" +visible = true diff --git a/programs/src/volume/stb_truetype_impl.cpp b/programs/src/volume/stb_truetype_impl.cpp new file mode 100644 index 0000000..9ce2266 --- /dev/null +++ b/programs/src/volume/stb_truetype_impl.cpp @@ -0,0 +1,35 @@ +/* + * stb_truetype_impl.cpp + * Single compilation unit for stb_truetype in MontaukOS freestanding environment + * Copyright (c) 2026 Daniel Hammer + */ + +#include +#include +#include +#include +#include + +// Override all stb_truetype dependencies before including the implementation + +#define STBTT_ifloor(x) ((int) stb_floor(x)) +#define STBTT_iceil(x) ((int) stb_ceil(x)) +#define STBTT_sqrt(x) stb_sqrt(x) +#define STBTT_pow(x,y) stb_pow(x,y) +#define STBTT_fmod(x,y) stb_fmod(x,y) +#define STBTT_cos(x) stb_cos(x) +#define STBTT_acos(x) stb_acos(x) +#define STBTT_fabs(x) stb_fabs(x) + +#define STBTT_malloc(x,u) ((void)(u), montauk::malloc(x)) +#define STBTT_free(x,u) ((void)(u), montauk::mfree(x)) + +#define STBTT_memcpy(d,s,n) montauk::memcpy(d,s,n) +#define STBTT_memset(d,v,n) montauk::memset(d,v,n) + +#define STBTT_strlen(x) montauk::slen(x) + +#define STBTT_assert(x) ((void)(x)) + +#define STB_TRUETYPE_IMPLEMENTATION +#include diff --git a/scripts/copy_icons.sh b/scripts/copy_icons.sh index 4eafe9b..93e4793 100755 --- a/scripts/copy_icons.sh +++ b/scripts/copy_icons.sh @@ -82,6 +82,14 @@ ICONS=( "apps/scalable/utilities-system-monitor.svg" # system monitor "apps/scalable/drive-harddisk.svg" "actions/16/trash-empty.svg" + "apps/scalable/pavucontrol.svg" # for volume control app + "actions/16/media-pause.svg" + "actions/16/media-play.svg" + "actions/16/media-stop.svg" + "actions/16/media-forward.svg" + "actions/16/media-rewind.svg" + "apps/scalable/multimedia-video-player.svg" + "apps/scalable/bluetooth.svg" ) copied=0 diff --git a/scripts/install_apps.sh b/scripts/install_apps.sh index 37a4a8d..f6222b9 100755 --- a/scripts/install_apps.sh +++ b/scripts/install_apps.sh @@ -25,6 +25,10 @@ APPS=( "disks|apps/scalable/gparted.svg" "devexplorer|apps/scalable/hardware.svg" "installer|mimetypes/scalable/text-x-install.svg" + "volume|apps/scalable/pavucontrol.svg" + "music|apps/scalable/audio-player.svg" + "video|apps/scalable/multimedia-video-player.svg" + "bluetooth|apps/scalable/bluetooth.svg" ) installed=0