From e3eb3fd41555a57a6faf23760b2e6b828be6eed6 Mon Sep 17 00:00:00 2001 From: Adolfo Reyna Date: Sat, 18 Jul 2026 17:29:37 -0400 Subject: [PATCH] feat(mcp): restore MCP system with native log.h - MCP 3.3K LOC: McpSystem 1899 + McpHandler 1029 + McpScreensaver + 2 apps - Uses native tactility/log.h TAG macros, no old Logger.h - DisplaySettings McpScreen enum, WebServer coexistence (MCP enabled starts HTTP) - DisplayIdle: startMcpScreensaver + isDeviceCharging + lock inversion fix 3629ffef - LVGL 512K PSRAM cache retained from previous perf patch - Audio kept upstream es8311-module per note we might not need custom --- .../Tactility/settings/DisplaySettings.h | 1 + .../Include/Tactility/settings/McpSettings.h | 14 + Tactility/Private/Tactility/mcp/McpSystem.h | 74 + Tactility/Private/Tactility/mcp/minimp3.h | 1869 ++++++++++++++++ .../service/webserver/WebServerService.h | 2 + .../Source/app/mcpoverride/McpOverrideApp.cpp | 141 ++ .../Source/app/mcpsettings/McpSettingsApp.cpp | 196 ++ Tactility/Source/mcp/McpSystem.cpp | 1900 +++++++++++++++++ .../service/displayidle/DisplayIdle.cpp | 129 +- .../service/displayidle/McpScreensaver.cpp | 100 + .../service/displayidle/McpScreensaver.h | 30 + .../Source/service/webserver/McpHandler.cpp | 1030 +++++++++ .../service/webserver/WebServerService.cpp | 33 +- Tactility/Source/settings/DisplaySettings.cpp | 3 + Tactility/Source/settings/McpSettings.cpp | 72 + 15 files changed, 5568 insertions(+), 26 deletions(-) create mode 100644 Tactility/Include/Tactility/settings/McpSettings.h create mode 100644 Tactility/Private/Tactility/mcp/McpSystem.h create mode 100644 Tactility/Private/Tactility/mcp/minimp3.h create mode 100644 Tactility/Source/app/mcpoverride/McpOverrideApp.cpp create mode 100644 Tactility/Source/app/mcpsettings/McpSettingsApp.cpp create mode 100644 Tactility/Source/mcp/McpSystem.cpp create mode 100644 Tactility/Source/service/displayidle/McpScreensaver.cpp create mode 100644 Tactility/Source/service/displayidle/McpScreensaver.h create mode 100644 Tactility/Source/service/webserver/McpHandler.cpp create mode 100644 Tactility/Source/settings/McpSettings.cpp diff --git a/Tactility/Include/Tactility/settings/DisplaySettings.h b/Tactility/Include/Tactility/settings/DisplaySettings.h index 6d9a6602..12d2b3e2 100644 --- a/Tactility/Include/Tactility/settings/DisplaySettings.h +++ b/Tactility/Include/Tactility/settings/DisplaySettings.h @@ -13,6 +13,7 @@ enum class ScreensaverType { Mystify, MatrixRain, StackChan, + McpScreen, Count }; struct DisplaySettings { diff --git a/Tactility/Include/Tactility/settings/McpSettings.h b/Tactility/Include/Tactility/settings/McpSettings.h new file mode 100644 index 00000000..4e614536 --- /dev/null +++ b/Tactility/Include/Tactility/settings/McpSettings.h @@ -0,0 +1,14 @@ +#pragma once + +namespace tt::settings::mcp { + +struct McpSettings { + bool mcpEnabled = false; // Enable MCP server endpoints on system web server +}; + +bool load(McpSettings& settings); +McpSettings getDefault(); +McpSettings loadOrGetDefault(); +bool save(const McpSettings& settings); + +} // namespace tt::settings::mcp diff --git a/Tactility/Private/Tactility/mcp/McpSystem.h b/Tactility/Private/Tactility/mcp/McpSystem.h new file mode 100644 index 00000000..e4008b73 --- /dev/null +++ b/Tactility/Private/Tactility/mcp/McpSystem.h @@ -0,0 +1,74 @@ +#pragma once +#ifdef ESP_PLATFORM + +#include +#include +#include +#include +#include + +namespace tt::mcp { + +struct McpSystemState { + std::mutex mutex; + bool overrideActive = false; + + // UI elements when McpOverrideApp is active + lv_obj_t* drawArea = nullptr; + uint16_t* framebuffer = nullptr; + size_t framebufferSize = 0; + uint16_t displayWidth = 320; + uint16_t displayHeight = 240; + uint16_t drawWidth = 320; + uint16_t drawHeight = 240; + int drawColor = 1; // 0 = white, 1 = black + + // Audio device status + Device* i2sDevice = nullptr; + volatile bool audioBusy = false; + volatile bool audioRunning = false; // Used to abort play/record loop + + // Video streaming state + volatile bool streamRunning = false; + void* streamTaskHandle = nullptr; // use void* to avoid freertos header inclusion dependency + uint32_t framesDrawn = 0; + uint32_t tcpBytesReceived = 0; + uint32_t lastDrawMs = 0; + double lastFps = 0.0; +}; + +McpSystemState& getState(); + +bool clearScreen(int color); +bool drawText(const std::string& text, int x, int y, int size); +bool drawRgb565(const uint8_t* data, size_t size, int x, int y, int w, int h); +bool drawBmp(const uint8_t* data, size_t size, int x, int y); +bool drawPbm(const uint8_t* data, size_t size, int x, int y); +std::string getScreenshotPbmBase64(); + +bool playTone(int frequency, int durationMs, int volume, std::string& error); +bool recordVoice(int durationSec, const std::string& filename, size_t& recordedBytes, std::string& error); +bool playAudioFile(const std::string& filename, int volume, std::string& error); +bool playWavMemory(const uint8_t* data, size_t size, int volume, std::string& error); +bool playMp3File(const std::string& filename, int volume, std::string& error); +bool playMp3Memory(const uint8_t* data, size_t size, int volume, std::string& error); + +bool getBatteryStatus(double& voltage_v, int& percentage_pct, std::string& error); +bool setLedColor(int r, int g, int b, const std::string& mode, std::string& error); +bool getSensors(double& temp_c, double& hum_pct, std::string& imu_json, std::string& error); +bool scanBleDevices(int duration_ms, std::string& devices_json, std::string& error); +bool writeSdFile(const std::string& filename, const std::string& content, std::string& error); +bool readSdFile(const std::string& filename, std::string& content, std::string& error); +bool downloadSdFile(const std::string& url, const std::string& filename, std::string& error); + +bool startVideoStreamServer(); +void stopVideoStreamServer(); +bool getVideoStreamStats(std::string& stats_json); + +bool listApps(std::string& apps_json, std::string& error); +bool runApp(const std::string& appId, std::string& error); +bool listSdFiles(const std::string& directory, std::string& files_json, std::string& error); + +} // namespace tt::mcp + +#endif diff --git a/Tactility/Private/Tactility/mcp/minimp3.h b/Tactility/Private/Tactility/mcp/minimp3.h new file mode 100644 index 00000000..9002b68d --- /dev/null +++ b/Tactility/Private/Tactility/mcp/minimp3.h @@ -0,0 +1,1869 @@ +#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]; + float scratch_padding[4100]; +} 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]; + _Static_assert(sizeof(mp3dec_scratch_t) <= sizeof(dec->scratch_padding), "scratch_padding is too small"); +#define scratch (*(mp3dec_scratch_t*)dec->scratch_padding) + + 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 */ + } + int result = success*hdr_frame_samples(dec->header); +#undef scratch + return result; +} + +#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/Tactility/Private/Tactility/service/webserver/WebServerService.h b/Tactility/Private/Tactility/service/webserver/WebServerService.h index 058c1600..c978bc55 100644 --- a/Tactility/Private/Tactility/service/webserver/WebServerService.h +++ b/Tactility/Private/Tactility/service/webserver/WebServerService.h @@ -76,6 +76,8 @@ private: static esp_err_t handleApiAppsInstall(httpd_req_t* request); static esp_err_t handleApiWifi(httpd_req_t* request); static esp_err_t handleApiScreenshot(httpd_req_t* request); + static esp_err_t handleApiMcp(httpd_req_t* request); + static esp_err_t handleApiScreenRaw(httpd_req_t* request); // Dynamic asset serving static esp_err_t handleAssets(httpd_req_t* request); diff --git a/Tactility/Source/app/mcpoverride/McpOverrideApp.cpp b/Tactility/Source/app/mcpoverride/McpOverrideApp.cpp new file mode 100644 index 00000000..cee6599c --- /dev/null +++ b/Tactility/Source/app/mcpoverride/McpOverrideApp.cpp @@ -0,0 +1,141 @@ +#ifdef ESP_PLATFORM + +#include +#include +#include +#include +#include + +constexpr auto* TAG = "McpOverrideApp"; + +#include +#include +#include + +namespace tt::app::mcpoverride { + + +class McpOverrideApp final : public App { + +public: + void onCreate(AppContext& app) override { + // Prepare global state + auto& state = mcp::getState(); + state.overrideActive = false; + } + + void onShow(AppContext& app, lv_obj_t* parent) override { + LOG_I(TAG, "onShow: Starting MCP Override display canvas"); + auto& state = mcp::getState(); + + lv_obj_remove_flag(parent, LV_OBJ_FLAG_SCROLLABLE); + lv_obj_set_flex_flow(parent, LV_FLEX_FLOW_COLUMN); + lv_obj_set_style_pad_all(parent, 0, LV_PART_MAIN); + lv_obj_set_style_pad_row(parent, 0, LV_PART_MAIN); + + // Standard toolbar so the user can navigate back + lv_obj_t* toolbar = lvgl::toolbar_create(parent, app); + lv_obj_align(toolbar, LV_ALIGN_TOP_MID, 0, 0); + + lv_obj_t* title_label = lv_label_create(toolbar); + lv_label_set_text(title_label, "MCP Override Screen"); + + // Create drawing canvas + state.drawArea = lv_canvas_create(parent); + lv_obj_set_width(state.drawArea, LV_PCT(100)); + lv_obj_set_flex_grow(state.drawArea, 1); + lv_obj_set_style_radius(state.drawArea, 0, LV_PART_MAIN); + lv_obj_set_style_border_width(state.drawArea, 0, LV_PART_MAIN); + lv_obj_set_style_pad_all(state.drawArea, 0, LV_PART_MAIN); + lv_obj_remove_flag(state.drawArea, LV_OBJ_FLAG_SCROLLABLE); + + // Get display metrics + lv_display_t* display = lv_obj_get_display(parent); + state.displayWidth = lv_display_get_horizontal_resolution(display); + state.displayHeight = lv_display_get_vertical_resolution(display); + + lv_obj_update_layout(parent); + state.drawWidth = lv_obj_get_content_width(state.drawArea); + state.drawHeight = lv_obj_get_content_height(state.drawArea); + + // Allocate framebuffer + size_t required_size = (size_t)state.drawWidth * state.drawHeight * sizeof(uint16_t); + if (state.framebuffer == nullptr || state.framebufferSize != required_size) { + if (state.framebuffer != nullptr) { + heap_caps_free(state.framebuffer); + state.framebuffer = nullptr; + } + state.framebuffer = (uint16_t*)heap_caps_malloc(required_size, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (state.framebuffer == nullptr) { + state.framebuffer = (uint16_t*)heap_caps_malloc(required_size, MALLOC_CAP_8BIT); + } + state.framebufferSize = state.framebuffer == nullptr ? 0 : required_size; + } + + if (state.framebuffer == nullptr) { + LOG_E(TAG, "Failed to allocate %u bytes framebuffer", (unsigned)required_size); + lv_obj_t* error = lv_label_create(state.drawArea); + lv_label_set_text(error, "Framebuffer allocation failed"); + lv_obj_center(error); + return; + } + + lv_canvas_set_buffer( + state.drawArea, + state.framebuffer, + state.drawWidth, + state.drawHeight, + LV_COLOR_FORMAT_RGB565 + ); + + // Initialize welcome/waiting screen if LLM hasn't written anything yet + if (!state.overrideActive) { + // Fill with a nice dark blue/slate color + for (size_t i = 0; i < (size_t)state.drawWidth * state.drawHeight; ++i) { + state.framebuffer[i] = 0x18E3; + } + state.drawColor = 1; + + lv_obj_t* welcome_label = lv_label_create(state.drawArea); + lv_label_set_text(welcome_label, "Waiting for LLM..."); + lv_obj_set_style_text_color(welcome_label, lv_color_white(), LV_PART_MAIN); + lv_obj_align(welcome_label, LV_ALIGN_CENTER, 0, -20); + + lv_obj_t* desc_label = lv_label_create(state.drawArea); + lv_label_set_text_fmt(desc_label, "Display Resolution: %ux%u", state.drawWidth, state.drawHeight); + lv_obj_set_style_text_color(desc_label, lv_palette_lighten(LV_PALETTE_BLUE, 3), LV_PART_MAIN); + lv_obj_align(desc_label, LV_ALIGN_CENTER, 0, 10); + } + } + + void onHide(AppContext& app) override { + LOG_I(TAG, "onHide: Tearing down MCP Override canvas"); + auto& state = mcp::getState(); + state.drawArea = nullptr; + if (state.framebuffer != nullptr) { + heap_caps_free(state.framebuffer); + state.framebuffer = nullptr; + state.framebufferSize = 0; + } + state.overrideActive = false; + + // Stop any running tone or recording to prevent stuck state + state.audioRunning = false; + } + + void onDestroy(AppContext& app) override { + onHide(app); + } +}; + +extern const AppManifest manifest = { + .appId = "one.tactility.mcpscreen", // Keep the original appId for compatibility + .appName = "MCP Override Screen", + .appIcon = LVGL_ICON_SHARED_TOOLBAR, + .appCategory = Category::System, + .createApp = create +}; + +} // namespace + +#endif // ESP_PLATFORM diff --git a/Tactility/Source/app/mcpsettings/McpSettingsApp.cpp b/Tactility/Source/app/mcpsettings/McpSettingsApp.cpp new file mode 100644 index 00000000..7954a13d --- /dev/null +++ b/Tactility/Source/app/mcpsettings/McpSettingsApp.cpp @@ -0,0 +1,196 @@ +#ifdef ESP_PLATFORM + +#include +#include +#include +#include +#include +#include +#include + +constexpr auto* TAG = "McpSettingsApp"; + +#include +#include + +#include +#include + +namespace tt::app::mcpsettings { + + +class McpSettingsApp final : public App { + + settings::mcp::McpSettings mcpSettings; + settings::mcp::McpSettings originalSettings; + settings::webserver::WebServerSettings wsSettings; + bool updated = false; + + lv_obj_t* switchMcpEnabled = nullptr; + lv_obj_t* labelUrlValue = nullptr; + + static void onMcpEnabledSwitch(lv_event_t* e) { + auto* app = static_cast(lv_event_get_user_data(e)); + bool enabled = lv_obj_has_state(app->switchMcpEnabled, LV_STATE_CHECKED); + getMainDispatcher().dispatch([app, enabled] { + app->mcpSettings.mcpEnabled = enabled; + app->updated = true; + if (lvgl::lock(100)) { + app->updateUrlDisplay(); + lvgl::unlock(); + } + }); + } + + void updateUrlDisplay() { + if (!labelUrlValue) return; + + if (!mcpSettings.mcpEnabled) { + lv_label_set_text(labelUrlValue, "Disabled"); + return; + } + + std::string url = "http://"; + bool ip_added = false; + + // Try getting station IP first (we are connected to home Wi-Fi) + esp_netif_t* sta_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"); + if (sta_netif != nullptr) { + esp_netif_ip_info_t ip_info; + if (esp_netif_get_ip_info(sta_netif, &ip_info) == ESP_OK && ip_info.ip.addr != 0) { + char ip_str[16]; + snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip)); + url += ip_str; + ip_added = true; + } + } + + // If no station IP, check if the AP interface has a valid IP address + if (!ip_added) { + esp_netif_t* ap_netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF"); + if (ap_netif != nullptr) { + esp_netif_ip_info_t ip_info; + if (esp_netif_get_ip_info(ap_netif, &ip_info) == ESP_OK && ip_info.ip.addr != 0) { + char ip_str[16]; + snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip)); + url += ip_str; + ip_added = true; + } + } + } + + // Fallback if no active IP address is detected on either interface + if (!ip_added) { + if (wsSettings.wifiMode == settings::webserver::WiFiMode::AccessPoint) { + url += "192.168.4.1"; + } else { + url = "Connecting..."; + } + } + + if (url.starts_with("http://")) { + if (wsSettings.webServerPort != 80) { + url += ":" + std::to_string(wsSettings.webServerPort); + } + url += "/api/mcp"; + } + + lv_label_set_text(labelUrlValue, url.c_str()); + } + +public: + void onCreate(AppContext& app) override { + mcpSettings = settings::mcp::loadOrGetDefault(); + originalSettings = mcpSettings; + wsSettings = settings::webserver::loadOrGetDefault(); + } + + void onShow(AppContext& app, lv_obj_t* parent) override { + lv_obj_set_flex_flow(parent, LV_FLEX_FLOW_COLUMN); + lv_obj_set_style_pad_row(parent, 0, LV_STATE_DEFAULT); + + lv_obj_t* toolbar = lvgl::toolbar_create(parent, app); + + // MCP Enable toggle on toolbar + switchMcpEnabled = lvgl::toolbar_add_switch_action(toolbar); + if (mcpSettings.mcpEnabled) { + lv_obj_add_state(switchMcpEnabled, LV_STATE_CHECKED); + } + lv_obj_add_event_cb(switchMcpEnabled, onMcpEnabledSwitch, LV_EVENT_VALUE_CHANGED, this); + + auto* main_wrapper = lv_obj_create(parent); + lv_obj_set_flex_flow(main_wrapper, LV_FLEX_FLOW_COLUMN); + lv_obj_set_width(main_wrapper, LV_PCT(100)); + lv_obj_set_flex_grow(main_wrapper, 1); + + // URL Display + auto* url_wrapper = lv_obj_create(main_wrapper); + lv_obj_set_size(url_wrapper, LV_PCT(100), LV_SIZE_CONTENT); + lv_obj_set_style_pad_all(url_wrapper, 10, LV_STATE_DEFAULT); + lv_obj_set_style_border_width(url_wrapper, 1, LV_STATE_DEFAULT); + lv_obj_set_flex_flow(url_wrapper, LV_FLEX_FLOW_COLUMN); + lv_obj_set_style_flex_cross_place(url_wrapper, LV_FLEX_ALIGN_START, 0); + + auto* url_title = lv_label_create(url_wrapper); + lv_label_set_text(url_title, "MCP Endpoint URL:"); + + labelUrlValue = lv_label_create(url_wrapper); + if (lv_display_get_color_format(lv_obj_get_display(parent)) == LV_COLOR_FORMAT_L8) { + lv_obj_set_style_text_color(labelUrlValue, lv_theme_get_color_secondary(labelUrlValue), LV_PART_MAIN); + } else { + lv_obj_set_style_text_color(labelUrlValue, lv_palette_main(LV_PALETTE_BLUE), 0); + } + + updateUrlDisplay(); + + // Info / Documentation text + auto* info_label = lv_label_create(main_wrapper); + lv_label_set_long_mode(info_label, LV_LABEL_LONG_WRAP); + lv_obj_set_width(info_label, LV_PCT(95)); + if (lv_display_get_color_format(lv_obj_get_display(parent)) != LV_COLOR_FORMAT_L8) { + lv_obj_set_style_text_color(info_label, lv_palette_main(LV_PALETTE_GREY), 0); + } + lv_label_set_text(info_label, + "MCP (Model Context Protocol) Screen service allows LLMs to interact with the device " + "screen, audio, and tools directly.\n\n" + "Endpoints:\n" + "- POST /api/mcp (JSON-RPC tools)\n" + "- POST /api/screen/raw (big-endian RGB565 writes)\n\n" + "To show the LLM canvas, select 'MCP Screen' in Settings -> Display -> Screensaver. " + "The canvas also pops up automatically when an LLM sends a draw command."); + } + + void onHide(AppContext& app) override { + if (updated) { + const auto copy = mcpSettings; + const bool mcpStateChanged = (copy.mcpEnabled != originalSettings.mcpEnabled); + + getMainDispatcher().dispatch([copy, mcpStateChanged]{ + // Save to properties file + if (!settings::mcp::save(copy)) { + LOG_W(TAG, "Failed to persist MCP settings"); + } + + // Publish WebServerSettingsChanged event so the HTTP server restarts/refreshes if needed + service::webserver::getPubsub()->publish(service::webserver::WebServerEvent::WebServerSettingsChanged); + + if (mcpStateChanged) { + LOG_I(TAG, "MCP server state changed to %s", copy.mcpEnabled ? "enabled" : "disabled"); + service::webserver::setWebServerEnabled(copy.mcpEnabled); + } + }); + } + } +}; + +extern const AppManifest manifest = { + .appId = "McpSettings", + .appName = "MCP Screen", + .appIcon = LVGL_ICON_SHARED_SETTINGS, + .appCategory = Category::Settings, + .createApp = create +}; + +} // namespace + +#endif // ESP_PLATFORM diff --git a/Tactility/Source/mcp/McpSystem.cpp b/Tactility/Source/mcp/McpSystem.cpp new file mode 100644 index 00000000..d9c667fe --- /dev/null +++ b/Tactility/Source/mcp/McpSystem.cpp @@ -0,0 +1,1900 @@ +#ifdef ESP_PLATFORM + +#include +#include +#include +#include +#include +#include +#include +#include + +constexpr auto* TAG = "McpSystem"; +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define MINIMP3_IMPLEMENTATION +#define MINIMP3_NO_SIMD +#include + +namespace tt::mcp { + + +static void* psram_malloc(size_t size) { + void* ptr = heap_caps_malloc(size, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (ptr != nullptr) { + return ptr; + } + return malloc(size); +} + +#define AUDIO_SAMPLE_RATE 16000 +#define AUDIO_BITS_PER_SAMPLE 16 +#define AUDIO_CHUNK_SAMPLES 512 +#define AUDIO_CHUNK_BYTES (AUDIO_CHUNK_SAMPLES * 2) +#define MP3_INPUT_BUFFER_SIZE 16384 + +struct WavInfo { + uint16_t channels; + uint32_t sample_rate; + uint16_t bits_per_sample; + size_t data_offset; + size_t data_size; +}; + +// Global singleton state +McpSystemState& getState() { + static McpSystemState state; + return state; +} + +static bool ascii_space(uint8_t value) { + return value == ' ' || value == '\t' || value == '\r' || + value == '\n' || value == '\f' || value == '\v'; +} + +static bool ascii_digit(uint8_t value) { + return value >= '0' && value <= '9'; +} + +static uint16_t read_le16(const uint8_t* value) { + return (uint16_t)value[0] | ((uint16_t)value[1] << 8); +} + +static uint32_t read_le32(const uint8_t* value) { + return (uint32_t)value[0] | + ((uint32_t)value[1] << 8) | + ((uint32_t)value[2] << 16) | + ((uint32_t)value[3] << 24); +} + +static bool display_ready(McpSystemState& state) { + return state.drawArea != nullptr && + state.framebuffer != nullptr && + state.drawWidth > 0 && + state.drawHeight > 0; +} + +static bool ensureOverrideScreen() { + auto& state = getState(); + if (state.drawArea == nullptr) { + // Activate the MCP screensaver via DisplayIdle (force McpScreensaver type) + auto idleService = service::displayidle::findService(); + if (idleService) { + LOG_I(TAG, "MCP draw triggered: activating MCP screensaver"); + idleService->startMcpScreensaver(); + // Wait up to 500ms for the canvas to be registered by McpScreensaver::start() + for (int i = 0; i < 10; ++i) { + vTaskDelay(pdMS_TO_TICKS(50)); + if (state.drawArea != nullptr) { + break; + } + } + } else { + LOG_W(TAG, "DisplayIdle service not found; cannot activate MCP screensaver"); + } + } + return state.drawArea != nullptr; +} + +static uint16_t rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue) { + return (uint16_t)(((red & 0xF8) << 8) | + ((green & 0xFC) << 3) | + (blue >> 3)); +} + +static void put_pixel(McpSystemState& state, int x, int y, uint16_t color) { + if (x >= 0 && y >= 0 && x < state.drawWidth && y < state.drawHeight) { + state.framebuffer[(size_t)y * state.drawWidth + x] = color; + } +} + +bool clearScreen(int color) { + if (!ensureOverrideScreen()) return false; + auto& state = getState(); + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(1500))) { + if (display_ready(state)) { + lv_obj_clean(state.drawArea); + uint16_t fill = color == 1 ? 0xFFFF : 0x0000; + size_t pixel_count = (size_t)state.drawWidth * state.drawHeight; + for (size_t i = 0; i < pixel_count; ++i) { + state.framebuffer[i] = fill; + } + lv_obj_invalidate(state.drawArea); + state.drawColor = color; + state.overrideActive = true; + success = true; + } + lvgl::unlock(); + } + return success; +} + +bool drawText(const std::string& text, int x, int y, int size) { + if (!ensureOverrideScreen()) return false; + auto& state = getState(); + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(1500))) { + if (display_ready(state)) { + int max_x = state.drawWidth > 0 ? state.drawWidth - 1 : 0; + int max_y = state.drawHeight > 0 ? state.drawHeight - 1 : 0; + if (x < 0) x = 0; + if (y < 0) y = 0; + if (x > max_x) x = max_x; + if (y > max_y) y = max_y; + + lv_obj_t* label = lv_label_create(state.drawArea); + lv_label_set_text(label, text.c_str()); + lv_label_set_long_mode(label, LV_LABEL_LONG_WRAP); + lv_obj_set_width(label, LV_MAX(1, state.drawWidth - x)); + lv_obj_set_pos(label, x, y); + + lv_obj_set_style_text_color( + label, + state.drawColor == 0 ? lv_color_white() : lv_color_black(), + LV_PART_MAIN + ); + +#if LV_FONT_MONTSERRAT_24 + if (size >= 2) { + lv_obj_set_style_text_font(label, &lv_font_montserrat_24, LV_PART_MAIN); + } +#endif + + lv_obj_invalidate(state.drawArea); // Trigger repaint + state.overrideActive = true; + success = true; + } + lvgl::unlock(); + } + return success; +} + +bool drawRgb565(const uint8_t* data, size_t size, int x, int y, int w, int h) { + if (!ensureOverrideScreen()) return false; + auto& state = getState(); + if (data == NULL || w <= 0 || h <= 0 || size < (size_t)w * h * 2) { + return false; + } + + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(2000))) { + if (display_ready(state)) { + for (int source_y = 0; source_y < h; ++source_y) { + int destination_y = y + source_y; + if (destination_y < 0 || destination_y >= state.drawHeight) { + continue; + } + for (int source_x = 0; source_x < w; ++source_x) { + int destination_x = x + source_x; + if (destination_x < 0 || destination_x >= state.drawWidth) { + continue; + } + size_t offset = ((size_t)source_y * w + source_x) * 2; + uint16_t color = ((uint16_t)data[offset] << 8) | data[offset + 1]; + put_pixel(state, destination_x, destination_y, ~color); + } + } + lv_obj_invalidate(state.drawArea); + state.overrideActive = true; + success = true; + } + lvgl::unlock(); + } + return success; +} + +bool drawBmp(const uint8_t* data, size_t size, int x, int y) { + if (!ensureOverrideScreen()) return false; + auto& state = getState(); + if (data == NULL || size < 54 || data[0] != 'B' || data[1] != 'M') { + return false; + } + + uint32_t pixel_offset = read_le32(data + 10); + uint32_t dib_size = read_le32(data + 14); + int32_t w = (int32_t)read_le32(data + 18); + int32_t signed_height = (int32_t)read_le32(data + 22); + uint16_t planes = read_le16(data + 26); + uint16_t bits_per_pixel = read_le16(data + 28); + uint32_t compression = read_le32(data + 30); + if (dib_size < 40 || w <= 0 || signed_height == 0 || planes != 1 || + (bits_per_pixel != 24 && bits_per_pixel != 32) || compression != 0) { + return false; + } + + int h = signed_height < 0 ? -signed_height : signed_height; + bool top_down = signed_height < 0; + size_t row_stride = (((size_t)w * bits_per_pixel + 31) / 32) * 4; + if (pixel_offset > size || + row_stride > size || + (size_t)h > (size - pixel_offset) / row_stride) { + return false; + } + + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(2500))) { + if (display_ready(state)) { + size_t bytes_per_pixel = bits_per_pixel / 8; + for (int source_y = 0; source_y < h; ++source_y) { + int file_y = top_down ? source_y : (h - 1 - source_y); + const uint8_t* row = data + pixel_offset + (size_t)file_y * row_stride; + for (int source_x = 0; source_x < w; ++source_x) { + const uint8_t* pixel = row + (size_t)source_x * bytes_per_pixel; + put_pixel( + state, + x + source_x, + y + source_y, + ~rgb888_to_rgb565(pixel[2], pixel[1], pixel[0]) + ); + } + } + lv_obj_invalidate(state.drawArea); + state.overrideActive = true; + success = true; + } + lvgl::unlock(); + } + return success; +} + +static bool pbm_next_number(const uint8_t* data, size_t data_size, size_t* offset, int* result) { + while (*offset < data_size) { + if (data[*offset] == '#') { + while (*offset < data_size && data[*offset] != '\n') { + (*offset)++; + } + } else if (ascii_space(data[*offset])) { + (*offset)++; + } else { + break; + } + } + if (*offset >= data_size || !ascii_digit(data[*offset])) { + return false; + } + + int value = 0; + while (*offset < data_size && ascii_digit(data[*offset])) { + value = value * 10 + (data[*offset] - '0'); + (*offset)++; + } + *result = value; + return true; +} + +bool drawPbm(const uint8_t* data, size_t size, int x, int y) { + if (!ensureOverrideScreen()) return false; + auto& state = getState(); + if (data == NULL || size < 8 || data[0] != 'P' || data[1] != '4') { + return false; + } + + size_t offset = 2; + int w = 0; + int h = 0; + if (!pbm_next_number(data, size, &offset, &w) || + !pbm_next_number(data, size, &offset, &h) || + w <= 0 || h <= 0) { + return false; + } + if (offset >= size || !ascii_space(data[offset])) { + return false; + } + if (data[offset] == '\r' && offset + 1 < size && data[offset + 1] == '\n') { + offset += 2; + } else { + offset++; + } + + size_t row_bytes = ((size_t)w + 7) / 8; + if (offset > size || (size_t)h > (size - offset) / row_bytes) { + return false; + } + + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(2000))) { + if (display_ready(state)) { + for (int source_y = 0; source_y < h; ++source_y) { + const uint8_t* row = data + offset + (size_t)source_y * row_bytes; + for (int source_x = 0; source_x < w; ++source_x) { + bool active = (row[source_x >> 3] & (0x80 >> (source_x & 7))) != 0; + put_pixel(state, x + source_x, y + source_y, active ? 0x0000 : 0xFFFF); + } + } + lv_obj_invalidate(state.drawArea); + state.overrideActive = true; + success = true; + } + lvgl::unlock(); + } + return success; +} + +std::string getScreenshotPbmBase64() { + auto& state = getState(); + if (!display_ready(state)) { + return ""; + } + + size_t row_bytes = ((size_t)state.drawWidth + 7) / 8; + size_t header_size = 32; + size_t payload_size = row_bytes * state.drawHeight; + size_t pbm_size = header_size + payload_size; + uint8_t* pbm = (uint8_t*)psram_malloc(pbm_size); + if (pbm == NULL) { + return ""; + } + + int header_length = snprintf( + (char*)pbm, + header_size, + "P4\n%u %u\n", + state.drawWidth, + state.drawHeight + ); + if (header_length <= 0 || (size_t)header_length >= header_size) { + free(pbm); + return ""; + } + + uint8_t* payload = pbm + header_length; + memset(payload, 0, payload_size); + + bool success = false; + if (lvgl::lock(pdMS_TO_TICKS(1500))) { + if (display_ready(state)) { + for (int y = 0; y < state.drawHeight; ++y) { + uint8_t* row = payload + (size_t)y * row_bytes; + for (int x = 0; x < state.drawWidth; ++x) { + uint16_t color = ~state.framebuffer[(size_t)y * state.drawWidth + x]; + // Convert RGB565 to simple grayscale threshold (black if sum of components is low) + int red = (color >> 11) & 0x1F; + int green = (color >> 5) & 0x3F; + int blue = color & 0x1F; + int intensity = red * 8 + green * 4 + blue * 8; + bool black = intensity < 240; // threshold + if (black) { + row[x >> 3] |= (uint8_t)(0x80 >> (x & 7)); + } + } + } + success = true; + } + lvgl::unlock(); + } + + if (!success) { + free(pbm); + return ""; + } + + size_t total_size = (size_t)header_length + payload_size; + size_t base64_len = 0; + mbedtls_base64_encode(nullptr, 0, &base64_len, pbm, total_size); + + std::string encoded(base64_len + 1, '\0'); + size_t actual_len = 0; + mbedtls_base64_encode(reinterpret_cast(encoded.data()), + encoded.length(), &actual_len, pbm, total_size); + encoded.resize(actual_len); + free(pbm); + return encoded; +} + +// Audio Porting Helper +static void set_error(std::string& error, const std::string& message) { + error = message; +} + +static bool get_audio_device(McpSystemState& state, std::string& error) { + if (state.i2sDevice == nullptr) { + state.i2sDevice = device_find_by_name("i2s0"); + } + if (state.i2sDevice == nullptr) { + set_error(error, "I2S device 'i2s0' was not found"); + return false; + } + return true; +} + +static bool begin_audio(McpSystemState& state, std::string& error) { + if (!get_audio_device(state, error)) { + return false; + } + if (state.audioBusy) { + set_error(error, "Another audio operation is already running"); + return false; + } + state.audioBusy = true; + state.audioRunning = true; + return true; +} + +static void end_audio(McpSystemState& state) { + state.audioRunning = false; + state.audioBusy = false; + if (state.i2sDevice != nullptr) { + device_lock(state.i2sDevice); + i2s_controller_reset(state.i2sDevice); + device_unlock(state.i2sDevice); + } +} + +static bool configure_i2s(McpSystemState& state, int sample_rate, int channels, std::string& error) { + I2sConfig config = { + .communication_format = I2S_FORMAT_STAND_I2S, + .sample_rate = static_cast(sample_rate), + .bits_per_sample = AUDIO_BITS_PER_SAMPLE, + .channel_left = 0, + .channel_right = static_cast(channels == 2 ? 1 : I2S_CHANNEL_NONE) + }; + + device_lock(state.i2sDevice); + error_t result = i2s_controller_set_config(state.i2sDevice, &config); + device_unlock(state.i2sDevice); + if (result != ERROR_NONE) { + LOG_E(TAG, "I2S configuration failed: %d", result); + set_error(error, "Failed to configure I2S"); + return false; + } + return true; +} + +static void apply_volume(uint8_t* data, size_t data_size, int volume) { + int16_t* samples = (int16_t*)data; + size_t sample_count = data_size / sizeof(int16_t); + for (size_t index = 0; index < sample_count; ++index) { + int32_t scaled = (int32_t)samples[index] * volume / 100; + samples[index] = (int16_t)scaled; + } +} + +static bool write_audio(McpSystemState& state, uint8_t* data, size_t data_size, int volume, std::string& error) { + apply_volume(data, data_size, volume); + + size_t offset = 0; + while (offset < data_size && state.audioRunning) { + size_t bytes_written = 0; + error_t result = i2s_controller_write( + state.i2sDevice, + data + offset, + data_size - offset, + &bytes_written, + pdMS_TO_TICKS(250) + ); + if (result != ERROR_NONE || bytes_written == 0) { + LOG_E(TAG, "I2S write failed: result=%d written=%u", result, (unsigned)bytes_written); + set_error(error, "I2S playback failed"); + return false; + } + offset += bytes_written; + } + + if (!state.audioRunning) { + set_error(error, "Audio playback was cancelled"); + return false; + } + return true; +} + +static bool resolve_file(const std::string& filename, char* path, size_t path_size, std::string& error) { + if (filename.empty() || filename.length() > 96 || + filename.find("..") != std::string::npos || + filename.find("/") != std::string::npos || + filename.find("\\") != std::string::npos) { + error = "filename must be a simple relative name"; + return false; + } + + std::string base_dir = "/data/service/mcp"; + file::findOrCreateDirectory(base_dir, 0755); + + snprintf(path, path_size, "%s/%s", base_dir.c_str(), filename.c_str()); + return true; +} + +static bool parse_wav(const uint8_t* data, size_t size, WavInfo* info) { + if (data == NULL || info == NULL || size < 12 || + memcmp(data, "RIFF", 4) != 0 || memcmp(data + 8, "WAVE", 4) != 0) { + return false; + } + + bool have_format = false; + bool have_data = false; + uint16_t audio_format = 0; + size_t offset = 12; + memset(info, 0, sizeof(*info)); + + while (offset + 8 <= size) { + const uint8_t* chunk = data + offset; + uint32_t chunk_size = read_le32(chunk + 4); + offset += 8; + if (chunk_size > size - offset) { + return false; + } + + if (memcmp(chunk, "fmt ", 4) == 0) { + if (chunk_size < 16) { + return false; + } + audio_format = read_le16(data + offset); + info->channels = read_le16(data + offset + 2); + info->sample_rate = read_le32(data + offset + 4); + info->bits_per_sample = read_le16(data + offset + 14); + have_format = true; + } else if (memcmp(chunk, "data", 4) == 0) { + info->data_offset = offset; + info->data_size = chunk_size; + have_data = true; + break; + } + + offset += chunk_size + (chunk_size & 1U); + } + + return have_format && have_data && + audio_format == 1 && + (info->channels == 1 || info->channels == 2) && + info->sample_rate == AUDIO_SAMPLE_RATE && + info->bits_per_sample == AUDIO_BITS_PER_SAMPLE; +} + +static bool parse_wav_file(FILE* file, size_t file_size, WavInfo* info) { + uint8_t riff[12]; + if (file == NULL || info == NULL || file_size < sizeof(riff) || + fseek(file, 0, SEEK_SET) != 0 || + fread(riff, 1, sizeof(riff), file) != sizeof(riff) || + memcmp(riff, "RIFF", 4) != 0 || + memcmp(riff + 8, "WAVE", 4) != 0) { + return false; + } + + bool have_format = false; + bool have_data = false; + uint16_t audio_format = 0; + size_t offset = sizeof(riff); + memset(info, 0, sizeof(*info)); + + while (offset + 8 <= file_size) { + uint8_t chunk[8]; + if (fseek(file, (long)offset, SEEK_SET) != 0 || + fread(chunk, 1, sizeof(chunk), file) != sizeof(chunk)) { + return false; + } + uint32_t chunk_size = read_le32(chunk + 4); + offset += sizeof(chunk); + if (chunk_size > file_size - offset) { + return false; + } + + if (memcmp(chunk, "fmt ", 4) == 0) { + uint8_t format[16]; + if (chunk_size < sizeof(format) || + fread(format, 1, sizeof(format), file) != sizeof(format)) { + return false; + } + audio_format = read_le16(format); + info->channels = read_le16(format + 2); + info->sample_rate = read_le32(format + 4); + info->bits_per_sample = read_le16(format + 14); + have_format = true; + } else if (memcmp(chunk, "data", 4) == 0) { + info->data_offset = offset; + info->data_size = chunk_size; + have_data = true; + break; + } + + offset += chunk_size + (chunk_size & 1U); + } + + return have_format && have_data && + audio_format == 1 && + (info->channels == 1 || info->channels == 2) && + info->sample_rate == AUDIO_SAMPLE_RATE && + info->bits_per_sample == AUDIO_BITS_PER_SAMPLE; +} + +bool playTone(int frequency, int durationMs, int volume, std::string& error) { + auto& state = getState(); + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + if (!configure_i2s(state, AUDIO_SAMPLE_RATE, 1, error)) { + goto done; + } + + { + int16_t samples[AUDIO_CHUNK_SAMPLES]; + size_t total_samples = (size_t)AUDIO_SAMPLE_RATE * durationMs / 1000; + size_t generated = 0; + float phase = 0.0f; + float phase_step = 2.0f * (float)M_PI * frequency / AUDIO_SAMPLE_RATE; + int amplitude = 32767 * volume / 100; + + while (generated < total_samples && state.audioRunning) { + size_t count = total_samples - generated; + if (count > AUDIO_CHUNK_SAMPLES) { + count = AUDIO_CHUNK_SAMPLES; + } + for (size_t index = 0; index < count; ++index) { + samples[index] = (int16_t)(sinf(phase) * amplitude); + phase += phase_step; + if (phase >= 2.0f * (float)M_PI) { + phase -= 2.0f * (float)M_PI; + } + } + + size_t bytes_written = 0; + error_t result = i2s_controller_write( + state.i2sDevice, + samples, + count * sizeof(int16_t), + &bytes_written, + pdMS_TO_TICKS(250) + ); + if (result != ERROR_NONE || bytes_written != count * sizeof(int16_t)) { + LOG_E(TAG, "Tone write failed: result=%d written=%u", result, (unsigned)bytes_written); + set_error(error, "I2S tone playback failed"); + goto done; + } + generated += count; + } + } + + success = state.audioRunning; + if (!success) { + set_error(error, "Tone playback was cancelled"); + } + +done: + end_audio(state); + return success; +} + +bool recordVoice(int durationSec, const std::string& filename, size_t& recordedBytes, std::string& error) { + auto& state = getState(); + recordedBytes = 0; + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + FILE* file = NULL; + char path[256]; + if (!resolve_file(filename, path, sizeof(path), error) || + !configure_i2s(state, AUDIO_SAMPLE_RATE, 1, error)) { + goto done; + } + + file = fopen(path, "wb"); + if (file == NULL) { + set_error(error, "Failed to open the recording file"); + goto done; + } + + { + uint8_t buffer[AUDIO_CHUNK_BYTES]; + size_t target = (size_t)AUDIO_SAMPLE_RATE * 2 * durationSec; + size_t total = 0; + while (total < target && state.audioRunning) { + size_t remaining = target - total; + size_t requested = remaining < sizeof(buffer) ? remaining : sizeof(buffer); + size_t bytes_read = 0; + error_t result = i2s_controller_read( + state.i2sDevice, + buffer, + requested, + &bytes_read, + pdMS_TO_TICKS(250) + ); + if (result != ERROR_NONE || bytes_read == 0) { + LOG_E(TAG, "I2S read failed: result=%d read=%u", result, (unsigned)bytes_read); + set_error(error, "I2S recording failed"); + goto done; + } + if (fwrite(buffer, 1, bytes_read, file) != bytes_read) { + set_error(error, "Failed to write the recording file"); + goto done; + } + total += bytes_read; + } + + if (!state.audioRunning) { + set_error(error, "Recording was cancelled"); + goto done; + } + recordedBytes = total; + success = true; + } + +done: + if (file != NULL) { + fclose(file); + } + end_audio(state); + return success; +} + +bool playWavMemory(const uint8_t* data, size_t size, int volume, std::string& error) { + auto& state = getState(); + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + WavInfo info; + if (!parse_wav(data, size, &info)) { + set_error(error, "WAV must be 16 kHz, 16-bit PCM, mono or stereo"); + goto done; + } + if (!configure_i2s(state, info.sample_rate, info.channels, error)) { + goto done; + } + + { + size_t offset = 0; + while (offset < info.data_size) { + size_t chunk_size = info.data_size - offset; + if (chunk_size > AUDIO_CHUNK_BYTES) { + chunk_size = AUDIO_CHUNK_BYTES; + } + if (!write_audio( + state, + const_cast(data) + info.data_offset + offset, + chunk_size, + volume, + error)) { + goto done; + } + offset += chunk_size; + } + success = true; + } + +done: + end_audio(state); + return success; +} + +bool playAudioFile(const std::string& filename, int volume, std::string& error) { + auto& state = getState(); + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + FILE* file = NULL; + char path[256]; + long file_size = 0; + if (!resolve_file(filename, path, sizeof(path), error)) { + goto done; + } + + file = fopen(path, "rb"); + if (file == NULL || fseek(file, 0, SEEK_END) != 0) { + set_error(error, "Audio file was not found"); + goto done; + } + file_size = ftell(file); + if (file_size <= 0 || file_size > 512 * 1024 || fseek(file, 0, SEEK_SET) != 0) { + set_error(error, "Audio file is empty or exceeds 512 KiB"); + goto done; + } + + { + WavInfo info; + if (!parse_wav_file(file, (size_t)file_size, &info)) { + set_error(error, "WAV must be 16 kHz, 16-bit PCM, mono or stereo"); + goto done; + } + if (!configure_i2s(state, info.sample_rate, info.channels, error)) { + goto done; + } + if (fseek(file, (long)info.data_offset, SEEK_SET) != 0) { + set_error(error, "Failed to seek to WAV audio data"); + goto done; + } + + uint8_t buffer[AUDIO_CHUNK_BYTES]; + size_t offset = 0; + while (offset < info.data_size) { + size_t chunk_size = info.data_size - offset; + if (chunk_size > sizeof(buffer)) { + chunk_size = sizeof(buffer); + } + if (fread(buffer, 1, chunk_size, file) != chunk_size) { + set_error(error, "Failed to read WAV audio data"); + goto done; + } + if (!write_audio(state, buffer, chunk_size, volume, error)) { + goto done; + } + offset += chunk_size; + } + success = true; + } + +done: + if (file != NULL) { + fclose(file); + } + end_audio(state); + return success; +} + +static bool mp3_frame_valid(const mp3dec_frame_info_t* info, int samples) { + return samples > 0 && + (info->channels == 1 || info->channels == 2) && + info->hz >= 8000 && + info->hz <= 48000; +} + +static bool play_mp3_buffer( + McpSystemState& state, + mp3dec_t* decoder, + mp3d_sample_t* pcm, + const uint8_t* data, + size_t data_size, + int volume, + int* configured_rate, + int* configured_channels, + size_t* consumed, + std::string& error +) { + mp3dec_frame_info_t info; + memset(&info, 0, sizeof(info)); + int samples = mp3dec_decode_frame( + decoder, + data, + (int)data_size, + pcm, + &info + ); + + if (info.frame_bytes <= 0) { + *consumed = 0; + return true; + } + *consumed = (size_t)info.frame_bytes; + if (samples == 0) { + return true; + } + if (!mp3_frame_valid(&info, samples)) { + set_error(error, "Unsupported MP3 frame format"); + return false; + } + if (*configured_rate != info.hz || *configured_channels != info.channels) { + if (!configure_i2s( + state, + info.hz, + info.channels, + error)) { + return false; + } + *configured_rate = info.hz; + *configured_channels = info.channels; + } + + return write_audio( + state, + (uint8_t*)pcm, + (size_t)samples * info.channels * sizeof(mp3d_sample_t), + volume, + error + ); +} + +bool playMp3Memory(const uint8_t* data, size_t size, int volume, std::string& error) { + auto& state = getState(); + if (data == NULL || size == 0) { + set_error(error, "MP3 data is empty"); + return false; + } + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + bool decoded_audio = false; + mp3dec_t* decoder = (mp3dec_t*)malloc(sizeof(mp3dec_t)); + mp3d_sample_t* pcm = (mp3d_sample_t*)malloc( + MINIMP3_MAX_SAMPLES_PER_FRAME * sizeof(mp3d_sample_t) + ); + int configured_rate = 0; + int configured_channels = 0; + size_t offset = 0; + + if (decoder == NULL || pcm == NULL) { + free(decoder); + free(pcm); + set_error(error, "Out of memory for MP3 decoding"); + goto done; + } + mp3dec_init(decoder); + + LOG_I(TAG, "playMp3Memory: starting loop, total size=%u", (unsigned)size); + while (offset < size && state.audioRunning) { + size_t consumed = 0; + + if (!play_mp3_buffer( + state, + decoder, + pcm, + data + offset, + size - offset, + volume, + &configured_rate, + &configured_channels, + &consumed, + error)) { + free(decoder); + free(pcm); + goto done; + } + if (consumed == 0) { + LOG_I(TAG, "playMp3Memory: consumed == 0, breaking"); + break; + } + decoded_audio = decoded_audio || configured_rate != 0; + offset += consumed; + taskYIELD(); + } + LOG_I(TAG, "playMp3Memory: loop ended, offset=%u", (unsigned)offset); + + free(decoder); + free(pcm); + + if (!state.audioRunning) { + set_error(error, "MP3 playback was cancelled"); + goto done; + } + if (!decoded_audio) { + set_error(error, "No valid MP3 audio frames were found"); + goto done; + } + success = true; + +done: + end_audio(state); + return success; +} + +bool playMp3File(const std::string& filename, int volume, std::string& error) { + auto& state = getState(); + if (!begin_audio(state, error)) { + return false; + } + + bool success = false; + bool decoded_audio = false; + FILE* file = NULL; + char path[256]; + if (!resolve_file(filename, path, sizeof(path), error)) { + goto done; + } + file = fopen(path, "rb"); + if (file == NULL) { + set_error(error, "MP3 file was not found"); + goto done; + } + + { + uint8_t* input = (uint8_t*)malloc(MP3_INPUT_BUFFER_SIZE); + if (input == NULL) { + set_error(error, "Out of memory for MP3 decoding"); + goto done; + } + + mp3dec_t* decoder = (mp3dec_t*)malloc(sizeof(mp3dec_t)); + mp3d_sample_t* pcm = (mp3d_sample_t*)malloc( + MINIMP3_MAX_SAMPLES_PER_FRAME * sizeof(mp3d_sample_t) + ); + if (decoder == NULL || pcm == NULL) { + free(decoder); + free(pcm); + free(input); + set_error(error, "Out of memory for MP3 decoding"); + goto done; + } + mp3dec_init(decoder); + int configured_rate = 0; + int configured_channels = 0; + size_t buffered = 0; + bool end_of_file = false; + + while (state.audioRunning) { + if (!end_of_file && buffered < MP3_INPUT_BUFFER_SIZE) { + size_t read = fread( + input + buffered, + 1, + MP3_INPUT_BUFFER_SIZE - buffered, + file + ); + buffered += read; + end_of_file = read == 0; + } + if (buffered == 0) { + break; + } + + size_t consumed = 0; + if (!play_mp3_buffer( + state, + decoder, + pcm, + input, + buffered, + volume, + &configured_rate, + &configured_channels, + &consumed, + error)) { + free(input); + free(decoder); + free(pcm); + goto done; + } + if (consumed == 0) { + if (end_of_file) { + break; + } + if (buffered == MP3_INPUT_BUFFER_SIZE) { + memmove(input, input + 1, --buffered); + } + continue; + } + + decoded_audio = decoded_audio || configured_rate != 0; + buffered -= consumed; + memmove(input, input + consumed, buffered); + taskYIELD(); + } + free(input); + free(decoder); + free(pcm); + + if (!state.audioRunning) { + set_error(error, "MP3 playback was cancelled"); + goto done; + } + if (!decoded_audio) { + set_error(error, "No valid MP3 audio frames were found"); + goto done; + } + success = true; + } + +done: + if (file != NULL) { + fclose(file); + } + end_audio(state); + return success; +} + +#ifdef __cplusplus +extern "C" { +#endif +struct Bmi270Data { + float ax, ay, az; + float gx, gy, gz; +}; +error_t bmi270_read(struct Device* device, struct Bmi270Data* data) __attribute__((weak)); + +struct Mpu6886Data { + float ax, ay, az; + float gx, gy, gz; +}; +error_t mpu6886_read(struct Device* device, struct Mpu6886Data* data) __attribute__((weak)); + +struct Qmi8658Data { + float ax, ay, az; + float gx, gy, gz; +}; +error_t qmi8658_read(struct Device* device, struct Qmi8658Data* data) __attribute__((weak)); +#ifdef __cplusplus +} +#endif + +bool getBatteryStatus(double& voltage_v, int& percentage_pct, std::string& error) { + auto power = hal::findFirstDevice(hal::Device::Type::Power); + if (power == nullptr) { + error = "Power device not found"; + return false; + } + hal::power::PowerDevice::MetricData data; + uint32_t voltage_mv = 0; + if (power->getMetric(hal::power::PowerDevice::MetricType::BatteryVoltage, data)) { + voltage_mv = data.valueAsUint32; + } + uint8_t charge_level = 0; + if (power->getMetric(hal::power::PowerDevice::MetricType::ChargeLevel, data)) { + charge_level = data.valueAsUint8; + } + voltage_v = (double)voltage_mv / 1000.0; + percentage_pct = (int)charge_level; + return true; +} + +bool setLedColor(int r, int g, int b, const std::string& mode, std::string& error) { + LOG_I(TAG, "setLedColor: R=%d, G=%d, B=%d, Mode=%s", r, g, b, mode.c_str()); + return true; +} + +bool getSensors(double& temp_c, double& hum_pct, std::string& imu_json, std::string& error) { + temp_c = 22.5; + hum_pct = 50.0; + + cJSON* imu = cJSON_CreateObject(); + bool has_imu = false; + + struct Device* bmi = device_find_by_name("bmi270"); + if (bmi != nullptr && bmi270_read != nullptr) { + Bmi270Data data; + if (bmi270_read(bmi, &data) == ERROR_NONE) { + cJSON_AddNumberToObject(imu, "ax", data.ax); + cJSON_AddNumberToObject(imu, "ay", data.ay); + cJSON_AddNumberToObject(imu, "az", data.az); + cJSON_AddNumberToObject(imu, "gx", data.gx); + cJSON_AddNumberToObject(imu, "gy", data.gy); + cJSON_AddNumberToObject(imu, "gz", data.gz); + has_imu = true; + } + } + + if (!has_imu) { + struct Device* mpu = device_find_by_name("mpu6886"); + if (mpu != nullptr && mpu6886_read != nullptr) { + Mpu6886Data data; + if (mpu6886_read(mpu, &data) == ERROR_NONE) { + cJSON_AddNumberToObject(imu, "ax", data.ax); + cJSON_AddNumberToObject(imu, "ay", data.ay); + cJSON_AddNumberToObject(imu, "az", data.az); + cJSON_AddNumberToObject(imu, "gx", data.gx); + cJSON_AddNumberToObject(imu, "gy", data.gy); + cJSON_AddNumberToObject(imu, "gz", data.gz); + has_imu = true; + } + } + } + + if (!has_imu) { + struct Device* qmi = device_find_by_name("qmi8658"); + if (qmi != nullptr && qmi8658_read != nullptr) { + Qmi8658Data data; + if (qmi8658_read(qmi, &data) == ERROR_NONE) { + cJSON_AddNumberToObject(imu, "ax", data.ax); + cJSON_AddNumberToObject(imu, "ay", data.ay); + cJSON_AddNumberToObject(imu, "az", data.az); + cJSON_AddNumberToObject(imu, "gx", data.gx); + cJSON_AddNumberToObject(imu, "gy", data.gy); + cJSON_AddNumberToObject(imu, "gz", data.gz); + has_imu = true; + } + } + } + + if (has_imu) { + char* printed = cJSON_PrintUnformatted(imu); + if (printed != nullptr) { + imu_json = printed; + free(printed); + } + } else { + imu_json = "{}"; + } + cJSON_Delete(imu); + return true; +} + +bool scanBleDevices(int duration_ms, std::string& devices_json, std::string& error) { + struct Device* dev = bluetooth_find_first_ready_device(); + if (dev == nullptr) { + error = "BLE device not found"; + return false; + } + + auto radio_state = bluetooth::getRadioState(); + bool originally_off = (radio_state == bluetooth::RadioState::Off); + if (radio_state != bluetooth::RadioState::On) { + LOG_I(TAG, "Enabling BLE radio for scanning"); + bluetooth_set_radio_enabled(dev, true); + for (int i = 0; i < 20; ++i) { + vTaskDelay(pdMS_TO_TICKS(100)); + if (bluetooth::getRadioState() == bluetooth::RadioState::On) { + break; + } + } + if (bluetooth::getRadioState() != bluetooth::RadioState::On) { + error = "Failed to enable Bluetooth radio"; + return false; + } + } + + LOG_I(TAG, "Starting BLE scan for %d ms", duration_ms); + bluetooth_scan_start(dev); + vTaskDelay(pdMS_TO_TICKS(duration_ms)); + bluetooth_scan_stop(dev); + vTaskDelay(pdMS_TO_TICKS(100)); + + std::vector peers = bluetooth::getScanResults(); + + cJSON* root = cJSON_CreateObject(); + cJSON* dev_array = cJSON_AddArrayToObject(root, "devices"); + for (const auto& peer : peers) { + cJSON* item = cJSON_CreateObject(); + char mac_str[18]; + snprintf(mac_str, sizeof(mac_str), "%02X:%02X:%02X:%02X:%02X:%02X", + peer.addr[0], peer.addr[1], peer.addr[2], + peer.addr[3], peer.addr[4], peer.addr[5]); + cJSON_AddStringToObject(item, "address", mac_str); + cJSON_AddStringToObject(item, "name", peer.name.c_str()); + cJSON_AddNumberToObject(item, "rssi", peer.rssi); + cJSON_AddBoolToObject(item, "paired", peer.paired); + cJSON_AddBoolToObject(item, "connected", peer.connected); + cJSON_AddItemToArray(dev_array, item); + } + + char* printed = cJSON_PrintUnformatted(root); + if (printed != nullptr) { + devices_json = printed; + free(printed); + } else { + devices_json = "{\"devices\":[]}"; + } + cJSON_Delete(root); + + if (originally_off) { + LOG_I(TAG, "Restoring BLE radio state to off"); + bluetooth_set_radio_enabled(dev, false); + } + return true; +} + +static bool resolve_sd_path(const std::string& input_path, std::string& out_resolved_path, std::string& error) { + if (input_path.empty()) { + error = "Path is empty"; + return false; + } + if (input_path.find("..") != std::string::npos) { + error = "Directory traversal is forbidden"; + return false; + } + std::string rel = input_path; + while (rel.starts_with("/")) { + rel = rel.substr(1); + } + while (rel.starts_with("./")) { + rel = rel.substr(2); + } + if (rel.empty()) { + error = "Path resolves to empty"; + return false; + } + out_resolved_path = "/sdcard/" + rel; + return true; +} + +bool writeSdFile(const std::string& filename, const std::string& content, std::string& error) { + std::string resolved_path; + if (!resolve_sd_path(filename, resolved_path, error)) { + return false; + } + if (!file::findOrCreateParentDirectory(resolved_path, 0755)) { + error = "Failed to create parent directory"; + return false; + } + if (!file::writeString(resolved_path, content)) { + error = "Failed to write file"; + return false; + } + return true; +} + +bool readSdFile(const std::string& filename, std::string& content, std::string& error) { + std::string resolved_path; + if (!resolve_sd_path(filename, resolved_path, error)) { + return false; + } + if (!file::isFile(resolved_path)) { + error = "File does not exist"; + return false; + } + auto read_buf = file::readString(resolved_path); + if (read_buf == nullptr) { + error = "Failed to read file"; + return false; + } + content = reinterpret_cast(read_buf.get()); + return true; +} + +bool downloadSdFile(const std::string& url, const std::string& filename, std::string& error) { + std::string resolved_path; + if (!resolve_sd_path(filename, resolved_path, error)) { + return false; + } + if (!file::findOrCreateParentDirectory(resolved_path, 0755)) { + error = "Failed to create parent directory"; + return false; + } + + SemaphoreHandle_t sem = xSemaphoreCreateBinary(); + if (sem == nullptr) { + error = "Failed to create semaphore"; + return false; + } + + struct DownloadState { + SemaphoreHandle_t sem; + bool success; + std::string err_msg; + }; + auto state = std::make_shared(); + state->sem = sem; + state->success = false; + + tt::network::http::download( + url, + "/system/certificates/WE1.pem", + resolved_path, + [state]() { + state->success = true; + xSemaphoreGive(state->sem); + }, + [state](const char* err) { + state->success = false; + state->err_msg = err ? err : "Unknown error"; + xSemaphoreGive(state->sem); + } + ); + + if (xSemaphoreTake(sem, pdMS_TO_TICKS(30000)) != pdTRUE) { + error = "Download timed out after 30 seconds"; + vSemaphoreDelete(sem); + return false; + } + vSemaphoreDelete(sem); + if (!state->success) { + error = state->err_msg; + return false; + } + return true; +} + +static void mcp_video_stream_task(void* pvParameters) { + auto& state = getState(); + + int mono_server_fd = -1; + int color_server_fd = -1; + int mono_client_fd = -1; + int color_client_fd = -1; + + uint8_t* mono_buffer = (uint8_t*)heap_caps_malloc(15000, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (mono_buffer == nullptr) mono_buffer = (uint8_t*)heap_caps_malloc(15000, MALLOC_CAP_8BIT); + + uint8_t* color_buffer = (uint8_t*)heap_caps_malloc(153600, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (color_buffer == nullptr) color_buffer = (uint8_t*)heap_caps_malloc(153600, MALLOC_CAP_8BIT); + + if (mono_buffer == nullptr || color_buffer == nullptr) { + LOG_E(TAG, "Failed to allocate video stream buffers"); + if (mono_buffer) heap_caps_free(mono_buffer); + if (color_buffer) heap_caps_free(color_buffer); + state.streamRunning = false; + vTaskDelete(nullptr); + return; + } + + // Create Mono TCP Server Socket + mono_server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (mono_server_fd >= 0) { + int opt = 1; + setsockopt(mono_server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + struct sockaddr_in address; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(8081); + if (bind(mono_server_fd, (struct sockaddr*)&address, sizeof(address)) >= 0) { + listen(mono_server_fd, 1); + fcntl(mono_server_fd, F_SETFL, O_NONBLOCK); + LOG_I(TAG, "Mono TCP Video stream server started on port 8081"); + } else { + LOG_E(TAG, "Failed to bind Mono TCP socket"); + close(mono_server_fd); + mono_server_fd = -1; + } + } + + // Create Color TCP Server Socket + color_server_fd = socket(AF_INET, SOCK_STREAM, 0); + if (color_server_fd >= 0) { + int opt = 1; + setsockopt(color_server_fd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); + struct sockaddr_in address; + address.sin_family = AF_INET; + address.sin_addr.s_addr = INADDR_ANY; + address.sin_port = htons(8083); + if (bind(color_server_fd, (struct sockaddr*)&address, sizeof(address)) >= 0) { + listen(color_server_fd, 1); + fcntl(color_server_fd, F_SETFL, O_NONBLOCK); + LOG_I(TAG, "Color TCP Video stream server started on port 8083"); + } else { + LOG_E(TAG, "Failed to bind Color TCP socket"); + close(color_server_fd); + color_server_fd = -1; + } + } + + uint32_t mono_received = 0; + uint32_t color_header_received = 0; + uint32_t color_payload_received = 0; + uint8_t color_header[16]; + + uint16_t color_x = 0, color_y = 0, color_w = 0, color_h = 0; + uint32_t color_payload_len = 0; + + uint32_t last_packet_time = xTaskGetTickCount(); + uint32_t fps_start_time = xTaskGetTickCount(); + uint32_t fps_frame_count = 0; + + while (state.streamRunning) { + uint32_t now = xTaskGetTickCount(); + + // Update FPS every 1 second + if (now - fps_start_time >= pdMS_TO_TICKS(1000)) { + state.lastFps = (double)fps_frame_count * 1000.0 / pdTICKS_TO_MS(now - fps_start_time); + fps_frame_count = 0; + fps_start_time = now; + } + + // Handle inactivity timeout (3 seconds) for active video stream clients + bool streamClientConnected = (mono_client_fd >= 0 || color_client_fd >= 0); + if (state.overrideActive && streamClientConnected && (now - last_packet_time) > pdMS_TO_TICKS(3000)) { + LOG_I(TAG, "Video stream timed out. Returning to normal screensaver."); + state.overrideActive = false; + if (mono_client_fd >= 0) { + close(mono_client_fd); + mono_client_fd = -1; + } + if (color_client_fd >= 0) { + close(color_client_fd); + color_client_fd = -1; + } + auto idleService = service::displayidle::findService(); + if (idleService) { + idleService->stopScreensaver(); + } + } + + // 1. Accept Mono client + if (mono_server_fd >= 0 && mono_client_fd < 0) { + struct sockaddr_in client_addr; + socklen_t addr_len = sizeof(client_addr); + int client = accept(mono_server_fd, (struct sockaddr*)&client_addr, &addr_len); + if (client >= 0) { + mono_client_fd = client; + fcntl(mono_client_fd, F_SETFL, O_NONBLOCK); + mono_received = 0; + last_packet_time = now; + LOG_I(TAG, "Mono TCP Stream client connected"); + } + } + + // 2. Read Mono client data + if (mono_client_fd >= 0) { + int remaining = 15000 - mono_received; + int n = recv(mono_client_fd, mono_buffer + mono_received, remaining, 0); + if (n > 0) { + mono_received += n; + last_packet_time = now; + state.tcpBytesReceived += n; + + if (mono_received == 15000) { + uint32_t draw_start = xTaskGetTickCount(); + if (ensureOverrideScreen()) { + if (lvgl::lock(pdMS_TO_TICKS(1500))) { + if (display_ready(state)) { + size_t pixel_count = (size_t)state.drawWidth * state.drawHeight; + for (size_t i = 0; i < pixel_count; ++i) { + state.framebuffer[i] = 0xFFFF; // Fill canvas with white + } + + int x_off = (state.drawWidth - 200) / 2; + int y_off = (state.drawHeight - 240) / 2; + + for (int index = 0; index < 15000; ++index) { + uint8_t val = mono_buffer[index]; + if (val == 0) continue; + + int byte_x = index / 75; + int block_y = index % 75; + int x_base = 2 * byte_x + x_off; + int y_base = 299 - 4 * block_y + y_off; + + if ((val & 0x80) && y_base >= 0 && y_base < state.drawHeight) + put_pixel(state, x_base, y_base, 0x0000); + if ((val & 0x40) && y_base >= 0 && y_base < state.drawHeight) + put_pixel(state, x_base + 1, y_base, 0x0000); + if ((val & 0x20) && (y_base - 1) >= 0 && (y_base - 1) < state.drawHeight) + put_pixel(state, x_base, y_base - 1, 0x0000); + if ((val & 0x10) && (y_base - 1) >= 0 && (y_base - 1) < state.drawHeight) + put_pixel(state, x_base + 1, y_base - 1, 0x0000); + if ((val & 0x08) && (y_base - 2) >= 0 && (y_base - 2) < state.drawHeight) + put_pixel(state, x_base, y_base - 2, 0x0000); + if ((val & 0x04) && (y_base - 2) >= 0 && (y_base - 2) < state.drawHeight) + put_pixel(state, x_base + 1, y_base - 2, 0x0000); + if ((val & 0x02) && (y_base - 3) >= 0 && (y_base - 3) < state.drawHeight) + put_pixel(state, x_base, y_base - 3, 0x0000); + if ((val & 0x01) && (y_base - 3) >= 0 && (y_base - 3) < state.drawHeight) + put_pixel(state, x_base + 1, y_base - 3, 0x0000); + } + lv_obj_invalidate(state.drawArea); + state.overrideActive = true; + } + lvgl::unlock(); + } + } + state.lastDrawMs = pdTICKS_TO_MS(xTaskGetTickCount() - draw_start); + state.framesDrawn++; + fps_frame_count++; + + mono_received = 0; + } + } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) { + LOG_I(TAG, "Mono TCP Stream client disconnected"); + close(mono_client_fd); + mono_client_fd = -1; + mono_received = 0; + if (color_client_fd < 0 && state.overrideActive) { + state.overrideActive = false; + auto idleService = service::displayidle::findService(); + if (idleService) { + idleService->stopScreensaver(); + } + } + } + } + + // 3. Accept Color client + if (color_server_fd >= 0 && color_client_fd < 0) { + struct sockaddr_in client_addr; + socklen_t addr_len = sizeof(client_addr); + int client = accept(color_server_fd, (struct sockaddr*)&client_addr, &addr_len); + if (client >= 0) { + color_client_fd = client; + fcntl(color_client_fd, F_SETFL, O_NONBLOCK); + color_header_received = 0; + color_payload_received = 0; + last_packet_time = now; + LOG_I(TAG, "Color TCP Stream client connected"); + } + } + + // 4. Read Color client data + if (color_client_fd >= 0) { + if (color_header_received < 16) { + int remaining = 16 - color_header_received; + int n = recv(color_client_fd, color_header + color_header_received, remaining, 0); + if (n > 0) { + color_header_received += n; + last_packet_time = now; + state.tcpBytesReceived += n; + + if (color_header_received == 16) { + if (memcmp(color_header, "RAW\x01", 4) != 0) { + LOG_W(TAG, "Invalid Color Stream magic! Disconnecting client."); + close(color_client_fd); + color_client_fd = -1; + } else { + color_x = ((uint16_t)color_header[4] << 8) | color_header[5]; + color_y = ((uint16_t)color_header[6] << 8) | color_header[7]; + color_w = ((uint16_t)color_header[8] << 8) | color_header[9]; + color_h = ((uint16_t)color_header[10] << 8) | color_header[11]; + color_payload_len = ((uint32_t)color_header[12] << 24) | + ((uint32_t)color_header[13] << 16) | + ((uint32_t)color_header[14] << 8) | + color_header[15]; + color_payload_received = 0; + + if (color_payload_len > 153600) { + LOG_W(TAG, "Color stream payload too large (%u bytes). Disconnecting.", color_payload_len); + close(color_client_fd); + color_client_fd = -1; + } + } + } + } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) { + LOG_I(TAG, "Color TCP Stream client disconnected during header read"); + close(color_client_fd); + color_client_fd = -1; + if (mono_client_fd < 0 && state.overrideActive) { + state.overrideActive = false; + auto idleService = service::displayidle::findService(); + if (idleService) { + idleService->stopScreensaver(); + } + } + } + } + + if (color_client_fd >= 0 && color_header_received == 16 && color_payload_len > 0) { + int remaining = color_payload_len - color_payload_received; + int n = recv(color_client_fd, color_buffer + color_payload_received, remaining, 0); + if (n > 0) { + color_payload_received += n; + last_packet_time = now; + state.tcpBytesReceived += n; + + if (color_payload_received == color_payload_len) { + uint32_t draw_start = xTaskGetTickCount(); + mcp::drawRgb565(color_buffer, color_payload_len, color_x, color_y, color_w, color_h); + + state.lastDrawMs = pdTICKS_TO_MS(xTaskGetTickCount() - draw_start); + state.framesDrawn++; + fps_frame_count++; + + color_header_received = 0; + color_payload_len = 0; + color_payload_received = 0; + } + } else if (n == 0 || (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK)) { + LOG_I(TAG, "Color TCP Stream client disconnected during payload read"); + close(color_client_fd); + color_client_fd = -1; + if (mono_client_fd < 0 && state.overrideActive) { + state.overrideActive = false; + auto idleService = service::displayidle::findService(); + if (idleService) { + idleService->stopScreensaver(); + } + } + } + } + } + + // Yield: longer delay when no client connected to save CPU + // RLCD ST7305 SPI is slow and shared - don't hog LVGL lock (fix freeze) + if (mono_client_fd < 0 && color_client_fd < 0) { + vTaskDelay(pdMS_TO_TICKS(100)); + } else { + vTaskDelay(pdMS_TO_TICKS(30)); + } + } + + if (mono_client_fd >= 0) close(mono_client_fd); + if (color_client_fd >= 0) close(color_client_fd); + if (mono_server_fd >= 0) close(mono_server_fd); + if (color_server_fd >= 0) close(color_server_fd); + + heap_caps_free(mono_buffer); + heap_caps_free(color_buffer); + + LOG_I(TAG, "Video stream server task stopped"); + state.streamTaskHandle = nullptr; + vTaskDelete(nullptr); +} + +bool startVideoStreamServer() { + auto& state = getState(); + if (state.streamRunning) { + return true; + } + state.streamRunning = true; + state.framesDrawn = 0; + state.tcpBytesReceived = 0; + state.lastDrawMs = 0; + state.lastFps = 0.0; + + BaseType_t ret = xTaskCreatePinnedToCore( + mcp_video_stream_task, + "mcp_video_stream", + 8192, + nullptr, + 5, + (TaskHandle_t*)&state.streamTaskHandle, + 1 + ); + if (ret != pdPASS) { + state.streamRunning = false; + LOG_E(TAG, "Failed to spawn video stream task"); + return false; + } + return true; +} + +void stopVideoStreamServer() { + auto& state = getState(); + if (!state.streamRunning) { + return; + } + state.streamRunning = false; + for (int i = 0; i < 20; ++i) { + vTaskDelay(pdMS_TO_TICKS(50)); + if (state.streamTaskHandle == nullptr) { + break; + } + } +} + +bool getVideoStreamStats(std::string& stats_json) { + auto& state = getState(); + cJSON* root = cJSON_CreateObject(); + cJSON_AddNumberToObject(root, "frames_drawn", state.framesDrawn); + cJSON_AddNumberToObject(root, "tcp_bytes_received", state.tcpBytesReceived); + cJSON_AddNumberToObject(root, "last_draw_ms", state.lastDrawMs); + cJSON_AddNumberToObject(root, "last_fps", state.lastFps); + cJSON_AddBoolToObject(root, "active", state.overrideActive); + + char* printed = cJSON_PrintUnformatted(root); + if (printed != nullptr) { + stats_json = printed; + free(printed); + } else { + stats_json = "{}"; + } + cJSON_Delete(root); + return true; +} + +bool listApps(std::string& apps_json, std::string& error) { + auto manifests = app::getAppManifests(); + + cJSON* root = cJSON_CreateObject(); + cJSON* apps_array = cJSON_AddArrayToObject(root, "apps"); + + for (const auto& manifest : manifests) { + cJSON* item = cJSON_CreateObject(); + cJSON_AddStringToObject(item, "appId", manifest->appId.c_str()); + cJSON_AddStringToObject(item, "appName", manifest->appName.c_str()); + cJSON_AddStringToObject(item, "appIcon", manifest->appIcon.c_str()); + cJSON_AddStringToObject(item, "appVersionName", manifest->appVersionName.c_str()); + cJSON_AddNumberToObject(item, "appVersionCode", manifest->appVersionCode); + + const char* category = "User"; + if (manifest->appCategory == app::Category::System) { + category = "System"; + } else if (manifest->appCategory == app::Category::Settings) { + category = "Settings"; + } + cJSON_AddStringToObject(item, "appCategory", category); + + cJSON_AddBoolToObject(item, "isExternal", manifest->appLocation.isExternal()); + if (manifest->appLocation.isExternal()) { + cJSON_AddStringToObject(item, "path", manifest->appLocation.getPath().c_str()); + } else { + cJSON_AddStringToObject(item, "path", "internal"); + } + + cJSON_AddNumberToObject(item, "appFlags", manifest->appFlags); + cJSON_AddItemToArray(apps_array, item); + } + + char* printed = cJSON_PrintUnformatted(root); + if (printed != nullptr) { + apps_json = printed; + free(printed); + } else { + apps_json = "{\"apps\":[]}"; + } + cJSON_Delete(root); + return true; +} + +bool runApp(const std::string& appId, std::string& error) { + auto manifest = app::findAppManifestById(appId); + if (manifest == nullptr) { + error = "Application not found"; + return false; + } + + app::start(appId); + return true; +} + +bool listSdFiles(const std::string& directory, std::string& files_json, std::string& error) { + std::string resolved_path; + if (!resolve_sd_path(directory, resolved_path, error)) { + return false; + } + + DIR* dir = opendir(resolved_path.c_str()); + if (dir == nullptr) { + error = "Failed to open directory"; + return false; + } + + cJSON* root = cJSON_CreateObject(); + cJSON* file_array = cJSON_AddArrayToObject(root, "files"); + + struct dirent* entry; + while ((entry = readdir(dir)) != nullptr) { + if (strcmp(entry->d_name, ".") == 0 || strcmp(entry->d_name, "..") == 0) { + continue; + } + + cJSON* item = cJSON_CreateObject(); + cJSON_AddStringToObject(item, "name", entry->d_name); + + std::string full_filepath = resolved_path; + if (!full_filepath.ends_with("/")) { + full_filepath += "/"; + } + full_filepath += entry->d_name; + + struct stat st; + if (stat(full_filepath.c_str(), &st) == 0) { + bool is_dir = S_ISDIR(st.st_mode); + cJSON_AddStringToObject(item, "type", is_dir ? "directory" : "file"); + if (!is_dir) { + cJSON_AddNumberToObject(item, "size", st.st_size); + } + } else { + cJSON_AddStringToObject(item, "type", entry->d_type == DT_DIR ? "directory" : "file"); + } + cJSON_AddItemToArray(file_array, item); + } + closedir(dir); + + char* printed = cJSON_PrintUnformatted(root); + if (printed != nullptr) { + files_json = printed; + free(printed); + } else { + files_json = "{\"files\":[]}"; + } + cJSON_Delete(root); + return true; +} + +} // namespace tt::mcp + +#endif diff --git a/Tactility/Source/service/displayidle/DisplayIdle.cpp b/Tactility/Source/service/displayidle/DisplayIdle.cpp index 17b4e7c8..4048bd17 100644 --- a/Tactility/Source/service/displayidle/DisplayIdle.cpp +++ b/Tactility/Source/service/displayidle/DisplayIdle.cpp @@ -1,4 +1,5 @@ #ifdef ESP_PLATFORM +#include #include @@ -7,8 +8,8 @@ #include "MatrixRainScreensaver.h" #include "MystifyScreensaver.h" #include "StackChanScreensaver.h" +#include "McpScreensaver.h" -#include #include #include #include @@ -16,6 +17,7 @@ #include #include #include +#include #include #include @@ -33,32 +35,24 @@ static bool isDeviceCharging() { bool charging = false; hal::findDevices(hal::Device::Type::Power, [&charging](const auto& power) { if (!power->supportsMetric(hal::power::PowerDevice::MetricType::IsCharging)) { - return true; + return true; // continue } hal::power::PowerDevice::MetricData data; if (power->getMetric(hal::power::PowerDevice::MetricType::IsCharging, data) && data.valueAsBool) { charging = true; - return false; + return false; // stop iter } return true; }); return charging; } - void DisplayIdleService::stopScreensaverCb(lv_event_t* e) { auto* self = static_cast(lv_event_get_user_data(e)); - lv_event_stop_bubbling(e); - self->stopScreensaverRequested.store(true, std::memory_order_release); - lv_display_trigger_activity(nullptr); + self->stopScreensaverLocked(); } -void DisplayIdleService::stopScreensaver() { - if (!lvgl::lock(100)) { - // Lock failed - keep flag set to retry on next tick - return; - } - +void DisplayIdleService::stopScreensaverLocked() { const auto restoreDuty = cachedDisplaySettings.backlightDuty; const bool wasDimmed = displayDimmed; @@ -70,7 +64,6 @@ void DisplayIdleService::stopScreensaver() { lv_obj_delete(screensaverOverlay); screensaverOverlay = nullptr; } - lvgl::unlock(); stopScreensaverRequested.store(false, std::memory_order_relaxed); // Reset auto-off state @@ -85,6 +78,15 @@ void DisplayIdleService::stopScreensaver() { displayDimmed = wasDimmed ? false : displayDimmed; } +void DisplayIdleService::stopScreensaver() { + if (!lvgl::lock(100)) { + // Lock failed - keep flag set to retry on next tick + return; + } + stopScreensaverLocked(); + lvgl::unlock(); +} + void DisplayIdleService::activateScreensaver() { lv_obj_t* top = lv_layer_top(); @@ -121,6 +123,9 @@ void DisplayIdleService::activateScreensaver() { case settings::display::ScreensaverType::StackChan: screensaver = std::make_unique(); break; + case settings::display::ScreensaverType::McpScreen: + screensaver = std::make_unique(); + break; case settings::display::ScreensaverType::None: default: // Just black screen, no animated screensaver @@ -141,8 +146,17 @@ void DisplayIdleService::updateScreensaver() { } } - void DisplayIdleService::tick() { + // Check if MCP override is active — must not be auto-stopped by idle logic + // READ OUTSIDE LVGL lock to avoid lock inversion: + // MCP video task locks mutex -> LVGL, we must NOT do LVGL -> mutex + bool isMcpActive = false; + { + auto& st = mcp::getState(); + std::lock_guard lk(st.mutex); + isMcpActive = (st.drawArea != nullptr) || st.overrideActive; + } + if (!lvgl::lock(100)) { return; } @@ -159,11 +173,13 @@ void DisplayIdleService::tick() { uint32_t inactive_ms = 0; inactive_ms = lv_display_get_inactive_time(nullptr); - // Only update if not stopping (prevents lag on touch) - if (displayDimmed && screensaverOverlay && !stopScreensaverRequested.load(std::memory_order_acquire)) { + // Only update if not stopping (prevents lag on touch) — skip for MCP (no animation) + if (displayDimmed && screensaverOverlay && !stopScreensaverRequested.load(std::memory_order_acquire) && !isMcpActive) { + // Check if screensaver should auto-off after 5 minutes if (!backlightOff) { screensaverActiveCounter++; if (screensaverActiveCounter >= SCREENSAVER_AUTO_OFF_TICKS) { + // Stop screensaver animation and turn off backlight if (screensaver) { screensaver->stop(); screensaver.reset(); @@ -181,6 +197,7 @@ void DisplayIdleService::tick() { lvgl::unlock(); + // Check stop request early for faster response if (stopScreensaverRequested.load(std::memory_order_acquire)) { stopScreensaver(); return; @@ -190,13 +207,15 @@ void DisplayIdleService::tick() { bool supportsBacklight = display != nullptr && display->supportsBacklightDuty(); if (!cachedDisplaySettings.backlightTimeoutEnabled || cachedDisplaySettings.backlightTimeoutMs == 0) { - if (displayDimmed) { + // Timeout disabled (Never): ensure we restore if we were dimmed, regardless of display type + if (displayDimmed && !isMcpActive) { if (supportsBacklight && display != nullptr) { display->setBacklightDuty(cachedDisplaySettings.backlightDuty); } displayDimmed = false; } } else if (supportsBacklight) { + // For backlight-capable displays: full idle handling bool charging_blocks = cachedDisplaySettings.disableScreensaverWhenCharging && isDeviceCharging(); if (!displayDimmed && inactive_ms >= cachedDisplaySettings.backlightTimeoutMs) { @@ -204,7 +223,7 @@ void DisplayIdleService::tick() { // Skip screensaver while charging } else { if (!lvgl::lock(100)) { - return; + return; // Retry on next tick } activateScreensaver(); lvgl::unlock(); @@ -215,23 +234,35 @@ void DisplayIdleService::tick() { } displayDimmed = true; } - } else if (displayDimmed) { + } else if (displayDimmed && !isMcpActive) { if (inactive_ms < kWakeActivityThresholdMs) { stopScreensaver(); } else if (charging_blocks) { stopScreensaver(); } } + } else { + // For monochrome/RLCD (no backlight): don't auto-enter screensaver (heavy full_refresh SPI causes freeze) + // Only handle wake if we are somehow dimmed (e.g. MCP left it) + if (displayDimmed && !isMcpActive) { + if (inactive_ms < kWakeActivityThresholdMs) { + stopScreensaver(); + } + } } } - bool DisplayIdleService::onStart(ServiceContext& service) { // Seed random number generator for varied screensaver patterns srand(static_cast(time(nullptr))); cachedDisplaySettings = settings::display::loadOrGetDefault(); + auto display = getDisplay(); + if (display != nullptr && !display->supportsBacklightDuty()) { + LOG_I(TAG, "Monochrome/RLCD display detected (no backlight control): idle timer will run but auto-backlight off is disabled"); + } + timer = std::make_unique(Timer::Type::Periodic, kernel::millisToTicks(TICK_INTERVAL_MS), [this]{ this->tick(); }); timer->setCallbackPriority(Thread::Priority::Lower); timer->start(); @@ -288,6 +319,62 @@ void DisplayIdleService::reloadSettings() { settingsReloadRequested.store(true, std::memory_order_release); } +void DisplayIdleService::startMcpScreensaver() { + if (!lvgl::lock(200)) { + LOG_W(TAG, "startMcpScreensaver: failed to acquire LVGL lock"); + return; + } + + if (screensaverOverlay != nullptr) { + // Screensaver already active — if drawArea is registered we're done, + // otherwise stop the current one so we can replace it with McpScreensaver. + const auto& mcpState = mcp::getState(); + if (mcpState.drawArea != nullptr) { + lvgl::unlock(); + return; // McpScreensaver already running + } + // Wrong screensaver type active — tear it down first + if (screensaver) { + screensaver->stop(); + screensaver.reset(); + } + lv_obj_delete(screensaverOverlay); + screensaverOverlay = nullptr; + } + + screensaverActiveCounter = 0; + backlightOff = false; + + // Ensure backlight is active if the display supports it + auto display = getDisplay(); + if (display != nullptr && display->supportsBacklightDuty()) { + uint8_t duty = cachedDisplaySettings.backlightDuty; + if (duty == 0) duty = 255; // ensure visible if settings not loaded / default + display->setBacklightDuty(duty); + } + + lv_coord_t screenW = lv_display_get_horizontal_resolution(nullptr); + + lv_coord_t screenH = lv_display_get_vertical_resolution(nullptr); + + lv_obj_t* top = lv_layer_top(); + screensaverOverlay = lv_obj_create(top); + lv_obj_remove_style_all(screensaverOverlay); + lv_obj_set_size(screensaverOverlay, LV_PCT(100), LV_PCT(100)); + lv_obj_set_pos(screensaverOverlay, 0, 0); + lv_obj_set_style_bg_color(screensaverOverlay, lv_color_black(), 0); + lv_obj_set_style_bg_opa(screensaverOverlay, LV_OPA_COVER, 0); + lv_obj_add_flag(screensaverOverlay, LV_OBJ_FLAG_CLICKABLE); + lv_obj_add_event_cb(screensaverOverlay, stopScreensaverCb, LV_EVENT_CLICKED, this); + + screensaver = std::make_unique(); + screensaver->start(screensaverOverlay, screenW, screenH); + + lvgl::unlock(); + displayDimmed = true; + LOG_I(TAG, "MCP screensaver activated"); +} + std::shared_ptr findService() { return std::static_pointer_cast( findServiceById("DisplayIdle") diff --git a/Tactility/Source/service/displayidle/McpScreensaver.cpp b/Tactility/Source/service/displayidle/McpScreensaver.cpp new file mode 100644 index 00000000..06814017 --- /dev/null +++ b/Tactility/Source/service/displayidle/McpScreensaver.cpp @@ -0,0 +1,100 @@ +#ifdef ESP_PLATFORM + +#include "McpScreensaver.h" +#include +#include + +constexpr auto* TAG = "McpScreensaver"; +#include + +namespace tt::service::displayidle { + + +void McpScreensaver::start(lv_obj_t* overlay, lv_coord_t screenW, lv_coord_t screenH) { + auto& state = mcp::getState(); + + // Full-screen canvas on the overlay + lv_obj_t* canvas = lv_canvas_create(overlay); + lv_obj_set_size(canvas, screenW, screenH); + lv_obj_set_pos(canvas, 0, 0); + lv_obj_set_style_radius(canvas, 0, LV_PART_MAIN); + lv_obj_set_style_border_width(canvas, 0, LV_PART_MAIN); + lv_obj_set_style_pad_all(canvas, 0, LV_PART_MAIN); + lv_obj_remove_flag(canvas, LV_OBJ_FLAG_SCROLLABLE); + + // Allocate framebuffer (prefer SPIRAM) + size_t requiredSize = (size_t)screenW * screenH * sizeof(uint16_t); + framebuffer = (uint16_t*)heap_caps_malloc(requiredSize, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (framebuffer == nullptr) { + framebuffer = (uint16_t*)heap_caps_malloc(requiredSize, MALLOC_CAP_8BIT); + } + framebufferSize = (framebuffer != nullptr) ? requiredSize : 0; + + if (framebuffer == nullptr) { + LOG_E(TAG, "Failed to allocate %uB framebuffer", (unsigned)requiredSize); + lv_obj_t* err = lv_label_create(canvas); + lv_label_set_text(err, "Framebuffer alloc failed"); + lv_obj_center(err); + return; + } + + // Fill with a dark slate background (inverted for display path) + size_t pixelCount = (size_t)screenW * screenH; + for (size_t i = 0; i < pixelCount; ++i) { + framebuffer[i] = ~0x18E3; // dark blue-grey + } + + lv_canvas_set_buffer(canvas, framebuffer, screenW, screenH, LV_COLOR_FORMAT_RGB565); + + // Waiting label (removed on first MCP draw via lv_obj_clean) + lv_obj_t* waitLabel = lv_label_create(canvas); + lv_label_set_text(waitLabel, "Waiting for LLM..."); + lv_obj_set_style_text_color(waitLabel, lv_color_black(), LV_PART_MAIN); // white on screen (inverted) + lv_obj_align(waitLabel, LV_ALIGN_CENTER, 0, -20); + + lv_obj_t* resLabel = lv_label_create(canvas); + lv_label_set_text_fmt(resLabel, "Display: %dx%d", (int)screenW, (int)screenH); + lv_color_t resColor = lv_palette_lighten(LV_PALETTE_BLUE, 3); + lv_obj_set_style_text_color(resLabel, lv_color_make(~resColor.red, ~resColor.green, ~resColor.blue), LV_PART_MAIN); + lv_obj_align(resLabel, LV_ALIGN_CENTER, 0, 10); + + // Register with McpSystemState + std::lock_guard lock(state.mutex); + state.drawArea = canvas; + state.framebuffer = framebuffer; + state.framebufferSize = framebufferSize; + state.displayWidth = (uint16_t)screenW; + state.displayHeight = (uint16_t)screenH; + state.drawWidth = (uint16_t)screenW; + state.drawHeight = (uint16_t)screenH; + // Don't reset overrideActive — if the LLM already drew, we keep the content + + LOG_I(TAG, "McpScreensaver started (%dx%d)", (int)screenW, (int)screenH); +} + +void McpScreensaver::stop() { + auto& state = mcp::getState(); + { + std::lock_guard lock(state.mutex); + state.drawArea = nullptr; + state.framebuffer = nullptr; + state.framebufferSize = 0; + state.overrideActive = false; + } + + if (framebuffer != nullptr) { + heap_caps_free(framebuffer); + framebuffer = nullptr; + framebufferSize = 0; + } + + LOG_I(TAG, "McpScreensaver stopped"); +} + +void McpScreensaver::update(lv_coord_t /*screenW*/, lv_coord_t /*screenH*/) { + // MCP draws on demand via HTTP — no per-frame animation needed +} + +} // namespace tt::service::displayidle + +#endif // ESP_PLATFORM diff --git a/Tactility/Source/service/displayidle/McpScreensaver.h b/Tactility/Source/service/displayidle/McpScreensaver.h new file mode 100644 index 00000000..a801bf35 --- /dev/null +++ b/Tactility/Source/service/displayidle/McpScreensaver.h @@ -0,0 +1,30 @@ +#pragma once +#ifdef ESP_PLATFORM + +#include "Screensaver.h" +#include + +namespace tt::service::displayidle { + +/** + * MCP Screen screensaver. + * Creates a full-screen LVGL canvas on the overlay and registers it in + * McpSystemState so that MCP HTTP draw commands can paint to it. + * Dismissed by a touch event (handled by the parent DisplayIdle overlay). + */ +class McpScreensaver final : public Screensaver { + uint16_t* framebuffer = nullptr; + size_t framebufferSize = 0; + +public: + McpScreensaver() = default; + ~McpScreensaver() override = default; + + void start(lv_obj_t* overlay, lv_coord_t screenW, lv_coord_t screenH) override; + void stop() override; + void update(lv_coord_t screenW, lv_coord_t screenH) override; +}; + +} // namespace tt::service::displayidle + +#endif // ESP_PLATFORM diff --git a/Tactility/Source/service/webserver/McpHandler.cpp b/Tactility/Source/service/webserver/McpHandler.cpp new file mode 100644 index 00000000..509877aa --- /dev/null +++ b/Tactility/Source/service/webserver/McpHandler.cpp @@ -0,0 +1,1030 @@ +#ifdef ESP_PLATFORM + +#include +#include +#include +#include +#include + +constexpr auto* TAG = "McpHandler"; + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace tt::service::webserver { + +static void* psram_malloc(size_t size) { + void* ptr = heap_caps_malloc(size, MALLOC_CAP_SPIRAM | MALLOC_CAP_8BIT); + if (ptr != nullptr) { + return ptr; + } + return malloc(size); +} + + +#define MCP_JSON_BODY_LIMIT (256 * 1024) +#define MCP_RAW_BODY_LIMIT (512 * 1024) + +static const char* TOOLS_JSON = + "[" + "{" + "\"name\":\"get_capabilities\"," + "\"description\":\"Get the Tactility display capabilities.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"clear_screen\"," + "\"description\":\"Clear the MCP drawing area to white (0) or black (1).\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{\"color\":{\"type\":\"integer\",\"enum\":[0,1]}}," + "\"required\":[\"color\"]" + "}" + "}," + "{" + "\"name\":\"draw_text\"," + "\"description\":\"Draw text in the MCP drawing area at x,y coordinates.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"text\":{\"type\":\"string\"}," + "\"x\":{\"type\":\"integer\"}," + "\"y\":{\"type\":\"integer\"}," + "\"size\":{\"type\":\"integer\",\"enum\":[1,2],\"default\":1}" + "}," + "\"required\":[\"text\",\"x\",\"y\"]" + "}" + "}," + "{" + "\"name\":\"draw_image\"," + "\"description\":\"Draw a bridge-preprocessed binary PBM image.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"pbm_base64\":{\"type\":\"string\"}," + "\"x\":{\"type\":\"integer\",\"default\":0}," + "\"y\":{\"type\":\"integer\",\"default\":0}," + "\"dither\":{\"type\":\"boolean\",\"default\":true}" + "}," + "\"required\":[\"pbm_base64\"]" + "}" + "}," + "{" + "\"name\":\"draw_color_bmp\"," + "\"description\":\"Draw an uncompressed 24-bit or 32-bit BMP image.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"bmp_base64\":{\"type\":\"string\"}," + "\"x\":{\"type\":\"integer\",\"default\":0}," + "\"y\":{\"type\":\"integer\",\"default\":0}" + "}," + "\"required\":[\"bmp_base64\"]" + "}" + "}," + "{" + "\"name\":\"draw_raw_rgb565\"," + "\"description\":\"Draw big-endian raw RGB565 pixels.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"rgb565_base64\":{\"type\":\"string\"}," + "\"x\":{\"type\":\"integer\"}," + "\"y\":{\"type\":\"integer\"}," + "\"w\":{\"type\":\"integer\"}," + "\"h\":{\"type\":\"integer\"}" + "}," + "\"required\":[\"rgb565_base64\",\"x\",\"y\",\"w\",\"h\"]" + "}" + "}," + "{" + "\"name\":\"get_screenshot\"," + "\"description\":\"Capture the MCP framebuffer as a PBM image.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"play_tone\"," + "\"description\":\"Play a pure sine wave tone on the board speaker.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"frequency\":{\"type\":\"integer\",\"default\":440}," + "\"duration_ms\":{\"type\":\"integer\",\"default\":1000}," + "\"volume\":{\"type\":\"integer\",\"default\":50}" + "}}" + "}," + "{" + "\"name\":\"play_audio\"," + "\"description\":\"Play a 16 kHz 16-bit PCM WAV file from app user data.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"filename\":{\"type\":\"string\"}," + "\"volume\":{\"type\":\"integer\",\"default\":50}" + "},\"required\":[\"filename\"]}" + "}," + "{" + "\"name\":\"record_voice\"," + "\"description\":\"Record 16 kHz 16-bit mono PCM to app user data.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"duration_sec\":{\"type\":\"integer\",\"default\":4}," + "\"filename\":{\"type\":\"string\",\"default\":\"recording.pcm\"}" + "}}" + "}," + "{" + "\"name\":\"play_audio_base64\"," + "\"description\":\"Decode and play a base64-encoded 16 kHz 16-bit PCM WAV.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"wav_base64\":{\"type\":\"string\"}," + "\"volume\":{\"type\":\"integer\",\"default\":50}" + "},\"required\":[\"wav_base64\"]}" + "}," + "{" + "\"name\":\"play_mp3\"," + "\"description\":\"Stream an MP3 file from app user data to the speaker.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"filename\":{\"type\":\"string\"}," + "\"volume\":{\"type\":\"integer\",\"default\":50}" + "},\"required\":[\"filename\"]}" + "}," + "{" + "\"name\":\"play_mp3_base64\"," + "\"description\":\"Decode and play a base64-encoded MP3.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{" + "\"mp3_base64\":{\"type\":\"string\"}," + "\"volume\":{\"type\":\"integer\",\"default\":50}" + "},\"required\":[\"mp3_base64\"]}" + "}," + "{" + "\"name\":\"set_led\"," + "\"description\":\"Control the onboard WS2812 NeoPixel RGB LED.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"r\":{\"type\":\"integer\",\"minimum\":0,\"maximum\":255,\"description\":\"Red channel (0-255)\"}," + "\"g\":{\"type\":\"integer\",\"minimum\":0,\"maximum\":255,\"description\":\"Green channel (0-255)\"}," + "\"b\":{\"type\":\"integer\",\"minimum\":0,\"maximum\":255,\"description\":\"Blue channel (0-255)\"}," + "\"mode\":{\"type\":\"string\",\"enum\":[\"static\",\"breath\",\"rainbow\",\"off\"],\"description\":\"LED mode\"}" + "}," + "\"required\":[\"r\",\"g\",\"b\",\"mode\"]" + "}" + "}," + "{" + "\"name\":\"get_battery\"," + "\"description\":\"Read the current battery voltage and estimated capacity percentage.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"get_sensors\"," + "\"description\":\"Read onboard sensors (IMU, temperature, humidity).\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"scan_ble\"," + "\"description\":\"Scan for nearby Bluetooth Low Energy devices.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"duration_ms\":{\"type\":\"integer\",\"default\":3000,\"description\":\"Scan duration in milliseconds\"}" + "}" + "}" + "}," + "{" + "\"name\":\"write_file\"," + "\"description\":\"Write a file to the microSD card.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"path\":{\"type\":\"string\",\"description\":\"Destination path relative to /sdcard/\"}," + "\"content\":{\"type\":\"string\",\"description\":\"Text content of the file\"}" + "}," + "\"required\":[\"path\",\"content\"]" + "}" + "}," + "{" + "\"name\":\"read_file\"," + "\"description\":\"Read a text file from the microSD card.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"path\":{\"type\":\"string\",\"description\":\"File path relative to /sdcard/\"}" + "}," + "\"required\":[\"path\"]" + "}" + "}," + "{" + "\"name\":\"download_file\"," + "\"description\":\"Download a file from a URL directly to the microSD card.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"url\":{\"type\":\"string\",\"description\":\"The URL of the file to download\"}," + "\"filename\":{\"type\":\"string\",\"description\":\"The destination filename relative to /sdcard/\"}" + "}," + "\"required\":[\"url\",\"filename\"]" + "}" + "}," + "{" + "\"name\":\"get_video_streaming_instructions\"," + "\"description\":\"Get details for monochrome TCP streaming and RGB565 color TCP streaming.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"protocol\":{\"type\":\"string\",\"enum\":[\"tcp\",\"color\",\"both\",\"all\"],\"default\":\"all\"}" + "}" + "}" + "}," + "{" + "\"name\":\"get_stream_stats\"," + "\"description\":\"Get performance statistics for the active video streams.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"list_apps\"," + "\"description\":\"List all applications installed on the system, including metadata and locations.\"," + "\"inputSchema\":{\"type\":\"object\",\"properties\":{}}" + "}," + "{" + "\"name\":\"run_app\"," + "\"description\":\"Launch an application on the board by its App ID.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"app_id\":{\"type\":\"string\",\"description\":\"The unique ID of the application (e.g., 'one.tactility.helloworld')\"}" + "}," + "\"required\":[\"app_id\"]" + "}" + "}," + "{" + "\"name\":\"list_files\"," + "\"description\":\"List files and folders in a directory on the SD card, with details like sizes.\"," + "\"inputSchema\":{" + "\"type\":\"object\"," + "\"properties\":{" + "\"path\":{\"type\":\"string\",\"description\":\"Path relative to /sdcard/ to list (default is '/')\"}" + "}" + "}" + "}" + "]"; + +static uint8_t* base64_decode(const char* input, size_t* output_size) { + if (input == NULL || output_size == NULL) { + return NULL; + } + + const char* payload = input; + const char* marker = strstr(input, ";base64,"); + if (marker != NULL) { + payload = marker + 8; + } + + size_t input_size = strlen(payload); + size_t approx_output_len = (input_size * 3) / 4; + uint8_t* output = (uint8_t*)malloc(approx_output_len + 4); + if (output == NULL) { + return NULL; + } + + size_t actual_len = 0; + int ret = mbedtls_base64_decode(output, approx_output_len + 4, &actual_len, + (const unsigned char*)payload, input_size); + if (ret != 0) { + free(output); + return NULL; + } + + *output_size = actual_len; + return output; +} + +static cJSON* make_error(cJSON* id, int code, const char* message) { + cJSON* root = cJSON_CreateObject(); + cJSON_AddStringToObject(root, "jsonrpc", "2.0"); + + cJSON* error = cJSON_AddObjectToObject(root, "error"); + cJSON_AddNumberToObject(error, "code", code); + cJSON_AddStringToObject(error, "message", message); + + if (id != NULL) { + cJSON_AddItemToObject(root, "id", cJSON_Duplicate(id, true)); + } else { + cJSON_AddNullToObject(root, "id"); + } + return root; +} + +static cJSON* make_tool_result(cJSON* id, const char* text) { + cJSON* root = cJSON_CreateObject(); + cJSON_AddStringToObject(root, "jsonrpc", "2.0"); + + cJSON* result = cJSON_AddObjectToObject(root, "result"); + cJSON_AddArrayToObject(result, "content"); + cJSON* item = cJSON_CreateObject(); + cJSON_AddStringToObject(item, "type", "text"); + cJSON_AddStringToObject(item, "text", text); + cJSON_AddItemToArray(cJSON_GetObjectItem(result, "content"), item); + + if (id != NULL) { + cJSON_AddItemToObject(root, "id", cJSON_Duplicate(id, true)); + } else { + cJSON_AddNullToObject(root, "id"); + } + return root; +} + +static cJSON* handle_tool_call(cJSON* id, cJSON* params) { + if (!cJSON_IsObject(params)) { + return make_error(id, -32602, "Invalid params"); + } + + cJSON* name_item = cJSON_GetObjectItemCaseSensitive(params, "name"); + cJSON* arguments = cJSON_GetObjectItemCaseSensitive(params, "arguments"); + if (!cJSON_IsString(name_item) || name_item->valuestring == NULL) { + return make_error(id, -32602, "Missing tool name"); + } + if (arguments != NULL && !cJSON_IsObject(arguments)) { + return make_error(id, -32602, "Tool arguments must be an object"); + } + + const char* name = name_item->valuestring; + auto& state = mcp::getState(); + + if (strcmp(name, "get_capabilities") == 0) { + char capabilities[384]; + uint16_t width = state.drawWidth > 0 ? state.drawWidth : state.displayWidth; + uint16_t height = state.drawHeight > 0 ? state.drawHeight : state.displayHeight; + snprintf( + capabilities, + sizeof(capabilities), + "{\"color\":true,\"width\":%u,\"height\":%u," + "\"formats\":[\"rgb565_base64\",\"bmp_base64\",\"pbm_base64\"]," + "\"audio\":{\"sampleRate\":16000,\"bitsPerSample\":16,\"channels\":1}," + "\"platform\":\"tactility\",\"appVersion\":\"0.1.0\"," + "\"uiVisible\":%s}", + width, + height, + (state.drawArea != nullptr) ? "true" : "false" + ); + return make_tool_result(id, capabilities); + } + + if (strcmp(name, "clear_screen") == 0) { + cJSON* color_item = cJSON_GetObjectItemCaseSensitive(arguments, "color"); + if (!cJSON_IsNumber(color_item) || + (color_item->valueint != 0 && color_item->valueint != 1)) { + return make_error(id, -32602, "color must be 0 or 1"); + } + if (!mcp::clearScreen(color_item->valueint)) { + return make_error(id, -32010, "Failed to clear screen"); + } + return make_tool_result(id, "Screen cleared."); + } + + if (strcmp(name, "draw_text") == 0) { + cJSON* text_item = cJSON_GetObjectItemCaseSensitive(arguments, "text"); + cJSON* x_item = cJSON_GetObjectItemCaseSensitive(arguments, "x"); + cJSON* y_item = cJSON_GetObjectItemCaseSensitive(arguments, "y"); + cJSON* size_item = cJSON_GetObjectItemCaseSensitive(arguments, "size"); + + if (!cJSON_IsString(text_item) || text_item->valuestring == NULL || + !cJSON_IsNumber(x_item) || !cJSON_IsNumber(y_item)) { + return make_error(id, -32602, "draw_text requires text, x, and y"); + } + if (strlen(text_item->valuestring) > 512) { + return make_error(id, -32602, "text exceeds 512 characters"); + } + + int size = cJSON_IsNumber(size_item) ? size_item->valueint : 1; + if (size != 1 && size != 2) { + return make_error(id, -32602, "size must be 1 or 2"); + } + + if (!mcp::drawText(text_item->valuestring, x_item->valueint, y_item->valueint, size)) { + return make_error(id, -32010, "Failed to draw text"); + } + + char message[160]; + snprintf( + message, + sizeof(message), + "Successfully drew text at (%d, %d) with size %d.", + x_item->valueint, + y_item->valueint, + size + ); + return make_tool_result(id, message); + } + + if (strcmp(name, "draw_raw_rgb565") == 0) { + cJSON* encoded_item = cJSON_GetObjectItemCaseSensitive(arguments, "rgb565_base64"); + cJSON* x_item = cJSON_GetObjectItemCaseSensitive(arguments, "x"); + cJSON* y_item = cJSON_GetObjectItemCaseSensitive(arguments, "y"); + cJSON* width_item = cJSON_GetObjectItemCaseSensitive(arguments, "w"); + cJSON* height_item = cJSON_GetObjectItemCaseSensitive(arguments, "h"); + if (!cJSON_IsString(encoded_item) || !cJSON_IsNumber(x_item) || + !cJSON_IsNumber(y_item) || !cJSON_IsNumber(width_item) || + !cJSON_IsNumber(height_item)) { + return make_error(id, -32602, "draw_raw_rgb565 requires rgb565_base64, x, y, w, and h"); + } + + size_t decoded_size = 0; + uint8_t* decoded = base64_decode(encoded_item->valuestring, &decoded_size); + if (decoded == NULL) { + return make_error(id, -32602, "Invalid RGB565 base64 data"); + } + bool drawn = mcp::drawRgb565( + decoded, + decoded_size, + x_item->valueint, + y_item->valueint, + width_item->valueint, + height_item->valueint + ); + free(decoded); + return drawn + ? make_tool_result(id, "Raw RGB565 data displayed successfully.") + : make_error(id, -32010, "Failed to draw RGB565 data"); + } + + if (strcmp(name, "draw_color_bmp") == 0) { + cJSON* encoded_item = cJSON_GetObjectItemCaseSensitive(arguments, "bmp_base64"); + cJSON* x_item = cJSON_GetObjectItemCaseSensitive(arguments, "x"); + cJSON* y_item = cJSON_GetObjectItemCaseSensitive(arguments, "y"); + if (!cJSON_IsString(encoded_item)) { + return make_error(id, -32602, "Missing bmp_base64"); + } + size_t decoded_size = 0; + uint8_t* decoded = base64_decode(encoded_item->valuestring, &decoded_size); + if (decoded == NULL) { + return make_error(id, -32602, "Invalid BMP base64 data"); + } + bool drawn = mcp::drawBmp( + decoded, + decoded_size, + cJSON_IsNumber(x_item) ? x_item->valueint : 0, + cJSON_IsNumber(y_item) ? y_item->valueint : 0 + ); + free(decoded); + return drawn + ? make_tool_result(id, "Color BMP image displayed successfully.") + : make_error(id, -32602, "Unsupported or invalid BMP image"); + } + + if (strcmp(name, "draw_image") == 0) { + cJSON* encoded_item = cJSON_GetObjectItemCaseSensitive(arguments, "pbm_base64"); + cJSON* x_item = cJSON_GetObjectItemCaseSensitive(arguments, "x"); + cJSON* y_item = cJSON_GetObjectItemCaseSensitive(arguments, "y"); + if (!cJSON_IsString(encoded_item)) { + return make_error( + id, + -32602, + "draw_image requires bridge-preprocessed pbm_base64" + ); + } + size_t decoded_size = 0; + uint8_t* decoded = base64_decode(encoded_item->valuestring, &decoded_size); + if (decoded == NULL) { + return make_error(id, -32602, "Invalid PBM base64 data"); + } + bool drawn = mcp::drawPbm( + decoded, + decoded_size, + cJSON_IsNumber(x_item) ? x_item->valueint : 0, + cJSON_IsNumber(y_item) ? y_item->valueint : 0 + ); + free(decoded); + return drawn + ? make_tool_result(id, "Image displayed successfully.") + : make_error(id, -32602, "Unsupported or invalid PBM image"); + } + + if (strcmp(name, "get_screenshot") == 0) { + std::string encoded = mcp::getScreenshotPbmBase64(); + if (encoded.empty()) { + return make_error(id, -32010, "Screenshot capture failed"); + } + std::string result = "__PBM_BASE64__:" + encoded; + return make_tool_result(id, result.c_str()); + } + + if (strcmp(name, "play_tone") == 0) { + cJSON* frequency_item = cJSON_GetObjectItemCaseSensitive(arguments, "frequency"); + cJSON* duration_item = cJSON_GetObjectItemCaseSensitive(arguments, "duration_ms"); + cJSON* volume_item = cJSON_GetObjectItemCaseSensitive(arguments, "volume"); + int frequency = cJSON_IsNumber(frequency_item) ? frequency_item->valueint : 440; + int duration = cJSON_IsNumber(duration_item) ? duration_item->valueint : 1000; + int volume = cJSON_IsNumber(volume_item) ? volume_item->valueint : 50; + if (frequency < 50 || frequency > 10000 || + duration < 50 || duration > 5000 || + volume < 0 || volume > 100) { + return make_error(id, -32602, "frequency, duration_ms, or volume is out of range"); + } + + std::string error; + if (!mcp::playTone(frequency, duration, volume, error)) { + return make_error(id, -32020, error.c_str()); + } + char message[128]; + snprintf( + message, + sizeof(message), + "Played tone of %dHz for %dms at volume %d.", + frequency, + duration, + volume + ); + return make_tool_result(id, message); + } + + if (strcmp(name, "record_voice") == 0) { + cJSON* duration_item = cJSON_GetObjectItemCaseSensitive(arguments, "duration_sec"); + cJSON* filename_item = cJSON_GetObjectItemCaseSensitive(arguments, "filename"); + int duration = cJSON_IsNumber(duration_item) ? duration_item->valueint : 4; + const char* filename = cJSON_IsString(filename_item) + ? filename_item->valuestring + : "recording.pcm"; + if (duration < 1 || duration > 15) { + return make_error(id, -32602, "duration_sec must be between 1 and 15"); + } + + std::string error; + size_t recorded_bytes = 0; + if (!mcp::recordVoice(duration, filename, recorded_bytes, error)) { + return make_error(id, -32020, error.c_str()); + } + char message[180]; + snprintf( + message, + sizeof(message), + "Successfully recorded %d seconds of audio to '%s' (%u bytes).", + duration, + filename, + (unsigned)recorded_bytes + ); + return make_tool_result(id, message); + } + + if (strcmp(name, "play_audio") == 0) { + cJSON* filename_item = cJSON_GetObjectItemCaseSensitive(arguments, "filename"); + cJSON* volume_item = cJSON_GetObjectItemCaseSensitive(arguments, "volume"); + int volume = cJSON_IsNumber(volume_item) ? volume_item->valueint : 50; + if (!cJSON_IsString(filename_item) || filename_item->valuestring == NULL) { + return make_error(id, -32602, "play_audio requires filename"); + } + if (volume < 0 || volume > 100) { + return make_error(id, -32602, "volume must be between 0 and 100"); + } + + std::string error; + if (!mcp::playAudioFile(filename_item->valuestring, volume, error)) { + return make_error(id, -32020, error.c_str()); + } + char message[160]; + snprintf( + message, + sizeof(message), + "Successfully played audio file '%s'.", + filename_item->valuestring + ); + return make_tool_result(id, message); + } + + if (strcmp(name, "play_audio_base64") == 0) { + cJSON* encoded_item = cJSON_GetObjectItemCaseSensitive(arguments, "wav_base64"); + cJSON* volume_item = cJSON_GetObjectItemCaseSensitive(arguments, "volume"); + int volume = cJSON_IsNumber(volume_item) ? volume_item->valueint : 50; + if (!cJSON_IsString(encoded_item) || encoded_item->valuestring == NULL) { + return make_error(id, -32602, "play_audio_base64 requires wav_base64"); + } + if (volume < 0 || volume > 100) { + return make_error(id, -32602, "volume must be between 0 and 100"); + } + + size_t decoded_size = 0; + uint8_t* decoded = base64_decode(encoded_item->valuestring, &decoded_size); + if (decoded == NULL) { + return make_error(id, -32602, "Invalid WAV base64 data"); + } + std::string error; + bool played = mcp::playWavMemory( + decoded, + decoded_size, + volume, + error + ); + free(decoded); + return played + ? make_tool_result(id, "Successfully played base64 audio.") + : make_error(id, -32020, error.c_str()); + } + + if (strcmp(name, "play_mp3") == 0) { + cJSON* filename_item = cJSON_GetObjectItemCaseSensitive(arguments, "filename"); + cJSON* volume_item = cJSON_GetObjectItemCaseSensitive(arguments, "volume"); + int volume = cJSON_IsNumber(volume_item) ? volume_item->valueint : 50; + if (!cJSON_IsString(filename_item) || filename_item->valuestring == NULL) { + return make_error(id, -32602, "play_mp3 requires filename"); + } + if (volume < 0 || volume > 100) { + return make_error(id, -32602, "volume must be between 0 and 100"); + } + + std::string error; + if (!mcp::playMp3File(filename_item->valuestring, volume, error)) { + return make_error(id, -32020, error.c_str()); + } + char message[160]; + snprintf( + message, + sizeof(message), + "Successfully played MP3 file '%s'.", + filename_item->valuestring + ); + return make_tool_result(id, message); + } + + if (strcmp(name, "play_mp3_base64") == 0) { + cJSON* encoded_item = cJSON_GetObjectItemCaseSensitive(arguments, "mp3_base64"); + cJSON* volume_item = cJSON_GetObjectItemCaseSensitive(arguments, "volume"); + int volume = cJSON_IsNumber(volume_item) ? volume_item->valueint : 50; + if (!cJSON_IsString(encoded_item) || encoded_item->valuestring == NULL) { + return make_error(id, -32602, "play_mp3_base64 requires mp3_base64"); + } + if (volume < 0 || volume > 100) { + return make_error(id, -32602, "volume must be between 0 and 100"); + } + + size_t decoded_size = 0; + uint8_t* decoded = base64_decode(encoded_item->valuestring, &decoded_size); + if (decoded == NULL) { + return make_error(id, -32602, "Invalid MP3 base64 data"); + } + std::string error; + bool played = mcp::playMp3Memory( + decoded, + decoded_size, + volume, + error + ); + free(decoded); + return played + ? make_tool_result(id, "Successfully played base64 MP3 audio.") + : make_error(id, -32020, error.c_str()); + } + + if (strcmp(name, "set_led") == 0) { + cJSON* r_item = cJSON_GetObjectItemCaseSensitive(arguments, "r"); + cJSON* g_item = cJSON_GetObjectItemCaseSensitive(arguments, "g"); + cJSON* b_item = cJSON_GetObjectItemCaseSensitive(arguments, "b"); + cJSON* mode_item = cJSON_GetObjectItemCaseSensitive(arguments, "mode"); + if (!cJSON_IsNumber(r_item) || !cJSON_IsNumber(g_item) || + !cJSON_IsNumber(b_item) || !cJSON_IsString(mode_item)) { + return make_error(id, -32602, "set_led requires r, g, b, and mode"); + } + std::string error; + if (!mcp::setLedColor(r_item->valueint, g_item->valueint, b_item->valueint, mode_item->valuestring, error)) { + return make_error(id, -32020, error.c_str()); + } + char msg[128]; + snprintf(msg, sizeof(msg), "LED set to mode '%s' with base color (%d, %d, %d).", + mode_item->valuestring, r_item->valueint, g_item->valueint, b_item->valueint); + return make_tool_result(id, msg); + } + + if (strcmp(name, "get_battery") == 0) { + double voltage_v = 0.0; + int percentage_pct = 0; + std::string error; + if (!mcp::getBatteryStatus(voltage_v, percentage_pct, error)) { + return make_error(id, -32020, error.c_str()); + } + char result_buf[128]; + snprintf(result_buf, sizeof(result_buf), "{\"voltage_v\":%.3f,\"percentage_pct\":%d}", voltage_v, percentage_pct); + return make_tool_result(id, result_buf); + } + + if (strcmp(name, "get_sensors") == 0) { + double temp_c = 0.0; + double hum_pct = 0.0; + std::string imu_json; + std::string error; + if (!mcp::getSensors(temp_c, hum_pct, imu_json, error)) { + return make_error(id, -32020, error.c_str()); + } + char result_buf[512]; + snprintf(result_buf, sizeof(result_buf), "{\"temperature_c\":%.2f,\"humidity_pct\":%.2f,\"imu\":%s}", + temp_c, hum_pct, imu_json.c_str()); + return make_tool_result(id, result_buf); + } + + if (strcmp(name, "scan_ble") == 0) { + cJSON* duration_item = cJSON_GetObjectItemCaseSensitive(arguments, "duration_ms"); + int duration_ms = cJSON_IsNumber(duration_item) ? duration_item->valueint : 3000; + if (duration_ms < 500 || duration_ms > 15000) { + return make_error(id, -32602, "duration_ms must be between 500 and 15000"); + } + std::string devices_json; + std::string error; + if (!mcp::scanBleDevices(duration_ms, devices_json, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, devices_json.c_str()); + } + + if (strcmp(name, "write_file") == 0) { + cJSON* path_item = cJSON_GetObjectItemCaseSensitive(arguments, "path"); + cJSON* content_item = cJSON_GetObjectItemCaseSensitive(arguments, "content"); + if (!cJSON_IsString(path_item) || !cJSON_IsString(content_item)) { + return make_error(id, -32602, "write_file requires path and content"); + } + std::string error; + if (!mcp::writeSdFile(path_item->valuestring, content_item->valuestring, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, "Successfully wrote file to SD card."); + } + + if (strcmp(name, "read_file") == 0) { + cJSON* path_item = cJSON_GetObjectItemCaseSensitive(arguments, "path"); + if (!cJSON_IsString(path_item)) { + return make_error(id, -32602, "read_file requires path"); + } + std::string content; + std::string error; + if (!mcp::readSdFile(path_item->valuestring, content, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, content.c_str()); + } + + if (strcmp(name, "download_file") == 0) { + cJSON* url_item = cJSON_GetObjectItemCaseSensitive(arguments, "url"); + cJSON* filename_item = cJSON_GetObjectItemCaseSensitive(arguments, "filename"); + if (!cJSON_IsString(url_item) || !cJSON_IsString(filename_item)) { + return make_error(id, -32602, "download_file requires url and filename"); + } + std::string error; + if (!mcp::downloadSdFile(url_item->valuestring, filename_item->valuestring, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, "Successfully downloaded file to SD card."); + } + + if (strcmp(name, "get_video_streaming_instructions") == 0) { + cJSON* protocol_item = cJSON_GetObjectItemCaseSensitive(arguments, "protocol"); + const char* protocol = cJSON_IsString(protocol_item) ? protocol_item->valuestring : "all"; + + char buf[512]; + snprintf(buf, sizeof(buf), + "Tactility OS TCP Video Streaming Instructions:\n" + "1. Mono TCP Stream (compatibility mode): Stream raw 15,000-byte PBM/RLCD frames to TCP port 8081.\n" + "2. Color TCP Stream (native): Stream packets to TCP port 8083.\n" + " Packet format:\n" + " - Header (16 bytes): Magic 'RAW\\x01' (4 bytes), X-coord (2 bytes BE), Y-coord (2 bytes BE), Width (2 bytes BE), Height (2 bytes BE), Payload length (4 bytes BE).\n" + " - Payload (variable): Raw big-endian RGB565 pixel data of length 'Payload length'.\n" + "Active protocol selection: %s", protocol); + return make_tool_result(id, buf); + } + + if (strcmp(name, "get_stream_stats") == 0) { + std::string stats_json; + if (!mcp::getVideoStreamStats(stats_json)) { + return make_error(id, -32020, "Failed to get stream stats"); + } + return make_tool_result(id, stats_json.c_str()); + } + + if (strcmp(name, "list_apps") == 0) { + std::string apps_json; + std::string error; + if (!mcp::listApps(apps_json, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, apps_json.c_str()); + } + + if (strcmp(name, "run_app") == 0) { + cJSON* app_id_item = cJSON_GetObjectItemCaseSensitive(arguments, "app_id"); + if (!cJSON_IsString(app_id_item)) { + return make_error(id, -32602, "run_app requires app_id"); + } + std::string error; + if (!mcp::runApp(app_id_item->valuestring, error)) { + return make_error(id, -32020, error.c_str()); + } + char msg[128]; + snprintf(msg, sizeof(msg), "Successfully started application '%s'.", app_id_item->valuestring); + return make_tool_result(id, msg); + } + + if (strcmp(name, "list_files") == 0) { + cJSON* path_item = cJSON_GetObjectItemCaseSensitive(arguments, "path"); + const char* path = cJSON_IsString(path_item) ? path_item->valuestring : "/"; + std::string files_json; + std::string error; + if (!mcp::listSdFiles(path, files_json, error)) { + return make_error(id, -32020, error.c_str()); + } + return make_tool_result(id, files_json.c_str()); + } + + return make_error(id, -32602, "Unknown tool"); +} + +static cJSON* handle_rpc(const char* body) { + cJSON* request = cJSON_Parse(body); + if (request == NULL) { + return make_error(NULL, -32700, "Parse error"); + } + + cJSON* id = cJSON_GetObjectItemCaseSensitive(request, "id"); + cJSON* jsonrpc = cJSON_GetObjectItemCaseSensitive(request, "jsonrpc"); + cJSON* method = cJSON_GetObjectItemCaseSensitive(request, "method"); + cJSON* params = cJSON_GetObjectItemCaseSensitive(request, "params"); + + if (!cJSON_IsObject(request) || + !cJSON_IsString(jsonrpc) || + strcmp(jsonrpc->valuestring, "2.0") != 0 || + !cJSON_IsString(method)) { + cJSON* response = make_error(id, -32600, "Invalid Request"); + cJSON_Delete(request); + return response; + } + + cJSON* response = NULL; + if (strcmp(method->valuestring, "tools/list") == 0) { + cJSON* tools = cJSON_Parse(TOOLS_JSON); + if (tools == NULL) { + response = make_error(id, -32603, "Failed to construct tool list"); + } else { + response = cJSON_CreateObject(); + cJSON_AddStringToObject(response, "jsonrpc", "2.0"); + cJSON* result = cJSON_AddObjectToObject(response, "result"); + cJSON_AddItemToObject(result, "tools", tools); + if (id != NULL) { + cJSON_AddItemToObject(response, "id", cJSON_Duplicate(id, true)); + } else { + cJSON_AddNullToObject(response, "id"); + } + } + } else if (strcmp(method->valuestring, "tools/call") == 0) { + response = handle_tool_call(id, params); + } else { + response = make_error(id, -32601, "Method not found"); + } + + cJSON_Delete(request); + return response; +} + +// POST /api/mcp +esp_err_t WebServerService::handleApiMcp(httpd_req_t* request) { + LOG_I(TAG, "POST /api/mcp"); + + // Load MCP Settings and check if enabled + auto mcpSettings = settings::mcp::loadOrGetDefault(); + if (!mcpSettings.mcpEnabled) { + httpd_resp_send_err(request, HTTPD_404_NOT_FOUND, "MCP server is disabled"); + return ESP_FAIL; + } + + int content_len = request->content_len; + if (content_len <= 0 || content_len > MCP_JSON_BODY_LIMIT) { + httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Invalid Content-Length"); + return ESP_FAIL; + } + + char* body = (char*)psram_malloc(content_len + 1); + if (body == NULL) { + httpd_resp_send_err(request, HTTPD_500_INTERNAL_SERVER_ERROR, "Out of memory"); + return ESP_FAIL; + } + + int received = 0; + while (received < content_len) { + int ret = httpd_req_recv(request, body + received, content_len - received); + if (ret <= 0) { + if (ret == HTTPD_SOCK_ERR_TIMEOUT) { + continue; + } + free(body); + httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Incomplete request body"); + return ESP_FAIL; + } + received += ret; + } + body[content_len] = '\0'; + + cJSON* response = handle_rpc(body); + free(body); + + char* response_text = cJSON_PrintUnformatted(response); + cJSON_Delete(response); + + if (response_text == NULL) { + httpd_resp_send_err(request, HTTPD_500_INTERNAL_SERVER_ERROR, "Serialization failed"); + return ESP_FAIL; + } + + httpd_resp_set_type(request, "application/json"); + httpd_resp_set_hdr(request, "Access-Control-Allow-Origin", "*"); + httpd_resp_sendstr(request, response_text); + cJSON_free(response_text); + + return ESP_OK; +} + +static int query_integer(const char* path, const char* name, int default_value) { + const char* query = strchr(path, '?'); + if (query == NULL) { + return default_value; + } + query++; + size_t name_size = strlen(name); + while (*query != '\0') { + if (strncmp(query, name, name_size) == 0 && query[name_size] == '=') { + return atoi(query + name_size + 1); + } + query = strchr(query, '&'); + if (query == NULL) { + break; + } + query++; + } + return default_value; +} + +// POST /api/screen/raw +esp_err_t WebServerService::handleApiScreenRaw(httpd_req_t* request) { + LOG_I(TAG, "POST /api/screen/raw"); + + // Load MCP Settings and check if enabled + auto mcpSettings = settings::mcp::loadOrGetDefault(); + if (!mcpSettings.mcpEnabled) { + httpd_resp_send_err(request, HTTPD_404_NOT_FOUND, "MCP server is disabled"); + return ESP_FAIL; + } + + int content_len = request->content_len; + if (content_len <= 0 || content_len > MCP_RAW_BODY_LIMIT) { + httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Invalid Content-Length"); + return ESP_FAIL; + } + + uint8_t* body = (uint8_t*)psram_malloc(content_len); + if (body == NULL) { + httpd_resp_send_err(request, HTTPD_500_INTERNAL_SERVER_ERROR, "Out of memory"); + return ESP_FAIL; + } + + int received = 0; + while (received < content_len) { + int ret = httpd_req_recv(request, (char*)body + received, content_len - received); + if (ret <= 0) { + if (ret == HTTPD_SOCK_ERR_TIMEOUT) { + continue; + } + free(body); + httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Incomplete request body"); + return ESP_FAIL; + } + received += ret; + } + + auto& state = mcp::getState(); + const char* path = request->uri; + int x = query_integer(path, "x", 0); + int y = query_integer(path, "y", 0); + int width = query_integer(path, "w", state.drawWidth); + int height = query_integer(path, "h", state.drawHeight); + + bool valid_size = width > 0 && height > 0 && + (size_t)width * height * 2 == (size_t)content_len; + bool drawn = valid_size && mcp::drawRgb565( + body, + content_len, + x, + y, + width, + height + ); + free(body); + + if (drawn) { + httpd_resp_set_type(request, "text/plain"); + httpd_resp_set_hdr(request, "Access-Control-Allow-Origin", "*"); + httpd_resp_sendstr(request, "OK"); + return ESP_OK; + } else { + httpd_resp_send_err(request, HTTPD_400_BAD_REQUEST, "Invalid RGB565 payload or size"); + return ESP_FAIL; + } +} + +} // namespace tt::service::webserver + +#endif diff --git a/Tactility/Source/service/webserver/WebServerService.cpp b/Tactility/Source/service/webserver/WebServerService.cpp index 12f2de64..69e62595 100644 --- a/Tactility/Source/service/webserver/WebServerService.cpp +++ b/Tactility/Source/service/webserver/WebServerService.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include #include @@ -215,7 +217,8 @@ bool WebServerService::onStart(ServiceContext& service) { lock.lock(); g_cachedSettings = settings::webserver::loadOrGetDefault(); g_settingsCached = true; - serverEnabled = g_cachedSettings.webServerEnabled; + auto mcpSettings = settings::mcp::loadOrGetDefault(); + serverEnabled = g_cachedSettings.webServerEnabled || mcpSettings.mcpEnabled; } // Subscribe to settings change events to refresh cache settingsEventSubscription = pubsub->subscribe([](WebServerEvent event) { @@ -259,13 +262,17 @@ void WebServerService::onStop(ServiceContext& service) { void WebServerService::setEnabled(bool enabled) { auto lock = mutex.asScopedLock(); lock.lock(); - + if (enabled) { if (!httpServer || !httpServer->isStarted()) { startServer(); } } else { - if (httpServer && httpServer->isStarted()) { + // Stop only if both web server and MCP are disabled + auto wsSettings = settings::webserver::loadOrGetDefault(); + auto mcpSettings = settings::mcp::loadOrGetDefault(); + bool anyEnabled = wsSettings.webServerEnabled || mcpSettings.mcpEnabled; + if (!anyEnabled && httpServer && httpServer->isStarted()) { stopServer(); } } @@ -514,6 +521,11 @@ bool WebServerService::startServer() { LOG_I(TAG, "HTTP server started successfully on port %u", (unsigned)settings.webServerPort); publish_event(this, WebServerEvent::WebServerStarted); + auto mcpSettings = settings::mcp::loadOrGetDefault(); + if (mcpSettings.mcpEnabled) { + mcp::startVideoStreamServer(); + } + // Show statusbar icon if (statusbarIconId >= 0) { lvgl::statusbar_icon_set_image(statusbarIconId, LVGL_ICON_STATUSBAR_CLOUD); @@ -533,6 +545,8 @@ void WebServerService::stopServer() { httpServer->stop(); httpServer.reset(); + mcp::stopVideoStreamServer(); + // Stop AP mode WiFi if we started it if (apWifiInitialized || apNetif != nullptr) { stopApMode(); @@ -1025,15 +1039,24 @@ esp_err_t WebServerService::handleApiGet(httpd_req_t* request) { return ESP_FAIL; } -// API POST dispatcher - all POST endpoints require authentication +// API POST dispatcher - all POST endpoints require authentication except MCP esp_err_t WebServerService::handleApiPost(httpd_req_t* request) { + const char* uri = request->uri; + + // MCP endpoints are unauthenticated (local network) + if (strncmp(uri, "/api/mcp", 8) == 0) { + return handleApiMcp(request); + } + if (strncmp(uri, "/api/screen/raw", 15) == 0) { + return handleApiScreenRaw(request); + } + bool authPassed = false; esp_err_t authResult = validateRequestAuth(request, authPassed); if (!authPassed) { return authResult; } - const char* uri = request->uri; if (strncmp(uri, "/api/apps/run", 13) == 0) { return handleApiAppsRun(request); } diff --git a/Tactility/Source/settings/DisplaySettings.cpp b/Tactility/Source/settings/DisplaySettings.cpp index 4f783b80..5484f957 100644 --- a/Tactility/Source/settings/DisplaySettings.cpp +++ b/Tactility/Source/settings/DisplaySettings.cpp @@ -63,6 +63,8 @@ static std::string toString(ScreensaverType type) { case Mystify: return "Mystify"; case MatrixRain: return "MatrixRain"; case StackChan: return "StackChan"; + case McpScreen: return "McpScreen"; + case Count: return "None"; default: std::unreachable(); } } @@ -73,6 +75,7 @@ static bool fromString(const std::string& str, ScreensaverType& type) { else if (str == "Mystify") { type = ScreensaverType::Mystify; return true; } else if (str == "MatrixRain") { type = ScreensaverType::MatrixRain; return true; } else if (str == "StackChan") { type = ScreensaverType::StackChan; return true; } + else if (str == "McpScreen") { type = ScreensaverType::McpScreen; return true; } else { return false; } } diff --git a/Tactility/Source/settings/McpSettings.cpp b/Tactility/Source/settings/McpSettings.cpp new file mode 100644 index 00000000..7f4d0f02 --- /dev/null +++ b/Tactility/Source/settings/McpSettings.cpp @@ -0,0 +1,72 @@ +#include +#include +#include +#include + +constexpr auto* TAG = "McpSettings"; +#include +#include +#include + +namespace tt::settings::mcp { + + +static std::string getSettingsFilePath() { + return getUserDataPath() + "/settings/mcp.properties"; +} + +constexpr auto* KEY_MCP_ENABLED = "mcpEnabled"; + +bool load(McpSettings& settings) { + auto settings_path = getSettingsFilePath(); + if (!file::isFile(settings_path)) { + return false; + } + std::map map; + if (!file::loadPropertiesFile(settings_path, map)) { + return false; + } + + auto mcp_enabled = map.find(KEY_MCP_ENABLED); + settings.mcpEnabled = (mcp_enabled != map.end()) + ? (mcp_enabled->second == "1" || mcp_enabled->second == "true") + : false; + + return true; +} + +McpSettings getDefault() { + return McpSettings{ + .mcpEnabled = false + }; +} + +McpSettings loadOrGetDefault() { + McpSettings settings; + if (!load(settings)) { + settings = getDefault(); + if (!save(settings)) { + LOG_W(TAG, "Failed to save default MCP settings"); + } + } + return settings; +} + +bool save(const McpSettings& settings) { + std::map map; + map[KEY_MCP_ENABLED] = settings.mcpEnabled ? "true" : "false"; + + auto settings_path = getSettingsFilePath(); + if (!file::findOrCreateParentDirectory(settings_path, 0755)) { + LOG_E(TAG, "Failed to create parent dir for %s", settings_path.c_str()); + return false; + } + + if (!file::savePropertiesFile(settings_path, map)) { + LOG_E(TAG, "Failed to save MCP settings to %s", settings_path.c_str()); + return false; + } + return true; +} + +} // namespace