From 6a49e8ad9c9ae1319696db42ef789a226774cee7 Mon Sep 17 00:00:00 2001 From: Alberto Mardegan Date: Sun, 8 Feb 2026 13:04:06 +0300 Subject: [PATCH 1/2] WPAD2: new library for Wiimote support --- .gitmodules | 0 CMakeLists.txt | 12 + wpad2/CMakeLists.txt | 19 + wpad2/classic.c | 162 +++++ wpad2/classic.h | 44 ++ wpad2/device.c | 698 +++++++++++++++++++++ wpad2/device.h | 171 ++++++ wpad2/dynamics.c | 370 +++++++++++ wpad2/dynamics.h | 9 + wpad2/event.c | 198 ++++++ wpad2/event.h | 18 + wpad2/guitar_hero_3.c | 106 ++++ wpad2/guitar_hero_3.h | 57 ++ wpad2/internals.h | 71 +++ wpad2/ir.c | 794 ++++++++++++++++++++++++ wpad2/ir.h | 17 + wpad2/motion_plus.c | 123 ++++ wpad2/motion_plus.h | 21 + wpad2/nunchuk.c | 117 ++++ wpad2/nunchuk.h | 10 + wpad2/speaker.c | 195 ++++++ wpad2/speaker.h | 25 + wpad2/wii_board.c | 68 +++ wpad2/wii_board.h | 43 ++ wpad2/wiiuse_internal.h | 278 +++++++++ wpad2/worker.c | 1283 +++++++++++++++++++++++++++++++++++++++ wpad2/worker.h | 59 ++ wpad2/wpad.c | 905 +++++++++++++++++++++++++++ 28 files changed, 5873 insertions(+) create mode 100644 .gitmodules create mode 100644 wpad2/CMakeLists.txt create mode 100644 wpad2/classic.c create mode 100644 wpad2/classic.h create mode 100644 wpad2/device.c create mode 100644 wpad2/device.h create mode 100644 wpad2/dynamics.c create mode 100644 wpad2/dynamics.h create mode 100644 wpad2/event.c create mode 100644 wpad2/event.h create mode 100644 wpad2/guitar_hero_3.c create mode 100644 wpad2/guitar_hero_3.h create mode 100644 wpad2/internals.h create mode 100644 wpad2/ir.c create mode 100644 wpad2/ir.h create mode 100644 wpad2/motion_plus.c create mode 100644 wpad2/motion_plus.h create mode 100644 wpad2/nunchuk.c create mode 100644 wpad2/nunchuk.h create mode 100644 wpad2/speaker.c create mode 100644 wpad2/speaker.h create mode 100644 wpad2/wii_board.c create mode 100644 wpad2/wii_board.h create mode 100644 wpad2/wiiuse_internal.h create mode 100644 wpad2/worker.c create mode 100644 wpad2/worker.h create mode 100644 wpad2/wpad.c diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..e69de29bb diff --git a/CMakeLists.txt b/CMakeLists.txt index 76ec888f6..249320b05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,18 @@ add_subdirectory(libiso9660) if(NINTENDO_GAMECUBE) add_subdirectory(lwip) elseif(NINTENDO_WII) + set(WITH_OPENOBEX OFF CACHE BOOL "Build with OpenOBEX" FORCE) + include_directories(gc) + if (NOT EXISTS "${CMAKE_SOURCE_DIR}/bt-embedded/CMakeLists.txt") + include(FetchContent) + FetchContent_Populate(BtEmbedded + GIT_REPOSITORY https://github.com/embedded-game-controller/bt-embedded.git + GIT_TAG fa0a213eab010b323958822b1f87e89823328b8e + SOURCE_DIR ${CMAKE_SOURCE_DIR}/bt-embedded + ) + endif() + add_subdirectory(bt-embedded) + add_subdirectory(wpad2) add_subdirectory(lwbt) add_subdirectory(wiiuse) add_subdirectory(libdi) diff --git a/wpad2/CMakeLists.txt b/wpad2/CMakeLists.txt new file mode 100644 index 000000000..795d9fe55 --- /dev/null +++ b/wpad2/CMakeLists.txt @@ -0,0 +1,19 @@ +add_library(wpad2 STATIC + classic.c + device.c + dynamics.c + event.c + guitar_hero_3.c + ir.c + motion_plus.c + nunchuk.c + speaker.c + wii_board.c + worker.c + wpad.c +) + +target_include_directories(wpad2 PRIVATE ../bt-embedded) +target_link_libraries(wpad2 PRIVATE libogc_inc) + +libogc_install_lib(wpad2) diff --git a/wpad2/classic.c b/wpad2/classic.c new file mode 100644 index 000000000..41e9e6c66 --- /dev/null +++ b/wpad2/classic.c @@ -0,0 +1,162 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/classic.c,v 1.7 2008-11-14 13:34:57 shagkur Exp $ + * + */ + +/** + * @file + * @brief Classic controller expansion device. + */ + +#include "classic.h" + +#include "dynamics.h" + +#include +#include +#include +#include + +static void fix_bad_calibration_values(struct joystick_t *js, short right_stick) { + if ((js->min.x >= js->center.x) || (js->max.x <= js->center.x)) { + js->min.x = 0; + js->max.x = right_stick ? 32 : 64; + js->center.x = right_stick ? 16 : 32; + } + if ((js->min.y >= js->center.y) || (js->max.y <= js->center.y)) { + js->min.y = 0; + js->max.y = right_stick ? 32 : 64; + js->center.y = right_stick ? 16 : 32; + } +} + +/** + * @brief Find what buttons are pressed. + * + * @param cc A pointer to a classic_ctrl_t structure. + * @param msg The message byte specified in the event packet. + */ +static void classic_ctrl_pressed_buttons(struct classic_ctrl_t *cc, const uint8_t *now) { + u32 buttons = (now[0] << 0x8) | now[1]; + + if (cc->type == CLASSIC_TYPE_WIIU) { + /* append Wii U Pro Controller stick buttons to top 16 bits */ + buttons |= (now[2] << 0x10); + + /* message is inverted (0 is active, 1 is inactive) */ + buttons = ~buttons & WII_U_PRO_CTRL_BUTTON_ALL; + } else { + /* message is inverted (0 is active, 1 is inactive) */ + buttons = ~buttons & CLASSIC_CTRL_BUTTON_ALL; + } + + /* preserve old btns pressed */ + cc->btns_last = cc->btns; + + /* pressed now & were pressed, then held */ + cc->btns_held = (buttons & cc->btns); + + /* were pressed or were held & not pressed now, then released */ + cc->btns_released = ((cc->btns | cc->btns_held) & ~buttons); + + /* buttons pressed now */ + cc->btns = buttons; +} + +void _wpad2_classic_calibrate(struct classic_ctrl_t *cc, + const WpadDeviceExpCalibrationData *cd) +{ + const uint8_t *data = cd->data; + + cc->btns = 0; + cc->btns_held = 0; + cc->btns_released = 0; + + /* is this a wiiu pro? */ + if (cc->type == CLASSIC_TYPE_WIIU) { + cc->ljs.max.x = cc->ljs.max.y = 208; + cc->ljs.min.x = cc->ljs.min.y = 48; + cc->ljs.center.x = cc->ljs.center.y = 0x80; + + cc->rjs = cc->ljs; + } else { + /* joystick stuff */ + cc->ljs.max.x = data[0] / 4 == 0 ? 64 : data[0] / 4; + cc->ljs.min.x = data[1] / 4; + cc->ljs.center.x = data[2] / 4 == 0 ? 32 : data[2] / 4; + cc->ljs.max.y = data[3] / 4 == 0 ? 64 : data[3] / 4; + cc->ljs.min.y = data[4] / 4; + cc->ljs.center.y = data[5] / 4 == 0 ? 32 : data[5] / 4; + + cc->rjs.max.x = data[6] / 8 == 0 ? 32 : data[6] / 8; + cc->rjs.min.x = data[7] / 8; + cc->rjs.center.x = data[8] / 8 == 0 ? 16 : data[8] / 8; + cc->rjs.max.y = data[9] / 8 == 0 ? 32 : data[9] / 8; + cc->rjs.min.y = data[10] / 8; + cc->rjs.center.y = data[11] / 8 == 0 ? 16 : data[11] / 8; + + fix_bad_calibration_values(&cc->ljs, 0); + fix_bad_calibration_values(&cc->rjs, 1); + } +} + +/** + * @brief Handle classic controller event. + * + * @param cc A pointer to a classic_ctrl_t structure. + * @param msg The message specified in the event packet. + */ +void _wpad2_classic_event(struct classic_ctrl_t *cc, const uint8_t *msg) { + if (cc->type == CLASSIC_TYPE_WIIU) { + classic_ctrl_pressed_buttons(cc, msg + 8); + + /* 12-bit little endian values adjusted to 8-bit */ + cc->ljs.pos.x = (msg[0] >> 4) | (msg[1] << 4); + cc->rjs.pos.x = (msg[2] >> 4) | (msg[3] << 4); + cc->ljs.pos.y = (msg[4] >> 4) | (msg[5] << 4); + cc->rjs.pos.y = (msg[6] >> 4) | (msg[7] << 4); + + cc->ls_raw = cc->btns & CLASSIC_CTRL_BUTTON_FULL_L ? 0x1F : 0; + cc->rs_raw = cc->btns & CLASSIC_CTRL_BUTTON_FULL_R ? 0x1F : 0; + + /* Wii U pro controller specific data */ + cc->charging = !(((msg[10] & WII_U_PRO_CTRL_CHARGING) >> 2) & 1); + cc->wired = !(((msg[10] & WII_U_PRO_CTRL_WIRED) >> 3) & 1); + cc->battery = ((msg[10] & WII_U_PRO_CTRL_BATTERY) >> 4) & 7; + } else { + classic_ctrl_pressed_buttons(cc, msg + 4); + + /* left/right triggers */ + cc->ls_raw = (((msg[2] & 0x60) >> 2) | ((msg[3] & 0xE0) >> 5)); + cc->rs_raw = (msg[3] & 0x1F); + + /* calculate joystick orientation */ + cc->ljs.pos.x = (msg[0] & 0x3F); + cc->ljs.pos.y = (msg[1] & 0x3F); + cc->rjs.pos.x = ((msg[0] & 0xC0) >> 3) | ((msg[1] & 0xC0) >> 5) | ((msg[2] & 0x80) >> 7); + cc->rjs.pos.y = (msg[2] & 0x1F); + } +} diff --git a/wpad2/classic.h b/wpad2/classic.h new file mode 100644 index 000000000..7fe5c8940 --- /dev/null +++ b/wpad2/classic.h @@ -0,0 +1,44 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/classic.h,v 1.1 2008-05-08 09:42:14 shagkur Exp $ + * + */ + +/** + * @file + * @brief Classic controller expansion device. + */ + +#ifndef WPAD2_CLASSIC_H +#define WPAD2_CLASSIC_H + +#include "device.h" + +void _wpad2_classic_calibrate(struct classic_ctrl_t *cc, + const WpadDeviceExpCalibrationData *cd); + +void _wpad2_classic_event(struct classic_ctrl_t *cc, const uint8_t *msg); + +#endif /* WPAD2_CLASSIC_H */ diff --git a/wpad2/device.c b/wpad2/device.c new file mode 100644 index 000000000..fb7cf7121 --- /dev/null +++ b/wpad2/device.c @@ -0,0 +1,698 @@ +/*------------------------------------------------------------- + +Copyright (C) 2008-2026 +Michael Wiedenbauer (shagkur) +Dave Murphy (WinterMute) +Hector Martin (marcan) +Zarithya +Alberto Mardegan (mardy) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you +must not claim that you wrote the original software. If you use +this software in a product, an acknowledgment in the product +documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +-------------------------------------------------------------*/ + +#include "device.h" + +#include "classic.h" +#include "ir.h" +#include "motion_plus.h" +#include "nunchuk.h" +#include "speaker.h" + +#include "bt-embedded/services/hid.h" + +#include +#include + +WpadDevice _wpad2_devices[WPAD2_MAX_DEVICES]; + +static Wpad2DeviceCalibrationCb s_device_calibration_cb; +static Wpad2DeviceExpCalibrationCb s_device_exp_calibration_cb; +static Wpad2DeviceExpDisconnectedCb s_device_exp_disconnected_cb; +static Wpad2DeviceReadyCb s_device_ready_cb; +static Wpad2DeviceStatusCb s_device_status_cb; +static Wpad2DeviceReportCb s_device_report_cb; +static Wpad2DeviceTimerHandlerCb s_timer_handler_cb; + +static bool device_io_write(WpadDevice *device, uint8_t *data, int len) +{ + if (!device->hid_intr) return false; + + BteBufferWriter writer; + bool ok = bte_l2cap_create_message(device->hid_intr, &writer, len + 1); + if (!ok) return false; + + uint8_t *buf = bte_buffer_writer_ptr_n(&writer, len + 1); + buf[0] = BTE_HID_TRANS_DATA | BTE_HID_REP_TYPE_OUTPUT; + memcpy(buf + 1, data, len); + int rc = bte_l2cap_send_message(device->hid_intr, + bte_buffer_writer_end(&writer)); + return rc >= 0; +} + +static bool device_send_command(WpadDevice *device, uint8_t *data, int len) +{ + if (device->rumble) data[1] |= 0x01; + return device_io_write(device, data, len); +} + +bool _wpad2_device_send_command(WpadDevice *device, uint8_t report_type, + uint8_t *data, int len) +{ + /* TODO: optimize this, avoid memcpy */ + uint8_t cmd[48]; + cmd[0] = report_type; + memcpy(cmd + 1, data, len); + if (report_type != WM_CMD_READ_DATA && + report_type != WM_CMD_CTRL_STATUS && + report_type != WM_CMD_REPORT_TYPE) { + cmd[1] |= 0x02; /* ACK requested */ + } + WPAD2_DEBUG("Pushing command: %02x %02x", cmd[0], cmd[1]); + return device_send_command(device, cmd, len + 1); +} + +bool _wpad2_device_request_status(WpadDevice *device) +{ + uint8_t buf = 0; + return _wpad2_device_send_command(device, WM_CMD_CTRL_STATUS, &buf, 1); +} + +static void event_data_read_completed(WpadDevice *device) +{ + if (device->last_read_data) { + free(device->last_read_data); + device->last_read_data = NULL; + } + /* This marks the read as completed */ + device->last_read_size = 0; +} + +static inline bool has_pending_read(WpadDevice *device) +{ + return device->last_read_size != 0; +} + +bool _wpad2_device_read_data(WpadDevice *device, uint32_t offset, uint16_t size) +{ + uint8_t msg[1 + 4 + 2]; + + WPAD2_DEBUG("offset %08x, size %d, pending read = %d", offset, size, has_pending_read(device)); + + /* Only one read at a time! */ + if (has_pending_read(device)) return false; + + msg[0] = WM_CMD_READ_DATA; + write_be32(offset, msg + 1); + write_be16(size, msg + 1 + 4); + bool ok = device_send_command(device, msg, sizeof(msg)); + if (ok) { + device->last_read_offset = offset; + device->last_read_size = size; + device->last_read_cursor = 0; + device->last_read_data = size > 16 ? malloc(size) : NULL; + } + return ok; +} + +bool _wpad2_device_write_data(WpadDevice *device, uint32_t offset, + const void *data, uint8_t size) +{ + uint8_t msg[1 + 4 + 1 + 16]; + + if (size > 16) return false; + + WPAD2_DEBUG("pending writes %d, offset %08x, size %d", device->num_pending_writes, offset, size); + msg[0] = WM_CMD_WRITE_DATA; + uint8_t *ptr = msg + 1; + write_be32(offset, ptr); + ptr += 4; + ptr[0] = size; + ptr++; + memcpy(ptr, data, size); + ptr += size; + memset(ptr, 0, 16 - size); + if (!device_send_command(device, msg, sizeof(msg))) return false; + + device->num_pending_writes++; + return true; +} + +bool _wpad2_device_stream_data(WpadDevice *device, const void *data, uint8_t size) +{ + uint8_t msg[1 + 1 + 20]; + + if (size > 20) return false; + + WPAD2_DEBUG("size %d", size); + msg[0] = WM_CMD_STREAM_DATA; + msg[1] = size << 3; + uint8_t *ptr = msg + 2; + memcpy(ptr, data, size); + ptr += size; + memset(ptr, 0, 20 - size); + if (!device_send_command(device, msg, sizeof(msg))) return false; + + return true; +} + +static bool device_set_report_type(WpadDevice *device) +{ + uint8_t buf[2]; + buf[0] = device->continuous ? 0x04 : 0x00; + + bool motion = device->accel_requested || device->ir_requested; + bool exp = device->exp_attached; + bool ir = device->ir_requested; + + if (motion && ir && exp) buf[1] = WM_RPT_BTN_ACC_IR_EXP; + else if (motion && exp) buf[1] = WM_RPT_BTN_ACC_EXP; + else if (motion && ir) buf[1] = WM_RPT_BTN_ACC_IR; + else if (ir && exp) buf[1] = WM_RPT_BTN_IR_EXP; + else if (ir) buf[1] = WM_RPT_BTN_ACC_IR; + else if (exp) buf[1] = WM_RPT_BTN_EXP; + else if (motion) buf[1] = WM_RPT_BTN_ACC; + else buf[1] = WM_RPT_BTN; + + if (buf[1] == device->report_type) return false; + + WPAD2_DEBUG("Setting report type: 0x%02x on device %d", buf[1], device->unid); + device->report_type = buf[1]; + + return _wpad2_device_send_command(device, WM_CMD_REPORT_TYPE, buf, 2); +} + +bool _wpad2_device_set_leds(WpadDevice *device, int leds) +{ + uint8_t buf; + leds &= 0xf0; + buf = leds; + return _wpad2_device_send_command(device, WM_CMD_LED, &buf, 1); +} + +void _wpad2_device_expansion_ready(WpadDevice *device) +{ + WPAD2_DEBUG("wiimote %d, expansion %d", _wpad2_device_get_slot(device), device->exp_type); + device->state = STATE_READY; + _wpad2_device_step(device); +} + +static bool device_expansion_failed(WpadDevice *device) +{ + WPAD2_WARNING("Expansion handshake failed for wiimote %d", + _wpad2_device_get_slot(device)); + device->exp_type = EXP_FAILED; + return false; +} + +static bool device_expansion_disable(WpadDevice *device) +{ + if (device->exp_type == EXP_NONE) return false; + + device->state = STATE_READY; + device->exp_type = EXP_NONE; + + s_device_exp_disconnected_cb(device); + return false; +} + +/* Returns true if a step was performed */ +static bool device_expansion_step(WpadDevice *device) +{ + uint8_t val; + + WPAD2_DEBUG("state: %d", device->state); + if (!device->exp_attached) { + return device_expansion_disable(device); + } + + switch (device->state) { + case STATE_EXP_ATTACHED: + device->state = STATE_EXP_ENABLE_1; + val = 0x55; + _wpad2_device_write_data(device, WM_EXP_MEM_ENABLE1, &val, 1); + break; + case STATE_EXP_ENABLE_1: + device->state = STATE_EXP_ENABLE_2; + val = 0x0; + _wpad2_device_write_data(device, WM_EXP_MEM_ENABLE2, &val, 1); + break; + case STATE_EXP_ENABLE_2: + if (_wpad2_device_read_data(device, WM_EXP_ID, 6)) { + device->state = STATE_EXP_IDENTIFICATION; + } + break; + case STATE_EXP_IDENTIFICATION: + if (_wpad2_device_read_data(device, WM_EXP_MEM_CALIBR, + EXP_CALIBRATION_DATA_LEN * 2)) { + device->state = STATE_EXP_READ_CALIBRATION; + } + break; + case STATE_EXP_READ_CALIBRATION: + _wpad2_device_expansion_ready(device); + break; + default: + return false; + } + return true; +} + +static bool device_expansion_calibrate( + WpadDevice *device, const uint8_t *data, uint16_t len) +{ + if (len < EXP_CALIBRATION_DATA_LEN * 2) { + return device_expansion_failed(device); + } + + if (data[0] == 0xff) { + /* This calibration data is invalid, let's try the backup copy */ + data += EXP_CALIBRATION_DATA_LEN; + } + + if (data[0] == 0xff) { + return device_expansion_failed(device); + } + + /* Send the calibration data to the client */ + s_device_exp_calibration_cb(device, (const WpadDeviceExpCalibrationData *)data); + + return false; +} + +void _wpad2_device_set_data_format(WpadDevice *device, uint8_t format) +{ + switch (format) { + case WPAD_FMT_BTNS: + device->accel_requested = false; + device->ir_requested = false; + break; + case WPAD_FMT_BTNS_ACC: + device->accel_requested = true; + device->ir_requested = false; + break; + case WPAD_FMT_BTNS_ACC_IR: + device->accel_requested = true; + device->ir_requested = true; + break; + default: + return; + } + + device->continuous = device->accel_requested; + if (device->initialized) _wpad2_device_step(device); +} + +static bool device_handshake_completed(WpadDevice *device) +{ + device->state = STATE_HANDSHAKE_COMPLETE; + WPAD2_DEBUG("state: %d", device->state); + + int8_t chan = device->unid; + bool active = _wpad2_device_set_leds(device, WIIMOTE_LED_1 << (chan % 4)); + + device->initialized = true; + s_device_ready_cb(device); + return active; +} + +static bool device_handshake_read_calibration(WpadDevice *device) +{ + device->state = STATE_HANDSHAKE_READ_CALIBRATION; + return _wpad2_device_read_data(device, WM_MEM_OFFSET_CALIBRATION, 8); +} + +static bool device_handshake_calibrated(WpadDevice *device, + const uint8_t *data, uint16_t len) +{ + if (len != 8) return false; + s_device_calibration_cb(device, (const WpadDeviceCalibrationData *)data); + + return device_handshake_completed(device); +} + +static bool device_handshake_step(WpadDevice *device) +{ + if (device->initialized) return false; + + bool active = false; + if (device->state == STATE_HANDSHAKE_LEDS_OFF) { + if (device->exp_attached) { + device->state = STATE_EXP_FIRST; + active = device_expansion_step(device); + } + } else if (device->state >= STATE_EXP_FIRST && + device->state <= STATE_EXP_LAST) { + active = device_expansion_step(device); + } + + if (active) return true; + + if (device->state != STATE_HANDSHAKE_READ_CALIBRATION) { + if (device_handshake_read_calibration(device)) return true; + } + + return false; +} + +bool _wpad2_device_step(WpadDevice *device) +{ + if (has_pending_read(device) || device->num_pending_writes > 0) { + WPAD2_DEBUG("state = %d, pending read %d, pending writes = %d", + device->state, has_pending_read(device), device->num_pending_writes); + /* let the pending operations complete first */ + return true; + } + + WPAD2_DEBUG("state = %d", device->state); + + if (device_handshake_step(device)) return true; + + /* First, complete any flow currently in progress */ + if (device->state >= STATE_EXP_FIRST && device->state <= STATE_EXP_LAST) { + if (device_expansion_step(device)) return true; + } else if (device->state >= STATE_IR_FIRST && device->state <= STATE_IR_LAST) { + if (_wpad2_device_ir_step(device)) return true; + } else if (device->state >= STATE_MP_FIRST && device->state <= STATE_MP_LAST) { + if (_wpad2_device_motion_plus_step(device)) return true; + } else if (device->state >= STATE_SPEAKER_FIRST && + device->state <= STATE_SPEAKER_LAST) { + if (_wpad2_device_speaker_step(device)) return true; + } + + if (device->ir_enabled != device->ir_requested) { + device->state = STATE_IR_FIRST; + if (_wpad2_device_ir_step(device)) return true; + } + + if (device->speaker_enabled != device->speaker_requested) { + device->state = STATE_SPEAKER_FIRST; + if (_wpad2_device_speaker_step(device)) return true; + } + + bool exp_setup = device->exp_type != EXP_NONE; + if (device->exp_attached != exp_setup) { + device->state = STATE_EXP_FIRST; + if (device_expansion_step(device)) return true; + } + + if (!device->motion_plus_probed || + (device->motion_plus_available && + device->motion_plus_enabled != device->motion_plus_requested)) { + device->state = STATE_MP_FIRST; + if (_wpad2_device_motion_plus_step(device)) return true; + } + + device->state = STATE_READY; + + return device_set_report_type(device); +} + +bool _wpad2_device_step_failed(WpadDevice *device) +{ + WPAD2_DEBUG(""); + bool active = false; + if (device->state >= STATE_EXP_FIRST && + device->state <= STATE_EXP_LAST) { + device_expansion_failed(device); + } else if (device->state >= STATE_IR_FIRST && + device->state <= STATE_IR_LAST) { + active = _wpad2_device_ir_failed(device); + } else if (device->state >= STATE_MP_FIRST && + device->state <= STATE_MP_LAST) { + active = _wpad2_device_motion_plus_failed(device); + } + + if (!active) { + /* Forget about pending writes */ + device->num_pending_writes = 0; + active = _wpad2_device_request_status(device); + } + + return active; +} + +static void event_status(WpadDevice *device, const uint8_t *msg, uint16_t len) +{ + if (len < 6) return; + + /* TODO; should we send en event for the buttons? */ + + bool critical = false; + bool attachment = false; + bool speaker = false; + bool ir = false; + if (msg[2] & WM_CTRL_STATUS_BYTE1_BATTERY_CRITICAL) critical = true; + if (msg[2] & WM_CTRL_STATUS_BYTE1_ATTACHMENT) attachment = true; + if (msg[2] & WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED) speaker = true; + if (msg[2] & WM_CTRL_STATUS_BYTE1_IR_ENABLED) ir = true; + + WPAD2_DEBUG("Status event, critical = %d, exp = %d, speaker = %d, ir = %d", + critical, attachment, speaker, ir); + uint8_t battery_level = msg[5]; + + if (battery_level != device->battery_level || + critical != device->battery_critical || + speaker != device->speaker_enabled) { + device->battery_level = battery_level; + device->battery_critical = critical; + device->speaker_enabled = speaker; + s_device_status_cb(device); + } + device->ir_enabled = ir; + device->exp_attached = attachment; + /* Reset the report mode, so that it will be recomputed */ + device->report_type = 0; + + _wpad2_device_step(device); +} + +static void parse_extension_type(WpadDevice *device, const uint8_t *data) +{ + uint32_t id = read_be32(data + 2); + switch (id) { + case EXP_ID_CODE_NUNCHUK: + device->exp_type = EXP_NUNCHUK; + break; + case EXP_ID_CODE_GUITAR: + device->exp_type = EXP_GUITAR_HERO_3; + break; + case EXP_ID_CODE_WIIBOARD: + device->exp_type = EXP_WII_BOARD; + break; + case EXP_ID_CODE_CLASSIC_CONTROLLER: + case EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING: + case EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING2: + case EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING3: + case EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC: + case EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC2: + case EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC3: + case EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC4: + case EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC5: + device->exp_type = EXP_CLASSIC; + device->exp_subtype = data[0] == 0 ? CLASSIC_TYPE_ORIG : CLASSIC_TYPE_PRO; + break; + case EXP_ID_CODE_CLASSIC_WIIU_PRO: + device->exp_type = EXP_CLASSIC; + device->exp_subtype = CLASSIC_TYPE_WIIU; + break; + case EXP_ID_CODE_MOTION_PLUS: + device->exp_type = EXP_MOTION_PLUS; + break; + default: + device->exp_type = EXP_NONE; + break; + } +} + +static void event_data_read(WpadDevice *device, + const uint8_t *msg, uint16_t len) +{ + if (len < 6) return; + + /* TODO; should we send an event for the buttons? */ + + uint8_t err = msg[2] & 0x0f; + WPAD2_DEBUG("len %d, err = %d, requested offset %08x req size %d", + len, err, device->last_read_offset, device->last_read_size); + if (err) { + event_data_read_completed(device); + _wpad2_device_step_failed(device); + return; + } + + uint16_t size = (msg[2] >> 4) + 1; + const uint8_t *data = msg + 5; + if (device->last_read_data) { + memcpy(device->last_read_data + device->last_read_cursor, data, size); + device->last_read_cursor += size; + if (device->last_read_cursor < device->last_read_size) { + /* We expect more read events. We'll handle the read when all the + * data has arrived. */ + return; + } + data = device->last_read_data; + size = device->last_read_size; + } + + uint32_t offset = device->last_read_offset; + bool active = false; + if (offset == WM_EXP_MEM_CALIBR) { + active = device_expansion_calibrate(device, data, size); + event_data_read_completed(device); + } else if (offset == WM_MEM_OFFSET_CALIBRATION) { + active = device_handshake_calibrated(device, data, size); + event_data_read_completed(device); + } else if (offset == WM_EXP_ID) { + parse_extension_type(device, data); + event_data_read_completed(device); + if (device->state < STATE_HANDSHAKE_COMPLETE) { + /* Give precedence to complete the wiimote initialization */ + active = device_handshake_read_calibration(device); + } else if (device->exp_type != EXP_MOTION_PLUS) { + active = device_expansion_step(device); + } + } else if (offset == WM_EXP_MOTION_PLUS_MODE) { + active = _wpad2_device_motion_plus_read_mode_cb(device, data, size); + event_data_read_completed(device); + } else { + event_data_read_completed(device); + } + + if (!active) { + _wpad2_device_step(device); + } +} + +static void event_ack(WpadDevice *device, const uint8_t *msg, uint16_t len) +{ + if (len < 4) return; + + uint8_t error = msg[3]; + uint8_t report_type = msg[2]; + WPAD2_DEBUG("reporty type %02x, error %d", report_type, error); + + if (report_type == WM_CMD_WRITE_DATA) { + if (device->num_pending_writes > 0) + device->num_pending_writes--; + } else if (report_type == WM_CMD_STREAM_DATA) { + if (error != 0) _wpad2_speaker_play(device, NULL); + } + + if (error == 0) { + _wpad2_device_step(device); + } else { + _wpad2_device_step_failed(device); + } +} + +void _wpad2_device_event(WpadDevice *device, + const uint8_t *report, uint16_t len) +{ + uint8_t event = report[0]; + const uint8_t *msg = report + 1; + + switch (event) { + case WM_RPT_CTRL_STATUS: + event_status(device, msg, len - 1); + break; + case WM_RPT_READ: + event_data_read(device, msg, len - 1); + break; + case WM_RPT_ACK: + event_ack(device, msg, len - 1); + break; + case WM_RPT_BTN: + case WM_RPT_BTN_ACC: + case WM_RPT_BTN_ACC_IR: + case WM_RPT_BTN_EXP: + case WM_RPT_BTN_ACC_EXP: + case WM_RPT_BTN_IR_EXP: + case WM_RPT_BTN_ACC_IR_EXP: + s_device_report_cb(device, report, len); + break; + default: + WPAD2_DEBUG("Event: %02x, length %d", event, len); + } +} + +void _wpad2_device_set_calibration_cb(Wpad2DeviceCalibrationCb callback) +{ + s_device_calibration_cb = callback; +} + +void _wpad2_device_set_exp_calibration_cb(Wpad2DeviceExpCalibrationCb callback) +{ + s_device_exp_calibration_cb = callback; +} + +void _wpad2_device_set_exp_disconnected_cb( + Wpad2DeviceExpDisconnectedCb callback) +{ + s_device_exp_disconnected_cb = callback; +} + +void _wpad2_device_set_ready_cb(Wpad2DeviceReadyCb callback) +{ + s_device_ready_cb = callback; +} + +void _wpad2_device_set_status_cb(Wpad2DeviceStatusCb callback) +{ + s_device_status_cb = callback; +} + +void _wpad2_device_set_report_cb(Wpad2DeviceReportCb callback) +{ + s_device_report_cb = callback; +} + +void _wpad2_device_set_timer_handler_cb(Wpad2DeviceTimerHandlerCb callback) +{ + s_timer_handler_cb = callback; +} + +void _wpad2_device_timer_event(uint64_t now) +{ + for (int i = 0; i < WPAD2_MAX_DEVICES; i++) { + WpadDevice *device = _wpad2_device_get(i); + if (device->sound) { + _wpad2_speaker_play_part(device); + } + } +} + +void _wpad2_device_set_rumble(WpadDevice *device, bool enable) +{ + WPAD2_DEBUG("enable: %d", enable); + device->rumble = enable; + _wpad2_device_request_status(device); +} + +void _wpad2_device_set_speaker(WpadDevice *device, bool enable) +{ + WPAD2_DEBUG("enable: %d", enable); + device->speaker_requested = enable; + _wpad2_device_step(device); +} + +void _wpad2_device_set_timer(WpadDevice *device, uint32_t period_us) +{ + s_timer_handler_cb(device, period_us); +} diff --git a/wpad2/device.h b/wpad2/device.h new file mode 100644 index 000000000..76675a546 --- /dev/null +++ b/wpad2/device.h @@ -0,0 +1,171 @@ +#ifndef WPAD2_DEVICE_H +#define WPAD2_DEVICE_H + +#include "internals.h" + +#define WPAD2_DEFAULT_SMOOTH_ALPHA 0.3f + +#define EXP_FAILED 0xff + +typedef struct { + uint8_t data[8]; +} WpadDeviceCalibrationData; + +typedef struct { + uint8_t data[EXP_CALIBRATION_DATA_LEN]; +} WpadDeviceExpCalibrationData; + +typedef struct { + int len; + int offset; + uint8_t samples[]; +} WpadSoundInfo; + +typedef enum { + STATE_HANDSHAKE_LEDS_OFF, + STATE_HANDSHAKE_READ_CALIBRATION, + STATE_HANDSHAKE_COMPLETE, + + STATE_EXP_FIRST = 10, + STATE_EXP_ATTACHED = STATE_EXP_FIRST, + /* These next two steps are for disabling encryption */ + STATE_EXP_ENABLE_1, + STATE_EXP_ENABLE_2, + STATE_EXP_IDENTIFICATION, + STATE_EXP_READ_CALIBRATION, + STATE_EXP_READY, + STATE_EXP_LAST = STATE_EXP_READY, + + /* IR states */ + STATE_IR_FIRST = 20, + STATE_IR_IDLE = STATE_IR_FIRST, + STATE_IR_ENABLING_1, + STATE_IR_ENABLING_2, + STATE_IR_SENSITIVITY_1, + STATE_IR_SENSITIVITY_2, + STATE_IR_SENSITIVITY_3, + STATE_IR_SET_MODE, + STATE_IR_LAST = STATE_IR_SET_MODE, + + /* Motion Plus states: */ + STATE_MP_FIRST = 30, + STATE_MP_PROBE = STATE_MP_FIRST, + STATE_MP_INITIALIZING, + STATE_MP_ENABLING, + STATE_MP_IDENTIFICATION, + STATE_MP_DISABLING_1, + STATE_MP_DISABLING_2, + STATE_MP_LAST = STATE_MP_DISABLING_2, + + STATE_SPEAKER_FIRST = 40, + STATE_SPEAKER_ENABLING_1, + STATE_SPEAKER_ENABLING_2, + STATE_SPEAKER_ENABLING_3, + STATE_SPEAKER_ENABLING_4, + STATE_SPEAKER_ENABLING_5, + STATE_SPEAKER_ENABLING_6, + STATE_SPEAKER_DISABLING, + STATE_SPEAKER_LAST = STATE_SPEAKER_DISABLING, + + STATE_READY = 50, +} WpadDeviceState; + +typedef struct wpad2_device_t WpadDevice; + +struct wpad2_device_t { + BteBdAddr address; + BteL2cap *hid_ctrl; + BteL2cap *hid_intr; + int unid; /* TODO: figure out if needed */ + WpadDeviceState state; + uint8_t battery_level; + uint8_t speaker_volume; + bool initialized : 1; + bool exp_attached : 1; + bool continuous : 1; + bool rumble : 1; + bool battery_critical : 1; + bool ir_enabled : 1; + bool ir_requested : 1; + bool accel_enabled : 1; + bool accel_requested : 1; + bool speaker_enabled : 1; + bool speaker_requested : 1; + bool motion_plus_probed : 1; + bool motion_plus_available : 1; + bool motion_plus_enabled : 1; + bool motion_plus_requested : 1; + unsigned ir_sensor_level : 3; + uint8_t exp_type; + uint8_t exp_subtype; + uint8_t num_pending_writes; + uint8_t report_type; + uint32_t last_read_offset; + uint16_t last_read_size; + uint16_t last_read_cursor; + uint8_t *last_read_data; /* NULL if data is less than 16 bytes */ + WpadSoundInfo *sound; +}; + +extern WpadDevice _wpad2_devices[WPAD2_MAX_DEVICES]; + +static inline WpadDevice *_wpad2_device_get(int slot) +{ + return &_wpad2_devices[slot]; +} + +static inline int _wpad2_device_get_slot(const WpadDevice *device) +{ + return device ? (device - _wpad2_devices) : -1; +} + +bool _wpad2_device_send_command(WpadDevice *device, uint8_t report_type, + uint8_t *data, int len); +bool _wpad2_device_read_data(WpadDevice *device, uint32_t offset, uint16_t size); +bool _wpad2_device_write_data(WpadDevice *device, uint32_t offset, + const void *data, uint8_t size); +bool _wpad2_device_stream_data(WpadDevice *device, const void *data, uint8_t size); +bool _wpad2_device_request_status(WpadDevice *device); +bool _wpad2_device_step(WpadDevice *device); +bool _wpad2_device_step_failed(WpadDevice *device); + +/* Initialization-related functions */ +void _wpad2_device_expansion_ready(WpadDevice *device); + +/* Event reporting */ +void _wpad2_device_event(WpadDevice *device, + const uint8_t *report, uint16_t len); + +typedef void (*Wpad2DeviceCalibrationCb)( + WpadDevice *device, const WpadDeviceCalibrationData *data); +void _wpad2_device_set_calibration_cb(Wpad2DeviceCalibrationCb callback); + +typedef void (*Wpad2DeviceExpCalibrationCb)( + WpadDevice *device, const WpadDeviceExpCalibrationData *data); +void _wpad2_device_set_exp_calibration_cb(Wpad2DeviceExpCalibrationCb callback); + +typedef void (*Wpad2DeviceExpDisconnectedCb)(WpadDevice *device); +void _wpad2_device_set_exp_disconnected_cb( + Wpad2DeviceExpDisconnectedCb callback); + +typedef void (*Wpad2DeviceReadyCb)(WpadDevice *device); +void _wpad2_device_set_ready_cb(Wpad2DeviceReadyCb callback); + +typedef void (*Wpad2DeviceStatusCb)(WpadDevice *device); +void _wpad2_device_set_status_cb(Wpad2DeviceStatusCb callback); + +typedef void (*Wpad2DeviceReportCb)( + WpadDevice *device, const uint8_t *data, uint8_t len); +void _wpad2_device_set_report_cb(Wpad2DeviceReportCb callback); + +typedef void (*Wpad2DeviceTimerHandlerCb)(WpadDevice *device, uint32_t period_us); +void _wpad2_device_set_timer_handler_cb(Wpad2DeviceTimerHandlerCb callback); +void _wpad2_device_timer_event(uint64_t now); + +bool _wpad2_device_set_leds(WpadDevice *device, int leds); +void _wpad2_device_set_data_format(WpadDevice *device, uint8_t format); +void _wpad2_device_set_rumble(WpadDevice *device, bool enable); +void _wpad2_device_set_speaker(WpadDevice *device, bool enable); +void _wpad2_device_set_timer(WpadDevice *device, uint32_t period_us); + +#endif /* WPAD2_DEVICE_H */ diff --git a/wpad2/dynamics.c b/wpad2/dynamics.c new file mode 100644 index 000000000..424c0771e --- /dev/null +++ b/wpad2/dynamics.c @@ -0,0 +1,370 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/dynamics.c,v 1.2 2008-11-14 13:34:57 shagkur Exp $ + * + */ + +/** + * @file + * @brief Handles the dynamics of the wiimote. + * + * The file includes functions that handle the dynamics + * of the wiimote. Such dynamics include orientation and + * motion sensing. + */ + +#include "dynamics.h" + +#include "guitar_hero_3.h" +#include "ir.h" + +#include +#include +#include + +static void apply_smoothing(struct accel_t *ac, struct orient_t *orient, int type) { + switch (type) { + case SMOOTH_ROLL: + { + /* it's possible last iteration was nan or inf, so set it to 0 if that happened */ + if (isnan(ac->st_roll) || isinf(ac->st_roll)) + ac->st_roll = 0.0f; + + /* + * If the sign changes (which will happen if going from -180 to 180) + * or from (-1 to 1) then don't smooth, just use the new angle. + */ + if (((ac->st_roll < 0) && (orient->roll > 0)) || ((ac->st_roll > 0) && (orient->roll < 0))) { + ac->st_roll = orient->roll; + } else { + orient->roll = ac->st_roll + (ac->st_alpha * (orient->a_roll - ac->st_roll)); + ac->st_roll = orient->roll; + } + + return; + } + + case SMOOTH_PITCH: + { + if (isnan(ac->st_pitch) || isinf(ac->st_pitch)) + ac->st_pitch = 0.0f; + + if (((ac->st_pitch < 0) && (orient->pitch > 0)) || ((ac->st_pitch > 0) && (orient->pitch < 0))) { + ac->st_pitch = orient->pitch; + } else { + orient->pitch = ac->st_pitch + (ac->st_alpha * (orient->a_pitch - ac->st_pitch)); + ac->st_pitch = orient->pitch; + } + + return; + } + } +} + +/** + * @brief Calculate the roll, pitch, yaw. + * + * @param ac An accelerometer (accel_t) structure. + * @param accel [in] Pointer to a vec3w_t structure that holds the raw acceleration data. + * @param orient [out] Pointer to a orient_t structure that will hold the orientation data. + * @param rorient [out] Pointer to a orient_t structure that will hold the non-smoothed orientation data. + * @param smooth If smoothing should be performed on the angles calculated. 1 to enable, 0 to disable. + * + * Given the raw acceleration data from the accelerometer struct, calculate + * the orientation of the device and set it in the \a orient parameter. + */ +void calculate_orientation(struct accel_t *ac, struct vec3w_t *accel, struct orient_t *orient, int smooth) { + float xg, yg, zg; + float x, y, z; + + /* + * roll - use atan(z / x) [ ranges from -180 to 180 ] + * pitch - use atan(z / y) [ ranges from -180 to 180 ] + * yaw - impossible to tell without IR + */ + + /* yaw - set to 0, IR will take care of it if it's enabled */ + orient->yaw = 0.0f; + + /* find out how much it has to move to be 1g */ + xg = (float)ac->cal_g.x; + yg = (float)ac->cal_g.y; + zg = (float)ac->cal_g.z; + + /* find out how much it actually moved and normalize to +/- 1g */ + x = ((float)accel->x - (float)ac->cal_zero.x) / xg; + y = ((float)accel->y - (float)ac->cal_zero.y) / yg; + z = ((float)accel->z - (float)ac->cal_zero.z) / zg; + + /* make sure x,y,z are between -1 and 1 for the tan functions */ + if (x < -1.0f) x = -1.0f; + else if (x > 1.0f) x = 1.0f; + if (y < -1.0f) y = -1.0f; + else if (y > 1.0f) y = 1.0f; + if (z < -1.0f) z = -1.0f; + else if (z > 1.0f) z = 1.0f; + + /* if it is over 1g then it is probably accelerating and not reliable */ + if (abs(accel->x - ac->cal_zero.x) <= (ac->cal_g.x + 10)) { + /* roll */ + x = RAD_TO_DEGREE(atan2f(x, z)); + if (isfinite(x)) { + orient->roll = x; + orient->a_roll = x; + } + } + + if (abs(accel->y - ac->cal_zero.y) <= (ac->cal_g.y + 10)) { + /* pitch */ + y = RAD_TO_DEGREE(atan2f(y, z)); + if (isfinite(y)) { + orient->pitch = y; + orient->a_pitch = y; + } + } + + /* smooth the angles if enabled */ + if (smooth) { + apply_smoothing(ac, orient, SMOOTH_ROLL); + apply_smoothing(ac, orient, SMOOTH_PITCH); + } +} + + +/** + * @brief Calculate the gravity forces on each axis. + * + * @param ac An accelerometer (accel_t) structure. + * @param accel [in] Pointer to a vec3w_t structure that holds the raw acceleration data. + * @param gforce [out] Pointer to a gforce_t structure that will hold the gravity force data. + */ +void calculate_gforce(struct accel_t *ac, struct vec3w_t *accel, struct gforce_t *gforce) { + float xg, yg, zg; + + /* find out how much it has to move to be 1g */ + xg = (float)ac->cal_g.x; + yg = (float)ac->cal_g.y; + zg = (float)ac->cal_g.z; + + /* find out how much it actually moved and normalize to +/- 1g */ + gforce->x = ((float)accel->x - (float)ac->cal_zero.x) / xg; + gforce->y = ((float)accel->y - (float)ac->cal_zero.y) / yg; + gforce->z = ((float)accel->z - (float)ac->cal_zero.z) / zg; +} + + +/** + * @brief Calculate the angle and magnitude of a joystick. + * + * @param js [out] Pointer to a joystick_t structure. + * @param x The raw x-axis value. + * @param y The raw y-axis value. + */ +void calc_joystick_state(struct joystick_t *js, float x, float y) { + float rx, ry; + + /* + * Since the joystick center may not be exactly: + * (min + max) / 2 + * Then the range from the min to the center and the center to the max + * may be different. + * Because of this, depending on if the current x or y value is greater + * or less than the assoicated axis center value, it needs to be interpolated + * between the center and the minimum or maxmimum rather than between + * the minimum and maximum. + * + * So we have something like this: + * (x min) [-1] ---------*------ [0] (x center) [0] -------- [1] (x max) + * Where the * is the current x value. + * The range is therefore -1 to 1, 0 being the exact center rather than + * the middle of min and max. + */ + if (x == js->center.x) + rx = 0; + else if (x >= js->center.x) + rx = ((float)(x - js->center.x) / (float)(js->max.x - js->center.x)); + else + rx = ((float)(x - js->min.x) / (float)(js->center.x - js->min.x)) - 1.0f; + + if (y == js->center.y) + ry = 0; + else if (y >= js->center.y) + ry = ((float)(y - js->center.y) / (float)(js->max.y - js->center.y)); + else + ry = ((float)(y - js->min.y) / (float)(js->center.y - js->min.y)) - 1.0f; + + /* calculate the joystick angle and magnitude */ + js->ang = RAD_TO_DEGREE(atan2f(rx, ry)); + js->mag = hypotf(rx, ry); +} + + +void calc_balanceboard_state(struct wii_board_t *wb) +{ + /* + Interpolate values + Calculations borrowed from wiili.org - No names to mention sadly :( http://www.wiili.org/index.php/Wii_Balance_Board_PC_Drivers + */ + + if (wb->rtr < wb->ctr[1]) { + wb->tr = 17.0f * (f32)(wb->rtr - wb->ctr[0]) / (f32)(wb->ctr[1] - wb->ctr[0]); + } else { + wb->tr = 17.0f * (f32)(wb->rtr - wb->ctr[1]) / (f32)(wb->ctr[2] - wb->ctr[1]) + 17.0f; + } + + if (wb->rtl < wb->ctl[1]) { + wb->tl = 17.0f * (f32)(wb->rtl - wb->ctl[0]) / (f32)(wb->ctl[1] - wb->ctl[0]); + } else { + wb->tl = 17.0f * (f32)(wb->rtl - wb->ctl[1]) / (f32)(wb->ctl[2] - wb->ctl[1]) + 17.0f; + } + + if (wb->rbr < wb->cbr[1]) { + wb->br = 17.0f * (f32)(wb->rbr - wb->cbr[0]) / (f32)(wb->cbr[1] - wb->cbr[0]); + } else { + wb->br = 17.0f * (f32)(wb->rbr - wb->cbr[1]) / (f32)(wb->cbr[2] - wb->cbr[1]) + 17.0f; + } + + if (wb->rbl < wb->cbl[1]) { + wb->bl = 17.0f * (f32)(wb->rbl - wb->cbl[0]) / (f32)(wb->cbl[1] - wb->cbl[0]); + } else { + wb->bl = 17.0f * (f32)(wb->rbl - wb->cbl[1]) / (f32)(wb->cbl[2] - wb->cbl[1]) + 17.0f; + } + + wb->x = ((wb->tr + wb->br) - (wb->tl + wb->bl)) / 2.0f; + wb->y = ((wb->bl + wb->br) - (wb->tl + wb->tr)) / 2.0f; +} + +void _wpad2_calc_data(WPADData *data, const WPADData *lstate, + struct accel_t *accel_calib, bool smoothed) +{ + if (data->err != WPAD_ERR_NONE) return; + + data->orient = lstate->orient; + + data->ir.state = lstate->ir.state; + data->ir.sensorbar = lstate->ir.sensorbar; + data->ir.x = lstate->ir.x; + data->ir.y = lstate->ir.y; + data->ir.sx = lstate->ir.sx; + data->ir.sy = lstate->ir.sy; + data->ir.ax = lstate->ir.ax; + data->ir.ay = lstate->ir.ay; + data->ir.distance = lstate->ir.distance; + data->ir.z = lstate->ir.z; + data->ir.angle = lstate->ir.angle; + data->ir.error_cnt = lstate->ir.error_cnt; + data->ir.glitch_cnt = lstate->ir.glitch_cnt; + data->ir.aspect = lstate->ir.aspect; + data->ir.vres[0] = lstate->ir.vres[0]; + data->ir.vres[1] = lstate->ir.vres[1]; + data->ir.offset[0] = lstate->ir.offset[0]; + data->ir.offset[1] = lstate->ir.offset[1]; + + data->btns_l = lstate->btns_h; + if (data->data_present & WPAD_DATA_ACCEL) { + calculate_orientation(accel_calib, &data->accel, &data->orient, smoothed); + calculate_gforce(accel_calib, &data->accel, &data->gforce); + } + if (data->data_present & WPAD_DATA_IR) { + _wpad2_interpret_ir_data(&data->ir, &data->orient); + } + if (data->data_present & WPAD_DATA_EXPANSION) { + WPAD2_DEBUG("calculating data with expansion %d", data->exp.type); + switch (data->exp.type) { + case EXP_NUNCHUK: + { + struct nunchuk_t *nc = &data->exp.nunchuk; + + nc->orient = lstate->exp.nunchuk.orient; + calc_joystick_state(&nc->js, nc->js.pos.x, nc->js.pos.y); + calculate_orientation(&nc->accel_calib, &nc->accel, &nc->orient, smoothed); + calculate_gforce(&nc->accel_calib, &nc->accel, &nc->gforce); + data->btns_h |= (data->exp.nunchuk.btns << 16); + } + break; + + case EXP_CLASSIC: + { + struct classic_ctrl_t *cc = &data->exp.classic; + + cc->r_shoulder = ((f32)cc->rs_raw / 0x1F); + cc->l_shoulder = ((f32)cc->ls_raw / 0x1F); + calc_joystick_state(&cc->ljs, cc->ljs.pos.x, cc->ljs.pos.y); + calc_joystick_state(&cc->rjs, cc->rjs.pos.x, cc->rjs.pos.y); + + /* overwrite Wiimote buttons (unused) with extra Wii U Pro Controller stick buttons */ + if (data->exp.classic.type == CLASSIC_TYPE_WIIU) + data->btns_h = (data->exp.classic.btns & WII_U_PRO_CTRL_BUTTON_EXTRA) >> 16; + + data->btns_h |= ((data->exp.classic.btns & CLASSIC_CTRL_BUTTON_ALL) << 16); + } + break; + + case EXP_GUITAR_HERO_3: + { + struct guitar_hero_3_t *gh3 = &data->exp.gh3; + + gh3->touch_bar = 0; + if (gh3->tb_raw > 0x1B) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_ORANGE; + else if (gh3->tb_raw > 0x18) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_ORANGE | GUITAR_HERO_3_TOUCH_BLUE; + else if (gh3->tb_raw > 0x15) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_BLUE; + else if (gh3->tb_raw > 0x13) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_BLUE | GUITAR_HERO_3_TOUCH_YELLOW; + else if (gh3->tb_raw > 0x10) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_YELLOW; + else if (gh3->tb_raw > 0x0D) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_AVAILABLE; + else if (gh3->tb_raw > 0x0B) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_YELLOW | GUITAR_HERO_3_TOUCH_RED; + else if (gh3->tb_raw > 0x08) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_RED; + else if (gh3->tb_raw > 0x05) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_RED | GUITAR_HERO_3_TOUCH_GREEN; + else if (gh3->tb_raw > 0x02) + gh3->touch_bar = GUITAR_HERO_3_TOUCH_GREEN; + + gh3->whammy_bar = (gh3->wb_raw - GUITAR_HERO_3_WHAMMY_BAR_MIN) / (float)(GUITAR_HERO_3_WHAMMY_BAR_MAX - GUITAR_HERO_3_WHAMMY_BAR_MIN); + calc_joystick_state(&gh3->js, gh3->js.pos.x, gh3->js.pos.y); + data->btns_h |= (data->exp.gh3.btns << 16); + } + break; + + case EXP_WII_BOARD: + { + struct wii_board_t *wb = &data->exp.wb; + calc_balanceboard_state(wb); + } + break; + + default: + break; + } + } + data->btns_d = data->btns_h & ~data->btns_l; + data->btns_u = ~data->btns_h & data->btns_l; +} diff --git a/wpad2/dynamics.h b/wpad2/dynamics.h new file mode 100644 index 000000000..8341c353d --- /dev/null +++ b/wpad2/dynamics.h @@ -0,0 +1,9 @@ +#ifndef WPAD2_DYNAMICS_H +#define WPAD2_DYNAMICS_H + +#include "internals.h" + +void _wpad2_calc_data(WPADData *data, const WPADData *lstate, + struct accel_t *accel_calib, bool smoothed); + +#endif /* WPAD2_DYNAMICS_H */ diff --git a/wpad2/event.c b/wpad2/event.c new file mode 100644 index 000000000..be3525232 --- /dev/null +++ b/wpad2/event.c @@ -0,0 +1,198 @@ +#include "event.h" + +#include "classic.h" +#include "guitar_hero_3.h" +#include "ir.h" +#include "motion_plus.h" +#include "nunchuk.h" +#include "wii_board.h" + +#define ABS(x) ((s32)(x) > 0 ? (s32)(x) : -((s32)(x))) + +#define CHECK_THRESHOLD(thresh, a, b) \ + (((thresh) > WPAD_THRESH_IGNORE) && (ABS((a) - (b)) > (thresh))) + +#define CHECK_THRESHOLD_SIMPLE(thresh, a, b) \ + (((thresh) > WPAD_THRESH_IGNORE) && ((a) != (b))) + +static bool check_threshold_js( + int threshold, const struct joystick_t *new, const struct joystick_t *old) +{ + if (CHECK_THRESHOLD(threshold, new->pos.x, old->pos.x)) return true; + if (CHECK_THRESHOLD(threshold, new->pos.y, old->pos.y)) return true; + return false; +} + +static bool check_threshold_accel( + int threshold, const struct vec3w_t *new, const struct vec3w_t *old) +{ + if (CHECK_THRESHOLD(threshold, new->x, old->x)) return true; + if (CHECK_THRESHOLD(threshold, new->y, old->y)) return true; + if (CHECK_THRESHOLD(threshold, new->z, old->z)) return true; + return false; +} + +static bool check_threshold_ir( + int threshold, const WPADData *new, const WPADData *old) +{ + for (int i = 0; i < WPAD_MAX_IR_DOTS; i++) { + if (new->ir.dot[i].visible != old->ir.dot[i].visible) return true; + if (CHECK_THRESHOLD(threshold, new->ir.dot[i].rx, old->ir.dot[i].rx)) return true; + if (CHECK_THRESHOLD(threshold, new->ir.dot[i].ry, old->ir.dot[i].ry)) return true; + } + return false; +} + +static bool handle_expansion(const WPADData *wpad_info, const uint8_t *msg, + const WpadThresholds *thresh, WPADData *out) +{ + WPAD2_DEBUG("exp type: %d", wpad_info->exp.type); + bool changed = false; + switch (out->exp.type) { + case EXP_NUNCHUK: + struct nunchuk_t *nc = &out->exp.nunchuk; + const struct nunchuk_t *nc_info = &wpad_info->exp.nunchuk; + _wpad2_nunchuk_event(nc, msg); + if (thresh && + (CHECK_THRESHOLD_SIMPLE(thresh->btns, nc->btns, nc_info->btns) || + check_threshold_js(thresh->js, &nc->js, &nc_info->js) || + check_threshold_accel(thresh->acc, &nc->accel, &nc_info->accel))) { + changed = true; + } + break; + case EXP_CLASSIC: + struct classic_ctrl_t *cc = &out->exp.classic; + const struct classic_ctrl_t *cc_info = &wpad_info->exp.classic; + cc->type = wpad_info->exp.classic.type; + _wpad2_classic_event(cc, msg); + if (thresh && + (CHECK_THRESHOLD_SIMPLE(thresh->btns, cc->btns, cc_info->btns) || + check_threshold_js(thresh->js, &cc->ljs, &cc_info->ljs) || + check_threshold_js(thresh->js, &cc->rjs, &cc_info->rjs) || + CHECK_THRESHOLD(thresh->js, cc->rs_raw, cc_info->rs_raw) || + CHECK_THRESHOLD(thresh->js, cc->ls_raw, cc_info->ls_raw))) { + changed = true; + } + break; + case EXP_GUITAR_HERO_3: + struct guitar_hero_3_t *gh = &out->exp.gh3; + const struct guitar_hero_3_t *gh_info = &wpad_info->exp.gh3; + _wpad2_guitar_hero_3_event(gh, msg); + if (thresh && + (CHECK_THRESHOLD_SIMPLE(thresh->btns, gh->btns, gh_info->btns) || + check_threshold_js(thresh->js, &gh->js, &gh_info->js) || + CHECK_THRESHOLD(thresh->js, gh->wb_raw, gh_info->wb_raw))) { + changed = true; + } + break; + case EXP_WII_BOARD: + struct wii_board_t *wb = &out->exp.wb; + const struct wii_board_t *wb_info = &wpad_info->exp.wb; + _wpad2_wii_board_event(wb, msg); + if (thresh && + (CHECK_THRESHOLD(thresh->wb, wb->rtl, wb_info->rtl) || + CHECK_THRESHOLD(thresh->wb, wb->rtr, wb_info->rtr) || + CHECK_THRESHOLD(thresh->wb, wb->rbl, wb_info->rbl) || + CHECK_THRESHOLD(thresh->wb, wb->rbr, wb_info->rbr))) { + changed = true; + } + break; + case EXP_MOTION_PLUS: + struct motion_plus_t *mp = &out->exp.mp; + const struct motion_plus_t *mp_info = &wpad_info->exp.mp; + _wpad2_motion_plus_event(mp, msg); + if (thresh && + (CHECK_THRESHOLD(thresh->mp, mp->rx, mp_info->rx) || + CHECK_THRESHOLD(thresh->mp, mp->ry, mp_info->ry) || + CHECK_THRESHOLD(thresh->mp, mp->rz, mp_info->rz))) { + changed = true; + } + break; + default: + return false; + } + out->data_present |= WPAD_DATA_EXPANSION; + return changed; +} + +static bool pressed_buttons(const WPADData *wpad_info, const uint8_t *msg, + const WpadThresholds *thresh, WPADData *out) +{ + uint16_t now = read_be16(msg) & WIIMOTE_BUTTON_ALL; + + /* buttons pressed now */ + out->btns_h = now; + out->data_present |= WPAD_DATA_BUTTONS; + WPAD2_DEBUG("Buttons %04x", now); + return CHECK_THRESHOLD_SIMPLE(thresh->btns, out->btns_h, wpad_info->btns_h); +} + +static bool parse_accel(const WPADData *wpad_info, const uint8_t *msg, + const WpadThresholds *thresh, WPADData *out) +{ + out->accel.x = (msg[2] << 2) | ((msg[0] >> 5) & 3); + out->accel.y = (msg[3] << 2) | ((msg[1] >> 4) & 2); + out->accel.z = (msg[4] << 2) | ((msg[1] >> 5) & 2); + out->data_present |= WPAD_DATA_ACCEL; + return thresh && check_threshold_accel(thresh->acc, &out->accel, &wpad_info->accel); +} + +/* Return true if the data has changed (taking the thresholds into account) */ +bool _wpad2_event_parse_report(const WPADData *wpad_info, const uint8_t *data, + const WpadThresholds *thresh, WPADData *out) +{ + uint8_t event = data[0]; + const uint8_t *msg = data + 1; + + out->data_present = 0; + out->exp = wpad_info->exp; + /* We set the "thresh" pointer to NULL if some data has changed, to avoid + * doing useless threshold checks. If "thresh" is NULL at the end of this + * function, it means that some data has significantly changed. */ + switch (event) { + case WM_RPT_BTN: + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + break; + case WM_RPT_BTN_ACC: + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + if (parse_accel(wpad_info, msg, thresh, out)) thresh = NULL; + break; + case WM_RPT_BTN_ACC_IR: + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + if (parse_accel(wpad_info, msg, thresh, out)) thresh = NULL; + _wpad2_ir_parse_extended(out, msg + 5); + if (thresh && check_threshold_ir(thresh->ir, out, wpad_info)) thresh = NULL; + break; + case WM_RPT_BTN_EXP: + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + if (handle_expansion(wpad_info, msg + 2, thresh, out)) thresh = NULL; + break; + case WM_RPT_BTN_ACC_EXP: + /* button - motion - expansion */ + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + if (parse_accel(wpad_info, msg, thresh, out)) thresh = NULL; + if (handle_expansion(wpad_info, msg + 5, thresh, out)) thresh = NULL; + break; + case WM_RPT_BTN_IR_EXP: + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + _wpad2_ir_parse_basic(out, msg + 2); + if (thresh && check_threshold_ir(thresh->ir, out, wpad_info)) thresh = NULL; + if (handle_expansion(wpad_info, msg + 12, thresh, out)) thresh = NULL; + break; + case WM_RPT_BTN_ACC_IR_EXP: + /* button - motion - ir - expansion */ + if (pressed_buttons(wpad_info, msg, thresh, out)) thresh = NULL; + if (parse_accel(wpad_info, msg, thresh, out)) thresh = NULL; + + /* ir */ + _wpad2_ir_parse_basic(out, msg + 5); + if (thresh && check_threshold_ir(thresh->ir, out, wpad_info)) thresh = NULL; + + if (handle_expansion(wpad_info, msg + 15, thresh, out)) thresh = NULL; + break; + default: + WPAD2_WARNING("Unknown event, can not handle it [Code 0x%02x].", event); + return false; + } + return thresh == NULL; +} diff --git a/wpad2/event.h b/wpad2/event.h new file mode 100644 index 000000000..a4c27bc37 --- /dev/null +++ b/wpad2/event.h @@ -0,0 +1,18 @@ +#ifndef WPAD2_EVENT_H +#define WPAD2_EVENT_H + +#include "internals.h" + +typedef struct { + int btns; + int ir; + int js; + int acc; + int wb; + int mp; +} WpadThresholds; + +bool _wpad2_event_parse_report(const WPADData *wpad_info, const uint8_t *data, + const WpadThresholds *thresh, WPADData *out); + +#endif /* WPAD2_EVENT_H */ diff --git a/wpad2/guitar_hero_3.c b/wpad2/guitar_hero_3.c new file mode 100644 index 000000000..09f2c6407 --- /dev/null +++ b/wpad2/guitar_hero_3.c @@ -0,0 +1,106 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/guitar_hero_3.c,v 1.7 2008-11-14 13:34:57 shagkur Exp $ + * + */ + +/** + * @file + * @brief Guitar Hero 3 expansion device. + */ + +#include "guitar_hero_3.h" + +#include +#include +#include +#include + +/** + * @brief Handle the handshake data from the guitar. + * + * @param cc A pointer to a classic_ctrl_t structure. + * @param data The data read in from the device. + * @param len The length of the data block, in bytes. + * + * @return Returns 1 if handshake was successful, 0 if not. + */ +void _wpad2_guitar_hero_3_calibrate(struct guitar_hero_3_t *gh3, + const WpadDeviceExpCalibrationData *) { + /* + * The good fellows that made the Guitar Hero 3 controller + * failed to factory calibrate the devices. There is no + * calibration data on the device. + */ + + gh3->btns = 0; + gh3->btns_held = 0; + gh3->btns_released = 0; + gh3->wb_raw = 0; + gh3->whammy_bar = 0.0f; + gh3->tb_raw = 0; + gh3->touch_bar = -1; + + /* joystick stuff */ + gh3->js.max.x = GUITAR_HERO_3_JS_MAX_X; + gh3->js.min.x = GUITAR_HERO_3_JS_MIN_X; + gh3->js.center.x = GUITAR_HERO_3_JS_CENTER_X; + gh3->js.max.y = GUITAR_HERO_3_JS_MAX_Y; + gh3->js.min.y = GUITAR_HERO_3_JS_MIN_Y; + gh3->js.center.y = GUITAR_HERO_3_JS_CENTER_Y; +} + +static void guitar_hero_3_pressed_buttons(struct guitar_hero_3_t *gh3, uint16_t now) { + /* message is inverted (0 is active, 1 is inactive) */ + now = ~now & GUITAR_HERO_3_BUTTON_ALL; + + /* preserve old btns pressed */ + gh3->btns_last = gh3->btns; + + /* pressed now & were pressed, then held */ + gh3->btns_held = (now & gh3->btns); + + /* were pressed or were held & not pressed now, then released */ + gh3->btns_released = ((gh3->btns | gh3->btns_held) & ~now); + + /* buttons pressed now */ + gh3->btns = now; +} + +/** + * @brief Handle guitar event. + * + * @param cc A pointer to a classic_ctrl_t structure. + * @param msg The message specified in the event packet. + */ +void _wpad2_guitar_hero_3_event(struct guitar_hero_3_t *gh3, const uint8_t *msg) { + uint16_t buttons = read_be16(msg + 4); + guitar_hero_3_pressed_buttons(gh3, buttons); + + gh3->js.pos.x = (msg[0] & GUITAR_HERO_3_JS_MASK); + gh3->js.pos.y = (msg[1] & GUITAR_HERO_3_JS_MASK); + gh3->tb_raw = (msg[2] & GUITAR_HERO_3_TOUCH_MASK); + gh3->wb_raw = (msg[3] & GUITAR_HERO_3_WHAMMY_MASK); +} diff --git a/wpad2/guitar_hero_3.h b/wpad2/guitar_hero_3.h new file mode 100644 index 000000000..43fd59d5a --- /dev/null +++ b/wpad2/guitar_hero_3.h @@ -0,0 +1,57 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/guitar_hero_3.h,v 1.1 2008-05-08 09:42:14 shagkur Exp $ + * + */ + +/** + * @file + * @brief Guitar Hero 3 expansion device. + */ + +#ifndef WPAD2_GUITAR_HERO_3_H +#define WPAD2_GUITAR_HERO_3_H + +#include "device.h" + +#define GUITAR_HERO_3_JS_MASK 0x3F +#define GUITAR_HERO_3_TOUCH_MASK 0x1F +#define GUITAR_HERO_3_WHAMMY_MASK 0x1F + +#define GUITAR_HERO_3_JS_MIN_X 0x05 +#define GUITAR_HERO_3_JS_MAX_X 0x3C +#define GUITAR_HERO_3_JS_CENTER_X 0x20 +#define GUITAR_HERO_3_JS_MIN_Y 0x05 +#define GUITAR_HERO_3_JS_MAX_Y 0x3A +#define GUITAR_HERO_3_JS_CENTER_Y 0x20 +#define GUITAR_HERO_3_WHAMMY_BAR_MIN 0x0F +#define GUITAR_HERO_3_WHAMMY_BAR_MAX 0x1A + +void _wpad2_guitar_hero_3_calibrate(struct guitar_hero_3_t *gh3, + const WpadDeviceExpCalibrationData *cd); + +void _wpad2_guitar_hero_3_event(struct guitar_hero_3_t *gh3, const uint8_t *msg); + +#endif /* WPAD2_GUITAR_HERO_3_H */ diff --git a/wpad2/internals.h b/wpad2/internals.h new file mode 100644 index 000000000..5d86258fa --- /dev/null +++ b/wpad2/internals.h @@ -0,0 +1,71 @@ +#ifndef WPAD2_INTERNALS_H +#define WPAD2_INTERNALS_H + +#define __BTE_H__ /* Prevent inclusion */ +#include "ogc/lwp_queue.h" +#include "bte/bd_addr.h" + +#include "wiiuse/wpad.h" +#include "wiiuse_internal.h" + +#include "bt-embedded/l2cap.h" + +#ifndef __wii__ +# include +#else +# include +#endif +#include + +#define read_be16(ptr) be16toh(*(uint16_t *)(ptr)) +#define read_be32(ptr) be32toh(*(uint32_t *)(ptr)) + +#define write_be16(n, ptr) *(uint16_t *)(ptr) = htobe16(n) +#define write_be32(n, ptr) *(uint32_t *)(ptr) = htobe32(n) + +#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0])) + +#define WIIMOTE_PI 3.14159265f +/* Convert between radians and degrees */ +#define RAD_TO_DEGREE(r) ((r * 180.0f) / WIIMOTE_PI) +#define DEGREE_TO_RAD(d) (d * (WIIMOTE_PI / 180.0f)) + +#define absf(x) ((x >= 0) ? (x) : (x * -1.0f)) +#define diff_f(x, y) ((x >= y) ? (absf(x - y)) : (absf(y - x))) + +//#define WITH_WPAD_DEBUG + +#ifdef WITH_WPAD_DEBUG + #define WPAD2_DEBUG(fmt, ...) SYS_Report("[DEBUG] %s:%i: " fmt "\n", __func__, __LINE__, ##__VA_ARGS__) + #define WPAD2_WARNING(fmt, ...) SYS_Report("[WARNING] " fmt "\n", ##__VA_ARGS__) + #define WPAD2_ERROR(fmt, ...) SYS_Report("[ERROR] " fmt "\n", ##__VA_ARGS__) +#else + #define WPAD2_DEBUG(fmt, ...) + #define WPAD2_WARNING(fmt, ...) + #define WPAD2_ERROR(fmt, ...) +#endif + +#define BD_ADDR_FROM_CONF(bdaddr, b) do { \ + (bdaddr)->bytes[0] = b[5]; \ + (bdaddr)->bytes[1] = b[4]; \ + (bdaddr)->bytes[2] = b[3]; \ + (bdaddr)->bytes[3] = b[2]; \ + (bdaddr)->bytes[4] = b[1]; \ + (bdaddr)->bytes[5] = b[0]; } while(0) + +#define BD_ADDR_TO_CONF(b, bdaddr) do { \ + b[0] = (bdaddr)->bytes[5]; \ + b[1] = (bdaddr)->bytes[4]; \ + b[2] = (bdaddr)->bytes[3]; \ + b[3] = (bdaddr)->bytes[2]; \ + b[4] = (bdaddr)->bytes[1]; \ + b[5] = (bdaddr)->bytes[0]; } while(0) + +#define BD_ADDR_FMT "%02x:%02x:%02x:%02x:%02x:%02x" +#define BD_ADDR_DATA(b) \ + (b)->bytes[5], (b)->bytes[4], (b)->bytes[3], \ + (b)->bytes[2], (b)->bytes[1], (b)->bytes[0] + +#define WPAD2_MAX_DEVICES (4 + 1) + +#endif /* WPAD2_INTERNALS_H */ diff --git a/wpad2/ir.c b/wpad2/ir.c new file mode 100644 index 000000000..64ab15a2c --- /dev/null +++ b/wpad2/ir.c @@ -0,0 +1,794 @@ +#include "ir.h" + +#include "ogc/irq.h" + +#include +#include +#include + +static vu32* const _ipcReg = (u32*)0xCD000000; + +static inline u32 ACR_ReadReg(u32 reg) +{ + return _ipcReg[reg >> 2]; +} + +static inline void ACR_WriteReg(u32 reg, u32 val) +{ + _ipcReg[reg >> 2] = val; +} + +/** + * @brief Correct for the IR bounding box. + * + * @param x [out] The current X, it will be updated if valid. + * @param y [out] The current Y, it will be updated if valid. + * @param aspect Aspect ratio of the screen. + * @param offset_x The X offset of the bounding box. + * @param offset_y The Y offset of the bounding box. + * + * @return Returns 1 if the point is valid and was updated. + * + * Nintendo was smart with this bit. They sacrifice a little + * precision for a big increase in usability. + */ +static int ir_correct_for_bounds(float *x, float *y, enum aspect_t aspect, int offset_x, int offset_y) { + float x0, y0; + int xs, ys; + + if (aspect == WIIUSE_ASPECT_16_9) { + xs = WM_ASPECT_16_9_X; + ys = WM_ASPECT_16_9_Y; + } else { + xs = WM_ASPECT_4_3_X; + ys = WM_ASPECT_4_3_Y; + } + + x0 = ((1024 - xs) / 2) + offset_x; + y0 = ((768 - ys) / 2) + offset_y; + + if ((*x >= x0) + && (*x <= (x0 + xs)) + && (*y >= y0) + && (*y <= (y0 + ys))) { + *x -= offset_x; + *y -= offset_y; + + return 1; + } + + return 0; +} + +/** + * @brief Interpolate the point to the user defined virtual screen resolution. + */ +static void ir_convert_to_vres(float *x, float *y, enum aspect_t aspect, unsigned int vx, unsigned int vy) { + int xs, ys; + + if (aspect == WIIUSE_ASPECT_16_9) { + xs = WM_ASPECT_16_9_X; + ys = WM_ASPECT_16_9_Y; + } else { + xs = WM_ASPECT_4_3_X; + ys = WM_ASPECT_4_3_Y; + } + + *x -= ((1024 - xs) / 2); + *y -= ((768 - ys) / 2); + + *x = (*x / (float)xs) * vx; + *y = (*y / (float)ys) * vy; +} + +/** + * @brief Get the IR sensitivity settings. + * + * @param wm Pointer to a wiimote_t structure. + * @param block1 [out] Pointer to where block1 will be set. + * @param block2 [out] Pointer to where block2 will be set. + * + * @return Returns the sensitivity level. + */ +static int get_ir_sens(uint8_t sensor_level, + const uint8_t **block1, const uint8_t **block2) { + switch (sensor_level) { + case 1: + *block1 = WM_IR_BLOCK1_LEVEL1; + *block2 = WM_IR_BLOCK2_LEVEL1; + return 1; + case 2: + *block1 = WM_IR_BLOCK1_LEVEL2; + *block2 = WM_IR_BLOCK2_LEVEL2; + return 2; + case 3: + *block1 = WM_IR_BLOCK1_LEVEL3; + *block2 = WM_IR_BLOCK2_LEVEL3; + return 3; + case 4: + *block1 = WM_IR_BLOCK1_LEVEL4; + *block2 = WM_IR_BLOCK2_LEVEL4; + return 4; + case 5: + *block1 = WM_IR_BLOCK1_LEVEL5; + *block2 = WM_IR_BLOCK2_LEVEL5; + return 5; + } + + *block1 = NULL; + *block2 = NULL; + return 0; +} + +static void rotate_dots(struct fdot_t *in, struct fdot_t *out, int count, float ang) { + float s, c; + int i; + + if (ang == 0) { + for (i = 0; i < count; ++i) { + out[i].x = in[i].x; + out[i].y = in[i].y; + } + return; + } + + s = sin(DEGREE_TO_RAD(ang)); + c = cos(DEGREE_TO_RAD(ang)); + + /* + * [ cos(theta) -sin(theta) ][ ir->rx ] + * [ sin(theta) cos(theta) ][ ir->ry ] + */ + + for (i = 0; i < count; ++i) { + out[i].x = (c * in[i].x) + (-s * in[i].y); + out[i].y = (s * in[i].x) + (c * in[i].y); + } +} + +static bool ir_set_mode(WpadDevice *device) +{ + uint8_t buf = device->exp_attached ? WM_IR_TYPE_BASIC : WM_IR_TYPE_EXTENDED; + if (!_wpad2_device_write_data(device, WM_REG_IR_MODENUM, &buf, 1)) return false; + device->state = STATE_IR_SET_MODE; + return true; +} + +static bool ir_enable(WpadDevice *device, bool enable) +{ + if (_wpad2_device_get_slot(device) == WPAD_BALANCE_BOARD) return false; + + /* + * Check to make sure a sensitivity setting is selected. + */ + const uint8_t *block1, *block2; + int ir_level = get_ir_sens(device->ir_sensor_level, &block1, &block2); + if (!ir_level) { + WPAD2_ERROR("No IR sensitivity setting selected."); + return false; + } + + if (enable == device->ir_enabled || + (device->exp_type == EXP_CLASSIC && + device->exp_subtype == CLASSIC_TYPE_WIIU)) { + return false; + } + + uint8_t buf = (enable ? 0x04 : 0x00); + device->state = STATE_IR_ENABLING_1; + _wpad2_device_send_command(device, WM_CMD_IR, &buf, 1); + + WPAD2_DEBUG("IR cameras for wiimote %i was set to %d", device->unid, enable); + return true; +} + +static bool ir_set_sensitivity(WpadDevice *device) +{ + const uint8_t *block1, *block2; + int ir_level = get_ir_sens(device->ir_sensor_level, &block1, &block2); + if (!ir_level) { + return false; + } + + if (device->state == STATE_IR_ENABLING_2) { + uint8_t buf = 0x08; + if (!_wpad2_device_write_data(device, WM_REG_IR, &buf, 1)) return false; + device->state = STATE_IR_SENSITIVITY_1; + } else if (device->state == STATE_IR_SENSITIVITY_1) { + if (!_wpad2_device_write_data(device, WM_REG_IR_BLOCK1, block1, 9)) return false; + device->state = STATE_IR_SENSITIVITY_2; + } else if (device->state == STATE_IR_SENSITIVITY_2) { + if (!_wpad2_device_write_data(device, WM_REG_IR_BLOCK2, block2, 2)) return false; + device->state = STATE_IR_SENSITIVITY_3; + } + + return true; +} + +/** + * @brief Set the virtual screen resolution for IR tracking. + * + * @param wm Pointer to a wiimote_t structure. + * @param status 1 to enable, 0 to disable. + */ +void _wpad2_ir_set_vres(WPADData *data, unsigned int x, unsigned int y) { + data->ir.vres[0] = x - 1; + data->ir.vres[1] = y - 1; +} + +/** + * @brief Set the XY position for the IR cursor. + * + * @param wm Pointer to a wiimote_t structure. + */ +void _wpad2_ir_set_position(WPADData *data, enum ir_position_t pos) { + data->ir.pos = pos; + + switch (pos) { + + case WIIUSE_IR_ABOVE: + data->ir.offset[0] = 0; + + if (data->ir.aspect == WIIUSE_ASPECT_16_9) + data->ir.offset[1] = WM_ASPECT_16_9_Y / 2 - 70; + else if (data->ir.aspect == WIIUSE_ASPECT_4_3) + data->ir.offset[1] = WM_ASPECT_4_3_Y / 2 - 100; + + return; + + case WIIUSE_IR_BELOW: + data->ir.offset[0] = 0; + + if (data->ir.aspect == WIIUSE_ASPECT_16_9) + data->ir.offset[1] = -WM_ASPECT_16_9_Y / 2 + 70; + else if (data->ir.aspect == WIIUSE_ASPECT_4_3) + data->ir.offset[1] = -WM_ASPECT_4_3_Y / 2 + 100; + + return; + + default: + return; + }; +} + +/** + * @brief Set the aspect ratio of the TV/monitor. + * + * @param wm Pointer to a wiimote_t structure. + * @param aspect Either WIIUSE_ASPECT_16_9 or WIIUSE_ASPECT_4_3 + */ +void _wpad2_ir_set_aspect_ratio(WPADData *data, enum aspect_t aspect) { + data->ir.aspect = aspect; + + if (aspect == WIIUSE_ASPECT_4_3) { + data->ir.vres[0] = WM_ASPECT_4_3_X; + data->ir.vres[1] = WM_ASPECT_4_3_Y; + } else { + data->ir.vres[0] = WM_ASPECT_16_9_X; + data->ir.vres[1] = WM_ASPECT_16_9_Y; + } + + /* reset the position offsets */ + _wpad2_ir_set_position(data, data->ir.pos); +} + +/** + * @brief Calculate the data from the IR spots. Basic IR mode. + * + * @param wm Pointer to a wiimote_t structure. + * @param data Data returned by the wiimote for the IR spots. + */ +void _wpad2_ir_parse_basic(WPADData *out, const uint8_t *data) { + struct ir_dot_t *dot = out->ir.dot; + int i; + + dot[0].rx = 1023 - (data[0] | ((data[2] & 0x30) << 4)); + dot[0].ry = data[1] | ((data[2] & 0xC0) << 2); + + dot[1].rx = 1023 - (data[3] | ((data[2] & 0x03) << 8)); + dot[1].ry = data[4] | ((data[2] & 0x0C) << 6); + + dot[2].rx = 1023 - (data[5] | ((data[7] & 0x30) << 4)); + dot[2].ry = data[6] | ((data[7] & 0xC0) << 2); + + dot[3].rx = 1023 - (data[8] | ((data[7] & 0x03) << 8)); + dot[3].ry = data[9] | ((data[7] & 0x0C) << 6); + + /* set each IR spot to visible if spot is in range */ + for (i = 0; i < 4; ++i) { + dot[i].rx = be16toh(dot[i].rx); + dot[i].ry = be16toh(dot[i].ry); + + if (dot[i].ry == 1023) + dot[i].visible = 0; + else { + dot[i].visible = 1; + dot[i].size = 0; /* since we don't know the size, set it as 0 */ + } + } + out->data_present |= WPAD_DATA_IR; +} + +/** + * @brief Calculate the data from the IR spots. Extended IR mode. + * + * @param wm Pointer to a wiimote_t structure. + * @param data Data returned by the wiimote for the IR spots. + */ +void _wpad2_ir_parse_extended(WPADData *out, const uint8_t *data) { + struct ir_dot_t *dot = out->ir.dot; + int i; + + for (i = 0; i < 4; ++i) { + dot[i].rx = 1023 - (data[3 * i] | ((data[(3 * i) + 2] & 0x30) << 4)); + dot[i].ry = data[(3 * i) + 1] | ((data[(3 * i) + 2] & 0xC0) << 2); + + dot[i].size = data[(3 * i) + 2]; + + dot[i].rx = be16toh(dot[i].rx); + dot[i].ry = be16toh(dot[i].ry); + + dot[i].size = dot[i].size & 0x0f; + + /* if in range set to visible */ + if (dot[i].ry == 1023) + dot[i].visible = 0; + else + dot[i].visible = 1; + } + out->data_present |= WPAD_DATA_IR; +} + +enum { + IR_STATE_DEAD = 0, + IR_STATE_GOOD, + IR_STATE_SINGLE, + IR_STATE_LOST, +}; + +/* half-height of the IR sensor if half-width is 1 */ +#define HEIGHT (384.0f / 512.0f) +/* maximum sensor bar slope (tan(35 degrees)) */ +#define MAX_SB_SLOPE 0.7f +/* minimum sensor bar width in view, relative to half of the IR sensor area */ +#define MIN_SB_WIDTH 0.1f +/* reject "sensor bars" that happen to have a dot towards the middle */ +#define SB_MIDDOT_REJECT 0.05f + +/* physical dimensions */ +/* cm center to center of emitters */ +#define SB_WIDTH 19.5f +/* half-width in cm of emitters */ +#define SB_DOT_WIDTH 2.25f +/* half-height in cm of emitters (with some tolerance) */ +#define SB_DOT_HEIGHT 1.0f + +#define SB_DOT_WIDTH_RATIO (SB_DOT_WIDTH / SB_WIDTH) +#define SB_DOT_HEIGHT_RATIO (SB_DOT_HEIGHT / SB_WIDTH) + +/* dots further out than these coords are allowed to not be picked up */ +/* otherwise assume something's wrong */ +/*#define SB_OFF_SCREEN_X 0.8f */ +/*#define SB_OFF_SCREEN_Y (0.8f * HEIGHT) */ + +/* disable, may be doing more harm than good due to sensor pickup glitches */ +#define SB_OFF_SCREEN_X 0.0f +#define SB_OFF_SCREEN_Y 0.0f + +/* if a point is closer than this to one of the previous SB points */ +/* when it reappears, consider it the same instead of trying to guess */ +/* which one of the two it is */ +#define SB_SINGLE_NOGUESS_DISTANCE (100.0 * 100.0) + +/* width of the sensor bar in pixels at one meter from the Wiimote */ +#define SB_Z_COEFFICIENT 256.0f + +/* distance in meters from the center of the FOV to the left or right edge, */ +/* when the wiimote is at one meter */ +#define WIIMOTE_FOV_COEFFICIENT 0.39f + +#define SQUARED(x) ((x)*(x)) +#define WMAX(x, y) ((x>y)?(x):(y)) +#define WMIN(x, y) ((xroll); + + /* count visible dots and populate dots structure */ + /* dots[] is in -1..1 units for width */ + ir->num_dots = 0; + for (i = 0; i < 4; i++) { + if (ir->dot[i].visible) { + dots[ir->num_dots].x = (ir->dot[i].rx - 512.0f) / 512.0f; + dots[ir->num_dots].y = (ir->dot[i].ry - 384.0f) / 512.0f; + WPAD2_DEBUG("IR: dot %d at (%d,%d) (%.03f,%.03f)", ir->num_dots, ir->dot[i].rx, ir->dot[i].ry, dots[ir->num_dots].x, dots[ir->num_dots].y); + ir->num_dots++; + } + } + + WPAD2_DEBUG("IR: found %d dots", ir->num_dots); + + /* nothing to track */ + if (ir->num_dots == 0) { + if (ir->state != IR_STATE_DEAD) + ir->state = IR_STATE_LOST; + ir->ax = 0; + ir->ay = 0; + ir->distance = 0.0f; + ir->raw_valid = 0; + return; + } + + /* ==== Find the Sensor Bar ==== */ + + /* first rotate according to accelerometer orientation */ + rotate_dots(dots, acc_dots, ir->num_dots, orient->roll); + if (ir->num_dots > 1) { + WPAD2_DEBUG("IR: locating sensor bar candidates"); + + /* iterate through all dot pairs */ + for (first = 0; first < (ir->num_dots - 1); first++) { + for (second = (first + 1); second < ir->num_dots; second++) { + WPAD2_DEBUG("IR: trying dots %d and %d", first, second); + /* order the dots leftmost first into cand */ + /* storing both the raw dots and the accel-rotated dots */ + if (acc_dots[first].x > acc_dots[second].x) { + cand.dots[0] = dots[second]; + cand.dots[1] = dots[first]; + cand.acc_dots[0] = acc_dots[second]; + cand.acc_dots[1] = acc_dots[first]; + } else { + cand.dots[0] = dots[first]; + cand.dots[1] = dots[second]; + cand.acc_dots[0] = acc_dots[first]; + cand.acc_dots[1] = acc_dots[second]; + } + difference.x = cand.acc_dots[1].x - cand.acc_dots[0].x; + difference.y = cand.acc_dots[1].y - cand.acc_dots[0].y; + + /* check angle */ + if (fabsf(difference.y / difference.x) > MAX_SB_SLOPE) + continue; + WPAD2_DEBUG("IR: passed angle check"); + /* rotate to the true sensor bar angle */ + cand.off_angle = -RAD_TO_DEGREE(atan2(difference.y, difference.x)); + cand.angle = cand.off_angle + orient->roll; + rotate_dots(cand.dots, cand.rot_dots, 2, cand.angle); + WPAD2_DEBUG("IR: off_angle: %.02f, angle: %.02f", cand.off_angle, cand.angle); + /* recalculate x distance - y should be zero now, so ignore it */ + difference.x = cand.rot_dots[1].x - cand.rot_dots[0].x; + + /* check distance */ + if (difference.x < MIN_SB_WIDTH) + continue; + /* middle dot check. If there's another source somewhere in the */ + /* middle of this candidate, then this can't be a sensor bar */ + + for (i = 0; i < ir->num_dots; i++) { + float wadj, hadj; + struct fdot_t tdot; + if (i == first || i == second) continue; + hadj = SB_DOT_HEIGHT_RATIO * difference.x; + wadj = SB_DOT_WIDTH_RATIO * difference.x; + rotate_dots(&dots[i], &tdot, 1, cand.angle); + if (((cand.rot_dots[0].x + wadj) < tdot.x) && + ((cand.rot_dots[1].x - wadj) > tdot.x) && + ((cand.rot_dots[0].y + hadj) > tdot.y) && + ((cand.rot_dots[0].y - hadj) < tdot.y)) + break; + } + /* failed middle dot check */ + if (i < ir->num_dots) continue; + WPAD2_DEBUG("IR: passed middle dot check"); + + cand.score = 1 / (cand.rot_dots[1].x - cand.rot_dots[0].x); + + /* we have a candidate, store it */ + WPAD2_DEBUG("IR: new candidate %d", num_candidates); + candidates[num_candidates++] = cand; + } + } + } + + if (num_candidates == 0) { + int closest = -1; + int closest_to = 0; + float best = 999.0f; + float d; + float dx[2]; + struct sb_t sbx[2]; + /* no sensor bar candidates, try to work with a lone dot */ + WPAD2_DEBUG("IR: no candidates"); + switch (ir->state) { + case IR_STATE_DEAD: + WPAD2_DEBUG("IR: we're dead"); + /* we've never seen a sensor bar before, so we're screwed */ + ir->ax = 0.0f; + ir->ay = 0.0f; + ir->distance = 0.0f; + ir->raw_valid = 0; + return; + case IR_STATE_GOOD: + case IR_STATE_SINGLE: + case IR_STATE_LOST: + WPAD2_DEBUG("IR: trying to keep track of single dot"); + /* try to find the dot closest to the previous sensor bar position */ + for (i = 0; i < ir->num_dots; i++) { + WPAD2_DEBUG("IR: checking dot %d (%.02f, %.02f)", i, acc_dots[i].x, acc_dots[i].y); + for (j = 0; j < 2; j++) { + WPAD2_DEBUG(" to dot %d (%.02f, %.02f)", j, ir->sensorbar.acc_dots[j].x, ir->sensorbar.acc_dots[j].y); + d = SQUARED(acc_dots[i].x - ir->sensorbar.acc_dots[j].x); + d += SQUARED(acc_dots[i].y - ir->sensorbar.acc_dots[j].y); + if (d < best) { + best = d; + closest_to = j; + closest = i; + } + } + } + WPAD2_DEBUG("IR: closest dot is %d to %d", closest, closest_to); + if (ir->state != IR_STATE_LOST || best < SB_SINGLE_NOGUESS_DISTANCE) { + /* now work out where the other dot would be, in the acc frame */ + sb.acc_dots[closest_to] = acc_dots[closest]; + sb.acc_dots[closest_to ^ 1].x = ir->sensorbar.acc_dots[closest_to ^ 1].x - ir->sensorbar.acc_dots[closest_to].x + acc_dots[closest].x; + sb.acc_dots[closest_to ^ 1].y = ir->sensorbar.acc_dots[closest_to ^ 1].y - ir->sensorbar.acc_dots[closest_to].y + acc_dots[closest].y; + /* get the raw frame */ + rotate_dots(sb.acc_dots, sb.dots, 2, -orient->roll); + if ((fabsf(sb.dots[closest_to ^ 1].x) < SB_OFF_SCREEN_X) && (fabsf(sb.dots[closest_to ^ 1].y) < SB_OFF_SCREEN_Y)) { + /* this dot should be visible but isn't, since the candidate section failed. */ + /* fall through and try to pick out the sensor bar without previous information */ + WPAD2_DEBUG("IR: dot falls on screen, falling through"); + } else { + /* calculate the rotated dots frame */ + /* angle tends to drift, so recalculate */ + sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x)); + sb.angle = ir->sensorbar.off_angle + orient->roll; + rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle); + WPAD2_DEBUG("IR: kept track of single dot\n"); + break; + } + } else { + WPAD2_DEBUG("IR: lost the dot and new one is too far away"); + } + /* try to find the dot closest to the sensor edge */ + WPAD2_DEBUG("IR: trying to find best dot"); + for (i = 0; i < ir->num_dots; i++) { + d = WMIN(1.0f - fabsf(dots[i].x), HEIGHT - fabsf(dots[i].y)); + if (d < best) { + best = d; + closest = i; + } + } + WPAD2_DEBUG("IR: best dot: %d", closest); + /* now try it as both places in the sensor bar */ + /* and pick the one that places the other dot furthest off-screen */ + for (i = 0; i < 2; i++) { + sbx[i].acc_dots[i] = acc_dots[closest]; + sbx[i].acc_dots[i ^ 1].x = ir->sensorbar.acc_dots[i ^ 1].x - ir->sensorbar.acc_dots[i].x + acc_dots[closest].x; + sbx[i].acc_dots[i ^ 1].y = ir->sensorbar.acc_dots[i ^ 1].y - ir->sensorbar.acc_dots[i].y + acc_dots[closest].y; + rotate_dots(sbx[i].acc_dots, sbx[i].dots, 2, -orient->roll); + dx[i] = WMAX(fabsf(sbx[i].dots[i ^ 1].x), fabsf(sbx[i].dots[i ^ 1].y / HEIGHT)); + } + if (dx[0] > dx[1]) { + WPAD2_DEBUG("IR: dot is LEFT: %.02f > %.02f", dx[0], dx[1]); + sb = sbx[0]; + } else { + WPAD2_DEBUG("IR: dot is RIGHT: %.02f < %.02f", dx[0], dx[1]); + sb = sbx[1]; + } + /* angle tends to drift, so recalculate */ + sb.off_angle = -RAD_TO_DEGREE(atan2(sb.acc_dots[1].y - sb.acc_dots[0].y, sb.acc_dots[1].x - sb.acc_dots[0].x)); + sb.angle = ir->sensorbar.off_angle + orient->roll; + rotate_dots(sb.acc_dots, sb.rot_dots, 2, ir->sensorbar.off_angle); + WPAD2_DEBUG("IR: found new dot to track"); + break; + } + sb.score = 0; + ir->state = IR_STATE_SINGLE; + } else { + int bestidx = 0; + float best = 0.0f; + WPAD2_DEBUG("IR: finding best candidate"); + /* look for the best candidate */ + /* for now, the formula is simple: pick the one with the smallest distance */ + for (i = 0; i < num_candidates; i++) { + if (candidates[i].score > best) { + bestidx = i; + best = candidates[i].score; + } + } + WPAD2_DEBUG("IR: best candidate: %d", bestidx); + sb = candidates[bestidx]; + ir->state = IR_STATE_GOOD; + } + + ir->raw_valid = 1; + ir->ax = ((sb.rot_dots[0].x + sb.rot_dots[1].x) / 2) * 512.0 + 512.0; + ir->ay = ((sb.rot_dots[0].y + sb.rot_dots[1].y) / 2) * 512.0 + 384.0; + ir->sensorbar = sb; + ir->distance = (sb.rot_dots[1].x - sb.rot_dots[0].x) * 512.0; + +} + +#define SMOOTH_IR_RADIUS 8.0f +#define SMOOTH_IR_SPEED 0.25f +#define SMOOTH_IR_DEADZONE 2.5f + +/** + * @brief Smooth the IR pointer position + * + * @param ir Pointer to an ir_t structure. + */ +static void apply_ir_smoothing(struct ir_t *ir) { + f32 dx, dy, d, theta; + + WPAD2_DEBUG("Smooth: OK (%.02f, %.02f) LAST (%.02f, %.02f) ", ir->ax, ir->ay, ir->sx, ir->sy); + dx = ir->ax - ir->sx; + dy = ir->ay - ir->sy; + d = sqrtf(dx * dx + dy * dy); + if (d > SMOOTH_IR_DEADZONE) { + if (d < SMOOTH_IR_RADIUS) { + WPAD2_DEBUG("INSIDE"); + ir->sx += dx * SMOOTH_IR_SPEED; + ir->sy += dy * SMOOTH_IR_SPEED; + } else { + WPAD2_DEBUG("OUTSIDE"); + theta = atan2f(dy, dx); + ir->sx = ir->ax - cosf(theta) * SMOOTH_IR_RADIUS; + ir->sy = ir->ay - sinf(theta) * SMOOTH_IR_RADIUS; + } + } else { + WPAD2_DEBUG("DEADZONE"); + } +} + +/* max number of errors before cooked data drops out */ +#define ERROR_MAX_COUNT 8 +/* max number of glitches before cooked data updates */ +#define GLITCH_MAX_COUNT 5 +/* squared delta over which we consider something a glitch */ +#define GLITCH_DIST (150.0f * 150.0f) + +/** + * @brief Calculate yaw given the IR data. + * + * @param ir IR data structure. + */ +float calc_yaw(struct ir_t *ir) { + float x; + + x = ir->ax - 512; + x *= WIIMOTE_FOV_COEFFICIENT / 512.0; + + return RAD_TO_DEGREE(atanf(x)); +} + +/** + * @brief Interpret IR data into more user friendly variables. + * + * @param ir Pointer to an ir_t structure. + * @param orient Pointer to an orient_t structure. + */ +void _wpad2_interpret_ir_data(struct ir_t *ir, struct orient_t *orient) { + + float x, y; + float d; + + find_sensorbar(ir, orient); + + if (ir->raw_valid) { + ir->angle = ir->sensorbar.angle; + ir->z = SB_Z_COEFFICIENT / ir->distance; + orient->yaw = calc_yaw(ir); + if (ir->error_cnt >= ERROR_MAX_COUNT) { + ir->sx = ir->ax; + ir->sy = ir->ay; + ir->glitch_cnt = 0; + } else { + d = SQUARED(ir->ax - ir->sx) + SQUARED(ir->ay - ir->sy); + if (d > GLITCH_DIST) { + if (ir->glitch_cnt > GLITCH_MAX_COUNT) { + apply_ir_smoothing(ir); + ir->glitch_cnt = 0; + } else { + ir->glitch_cnt++; + } + } else { + ir->glitch_cnt = 0; + apply_ir_smoothing(ir); + } + } + ir->smooth_valid = 1; + ir->error_cnt = 0; + } else { + if (ir->error_cnt >= ERROR_MAX_COUNT) { + ir->smooth_valid = 0; + } else { + ir->smooth_valid = 1; + ir->error_cnt++; + } + } + if (ir->smooth_valid) { + x = ir->sx; + y = ir->sy; + if (ir_correct_for_bounds(&x, &y, ir->aspect, ir->offset[0], ir->offset[1])) { + ir_convert_to_vres(&x, &y, ir->aspect, ir->vres[0], ir->vres[1]); + ir->x = x; + ir->y = y; + ir->valid = 1; + } else { + ir->valid = 0; + } + } else { + ir->valid = 0; + } +} + +bool _wpad2_device_ir_step(WpadDevice *device) +{ + WPAD2_DEBUG("state = %d", device->state); + /* My original Wiimote responds with error 0x04 if sensitivity writes are + * issued before the previous has been acknowledged, so we separate all the + * writes into different steps. */ + if (device->state == STATE_IR_IDLE) { + if (ir_enable(device, device->ir_requested)) return true; + device->state = STATE_READY; + } else if (device->state == STATE_IR_ENABLING_1) { + uint8_t buf = device->ir_requested ? 0x04 : 0x00; + device->state = STATE_IR_ENABLING_2; + if (_wpad2_device_send_command(device, WM_CMD_IR_2, &buf, 1)) return true; + } else if (device->state == STATE_IR_ENABLING_2) { + if (device->ir_requested) { + if (ir_set_sensitivity(device)) return true; + } else { + device->ir_enabled = false; + device->state = STATE_READY; + } + } else if (device->state >= STATE_IR_SENSITIVITY_1 && + device->state < STATE_IR_SENSITIVITY_3) { + if (ir_set_sensitivity(device)) return true; + } else if (device->state == STATE_IR_SENSITIVITY_3) { + if (ir_set_mode(device)) return true; + } else if (device->state == STATE_IR_SET_MODE) { + device->ir_enabled = true; + device->state = STATE_READY; + } + + return false; +} + +bool _wpad2_device_ir_failed(WpadDevice *device) +{ + device->state = STATE_READY; + return false; +} + +void _wpad2_ir_sensor_bar_enable(bool enable) +{ + u32 level; + + level = IRQ_Disable(); + u32 val = (ACR_ReadReg(0xc0) & ~0x100); + if (enable) val |= 0x100; + ACR_WriteReg(0xc0, val); + IRQ_Restore(level); +} diff --git a/wpad2/ir.h b/wpad2/ir.h new file mode 100644 index 000000000..8659c2c4c --- /dev/null +++ b/wpad2/ir.h @@ -0,0 +1,17 @@ +#ifndef WPAD2_IR_H +#define WPAD2_IR_H + +#include "device.h" + +void _wpad2_interpret_ir_data(struct ir_t *ir, struct orient_t *orient); +void _wpad2_ir_parse_basic(WPADData *out, const uint8_t *data); +void _wpad2_ir_parse_extended(WPADData *out, const uint8_t *data); +void _wpad2_ir_set_vres(WPADData *data, unsigned int x, unsigned int y); +void _wpad2_ir_set_position(WPADData *data, enum ir_position_t pos); +void _wpad2_ir_set_aspect_ratio(WPADData *data, enum aspect_t aspect); +void _wpad2_ir_sensor_bar_enable(bool enable); + +bool _wpad2_device_ir_step(WpadDevice *device); +bool _wpad2_device_ir_failed(WpadDevice *device); + +#endif /* WPAD2_IR_H */ diff --git a/wpad2/motion_plus.c b/wpad2/motion_plus.c new file mode 100644 index 000000000..bbf9807b9 --- /dev/null +++ b/wpad2/motion_plus.c @@ -0,0 +1,123 @@ +#include "motion_plus.h" + +#include +#include +#include +#include + +bool _wpad2_device_motion_plus_read_mode_cb(WpadDevice *device, + const uint8_t *data, uint16_t len) +{ + WPAD2_DEBUG("got byte %02x", data[1]); + device->motion_plus_probed = true; + if (data[1] != 0x05) { + device->motion_plus_available = false; + device->motion_plus_enabled = false; + return false; + } + uint8_t val = 0x55; + if (!_wpad2_device_write_data(device, WM_EXP_MOTION_PLUS_ENABLE, &val, 1)) + return false; + + device->state = STATE_MP_INITIALIZING; + return true; +} + +static void probe_motion_plus(WpadDevice *device) +{ + device->state = STATE_MP_PROBE; + _wpad2_device_read_data(device, WM_EXP_MOTION_PLUS_MODE, 2); +} + +static bool motion_plus_check(WpadDevice *device) +{ + device->state = STATE_MP_IDENTIFICATION; + return _wpad2_device_read_data(device, WM_EXP_ID, 6); +} + +static bool set_clear1(WpadDevice *device) +{ + ubyte val = 0x00; + return _wpad2_device_write_data(device, WM_EXP_MEM_ENABLE1, &val, 1); +} + +bool _wpad2_device_motion_plus_step(WpadDevice *device) +{ + WPAD2_DEBUG("state = %d", device->state); + + if (!device->motion_plus_probed) { + probe_motion_plus(device); + return true; + } + + bool active = false; + if (device->state == STATE_MP_INITIALIZING) { + device->motion_plus_available = true; + } else if (device->state == STATE_MP_ENABLING) { + active = motion_plus_check(device); + } else if (device->state == STATE_MP_IDENTIFICATION) { + device->motion_plus_enabled = true; + /* Now that the motion plus is enabled, continue its setup as an + * ordinary expansion. Issue a status request, and this will report + * that the expansion is attached. */ + device->state = STATE_EXP_IDENTIFICATION; + active = _wpad2_device_request_status(device); + } else if (device->state == STATE_MP_DISABLING_1) { + active = set_clear1(device); + } else if (device->state == STATE_MP_DISABLING_2) { + device->motion_plus_enabled = false; + } + + if (active) return true; + + if (device->motion_plus_enabled == device->motion_plus_requested) + return false; + + uint8_t val; + + if (device->motion_plus_requested) { + val = 0x04; + active = _wpad2_device_write_data(device, WM_EXP_MOTION_PLUS_MODE, &val, 1); + device->state = STATE_MP_ENABLING; + } else { + val = 0x55; + active = _wpad2_device_write_data(device, WM_EXP_MEM_ENABLE1, &val, 1); + device->state = STATE_MP_DISABLING_1; + } + return active; +} + +bool _wpad2_device_motion_plus_failed(WpadDevice *device) +{ + WPAD2_DEBUG("state = %d", device->state); + + if (device->state == STATE_MP_PROBE) { + device->motion_plus_probed = true; + device->motion_plus_available = false; + } + + device->state = STATE_READY; + return false; +} + +void _wpad2_device_motion_plus_enable(WpadDevice *device, bool enable) +{ + device->motion_plus_requested = enable; + if (device->initialized) _wpad2_device_step(device); +} + +void _wpad2_motion_plus_disconnected(struct motion_plus_t *mp) +{ + WPAD2_DEBUG("Motion plus disconnected"); + memset(mp, 0, sizeof(struct motion_plus_t)); +} + +void _wpad2_motion_plus_event(struct motion_plus_t *mp, const uint8_t *msg) +{ + mp->rx = ((msg[5] & 0xFC) << 6) | msg[2]; /* Pitch */ + mp->ry = ((msg[4] & 0xFC) << 6) | msg[1]; /* Roll */ + mp->rz = ((msg[3] & 0xFC) << 6) | msg[0]; /* Yaw */ + + mp->ext = msg[4] & 0x1; + mp->status = (msg[3] & 0x3) | ((msg[4] & 0x2) << 1); /* roll, yaw, pitch */ +} diff --git a/wpad2/motion_plus.h b/wpad2/motion_plus.h new file mode 100644 index 000000000..05c16d097 --- /dev/null +++ b/wpad2/motion_plus.h @@ -0,0 +1,21 @@ +/** + * @file + * @brief Motion plus extension + */ + +#ifndef WPAD2_MOTION_PLUS_H +#define WPAD2_MOTION_PLUS_H + +#include "device.h" + +void _wpad2_motion_plus_disconnected(struct motion_plus_t *mp); +void _wpad2_motion_plus_event(struct motion_plus_t *mp, const uint8_t *msg); + +bool _wpad2_device_motion_plus_step(WpadDevice *device); +bool _wpad2_device_motion_plus_failed(WpadDevice *device); +void _wpad2_device_motion_plus_enable(WpadDevice *device, bool enable); + +bool _wpad2_device_motion_plus_read_mode_cb(WpadDevice *device, + const uint8_t *data, uint16_t len); + +#endif /* WPAD2_MOTION_PLUS_H */ diff --git a/wpad2/nunchuk.c b/wpad2/nunchuk.c new file mode 100644 index 000000000..29158cd42 --- /dev/null +++ b/wpad2/nunchuk.c @@ -0,0 +1,117 @@ +#include "nunchuk.h" + +#include "dynamics.h" + +#include +#include +#include +#include + +/** + * @brief Find what buttons are pressed. + * + * @param nc Pointer to a nunchuk_t structure. + * @param msg The message byte specified in the event packet. + */ +static void nunchuk_pressed_buttons(struct nunchuk_t *nc, uint8_t now) { + /* message is inverted (0 is active, 1 is inactive) */ + now = ~now & NUNCHUK_BUTTON_ALL; + + /* preserve old btns pressed */ + nc->btns_last = nc->btns; + + /* pressed now & were pressed, then held */ + nc->btns_held = (now & nc->btns); + + /* were pressed or were held & not pressed now, then released */ + nc->btns_released = ((nc->btns | nc->btns_held) & ~now); + + /* buttons pressed now */ + nc->btns = now; +} + +void _wpad2_nunchuk_calibrate(struct nunchuk_t *nc, + const WpadDeviceExpCalibrationData *cd) +{ + const uint8_t *data = cd->data; + + memset(nc, 0, sizeof(*nc)); + nc->accel_calib.cal_zero.x = (data[0] << 2) | ((data[3] >> 4) & 3); + nc->accel_calib.cal_zero.y = (data[1] << 2) | ((data[3] >> 2) & 3); + nc->accel_calib.cal_zero.z = (data[2] << 2) | (data[3] & 3); + + nc->accel_calib.cal_g.x = (((data[4] << 2) | ((data[7] >> 4) & 3)) - nc->accel_calib.cal_zero.x); + nc->accel_calib.cal_g.y = (((data[5] << 2) | ((data[7] >> 2) & 3)) - nc->accel_calib.cal_zero.y); + nc->accel_calib.cal_g.z = (((data[6] << 2) | (data[7] & 3)) - nc->accel_calib.cal_zero.z); + + nc->js.max.x = data[8]; + nc->js.min.x = data[9]; + nc->js.center.x = data[10]; + nc->js.max.y = data[11]; + nc->js.min.y = data[12]; + nc->js.center.y = data[13]; + + /* set to defaults (averages from 5 nunchuks) if calibration data is invalid */ + if (nc->accel_calib.cal_zero.x == 0) + nc->accel_calib.cal_zero.x = 499; + if (nc->accel_calib.cal_zero.y == 0) + nc->accel_calib.cal_zero.y = 509; + if (nc->accel_calib.cal_zero.z == 0) + nc->accel_calib.cal_zero.z = 507; + if (nc->accel_calib.cal_g.x == 0) + nc->accel_calib.cal_g.x = 703; + if (nc->accel_calib.cal_g.y == 0) + nc->accel_calib.cal_g.y = 709; + if (nc->accel_calib.cal_g.z == 0) + nc->accel_calib.cal_g.z = 709; + if (nc->js.max.x == 0) + nc->js.max.x = 223; + if (nc->js.min.x == 0) + nc->js.min.x = 27; + if (nc->js.center.x == 0) + nc->js.center.x = 126; + if (nc->js.max.y == 0) + nc->js.max.y = 222; + if (nc->js.min.y == 0) + nc->js.min.y = 30; + if (nc->js.center.y == 0) + nc->js.center.y = 131; + + nc->accel_calib.st_alpha = WPAD2_DEFAULT_SMOOTH_ALPHA; +} + +/** + * @brief Handle nunchuk event. + * + * @param nc A pointer to a nunchuk_t structure. + * @param msg The message specified in the event packet. + */ +void _wpad2_nunchuk_event(struct nunchuk_t *nc, const uint8_t *msg) { + /*int i; */ + + /* decrypt data */ + /* + for (i = 0; i < 6; ++i) + msg[i] = (msg[i] ^ 0x17) + 0x17; + */ + /* get button states */ + nunchuk_pressed_buttons(nc, msg[5]); + + nc->js.pos.x = msg[0]; + nc->js.pos.y = msg[1]; + + /* extend min and max values to physical range of motion */ + if (nc->js.center.x) { + if (nc->js.min.x > nc->js.pos.x) nc->js.min.x = nc->js.pos.x; + if (nc->js.max.x < nc->js.pos.x) nc->js.max.x = nc->js.pos.x; + } + if (nc->js.center.y) { + if (nc->js.min.y > nc->js.pos.y) nc->js.min.y = nc->js.pos.y; + if (nc->js.max.y < nc->js.pos.y) nc->js.max.y = nc->js.pos.y; + } + + /* calculate orientation */ + nc->accel.x = (msg[2] << 2) + ((msg[5] >> 2) & 3); + nc->accel.y = (msg[3] << 2) + ((msg[5] >> 4) & 3); + nc->accel.z = (msg[4] << 2) + ((msg[5] >> 6) & 3); +} diff --git a/wpad2/nunchuk.h b/wpad2/nunchuk.h new file mode 100644 index 000000000..ed6a6d3b4 --- /dev/null +++ b/wpad2/nunchuk.h @@ -0,0 +1,10 @@ +#ifndef WPAD2_NUNCHUK_H +#define WPAD2_NUNCHUK_H + +#include "device.h" + +void _wpad2_nunchuk_calibrate(struct nunchuk_t *nc, + const WpadDeviceExpCalibrationData *cd); +void _wpad2_nunchuk_event(struct nunchuk_t *nc, const uint8_t *msg); + +#endif /* WPAD2_NUNCHUK_H */ diff --git a/wpad2/speaker.c b/wpad2/speaker.c new file mode 100644 index 000000000..659cc6244 --- /dev/null +++ b/wpad2/speaker.c @@ -0,0 +1,195 @@ +/*------------------------------------------------------------- + +Copyright (C) 2008-2026 +Michael Wiedenbauer (shagkur) +Dave Murphy (WinterMute) +Hector Martin (marcan) +Zarithya +Alberto Mardegan (mardy) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you +must not claim that you wrote the original software. If you use +this software in a product, an acknowledgment in the product +documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +-------------------------------------------------------------*/ + +#include "speaker.h" + +#include +#include + +#define WENCMIN(a,b) ((a)>(b)?(b):(a)) +#define ABS(x) ((s32)(x)>0?(s32)(x):-((s32)(x))) + +static const int yamaha_indexscale[] = { + 230, 230, 230, 230, 307, 409, 512, 614, + 230, 230, 230, 230, 307, 409, 512, 614 +}; + +static const int yamaha_difflookup[] = { + 1, 3, 5, 7, 9, 11, 13, 15, + -1, -3, -5, -7, -9, -11, -13, -15 +}; + +static const uint8_t s_speaker_defconf[7] = { + 0x00, 0x00, 0xD0, 0x07, 0x40, 0x0C, 0x0E +}; + +static inline int16_t wenc_clip_short(int a) +{ + if ((a + 32768) & ~65535) return (a >> 31) ^ 32767; + else return a; +} + +static inline int wenc_clip(int a, int amin, int amax) +{ + if (a < amin) return amin; + else if (a > amax) return amax; + else return a; +} + +static uint8_t wencdata(WENCStatus *info, short sample) +{ + if (!info->step) { + info->predictor = 0; + info->step = 127; + } + + int delta = sample - info->predictor; + int nibble = WENCMIN(7, (ABS(delta) * 4) / info->step) + (delta < 0) * 8; + + info->predictor += (info->step * yamaha_difflookup[nibble]) / 8; + info->predictor = wenc_clip_short(info->predictor); + info->step = (info->step * yamaha_indexscale[nibble]) >> 8; + info->step = wenc_clip(info->step, 127, 24576); + + return nibble; +} + +void _wpad2_speaker_encode(WENCStatus *status, uint32_t flag, + const int16_t *pcm_samples, int num_samples, uint8_t *out) +{ + if (!(flag & WPAD_ENC_CONT)) status->step = 0; + + for (int n = (num_samples + 1) / 2; n > 0; n--) { + uint8_t nibble; + nibble = (wencdata(status, pcm_samples[0])) << 4; + nibble |= (wencdata(status, pcm_samples[1])); + *out++ = nibble; + pcm_samples += 2; + } +} + +bool _wpad2_device_speaker_step(WpadDevice *device) +{ + if (device->speaker_enabled == device->speaker_requested) { + /* Nothing to do */ + return false; + } + + if (device->state == STATE_SPEAKER_DISABLING) { + device->state = STATE_READY; + return false; + } + + uint8_t buf = 0x04; + _wpad2_device_send_command(device, WM_CMD_SPEAKER_MUTE, &buf, 1); + + if (!device->speaker_requested) { + device->state = STATE_SPEAKER_DISABLING; + WPAD2_DEBUG("Disabling speaker for wiimote id %d", device->unid); + + buf = 0x01; + _wpad2_device_write_data(device, WM_REG_SPEAKER_REG1, &buf, 1); + + buf = 0x00; + _wpad2_device_write_data(device, WM_REG_SPEAKER_REG3, &buf, 1); + + buf = 0x00; + _wpad2_device_send_command(device, WM_CMD_SPEAKER_ENABLE, &buf, 1); + device->speaker_enabled = false; + return true; + } + + if (device->state == STATE_SPEAKER_FIRST) { + WPAD2_DEBUG("Enabling speaker for wiimote id %d", device->unid); + device->state = STATE_SPEAKER_ENABLING_1; + buf = 0x04; + _wpad2_device_send_command(device, WM_CMD_SPEAKER_ENABLE, &buf, 1); + } else if (device->state == STATE_SPEAKER_ENABLING_1) { + device->state = STATE_SPEAKER_ENABLING_2; + buf = 0x01; + _wpad2_device_write_data(device, WM_REG_SPEAKER_REG3, &buf, 1); + } else if (device->state == STATE_SPEAKER_ENABLING_2) { + device->state = STATE_SPEAKER_ENABLING_3; + buf = 0x08; + _wpad2_device_write_data(device, WM_REG_SPEAKER_REG1, &buf, 1); + } else if (device->state == STATE_SPEAKER_ENABLING_3) { + device->state = STATE_SPEAKER_ENABLING_4; + uint8_t conf[7]; + memcpy(conf, s_speaker_defconf, 7); + conf[4] = device->speaker_volume; + _wpad2_device_write_data(device, WM_REG_SPEAKER_BLOCK, conf, 7); + } else if (device->state == STATE_SPEAKER_ENABLING_4) { + device->state = STATE_SPEAKER_ENABLING_5; + buf = 0x01; + _wpad2_device_write_data(device, WM_REG_SPEAKER_REG2, &buf, 1); + } else if (device->state == STATE_SPEAKER_ENABLING_5) { + device->state = STATE_SPEAKER_ENABLING_6; + buf = 0x00; + _wpad2_device_send_command(device, WM_CMD_SPEAKER_MUTE, &buf, 1); + } else if (device->state == STATE_SPEAKER_ENABLING_6) { + device->state = STATE_READY; + _wpad2_device_request_status(device); + } + + return true; +} + +void _wpad2_speaker_play_part(WpadDevice *device) +{ + WPAD2_DEBUG("Playing at offset %d", device->sound->offset); + WpadSoundInfo *sound = device->sound; + + if (sound->offset < sound->len) { + int size = sound->len - sound->offset; + if (size > 20) size = 20; + _wpad2_device_stream_data(device, sound->samples + sound->offset, size); + sound->offset += size; + } + if (sound->offset >= sound->len) { + device->sound = NULL; + free(sound); + _wpad2_device_set_timer(device, 0); + } +} + +void _wpad2_speaker_play(WpadDevice *device, WpadSoundInfo *sound) +{ + /* If currently playing, stop */ + if (device->sound) { + free(device->sound); + device->sound = NULL; + _wpad2_device_set_timer(device, 0); + } + + if (!sound) return; + + device->sound = sound; + _wpad2_device_set_timer(device, 6667); +} diff --git a/wpad2/speaker.h b/wpad2/speaker.h new file mode 100644 index 000000000..dbb782080 --- /dev/null +++ b/wpad2/speaker.h @@ -0,0 +1,25 @@ +#ifndef WPAD2_SPEAKER_H +#define WPAD2_SPEAKER_H + +#include "device.h" + +typedef struct { + s32 predictor; + s16 step_index; + s32 step; + s32 prev_sample; + s16 sample1; + s16 sample2; + s32 coeff1; + s32 coeff2; + s32 idelta; +} WENCStatus; + +void _wpad2_speaker_encode(WENCStatus *info, uint32_t flag, + const int16_t *pcm_samples, int num_samples, uint8_t *out); +bool _wpad2_device_speaker_step(WpadDevice *device); + +void _wpad2_speaker_play_part(WpadDevice *device); +void _wpad2_speaker_play(WpadDevice *device, WpadSoundInfo *sound); + +#endif /* WPAD2_SPEAKER_H */ diff --git a/wpad2/wii_board.c b/wpad2/wii_board.c new file mode 100644 index 000000000..02cb4adae --- /dev/null +++ b/wpad2/wii_board.c @@ -0,0 +1,68 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /cvsroot/devkitpro/libogc/wiiuse/wiiboard.c,v 1.6 2008/05/26 19:24:53 shagkur Exp $ + * + */ + +/** + * @file + * @brief Wiiboard expansion device. + */ + +#include "wii_board.h" + +#include +#include +#include +#include + +void _wpad2_wii_board_calibrate(struct wii_board_t *wb, + const WpadDeviceExpCalibrationData *cd) +{ + const uint8_t *data = cd->data; + + wb->ctr[0] = (data[4] << 8) | data[5]; + wb->cbr[0] = (data[6] << 8) | data[7]; + wb->ctl[0] = (data[8] << 8) | data[9]; + wb->cbl[0] = (data[10] << 8) | data[11]; + + wb->ctr[1] = (data[12] << 8) | data[13]; + wb->cbr[1] = (data[14] << 8) | data[15]; + wb->ctl[1] = (data[16] << 8) | data[17]; + wb->cbl[1] = (data[18] << 8) | data[19]; + + wb->ctr[2] = (data[20] << 8) | data[21]; + wb->cbr[2] = (data[22] << 8) | data[23]; + wb->ctl[2] = (data[24] << 8) | data[25]; + wb->cbl[2] = (data[26] << 8) | data[27]; +} + +void _wpad2_wii_board_event(struct wii_board_t *wb, const uint8_t *msg) +{ + wb->rtr = (msg[0] << 8) | msg[1]; + wb->rbr = (msg[2] << 8) | msg[3]; + wb->rtl = (msg[4] << 8) | msg[5]; + wb->rbl = (msg[6] << 8) | msg[7]; +} diff --git a/wpad2/wii_board.h b/wpad2/wii_board.h new file mode 100644 index 000000000..96248951c --- /dev/null +++ b/wpad2/wii_board.h @@ -0,0 +1,43 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /cvsroot/devkitpro/libogc/wiiuse/wiiboard.h,v 1.1 2008/05/08 09:42:14 shagkur Exp $ + * + */ + +/** + * @file + * @brief Wii board expansion device. + */ + +#ifndef WII_BOARD_H +#define WII_BOARD_H + +#include "device.h" + +void _wpad2_wii_board_calibrate(struct wii_board_t *wb, + const WpadDeviceExpCalibrationData *cd); +void _wpad2_wii_board_event(struct wii_board_t *wb, const uint8_t *msg); + +#endif /* WPAD2_WII_BOARD_H */ diff --git a/wpad2/wiiuse_internal.h b/wpad2/wiiuse_internal.h new file mode 100644 index 000000000..7fea55622 --- /dev/null +++ b/wpad2/wiiuse_internal.h @@ -0,0 +1,278 @@ +/* + * wiiuse + * + * Written By: + * Michael Laforest < para > + * Email: < thepara (--AT--) g m a i l [--DOT--] com > + * + * Copyright 2006-2007 + * + * This file is part of wiiuse. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * $Header: /lvm/shared/ds/ds/cvs/devkitpro-cvsbackup/libogc/wiiuse/wiiuse_internal.h,v 1.8 2008-12-10 16:16:40 shagkur Exp $ + * + */ + +/** + * @file + * @brief General internal wiiuse stuff. + * + * Since Wiiuse is a library, wiiuse.h is a duplicate + * of the API header. + * + * The code that would normally go in that file, but + * which is not needed by third party developers, + * is put here. + * + * So wiiuse_internal.h is included by other files + * internally, wiiuse.h is included only here. + */ + +#ifndef WIIUSE_INTERNAL_H_INCLUDED +#define WIIUSE_INTERNAL_H_INCLUDED + +#if defined(__linux__) + #include /* htons() */ + #include +#endif + +/* wiiuse version */ +#define WIIUSE_VERSION "0.12" + +/******************** + * + * Wiimote internal codes + * + ********************/ + +/* Communication channels */ +#define WM_OUTPUT_CHANNEL 0x11 +#define WM_INPUT_CHANNEL 0x13 + +#define WM_SET_REPORT 0x50 +#define WM_DATA 0xA0 + +/* commands */ +#define WM_CMD_RUMBLE 0x10 +#define WM_CMD_LED 0x11 +#define WM_CMD_REPORT_TYPE 0x12 +#define WM_CMD_IR 0x13 +#define WM_CMD_SPEAKER_ENABLE 0x14 +#define WM_CMD_CTRL_STATUS 0x15 +#define WM_CMD_WRITE_DATA 0x16 +#define WM_CMD_READ_DATA 0x17 +#define WM_CMD_STREAM_DATA 0x18 +#define WM_CMD_SPEAKER_MUTE 0x19 +#define WM_CMD_IR_2 0x1A + +/* input report ids */ +#define WM_RPT_CTRL_STATUS 0x20 +#define WM_RPT_READ 0x21 +#define WM_RPT_ACK 0x22 +#define WM_RPT_BTN 0x30 +#define WM_RPT_BTN_ACC 0x31 +#define WM_RPT_BTN_ACC_IR 0x33 +#define WM_RPT_BTN_EXP 0x34 +#define WM_RPT_BTN_ACC_EXP 0x35 +#define WM_RPT_BTN_IR_EXP 0x36 +#define WM_RPT_BTN_ACC_IR_EXP 0x37 + +#define WM_BT_INPUT 0x01 +#define WM_BT_OUTPUT 0x02 + +/* Identify the wiimote device by its class */ +#define WM_DEV_CLASS_0 0x04 +#define WM_DEV_CLASS_1 0x25 +#define WM_DEV_CLASS_2 0x00 +#define WM_VENDOR_ID 0x057E +#define WM_PRODUCT_ID 0x0306 + +/* controller status stuff */ +#define WM_MAX_BATTERY_CODE 0xC8 + +/* offsets in wiimote memory */ +#define WM_MEM_OFFSET_CALIBRATION 0x16 +#define WM_EXP_MEM_BASE 0x04A40000 +#define WM_EXP_MEM_ENABLE1 0x04A400F0 +#define WM_EXP_MEM_ENABLE2 0x04A400FB +#define WM_EXP_MEM_KEY 0x04A40040 +#define WM_EXP_MEM_CALIBR 0x04A40020 +#define WM_EXP_MOTION_PLUS_ENABLE 0x04A600F0 +#define WM_EXP_MOTION_PLUS_MODE 0x04A600FE +#define WM_EXP_ID 0x04A400FA + +#define WM_REG_IR 0x04B00030 +#define WM_REG_IR_BLOCK1 0x04B00000 +#define WM_REG_IR_BLOCK2 0x04B0001A +#define WM_REG_IR_MODENUM 0x04B00033 + +#define WM_REG_SPEAKER_REG1 0x04A20001 +#define WM_REG_SPEAKER_REG2 0x04A20008 +#define WM_REG_SPEAKER_REG3 0x04A20009 +#define WM_REG_SPEAKER_BLOCK 0x04A20001 + +/* ir block data */ +#define WM_IR_BLOCK1_LEVEL1 (const uint8_t *)"\x02\x00\x00\x71\x01\x00\x64\x00\xfe" +#define WM_IR_BLOCK2_LEVEL1 (const uint8_t *)"\xfd\x05" +#define WM_IR_BLOCK1_LEVEL2 (const uint8_t *)"\x02\x00\x00\x71\x01\x00\x96\x00\xb4" +#define WM_IR_BLOCK2_LEVEL2 (const uint8_t *)"\xb3\x04" +#define WM_IR_BLOCK1_LEVEL3 (const uint8_t *)"\x02\x00\x00\x71\x01\x00\xaa\x00\x64" +#define WM_IR_BLOCK2_LEVEL3 (const uint8_t *)"\x63\x03" +#define WM_IR_BLOCK1_LEVEL4 (const uint8_t *)"\x02\x00\x00\x71\x01\x00\xc8\x00\x36" +#define WM_IR_BLOCK2_LEVEL4 (const uint8_t *)"\x35\x03" +#define WM_IR_BLOCK1_LEVEL5 (const uint8_t *)"\x07\x00\x00\x71\x01\x00\x72\x00\x20" +#define WM_IR_BLOCK2_LEVEL5 (const uint8_t *)"\x1f\x03" + +#define WM_IR_TYPE_BASIC 0x01 +#define WM_IR_TYPE_EXTENDED 0x03 +#define WM_IR_TYPE_FULL 0x05 + +/* controller status flags for the first message byte */ +#define WM_CTRL_STATUS_BYTE1_BATTERY_CRITICAL 0x01 +#define WM_CTRL_STATUS_BYTE1_ATTACHMENT 0x02 +#define WM_CTRL_STATUS_BYTE1_SPEAKER_ENABLED 0x04 +#define WM_CTRL_STATUS_BYTE1_IR_ENABLED 0x08 +#define WM_CTRL_STATUS_BYTE1_LED_1 0x10 +#define WM_CTRL_STATUS_BYTE1_LED_2 0x20 +#define WM_CTRL_STATUS_BYTE1_LED_3 0x40 +#define WM_CTRL_STATUS_BYTE1_LED_4 0x80 + +/* aspect ratio */ +#define WM_ASPECT_16_9_X 660 +#define WM_ASPECT_16_9_Y 370 +#define WM_ASPECT_4_3_X 560 +#define WM_ASPECT_4_3_Y 420 + + +/** + * Expansion stuff + */ + +/* encrypted expansion id codes (located at 0x04A400FC) */ +#define EXP_ID_CODE_NUNCHUK 0xa4200000 +#define EXP_ID_CODE_CLASSIC_CONTROLLER 0xa4200101 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING 0x90908f00 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING2 0x9e9f9c00 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_NYKOWING3 0x908f8f00 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC 0xa5a2a300 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC2 0x98999900 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC3 0xa0a1a000 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC4 0x8d8d8e00 +#define EXP_ID_CODE_CLASSIC_CONTROLLER_GENERIC5 0x93949400 +#define EXP_ID_CODE_CLASSIC_WIIU_PRO 0xa4200120 +#define EXP_ID_CODE_GUITAR 0xa4200103 +#define EXP_ID_CODE_WIIBOARD 0xa4200402 +#define EXP_ID_CODE_MOTION_PLUS 0xa4200405 + +#define EXP_HANDSHAKE_LEN 224 +#define EXP_CALIBRATION_DATA_LEN 16 + +/******************** + * + * End Wiimote internal codes + * + ********************/ + +/* wiimote state flags - (some duplicated in wiiuse.h)*/ +#define WIIMOTE_STATE_DEV_FOUND 0x000001 +#define WIIMOTE_STATE_BATTERY_CRITICAL 0x000002 +#define WIIMOTE_STATE_HANDSHAKE 0x000004 /* actual connection exists but no handshake yet */ +#define WIIMOTE_STATE_HANDSHAKE_COMPLETE 0x000008 /* actual connection exists but no handshake yet */ +#define WIIMOTE_STATE_CONNECTED 0x000010 +#define WIIMOTE_STATE_EXP_HANDSHAKE 0x000020 /* actual connection exists but no handshake yet */ +#define WIIMOTE_STATE_EXP_FAILED 0x000040 /* actual connection exists but no handshake yet */ +#define WIIMOTE_STATE_RUMBLE 0x000080 +#define WIIMOTE_STATE_ACC 0x000100 +#define WIIMOTE_STATE_EXP 0x000200 +#define WIIMOTE_STATE_IR 0x000400 +#define WIIMOTE_STATE_SPEAKER 0x000800 +#define WIIMOTE_STATE_IR_SENS_LVL1 0x001000 +#define WIIMOTE_STATE_IR_SENS_LVL2 0x002000 +#define WIIMOTE_STATE_IR_SENS_LVL3 0x004000 +#define WIIMOTE_STATE_IR_SENS_LVL4 0x008000 +#define WIIMOTE_STATE_IR_SENS_LVL5 0x010000 +#define WIIMOTE_STATE_IR_INIT 0x020000 +#define WIIMOTE_STATE_SPEAKER_INIT 0x040000 +#define WIIMOTE_STATE_WIIU_PRO 0x080000 +#define WIIMOTE_STATE_MPLUS_PRESENT 0x100000 + +#define WIIMOTE_INIT_STATES (WIIMOTE_STATE_IR_SENS_LVL3) + +/* macro to manage states */ +#define WIIMOTE_IS_SET(wm, s) ((wm->state & (s)) == (s)) +#define WIIMOTE_ENABLE_STATE(wm, s) (wm->state |= (s)) +#define WIIMOTE_DISABLE_STATE(wm, s) (wm->state &= ~(s)) +#define WIIMOTE_TOGGLE_STATE(wm, s) ((wm->state & (s)) ? WIIMOTE_DISABLE_STATE(wm, s) : WIIMOTE_ENABLE_STATE(wm, s)) + +#define WIIMOTE_IS_FLAG_SET(wm, s) ((wm->flags & (s)) == (s)) +#define WIIMOTE_ENABLE_FLAG(wm, s) (wm->flags |= (s)) +#define WIIMOTE_DISABLE_FLAG(wm, s) (wm->flags &= ~(s)) +#define WIIMOTE_TOGGLE_FLAG(wm, s) ((wm->flags & (s)) ? WIIMOTE_DISABLE_FLAG(wm, s) : WIIMOTE_ENABLE_FLAG(wm, s)) + +#define NUNCHUK_IS_FLAG_SET(wm, s) ((*(wm->flags) & (s)) == (s)) + +/* misc macros */ +#define WIIMOTE_ID(wm) (wm->unid) +#define WIIMOTE_IS_CONNECTED(wm) (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_CONNECTED)) + +/* + * Smooth tilt calculations are computed with the + * exponential moving average formula: + * St = St_last + (alpha * (tilt - St_last)) + * alpha is between 0 and 1 + */ +#define WIIUSE_DEFAULT_SMOOTH_ALPHA 0.3f + +#define SMOOTH_ROLL 0x01 +#define SMOOTH_PITCH 0x02 + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +struct op_t +{ + ubyte cmd; + union { + struct { + uint addr; + uword size; + } readdata; + struct { + uint addr; + ubyte size; + ubyte data[16]; + } writedata; + ubyte __data[MAX_PAYLOAD]; + }; + + void *buffer; + int wait; +} __attribute__((packed)); + +/* not part of the api */ +void wiiuse_init_cmd_queue(struct wiimote_t *wm); +void wiiuse_send_next_command(struct wiimote_t *wm); +int wiiuse_set_report_type(struct wiimote_t *wm, cmd_blk_cb cb); +int wiiuse_sendcmd(struct wiimote_t *wm, ubyte report_type, ubyte *msg, int len, cmd_blk_cb cb); + +#ifdef __cplusplus +} +#endif + +#endif /* WIIUSE_INTERNAL_H_INCLUDED */ diff --git a/wpad2/worker.c b/wpad2/worker.c new file mode 100644 index 000000000..16ecf63ac --- /dev/null +++ b/wpad2/worker.c @@ -0,0 +1,1283 @@ +/*------------------------------------------------------------- + +Copyright (C) 2008-2026 +Michael Wiedenbauer (shagkur) +Dave Murphy (WinterMute) +Hector Martin (marcan) +Zarithya +Alberto Mardegan (mardy) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you +must not claim that you wrote the original software. If you use +this software in a product, an acknowledgment in the product +documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +-------------------------------------------------------------*/ + +#include "worker.h" + +#include "device.h" +#include "ir.h" +#include "motion_plus.h" +#include "speaker.h" + +#include "conf.h" +#include "bt-embedded/bte.h" +#include "bt-embedded/client.h" +#include "bt-embedded/l2cap_server.h" +#include "bt-embedded/services/hid.h" +#include "tuxedo/mailbox.h" +#include "tuxedo/thread.h" + +#include +#include +#include + +#define MAX_INQUIRY_RESPONSES 8 +#define CONF_PAD_MAX_ACTIVE 4 +#define MAX_LINK_KEYS WPAD2_MAX_DEVICES + +#define WPAD2_VENDOR_EVENT_PAIRING 0x08 +#define WPAD2_VENDOR_EVENT_WIPE_PAIRED 0x09 + +static struct { + int num_responses; + int num_names_queried; + BteHciInquiryResponse responses[MAX_INQUIRY_RESPONSES]; +} *s_inquiry_responses = NULL; + +static BteClient *s_client; +static BteL2capServer *s_l2cap_server_hid_ctrl; +static BteL2capServer *s_l2cap_server_hid_intr; +static enum { + WPAD_PAIR_MODE_NORMAL, + WPAD_PAIR_MODE_TEMPORARY, +} s_pair_mode; +static BteBdAddr s_local_address; +static int8_t s_num_stored_keys = -1; /* Not retrieved */ +static BteHciStoredLinkKey s_stored_keys[MAX_LINK_KEYS]; + +static BtePacketType s_packet_types = 0; +static char s_nintendo_rvl[] = "Nintendo RVL-"; + +static conf_pads s_wpad_paired = {0}; +static conf_pad_guests s_wpad_guests = {0}; + +#define WORKER_THREAD_STACK_SIZE (4 * 1024) +static KThread s_worker_thread; +static uint8_t *s_worker_thread_stack = NULL; +static bool s_worker_thread_done = false; +static WpadEventCb s_worker_thread_event_cb; + +/* Incoming commands. */ +#define WORKER_CMD_OPCODE_MASK 0x0000FFFF +#define WORKER_CMD_PARAM_MASK 0xFFFF0000 +#define WORKER_CMD_OPCODE(cmd) ((cmd) & WORKER_CMD_OPCODE_MASK) +#define WORKER_CMD_PARAMS(cmd) ((cmd) >> 16) +#define WORKER_CMD(opcode, params) (WORKER_CMD_##opcode | ((params) << 16)) +enum { + WORKER_CMD_SET_SEARCH, + WORKER_CMD_SET_FORMAT, + WORKER_CMD_SET_MOTION_PLUS, + WORKER_CMD_SET_RUMBLE, + WORKER_CMD_SET_SPEAKER, + WORKER_CMD_PLAY_SOUND, + WORKER_CMD_DISCONNECT, + WORKER_CMD_WIPE_CONTROLLERS, + WORKER_CMD_START_PAIRING, +}; +static KMailbox s_mailbox_in; +static uptr s_mailbox_in_slots[16]; + +static uint32_t s_timer_period = 0; +static uint64_t s_timer_last_trigger; +static int s_timer_clients = 0; + +static inline uint64_t get_time_us() +{ + return PPCTicksToUs(PPCGetTickCount()); +} + +static inline bool bd_address_is_equal(const BteBdAddr *dest, + const BteBdAddr *src) +{ + return memcmp(dest, src, sizeof(BteBdAddr)) == 0; +} + +static inline void bd_address_copy(BteBdAddr *dest, const BteBdAddr *src) +{ + *dest = *src; +} + +static inline void bd_address_to_conf(const BteBdAddr *a, u8 *conf) +{ + const uint8_t *b = a->bytes; + conf[0] = b[5]; + conf[1] = b[4]; + conf[2] = b[3]; + conf[3] = b[2]; + conf[4] = b[1]; + conf[5] = b[0]; +} + +static inline BteBdAddr bd_address_from_conf(const u8 *conf) +{ + BteBdAddr addr = {{ conf[5], conf[4], conf[3], conf[2], conf[1], conf[0] }}; + return addr; +} + +static void query_name_next(BteHci *hci); + +static inline bool is_initialized() +{ + return s_num_stored_keys >= 0; +} + +static WpadDevice *wpad_device_from_addr(const BteBdAddr *address) +{ + for (int i = 0; i < WPAD2_MAX_DEVICES; i++) { + WpadDevice *device = _wpad2_device_get(i); + if (bd_address_is_equal(address, &device->address)) { + return device; + } + } + return NULL; +} + +static bool wpad_device_is_pairing(const BteBdAddr *address) +{ + WpadDevice *device = wpad_device_from_addr(address); + return device ? (device->hid_ctrl == NULL) : false; +} + +static void device_event_connected(WpadDevice *device) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_CONNECTED; + s_worker_thread_event_cb(&event); +} + +static int GetActiveSlot(const BteBdAddr *pad_addr) +{ + for (int i = 0; i < CONF_PAD_MAX_ACTIVE; i++) { + BteBdAddr bdaddr = bd_address_from_conf(s_wpad_paired.active[i].bdaddr); + if (bd_address_is_equal(pad_addr, &bdaddr)) { + return i; + } + } + + return -1; +} + +static void process_handshake(WpadDevice *device, uint8_t param, + BteBufferReader *reader) +{ + WPAD2_DEBUG("param %02x, length: %d", param, reader->packet->size - reader->pos_in_packet); +} + +static void process_data(WpadDevice *device, uint8_t param, + BteBufferReader *reader) +{ + WPAD2_DEBUG("param %02x, length: %d", param, reader->packet->size - reader->pos_in_packet); + if (param != BTE_HID_REP_TYPE_INPUT) { + /* Ignore */ + return; + } + + /* Here it's as being in __wiiuse_receive */ + uint8_t buffer[MAX_PAYLOAD] = { 0, }; + uint16_t len = bte_buffer_reader_read(reader, buffer, sizeof(buffer)); + if (len < 3) return; + + _wpad2_device_event(device, buffer, len); +} + +static void message_received_cb(BteL2cap *l2cap, BteBufferReader *reader, + void *userdata) +{ + WpadDevice *device = userdata; + uint8_t *hdr_ptr = bte_buffer_reader_read_n(reader, 1); + if (!hdr_ptr) return; + + uint8_t hdr = *hdr_ptr; + uint8_t type = hdr & BTE_HID_HDR_TRANS_MASK; + uint8_t param = hdr & BTE_HID_HDR_PARAM_MASK; + switch (type) { + case BTE_HID_TRANS_HANDSHAKE: + process_handshake(device, param, reader); + break; + case BTE_HID_TRANS_DATA: + process_data(device, param, reader); + break; + default: + WPAD2_DEBUG("got transaction %02x", type); + } +} + +static void device_set_active(WpadDevice *device) +{ + int slot = _wpad2_device_get_slot(device); + uint8_t conf_addr[6]; + bd_address_to_conf(&device->address, conf_addr); + uint8_t *dest = s_wpad_paired.active[slot].bdaddr; + if (memcmp(dest, conf_addr, sizeof(conf_addr)) != 0) { + memcpy(dest, conf_addr, sizeof(conf_addr)); + CONF_SetPadDevices(&s_wpad_paired); + CONF_SaveChanges(); + } +} + +static void device_register_paired(const BteBdAddr *address, const char *name) +{ + /* Store the key on the guest CONF data */ + int slot = -1; + uint8_t conf_addr[6]; + bd_address_to_conf(address, conf_addr); + for (int i = 0; i < s_wpad_paired.num_registered; i++) { + if (memcmp(conf_addr, s_wpad_paired.registered[i].bdaddr, 6) == 0) { + slot = i; + break; + } + } + + /* move the slots so that this gets into the first position, since we + * want the most recent devices first */ + int n_move = 0; + if (slot > 0) { + n_move = slot; + } else if (slot < 0) { + if (s_wpad_paired.num_registered < CONF_PAD_MAX_REGISTERED) { + n_move = s_wpad_paired.num_registered; + s_wpad_paired.num_registered++; + } else { + n_move = CONF_PAD_MAX_REGISTERED - 1; + } + } + + if (n_move > 0) { + memmove(&s_wpad_paired.registered[1], + &s_wpad_paired.registered[0], + sizeof(conf_pad_device) * n_move); + } + + memcpy(s_wpad_paired.registered[0].bdaddr, conf_addr, 6); + const int name_size = sizeof(s_wpad_paired.registered[0].name); + strncpy(s_wpad_paired.registered[0].name, name, name_size); + /* Make sure the string is zero-terminated */ + s_wpad_paired.registered[0].name[name_size - 1] = '\0'; +} + +static void device_register_guest(const BteBdAddr *address, const char *name) +{ + /* Store the key on the guest CONF data */ + int slot = -1; + uint8_t conf_addr[6]; + bd_address_to_conf(address, conf_addr); + for (int i = 0; i < s_wpad_guests.num_guests; i++) { + if (memcmp(conf_addr, s_wpad_guests.guests[i].bdaddr, 6) == 0) { + slot = i; + break; + } + } + + /* move the slots so that this gets into the first position, since we + * want the most recent guests first */ + int n_move = 0; + if (slot > 0) { + n_move = slot; + } else if (slot < 0) { + if (s_wpad_guests.num_guests < CONF_PAD_MAX_ACTIVE) { + n_move = s_wpad_guests.num_guests; + s_wpad_guests.num_guests++; + } else { + n_move = CONF_PAD_MAX_ACTIVE - 1; + } + } + + if (n_move > 0) { + memmove(&s_wpad_guests.guests[1], + &s_wpad_guests.guests[0], + sizeof(conf_pad_guest_device) * n_move); + } + + memcpy(s_wpad_guests.guests[0].bdaddr, conf_addr, 6); + const int name_size = sizeof(s_wpad_guests.guests[0].name); + strncpy(s_wpad_guests.guests[0].name, name, name_size); + /* Make sure the string is zero-terminated */ + s_wpad_guests.guests[0].name[name_size - 1] = '\0'; +} + +static conf_pad_device *device_get_paired(const BteBdAddr *address) +{ + uint8_t conf_addr[6]; + bd_address_to_conf(address, conf_addr); + for (int i = 0; i < s_wpad_paired.num_registered; i++) { + if (memcmp(conf_addr, s_wpad_paired.registered[i].bdaddr, 6) == 0) { + return &s_wpad_paired.registered[i]; + } + } + return NULL; +} + +static conf_pad_guest_device *device_get_guest(const BteBdAddr *address) +{ + uint8_t conf_addr[6]; + bd_address_to_conf(address, conf_addr); + for (int i = 0; i < s_wpad_guests.num_guests; i++) { + if (memcmp(conf_addr, s_wpad_guests.guests[i].bdaddr, 6) == 0) { + return &s_wpad_guests.guests[i]; + } + } + return NULL; +} + +static void on_channels_connected(WpadDevice *device) +{ + /* The BT-HID header tells us which packet type we are reading, + * therefore we can assing the same callback to both channels */ + bte_l2cap_on_message_received(device->hid_ctrl, message_received_cb); + bte_l2cap_on_message_received(device->hid_intr, message_received_cb); + + device_set_active(device); + + device_event_connected(device); + + device->state = STATE_HANDSHAKE_LEDS_OFF; + _wpad2_device_set_leds(device, WIIMOTE_LED_NONE); + _wpad2_device_request_status(device); +} + +static void hid_intr_state_changed_cb(BteL2cap *l2cap, BteL2capState state, + void *userdata) +{ + WpadDevice *device = userdata; + + printf("%s: state %d\n", __func__, state); + if (state == BTE_L2CAP_OPEN) { + on_channels_connected(device); + } +} + +static void l2cap_configure_cb( + BteL2cap *l2cap, const BteL2capConfigureReply *reply, void *userdata) +{ + printf("%s: rejected mask: %08x\n", __func__, reply->rejected_mask); + if (reply->rejected_mask != 0) { + bte_l2cap_disconnect(l2cap); + } +} + +static void device_free(WpadDevice *device) +{ + BteL2cap *hid_ctrl = device->hid_ctrl; + device->hid_ctrl = NULL; + BteL2cap *hid_intr = device->hid_intr; + device->hid_intr = NULL; + if (hid_ctrl) bte_l2cap_unref(hid_ctrl); + if (hid_intr) bte_l2cap_unref(hid_intr); + if (device->last_read_data) { + free(device->last_read_data); + device->last_read_data = NULL; + } +} + +static void disconnect_notify_and_free(WpadDevice *device, uint8_t reason) +{ + int slot = _wpad2_device_get_slot(device); + conf_pad_device *active_slot = &s_wpad_paired.active[slot]; + /* Just to be extra safe, check that the address in the active devices list + * matches */ + BteBdAddr conf_address = bd_address_from_conf(active_slot->bdaddr); + if (bd_address_is_equal(&device->address, &conf_address)) { + memset(active_slot, 0, sizeof(*active_slot)); + CONF_SetPadDevices(&s_wpad_paired); + CONF_SaveChanges(); + } + + WpadEvent event; + event.channel = slot; + event.type = WPAD2_EVENT_TYPE_DISCONNECTED; + /* WPAD_DISCON_* reasons map 1:1 to HCI errors */ + event.u.disconnection_reason = reason; + s_worker_thread_event_cb(&event); + + device_free(device); +} + +static void acl_disconnected_cb(BteL2cap *l2cap, uint8_t reason, void *userdata) +{ + WpadDevice *device = userdata; + + WPAD2_DEBUG("reason %02x", reason); + disconnect_notify_and_free(device, reason); +} + +static void l2cap_disconnected_cb(BteL2cap *l2cap, uint8_t reason, void *userdata) +{ + WPAD2_DEBUG("reason %x", reason); +} + +static void device_hid_intr_connected(BteL2cap *l2cap) +{ + WpadDevice *device = wpad_device_from_addr(bte_l2cap_get_address(l2cap)); + if (!device) return; /* Should never happen */ + + device->hid_intr = bte_l2cap_ref(l2cap); + bte_l2cap_set_userdata(l2cap, device); + /* Default configuration parameters are fine */ + bte_l2cap_configure(l2cap, NULL, l2cap_configure_cb, NULL); + bte_l2cap_on_state_changed(l2cap, hid_intr_state_changed_cb); + bte_l2cap_on_disconnected(l2cap, l2cap_disconnected_cb, device); +} + +static void hid_intr_connect_cb( + BteL2cap *l2cap, const BteL2capConnectionResponse *reply, void *userdata) +{ + printf("%s: result %d, status %d\n", __func__, reply->result, reply->status); + + if (reply->result != BTE_L2CAP_CONN_RESP_RES_OK) { + return; + } + + WpadDevice *device = wpad_device_from_addr(bte_l2cap_get_address(l2cap)); + if (!device) return; /* Should never happen */ + + device_hid_intr_connected(l2cap); +} + +static void hid_ctrl_state_changed_cb(BteL2cap *l2cap, BteL2capState state, + void *userdata) +{ + WpadDevice *device = userdata; + printf("%s: state %d\n", __func__, state); + if (state == BTE_L2CAP_OPEN) { + /* Connect the HID data channel. Since the ACL is already there, the + * connection parameters are ignored. */ + bte_l2cap_new_outgoing(s_client, &device->address, BTE_L2CAP_PSM_HID_INTR, + NULL, 0, hid_intr_connect_cb, NULL); + } +} + +static void device_hid_ctrl_connected(BteL2cap *l2cap) +{ + WpadDevice *device = wpad_device_from_addr(bte_l2cap_get_address(l2cap)); + if (!device) return; /* Should never happen */ + + device->hid_ctrl = bte_l2cap_ref(l2cap); + bte_l2cap_set_userdata(l2cap, device); + /* Default configuration parameters are fine */ + bte_l2cap_configure(l2cap, NULL, l2cap_configure_cb, NULL); + bte_l2cap_on_acl_disconnected(l2cap, acl_disconnected_cb); +} + +static void hid_ctrl_connect_cb( + BteL2cap *l2cap, const BteL2capConnectionResponse *reply, void *userdata) +{ + WpadDevice *device = userdata; + + WPAD2_DEBUG("result %d, status %d", reply->result, reply->status); + + if (reply->result != BTE_L2CAP_CONN_RESP_RES_OK) { + return; + } + + device_hid_ctrl_connected(l2cap); + bte_l2cap_on_state_changed(l2cap, hid_ctrl_state_changed_cb); + bte_l2cap_on_disconnected(l2cap, l2cap_disconnected_cb, device); +} + +static void device_init(WpadDevice *device, const BteBdAddr *address) +{ + /* Save the information that should persist, and restore it after the memset */ + bool accel_requested = device->accel_requested; + bool ir_requested = device->ir_requested; + bool speaker_requested = device->speaker_requested; + bool motion_plus_requested = device->motion_plus_requested; + memset(device, 0, sizeof(*device)); + bd_address_copy(&device->address, address); + device->unid = _wpad2_device_get_slot(device); + device->speaker_volume = 0x40; + device->ir_sensor_level = 3; + + device->accel_requested = accel_requested; + device->ir_requested = ir_requested; + device->speaker_requested = speaker_requested; + device->motion_plus_requested = motion_plus_requested; +} + +static WpadDevice *device_allocate(const BteBdAddr *address) +{ + for (int slot = 0; slot < WPAD2_MAX_DEVICES; slot++) { + WpadDevice *device = _wpad2_device_get(slot); + if (device->hid_ctrl == NULL) { + device_init(device, address); + return device; + } + } + + return NULL; +} + +static bool add_new_device(BteHci *hci, const BteHciInquiryResponse *info, + const char *name) +{ + if (strncmp(name, s_nintendo_rvl, sizeof(s_nintendo_rvl) - 1) != 0) { + return false; + } + + int slot = -1; + + /* Found Wii accessory, is it controller or something else? */ + const char *suffix = name + sizeof(s_nintendo_rvl) - 1; + if (strncmp(suffix, "CNT-", 4) == 0) { + /* It's an ordinary controller */ + slot = GetActiveSlot(&info->address); + if (slot >= 0) { + WPAD2_DEBUG("Already active in slot %d", slot); + } else { + /* Get a free slot */ + BteBdAddr null_addr = { 0, }; + WpadDevice *device = wpad_device_from_addr(&null_addr); + if (device) { + slot = _wpad2_device_get_slot(device); + } + } + } else { + /* Assume balance board */ + WpadDevice *device = _wpad2_device_get(WPAD_BALANCE_BOARD); + BteBdAddr null_addr = { 0, }; + if (bd_address_is_equal(&device->address, &info->address) || + bd_address_is_equal(&device->address, &null_addr)) { + slot = WPAD_BALANCE_BOARD; + } + } + + if (slot < 0) { + WPAD2_WARNING("all device slots used"); + return false; + } + + WpadDevice *device = _wpad2_device_get(slot); + device_init(device, &info->address); + + if (s_pair_mode == WPAD_PAIR_MODE_NORMAL) { + device_register_paired(&info->address, name); + /* Don't store the CONF now, that will happen in device_set_active() */ + } else { + device_register_guest(&info->address, name); + /* Don't store the CONF now, we'll do that once we get the link key */ + } + BteHciConnectParams params = { + s_packet_types, + info->clock_offset, + info->page_scan_rep_mode, + true, /* Allow role switch */ + }; + BteL2CapConnectFlags flags = BTE_L2CAP_CONNECT_FLAG_AUTH; + BteClient *client = bte_hci_get_client(hci); + bte_l2cap_new_outgoing(client, &info->address, BTE_L2CAP_PSM_HID_CTRL, + ¶ms, flags, hid_ctrl_connect_cb, device); + return true; +} + +static void read_remote_name_cb(BteHci *hci, const BteHciReadRemoteNameReply *r, + void *userdata) +{ + if (r->status == 0) { + WPAD2_DEBUG("Got name %s for " BD_ADDR_FMT, r->name, + BD_ADDR_DATA(&r->address)); + + if (add_new_device(hci, &s_inquiry_responses->responses[s_inquiry_responses->num_names_queried], r->name)) { + /* We only connect one Wiimote at a time: quit querying for names */ + return; + } + } + + s_inquiry_responses->num_names_queried++; + query_name_next(hci); +} + +static void query_name_next(BteHci *hci) +{ + if (s_inquiry_responses->num_names_queried >= + s_inquiry_responses->num_responses) { + /* All names have been queried */ + return; + } + + BteHciInquiryResponse *r = + &s_inquiry_responses->responses[s_inquiry_responses->num_names_queried]; + bte_hci_read_remote_name(hci, &r->address, r->page_scan_rep_mode, r->clock_offset, + NULL, read_remote_name_cb, NULL); +} + +static void inquiry_cb(BteHci *hci, const BteHciInquiryReply *reply, void *) +{ + if (reply->status != 0) return; + + WPAD2_DEBUG("Results: %d", reply->num_responses); + + BteBdAddr known_devices[MAX_INQUIRY_RESPONSES]; + int num_known_devices = s_inquiry_responses->num_names_queried; + for (int i = 0; i < num_known_devices; i++) { + bd_address_copy(&known_devices[i], + &s_inquiry_responses->responses[i].address); + } + + s_inquiry_responses->num_responses = 0; + s_inquiry_responses->num_names_queried = 0; + for (int i = 0; i < reply->num_responses; i++) { + const BteHciInquiryResponse *r = &reply->responses[i]; + + bool skip = false; + for (int j = 0; j < num_known_devices; j++) { + if (bd_address_is_equal(&r->address, &known_devices[j])) { + /* Ignore this device, we've already queried its name and it's + * not an interesting device. */ + skip = true; + break; + } + } + + const uint8_t *b = r->class_of_device.bytes; + WPAD2_DEBUG(" - " BD_ADDR_FMT " COD %02x%02x%02x offs %d RSSI %d (skip = %d)", + BD_ADDR_DATA(&r->address), + b[2], b[1], b[0], + r->clock_offset, r->rssi, skip); + if (skip) continue; + + /* New device, we'll query its name */ + if (s_inquiry_responses->num_responses >= MAX_INQUIRY_RESPONSES) break; + BteHciInquiryResponse *resp = + &s_inquiry_responses->responses[s_inquiry_responses->num_responses++]; + memcpy(resp, r, sizeof(*r)); + } + + query_name_next(hci); +} + +static bool link_key_request_cb(BteHci *hci, const BteBdAddr *address, + void *userdata) +{ + WPAD2_DEBUG("Link key requested from " BD_ADDR_FMT, BD_ADDR_DATA(address)); + if (!wpad_device_is_pairing(address)) return false; + + const BteLinkKey *key = NULL; + for (int i = 0; i < s_num_stored_keys; i++) { + if (bd_address_is_equal(address, &s_stored_keys[i].address)) { + key = &s_stored_keys[i].key; + break; + } + } + + if (!key) { + /* TODO: know if this is a registered device or a guest one */ + conf_pad_guest_device *guest = device_get_guest(address); + if (guest) { + key = (BteLinkKey *)guest->link_key; + } + } + + if (key) { + bte_hci_link_key_req_reply(hci, address, key, NULL, NULL); + } else { + bte_hci_link_key_req_neg_reply(hci, address, NULL, NULL); + } + return true; +} + +static bool pin_code_request_cb(BteHci *hci, const BteBdAddr *address, + void *userdata) +{ + WPAD2_DEBUG("PIN code requested from " BD_ADDR_FMT, BD_ADDR_DATA(address)); + if (!wpad_device_is_pairing(address)) return false; + + const BteBdAddr *pin = s_pair_mode == WPAD_PAIR_MODE_TEMPORARY ? + address : &s_local_address; + bte_hci_pin_code_req_reply(hci, address, (uint8_t *)pin, sizeof(*pin), NULL, NULL); + return true; +} + +static bool link_key_notification_cb( + BteHci *hci, const BteHciLinkKeyNotificationData *data, void *userdata) +{ + WPAD2_DEBUG("Link key notification from " BD_ADDR_FMT ", type %d", + BD_ADDR_DATA(&data->address), data->key_type); + conf_pad_device *paired = device_get_paired(&data->address); + + if (paired) { + /* Store the key on the BT controller */ + BteHciStoredLinkKey stored_key = { + data->address, + data->key, + }; + bte_hci_write_stored_link_key(hci, 1, &stored_key, NULL, NULL); + } else { + conf_pad_guest_device *guest = device_get_guest(&data->address); + if (!guest) return false; + + /* keys are stored backwards */ + const int key_size = sizeof(s_wpad_guests.guests[0].link_key); + for (int i = 0; i < key_size; i++) { + guest->link_key[i] = data->key.bytes[key_size - i]; + } + + CONF_SetPadGuestDevices(&s_wpad_guests); + } + return true; +} + +static bool vendor_event_cb(BteHci *hci, BteBuffer *event_data, void *) +{ + if (event_data->size < 1) return false; + + const uint8_t *data = event_data->data + 2; + bool held; + if (data[0] == WPAD2_VENDOR_EVENT_PAIRING) { + held = false; + } else if (data[0] == WPAD2_VENDOR_EVENT_WIPE_PAIRED) { + held = true; + } else { + return false; + } + + WpadEvent event; + event.channel = 0; + event.type = WPAD2_EVENT_TYPE_HOST_SYNC_BTN; + event.u.host_sync_button_held = held; + s_worker_thread_event_cb(&event); + return true; +} + +typedef void (*HciNextFunction)(BteHci *hci); + +static void generic_command_cb(BteHci *hci, const BteHciReply *reply, void *userdata) +{ + if (reply->status != 0) { + WPAD2_ERROR("Command completed with status %d", reply->status); + return; + } + + HciNextFunction f = userdata; + f(hci); +} + +static void init_done(BteHci *hci) +{ + WpadEvent event; + event.channel = 0; + event.type = WPAD2_EVENT_TYPE_BT_INITIALIZED; + s_worker_thread_event_cb(&event); +} + +static void init_set_page_timeout(BteHci *hci) +{ + bte_hci_write_page_timeout(hci, 0x2000, generic_command_cb, init_done); +} + +static void init_set_cod(BteHci *hci) +{ + BteClassOfDevice cod = {{0x04, 0x02, 0x40}}; + bte_hci_write_class_of_device(hci, &cod, generic_command_cb, + init_set_page_timeout); +} + +static void init_set_inquiry_scan_type(BteHci *hci) +{ + bte_hci_write_inquiry_scan_type(hci, BTE_HCI_INQUIRY_SCAN_TYPE_INTERLACED, + generic_command_cb, init_set_cod); +} + +static void init_set_page_scan_type(BteHci *hci) +{ + bte_hci_write_page_scan_type(hci, BTE_HCI_PAGE_SCAN_TYPE_INTERLACED, + generic_command_cb, init_set_inquiry_scan_type); +} + +static void init_set_inquiry_mode(BteHci *hci) +{ + bte_hci_write_inquiry_mode(hci, BTE_HCI_INQUIRY_MODE_RSSI, + generic_command_cb, init_set_page_scan_type); +} + +static void init_set_scan_enable(BteHci *hci) +{ + bte_hci_write_scan_enable(hci, BTE_HCI_SCAN_ENABLE_PAGE, + generic_command_cb, init_set_inquiry_mode); +} + +static void read_stored_link_keys_cb( + BteHci *hci, const BteHciReadStoredLinkKeyReply *reply, void *) +{ + WPAD2_DEBUG("status %d, num keys %d, max %d", reply->status, reply->num_keys, reply->max_keys); + if (reply->status == 0) { + s_num_stored_keys = MIN(reply->num_keys, MAX_LINK_KEYS); + for (int i = 0; i < s_num_stored_keys; i++) { + s_stored_keys[i] = reply->stored_keys[i]; + } + } + init_set_scan_enable(hci); +} + +static void init_read_stored_link_keys(BteHci *hci) +{ + bte_hci_read_stored_link_key(hci, NULL, read_stored_link_keys_cb, NULL); +} + +static void init_set_local_name(BteHci *hci) +{ + bte_hci_write_local_name(hci, "Wii", + generic_command_cb, init_read_stored_link_keys); +} + +static void read_bd_addr_cb(BteHci *hci, const BteHciReadBdAddrReply *reply, void *userdata) +{ + s_local_address = reply->address; + init_set_local_name(hci); +} + +static bool connection_request_cb(BteL2capServer *l2cap_server, + const BteBdAddr *address, + const BteClassOfDevice *cod, + void *userdata) +{ + WPAD2_DEBUG("from " BD_ADDR_FMT, BD_ADDR_DATA(address)); + + + /* If the device was already in the active list, allow it (and preserve its + * slot) */ + int active_slot = GetActiveSlot(address); + if (active_slot >= 0) { + WpadDevice *device = _wpad2_device_get(active_slot); + if (device->hid_ctrl != NULL) { + /* This can only occur if our structures are screwed up */ + WPAD2_WARNING("Conn request from " BD_ADDR_FMT " is already active", + BD_ADDR_DATA(address)); + } + + device_init(device, address); + return true; + } + + /* TODO: check device type */ + bool accepted = false; + for (int i = 0; i < s_wpad_paired.num_registered; i++) { + BteBdAddr stored = bd_address_from_conf(s_wpad_paired.registered[i].bdaddr); + WPAD2_DEBUG("Stored %d: " BD_ADDR_FMT, i, BD_ADDR_DATA(&stored)); + if (bd_address_is_equal(address, &stored)) { + accepted = true; + } + } + + /* Note that we don't check the guest device list here, because guest + * devices do not request a connection to the Wii, but listen in page mode. + */ + if (!accepted) return false; + + WpadDevice *device = device_allocate(address); + if (!device) return false; /* No more slots */ + + return true; +} + +static bool decline_connection(BteL2capServer *l2cap_server, + const BteBdAddr *address, + const BteClassOfDevice *cod, + void *userdata) +{ + WPAD2_DEBUG("from " BD_ADDR_FMT, BD_ADDR_DATA(address)); + return false; +} + +static void incoming_ctrl_connected_cb( + BteL2capServer *l2cap_server, BteL2cap *l2cap, void *userdata) +{ + WPAD2_DEBUG("l2cap != NULL %d", l2cap != NULL); + if (!l2cap) { + return; + } + + device_hid_ctrl_connected(l2cap); +} + +static void incoming_intr_connected_cb( + BteL2capServer *l2cap_server, BteL2cap *l2cap, void *userdata) +{ + WPAD2_DEBUG("l2cap != NULL %d", l2cap != NULL); + if (!l2cap) { + /* TODO: deallocate the WpadDevice */ + return; + } + device_hid_intr_connected(l2cap); +} + +static void initialized_cb(BteHci *hci, bool success, void *) +{ + printf("Initialized, OK = %d\n", success); + printf("ACL MTU=%d, max packets=%d\n", + bte_hci_get_acl_mtu(hci), + bte_hci_get_acl_max_packets(hci)); + BteHciFeatures features = bte_hci_get_supported_features(hci); + s_packet_types = bte_hci_packet_types_from_features(features); + bte_hci_on_link_key_request(hci, link_key_request_cb); + bte_hci_on_pin_code_request(hci, pin_code_request_cb); + bte_hci_on_link_key_notification(hci, link_key_notification_cb); + bte_hci_on_vendor_event(hci, vendor_event_cb); + bte_hci_read_bd_addr(hci, read_bd_addr_cb, NULL); + + s_l2cap_server_hid_ctrl = bte_l2cap_server_new(s_client, + BTE_L2CAP_PSM_HID_CTRL); + s_l2cap_server_hid_intr = bte_l2cap_server_new(s_client, + BTE_L2CAP_PSM_HID_INTR); + bte_l2cap_server_set_needs_auth(s_l2cap_server_hid_ctrl, true); + bte_l2cap_server_set_role(s_l2cap_server_hid_ctrl, BTE_HCI_ROLE_MASTER); + bte_l2cap_server_on_connected(s_l2cap_server_hid_ctrl, + incoming_ctrl_connected_cb, NULL); + bte_l2cap_server_on_connected(s_l2cap_server_hid_intr, + incoming_intr_connected_cb, NULL); + bte_l2cap_server_on_connection_request(s_l2cap_server_hid_ctrl, + connection_request_cb, NULL); + /* Since HID clients are required to connect to the control PSM first, the + * ACL connection is always received on the BteL2capServer handling the + * control connection. */ + bte_l2cap_server_on_connection_request(s_l2cap_server_hid_intr, + decline_connection, NULL); +} + +static void set_search_active(bool active) +{ + BteHci *hci = bte_hci_get(s_client); + if (active) { + /* wiiuse also clears the list of paired devices, but that doesn't seam + * entirely correct, since we are looking for guests here. */ + memset(&s_wpad_guests, 0, sizeof(s_wpad_guests)); + s_pair_mode = WPAD_PAIR_MODE_TEMPORARY; + if (!s_inquiry_responses) { + s_inquiry_responses = malloc(sizeof(*s_inquiry_responses)); + if (!s_inquiry_responses) return; + } + memset(s_inquiry_responses, 0, sizeof(*s_inquiry_responses)); + bte_hci_periodic_inquiry(hci, 4, 5, BTE_LAP_LIAC, 3, 0, + NULL, inquiry_cb, NULL); + } else { + bte_hci_exit_periodic_inquiry(hci, NULL, NULL); + } +} + +static void device_disconnect(WpadDevice *device) +{ + if (device->hid_ctrl) { + bte_l2cap_disconnect(device->hid_ctrl); + } + if (device->hid_intr) { + bte_l2cap_disconnect(device->hid_intr); + } + + disconnect_notify_and_free(device, BTE_HCI_CONN_TERMINATED_BY_LOCAL_HOST); +} + +static void wipe_saved_controllers() +{ + WPAD2_DEBUG(""); + + memset(&s_wpad_paired, 0, sizeof(s_wpad_paired)); + memset(&s_wpad_guests, 0, sizeof(s_wpad_guests)); + CONF_SetPadDevices(&s_wpad_paired); + CONF_SetPadGuestDevices(&s_wpad_guests); + CONF_SaveChanges(); + + memset(&s_stored_keys, 0, sizeof(s_stored_keys)); + BteHci *hci = bte_hci_get(s_client); + bte_hci_delete_stored_link_key(hci, NULL, NULL, NULL); +} + +static void start_pairing() +{ + s_pair_mode = WPAD_PAIR_MODE_NORMAL; + BteHci *hci = bte_hci_get(s_client); + if (!s_inquiry_responses) { + s_inquiry_responses = malloc(sizeof(*s_inquiry_responses)); + if (!s_inquiry_responses) return; + } + memset(s_inquiry_responses, 0, sizeof(*s_inquiry_responses)); + bte_hci_inquiry(hci, BTE_LAP_LIAC, 3, 0, NULL, inquiry_cb, NULL); +} + +void process_client_message(uptr msg) +{ + uint32_t cmd = msg; + uint32_t opcode = WORKER_CMD_OPCODE(cmd); + uint32_t params = WORKER_CMD_PARAMS(cmd); + int channel; + switch (opcode) { + case WORKER_CMD_SET_SEARCH: + set_search_active(params); + break; + case WORKER_CMD_SET_FORMAT: + channel = params >> 8; + _wpad2_device_set_data_format(_wpad2_device_get(channel), + params & 0xff); + break; + case WORKER_CMD_SET_MOTION_PLUS: + channel = params >> 8; + _wpad2_device_motion_plus_enable(_wpad2_device_get(channel), + params & 0xff); + break; + case WORKER_CMD_SET_RUMBLE: + channel = params >> 8; + _wpad2_device_set_rumble(_wpad2_device_get(channel), params & 0xff); + break; + case WORKER_CMD_SET_SPEAKER: + channel = params >> 8; + _wpad2_device_set_speaker(_wpad2_device_get(channel), params & 0xff); + break; + case WORKER_CMD_PLAY_SOUND: + channel = params >> 8; + WpadSoundInfo *sound_info = (void*)KMailboxRecv(&s_mailbox_in); + _wpad2_speaker_play(_wpad2_device_get(channel), sound_info); + break; + case WORKER_CMD_DISCONNECT: + channel = params >> 8; + device_disconnect(_wpad2_device_get(channel)); + break; + case WORKER_CMD_WIPE_CONTROLLERS: + wipe_saved_controllers(); + break; + case WORKER_CMD_START_PAIRING: + start_pairing(); + break; + } +} + +static void device_calibration_cb(WpadDevice *device, + const WpadDeviceCalibrationData *data) +{ + WpadEvent event; + event.type = WPAD2_EVENT_TYPE_CALIBRATION; + event.channel = _wpad2_device_get_slot(device); + event.u.calibration_data = data; + s_worker_thread_event_cb(&event); +} + +static void device_exp_calibration_cb(WpadDevice *device, + const WpadDeviceExpCalibrationData *data) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_EXP_CONNECTED; + event.u.exp_connected.exp_type = device->exp_type; + event.u.exp_connected.exp_subtype = device->exp_subtype; + event.u.exp_connected.calibration_data = data; + s_worker_thread_event_cb(&event); +} + +static void device_exp_disconnected_cb(WpadDevice *device) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_EXP_DISCONNECTED; + s_worker_thread_event_cb(&event); +} + +static void device_ready_cb(WpadDevice *device) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_READY; + s_worker_thread_event_cb(&event); +} + +static void device_status_cb(WpadDevice *device) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_STATUS; + event.u.status.battery_level = device->battery_level; + event.u.status.battery_critical = device->battery_critical; + event.u.status.speaker_enabled = device->speaker_enabled; + s_worker_thread_event_cb(&event); +} + +static void device_report_cb(WpadDevice *device, const uint8_t *data, uint8_t len) +{ + WpadEvent event; + event.channel = _wpad2_device_get_slot(device); + event.type = WPAD2_EVENT_TYPE_REPORT; + event.u.report.data = data; + event.u.report.len = len; + s_worker_thread_event_cb(&event); +} + +static void timer_handler_cb(WpadDevice *device, uint32_t period_us) +{ + WPAD2_DEBUG("device %p, period %d", device, period_us); + /* Here we assume that we can use the same timer for all devices, that it + * that they all have the same period and start at the same time. + * This is fine as long as we don't support different audio formats for the + * speaker, because in that case we will have different periods. */ + if (period_us != 0) { + s_timer_clients++; + } else { + s_timer_clients--; + } + + if (s_timer_clients <= 0) { + s_timer_period = 0; + } else { + if (s_timer_period == 0) { + /* Here we assume that if a timer is already running, we don't have to + * restart it. */ + s_timer_last_trigger = get_time_us(); + } + s_timer_period = period_us; + } +} + +static sptr worker_thread_func(void *) +{ + s_client = bte_client_new(); + BteHci *hci = bte_hci_get(s_client); + bte_hci_on_initialized(hci, initialized_cb, NULL); + + _wpad2_device_set_calibration_cb(device_calibration_cb); + _wpad2_device_set_exp_calibration_cb(device_exp_calibration_cb); + _wpad2_device_set_exp_disconnected_cb(device_exp_disconnected_cb); + _wpad2_device_set_ready_cb(device_ready_cb); + _wpad2_device_set_status_cb(device_status_cb); + _wpad2_device_set_report_cb(device_report_cb); + _wpad2_device_set_timer_handler_cb(timer_handler_cb); + + CONF_GetPadDevices(&s_wpad_paired); + CONF_GetPadGuestDevices(&s_wpad_guests); + + while (!s_worker_thread_done) { + if (s_timer_period != 0) { + uint64_t now = get_time_us(); + int32_t time_left = s_timer_period - (now - s_timer_last_trigger); + if (time_left < 0) { + /* In the (hopefully unlikely) case that we have been busy and + * are late on our timer, check for events without any sleep. + */ + bte_handle_events(); + } else { + bte_wait_events(time_left); + } + now = get_time_us(); + if (now >= s_timer_last_trigger + s_timer_period) { + s_timer_last_trigger = now; + /* Trigger timer */ + _wpad2_device_timer_event(now); + } + } else { + bte_wait_events(1000000); + } + + if (is_initialized()) { + uptr msg; + while (KMailboxTryRecv(&s_mailbox_in, &msg)) { + process_client_message(msg); + } + } + } + + bte_client_unref(s_client); + return 0; +} + +void _wpad2_worker_start() +{ + KMailboxPrepare(&s_mailbox_in, s_mailbox_in_slots, + ARRAY_SIZE(s_mailbox_in_slots)); + + s_worker_thread_stack = malloc(WORKER_THREAD_STACK_SIZE); + /* TODO: figure out optimal priority */ + KThreadPrepare(&s_worker_thread, worker_thread_func, NULL, + s_worker_thread_stack + WORKER_THREAD_STACK_SIZE, KTHR_MAIN_PRIO); + KThreadResume(&s_worker_thread); +} + +void _wpad2_worker_on_event(WpadEventCb callback) +{ + s_worker_thread_event_cb = callback; +} + +void _wpad2_worker_set_search_active(bool active) +{ + KMailboxTrySend(&s_mailbox_in, WORKER_CMD(SET_SEARCH, active)); +} + +void _wpad2_worker_set_format(uint8_t channel, uint8_t format) +{ + KMailboxTrySend(&s_mailbox_in, + WORKER_CMD(SET_FORMAT, (channel << 8) | format)); +} + +void _wpad2_worker_set_motion_plus(uint8_t channel, bool enable) +{ + KMailboxTrySend(&s_mailbox_in, + WORKER_CMD(SET_MOTION_PLUS, (channel << 8) | enable)); +} + +void _wpad2_worker_set_rumble(uint8_t channel, bool enable) +{ + KMailboxTrySend(&s_mailbox_in, + WORKER_CMD(SET_RUMBLE, (channel << 8) | enable)); +} + +void _wpad2_worker_set_speaker(uint8_t channel, bool enable) +{ + KMailboxTrySend(&s_mailbox_in, + WORKER_CMD(SET_SPEAKER, (channel << 8) | enable)); +} + +void _wpad2_worker_disconnect(uint8_t channel) +{ + KMailboxTrySend(&s_mailbox_in, WORKER_CMD(DISCONNECT, channel << 8)); +} + +void _wpad2_worker_stop() +{ + s_worker_thread_done = true; + KThreadJoin(&s_worker_thread); +} + +void _wpad2_worker_wipe_saved_controllers() +{ + KMailboxTrySend(&s_mailbox_in, WORKER_CMD_WIPE_CONTROLLERS); +} + +void _wpad2_worker_start_pairing() +{ + KMailboxTrySend(&s_mailbox_in, WORKER_CMD_START_PAIRING); +} + +void _wpad2_worker_play_sound(uint8_t channel, void *buffer, int len) +{ + WpadSoundInfo *info = malloc(sizeof(WpadSoundInfo) + len); + if (!info) { + WPAD2_WARNING("Could not allocate sound buffer (len = %d)", len); + return; + } + + info->len = len; + info->offset = 0; + memcpy(info->samples, buffer, len); + + KMailboxTrySend(&s_mailbox_in, WORKER_CMD(PLAY_SOUND, channel << 8)); + KMailboxTrySend(&s_mailbox_in, (uptr)info); +} diff --git a/wpad2/worker.h b/wpad2/worker.h new file mode 100644 index 000000000..d87ac0481 --- /dev/null +++ b/wpad2/worker.h @@ -0,0 +1,59 @@ +#ifndef WPAD2_WORKER_H +#define WPAD2_WORKER_H + +#include "device.h" + +typedef uint8_t WpadEventType; +enum { + WPAD2_EVENT_TYPE_BT_INITIALIZED, + WPAD2_EVENT_TYPE_CONNECTED, + WPAD2_EVENT_TYPE_CALIBRATION, + WPAD2_EVENT_TYPE_EXP_CONNECTED, + WPAD2_EVENT_TYPE_READY, + WPAD2_EVENT_TYPE_REPORT, + WPAD2_EVENT_TYPE_STATUS, + WPAD2_EVENT_TYPE_HOST_SYNC_BTN, + WPAD2_EVENT_TYPE_EXP_DISCONNECTED, + WPAD2_EVENT_TYPE_DISCONNECTED, +}; + +typedef struct { + uint8_t channel; + WpadEventType type; + union { + struct { + const uint8_t *data; + uint8_t len; + } report; + const WpadDeviceCalibrationData *calibration_data; + struct { + uint8_t exp_type; + uint8_t exp_subtype; + const WpadDeviceExpCalibrationData *calibration_data; + } exp_connected; + uint8_t disconnection_reason; + bool host_sync_button_held; + struct { + uint8_t battery_level; + bool battery_critical; + bool speaker_enabled; + } status; + } u; +} WpadEvent; + +typedef void (*WpadEventCb)(const WpadEvent *event); + +void _wpad2_worker_start(); +void _wpad2_worker_on_event(WpadEventCb callback); +void _wpad2_worker_set_search_active(bool active); +void _wpad2_worker_set_format(uint8_t channel, uint8_t format); +void _wpad2_worker_set_motion_plus(uint8_t channel, bool enable); +void _wpad2_worker_set_rumble(uint8_t channel, bool enable); +void _wpad2_worker_set_speaker(uint8_t channel, bool enable); +void _wpad2_worker_disconnect(uint8_t channel); +void _wpad2_worker_stop(); +void _wpad2_worker_wipe_saved_controllers(); +void _wpad2_worker_start_pairing(); +void _wpad2_worker_play_sound(uint8_t channel, void *buffer, int len); + +#endif /* WPAD2_WORKER_H */ diff --git a/wpad2/wpad.c b/wpad2/wpad.c new file mode 100644 index 000000000..bb8396b0d --- /dev/null +++ b/wpad2/wpad.c @@ -0,0 +1,905 @@ +/*------------------------------------------------------------- + +wpad.c -- Wiimote Application Programmers Interface + +Copyright (C) 2008-2026 +Michael Wiedenbauer (shagkur) +Dave Murphy (WinterMute) +Hector Martin (marcan) +Zarithya +Alberto Mardegan (mardy) + +This software is provided 'as-is', without any express or implied +warranty. In no event will the authors be held liable for any +damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you +must not claim that you wrote the original software. If you use +this software in a product, an acknowledgment in the product +documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and +must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source +distribution. + +-------------------------------------------------------------*/ + +#include "internals.h" + +#include "classic.h" +#include "dynamics.h" +#include "event.h" +#include "guitar_hero_3.h" +#include "ir.h" +#include "nunchuk.h" +#include "processor.h" +#include "speaker.h" +#include "tuxedo/sync.h" +#include "tuxedo/tick.h" +#include "wii_board.h" +#include "wiiuse_internal.h" +#include "worker.h" + +#include +#include +#include +#include +#include + +#include "ogcsys.h" + +typedef struct { + WpadThresholds thresholds; + int idle_time; +} WpadIdleInfo; + +#define MAX_REPORT_SIZE 22 +typedef struct { + uint8_t channel; + WpadEventType type; + uint8_t report[MAX_REPORT_SIZE]; +} WpadStoredEvent; + +#define EVENTQUEUE_LENGTH 16 +typedef struct { + KMutex mutex; + bool connected; + bool ready; + bool calibration_data_changed; + bool exp_calibration_data_changed; + bool battery_critical; + bool speaker_enabled; + uint8_t battery_level; + uint8_t exp_type; + union { + uint8_t exp_subtype; /* when connected */ + uint8_t disconnection_reason; /* when disconnected */ + }; + int idx_head; + int idx_tail; + int n_events; + int n_dropped; + WpadStoredEvent events[EVENTQUEUE_LENGTH]; + WpadDeviceCalibrationData calibration_data; + WpadDeviceExpCalibrationData exp_calibration_data; +} WpadEventQueue; + +enum { + WPAD2_HOST_SYNC_BUTTON_REST, + WPAD2_HOST_SYNC_BUTTON_PRESSED, + WPAD2_HOST_SYNC_BUTTON_HELD, +}; + +typedef struct { + uint8_t host_sync_button_state; +} WpadHostState; + +static WpadEventQueue s_event_queue[WPAD2_MAX_DEVICES]; +static WpadHostState s_host_state; +static WPADData s_wpaddata[WPAD2_MAX_DEVICES]; +static WpadIdleInfo s_idle_info[WPAD2_MAX_DEVICES]; +static struct KTickTask s_idle_task; +static int s_idle_timeout_secs = 300; +static accel_t s_calibration[WPAD2_MAX_DEVICES]; +static int32_t s_bt_status = WPAD_STATE_DISABLED; + +static WPADDisconnectCallback s_disconnect_cb; +static WPADShutdownCallback s_poweroff_cb; +static WPADShutdownCallback s_battery_dead_cb; +static WPADHostSyncBtnCallback s_sync_button_cb; +static WPADStatusCallback s_status_cb; + +/* This is invoked as a ISR, so we don't perform lengthy tasks here */ +static void idle_cb(KTickTask *task) +{ + for (int chan = 0; chan < WPAD2_MAX_DEVICES; chan++) { + WpadIdleInfo *idle_info = &s_idle_info[chan]; + if (idle_info->idle_time >= 0) + idle_info->idle_time++; + } +} + +static s32 reset_cb(s32 final) +{ + WPAD2_DEBUG("final: %d", final); + if (!final) { + WPAD_Shutdown(); + } + return 1; +} + +static sys_resetinfo s_reset_info = { + {}, + reset_cb, + 127 +}; + +static void default_poweroff_cb(s32 chan) +{ + SYS_DoPowerCB(); +} + +static void default_sync_button_cb(u32 held) +{ + if (held) { + WPAD_WipeSavedControllers(); + } else { + WPAD_StartPairing(); + } +} + +static void parse_calibration_data(uint8_t channel, + const WpadDeviceCalibrationData *cd) +{ + struct accel_t *accel = &s_calibration[channel]; + const uint8_t *data = cd->data; + + accel->cal_zero.x = (data[0] << 2) | ((data[3] >> 4) & 3); + accel->cal_zero.y = (data[1] << 2) | ((data[3] >> 2) & 3); + accel->cal_zero.z = (data[2] << 2) | (data[3] & 3); + + accel->cal_g.x = ((data[4] << 2) | ((data[7] >> 4) & 3)) - accel->cal_zero.x; + accel->cal_g.y = ((data[5] << 2) | ((data[7] >> 2) & 3)) - accel->cal_zero.y; + accel->cal_g.z = ((data[6] << 2) | (data[7] & 3)) - accel->cal_zero.z; + accel->st_alpha = WPAD2_DEFAULT_SMOOTH_ALPHA; +} + +/* Note that this is invoked from the worker thread, so access to common data + * needs to be synchronized */ +static void event_cb(const WpadEvent *event) +{ + WpadEventQueue *q = &s_event_queue[event->channel]; + + bool yield = false; + KMutexLock(&q->mutex); + if (event->type == WPAD2_EVENT_TYPE_CONNECTED) { + q->connected = true; + q->exp_subtype = 0; + } else if (event->type == WPAD2_EVENT_TYPE_CALIBRATION) { + q->calibration_data = *event->u.calibration_data; + q->calibration_data_changed = true; + } else if (event->type == WPAD2_EVENT_TYPE_EXP_CONNECTED) { + q->exp_type = event->u.exp_connected.exp_type; + q->exp_subtype = event->u.exp_connected.exp_subtype; + q->exp_calibration_data = *event->u.exp_connected.calibration_data; + q->exp_calibration_data_changed = true; + } else if (event->type == WPAD2_EVENT_TYPE_EXP_DISCONNECTED) { + q->exp_type = EXP_NONE; + } else if (event->type == WPAD2_EVENT_TYPE_READY) { + q->ready = true; + } else if (event->type == WPAD2_EVENT_TYPE_DISCONNECTED) { + q->connected = false; + q->disconnection_reason = event->u.disconnection_reason; + q->ready = false; + q->idx_tail = q->idx_head = 0; + q->n_events = 0; + if (q->n_dropped) { + WPAD2_WARNING("Wiimote %d: dropped %d events", + event->channel, q->n_dropped); + q->n_dropped = 0; + } + } else if (event->type == WPAD2_EVENT_TYPE_REPORT) { + q->idx_tail = (q->idx_tail + 1) % EVENTQUEUE_LENGTH; + if (q->n_events >= EVENTQUEUE_LENGTH) { + q->idx_head = (q->idx_head + 1) % EVENTQUEUE_LENGTH; + q->n_dropped++; + } else { + if (q->n_events == 0) q->idx_head = q->idx_tail; + q->n_events++; + } + WpadStoredEvent *e = &q->events[q->idx_tail]; + e->channel = event->channel; + e->type = event->type; /* TODO is this needed? */ + WPAD2_DEBUG("worker rep %d buttons %04x head %d tail %d, count %d", event->u.report.len, *(uint16_t *)(event->u.report.data + 1), q->idx_head, q->idx_tail, q->n_events); + memcpy(e->report, event->u.report.data, event->u.report.len); + if (q->n_events >= EVENTQUEUE_LENGTH / 2) yield = true; + } else if (event->type == WPAD2_EVENT_TYPE_BT_INITIALIZED) { + s_bt_status = WPAD_STATE_ENABLED; + } else if (event->type == WPAD2_EVENT_TYPE_HOST_SYNC_BTN) { + s_host_state.host_sync_button_state = event->u.host_sync_button_held ? + WPAD2_HOST_SYNC_BUTTON_HELD : WPAD2_HOST_SYNC_BUTTON_PRESSED; + } else if (event->type == WPAD2_EVENT_TYPE_STATUS) { + q->battery_level = event->u.status.battery_level; + q->battery_critical = event->u.status.battery_critical; + q->speaker_enabled = event->u.status.speaker_enabled; + } + KMutexUnlock(&q->mutex); + if (yield) KThreadYield(); +} + +s32 WPAD_StartPairing(void) +{ + WPAD2_DEBUG(""); + + _wpad2_worker_start_pairing(); + return WPAD_ERR_NONE; +} + +s32 WPAD_WipeSavedControllers(void) +{ + WPAD2_DEBUG(""); + + for (int i = 0; i < WPAD2_MAX_DEVICES; i++) { + WPAD_Disconnect(i); + } + + _wpad2_worker_wipe_saved_controllers(); + return WPAD_ERR_NONE; +} + +s32 WPAD_Search(void) +{ + _wpad2_worker_set_search_active(true); + return WPAD_ERR_NONE; +} + +s32 WPAD_StopSearch(void) +{ + _wpad2_worker_set_search_active(false); + return WPAD_ERR_NONE; +} + +s32 WPAD_Init(void) +{ + for (int i = 0; i < ARRAY_SIZE(s_wpaddata); i++) { + WPADData *data = &s_wpaddata[i]; + _wpad2_ir_set_aspect_ratio(data, WIIUSE_ASPECT_4_3); + _wpad2_ir_set_position(data, WIIUSE_IR_ABOVE); + data->err = WPAD_ERR_NOT_READY; + } + + s_bt_status = WPAD_STATE_ENABLING; + for (int i = 0; i < ARRAY_SIZE(s_idle_info); i++) { + s_idle_info[i].thresholds.btns = WPAD_THRESH_DEFAULT_BUTTONS; + s_idle_info[i].thresholds.ir = WPAD_THRESH_DEFAULT_IR; + s_idle_info[i].thresholds.acc = WPAD_THRESH_DEFAULT_ACCEL; + s_idle_info[i].thresholds.js = WPAD_THRESH_DEFAULT_JOYSTICK; + s_idle_info[i].thresholds.wb = WPAD_THRESH_DEFAULT_BALANCEBOARD; + s_idle_info[i].thresholds.mp = WPAD_THRESH_DEFAULT_MOTION_PLUS; + s_idle_info[i].idle_time = -1; + } + s_poweroff_cb = default_poweroff_cb; + s_sync_button_cb = default_sync_button_cb; + _wpad2_ir_sensor_bar_enable(true); + _wpad2_worker_start(); + _wpad2_worker_on_event(event_cb); + + u64 ticks_per_sec = PPCMsToTicks(1000); + KTickTaskStart(&s_idle_task, idle_cb, ticks_per_sec, ticks_per_sec); + + SYS_RegisterResetFunc(&s_reset_info); + + return WPAD_ERR_NONE; +} + +s32 WPAD_ReadEvent(s32 chan, WPADData *data) +{ + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + WpadEventQueue *q = &s_event_queue[chan]; + WPADData *our_data = &s_wpaddata[chan]; + WpadIdleInfo *idle_info = &s_idle_info[chan]; + + /* Since this mutex blocks the worker thread, let's try to hold it for as + * little as possible */ + s32 ret = WPAD_ERR_NONE; + WpadStoredEvent event; + union { + WpadDeviceCalibrationData wiimote; + WpadDeviceExpCalibrationData exp; + } calibration_data; + bool calibration_data_changed = false; + bool exp_calibration_data_changed = false; + uint8_t disconnection_reason = 0; /* 0 means we were not disconnected */ + uint8_t exp_old = our_data->exp.type; + + KMutexLock(&q->mutex); + + WpadHostState host_state = s_host_state; + s_host_state.host_sync_button_state = WPAD2_HOST_SYNC_BUTTON_REST; + + our_data->exp.type = q->exp_type; + if (q->calibration_data_changed) { + calibration_data.wiimote = q->calibration_data; + calibration_data_changed = true; + q->calibration_data_changed = false; + } else if (q->exp_calibration_data_changed) { + /* We have an "else" here in the condition to save some space on the + * stack, or we would not be able to use a union for the calibration + * data. Even in the unlikely case that we have calibration data for + * both the wiimote and the expansion, nothing bad will happen: we'll + * process the expansion data in the next call of this function. */ + calibration_data.exp = q->exp_calibration_data; + if (our_data->exp.type == EXP_CLASSIC) { + our_data->exp.classic.type = q->exp_subtype; + } + exp_calibration_data_changed = true; + q->exp_calibration_data_changed = false; + } + + if (!q->connected) { + ret = WPAD_ERR_NO_CONTROLLER; + if (our_data->err != WPAD_ERR_NO_CONTROLLER) { + /* We were disconnected just now */ + disconnection_reason = q->disconnection_reason; + } + idle_info->idle_time = -1; + } else if (!q->ready) { + ret = WPAD_ERR_NOT_READY; + } else if (q->n_events == 0) { + ret = WPAD_ERR_QUEUE_EMPTY; + } else { + event = q->events[q->idx_head]; + q->idx_head = (q->idx_head + 1) % EVENTQUEUE_LENGTH; + q->n_events--; + } + our_data->battery_level = q->battery_level; + KMutexUnlock(&q->mutex); + + if (idle_info->idle_time >= s_idle_timeout_secs) { + _wpad2_worker_disconnect(chan); + idle_info->idle_time = -1; + } + + if (host_state.host_sync_button_state) { + bool held = + host_state.host_sync_button_state == WPAD2_HOST_SYNC_BUTTON_HELD; + s_sync_button_cb(held); + } + + if (disconnection_reason != 0) { + memset(our_data, 0, sizeof(*our_data)); + if (s_disconnect_cb) { + s_disconnect_cb(chan, disconnection_reason); + } else if (disconnection_reason == WPAD_DISCON_POWER_OFF) { + s_poweroff_cb(chan); + } else if (disconnection_reason == WPAD_DISCON_BATTERY_DIED) { + if (s_battery_dead_cb) s_battery_dead_cb(chan); + } + } + + if (calibration_data_changed) { + idle_info->idle_time = 0; + parse_calibration_data(chan, &calibration_data.wiimote); + } + + if (our_data->exp.type != exp_old) { + if (our_data->exp.type == EXP_NONE) { + memset(&our_data->exp, 0, sizeof(our_data->exp)); + } + } + + if (exp_calibration_data_changed) { + const WpadDeviceExpCalibrationData *cal = &calibration_data.exp; + switch (our_data->exp.type) { + case EXP_NUNCHUK: + _wpad2_nunchuk_calibrate(&our_data->exp.nunchuk, cal); + break; + case EXP_CLASSIC: + _wpad2_classic_calibrate(&our_data->exp.classic, cal); + break; + case EXP_GUITAR_HERO_3: + _wpad2_guitar_hero_3_calibrate(&our_data->exp.gh3, cal); + break; + case EXP_WII_BOARD: + _wpad2_wii_board_calibrate(&our_data->exp.wb, cal); + break; + } + } + + /* TODO: smoothing */ + if (data) { + data->err = ret; + if (ret != WPAD_ERR_NONE) { + if (ret != WPAD_ERR_QUEUE_EMPTY) { + our_data->data_present = data->data_present = 0; + our_data->btns_h = data->btns_h = 0; + our_data->btns_l = data->btns_l = 0; + our_data->btns_d = data->btns_d = 0; + our_data->btns_u = data->btns_u = 0; + } + our_data->err = data->err; + return ret; + } + + bool changed = _wpad2_event_parse_report(our_data, event.report, + &idle_info->thresholds, data); + if (changed) idle_info->idle_time = 0; + + bool smoothed = true; + _wpad2_calc_data(data, our_data, &s_calibration[chan], smoothed); + *our_data = *data; + } + return ret; +} + +s32 WPAD_DroppedEvents(s32 chan) +{ + int dropped = 0; + s32 ret; + + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD2_MAX_DEVICES; i++) + if ((ret = WPAD_DroppedEvents(i)) < WPAD_ERR_NONE) + return ret; + else + dropped += ret; + return dropped; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) + return WPAD_ERR_BAD_CHANNEL; + + WpadEventQueue *q = &s_event_queue[chan]; + KMutexLock(&q->mutex); + if (!q->ready) { + KMutexUnlock(&q->mutex); + return WPAD_ERR_NOT_READY; + } + + dropped = q->n_dropped; + q->n_dropped = 0; + KMutexUnlock(&q->mutex); + return dropped; +} + +s32 WPAD_Flush(s32 chan) +{ + s32 ret; + int count = 0; + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD_MAX_DEVICES; i++) + if ((ret = WPAD_Flush(i)) < WPAD_ERR_NONE) + return ret; + else + count += ret; + return count; + } + + while ((ret = WPAD_ReadEvent(chan, NULL)) >= 0) + count++; + if (ret == WPAD_ERR_QUEUE_EMPTY) return count; + return ret; +} + +s32 WPAD_ReadPending(s32 chan, WPADDataCallback datacb) +{ + s32 ret; + s32 count = 0; + + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD2_MAX_DEVICES; i++) + if ((ret = WPAD_ReadPending(i, datacb)) >= WPAD_ERR_NONE) + count += ret; + return count; + } + + u32 btns_p = 0; + u32 btns_l = 0; + u32 btns_ev = 0; + u32 btns_nh = 0; + + WPADData data; + btns_p = btns_nh = btns_l = s_wpaddata[chan].btns_h; + while (true) { + ret = WPAD_ReadEvent(chan, &data); + if (ret < WPAD_ERR_NONE) break; + if (datacb) + datacb(chan, &s_wpaddata[chan]); + + /* we ignore everything except _h, since we have our */ + /* own "fake" _l and everything gets recalculated at */ + /* the end of the function */ + u32 btns_h = data.btns_h; + + /* Button event coalescing: + * What we're doing here is get the very first button event + * (press or release) for each button. This gets propagated + * to the output. Held will therefore report an "old" state + * for every button that has changed more than once. This is + * intentional: next call to WPAD_ReadPending, if this button + * hasn't again changed, the opposite event will fire. This + * is the behavior that preserves the most information, + * within the limitations of trying to coalesce multiple events + * into one. It also keeps the output consistent, if possibly + * not fully up to date. + */ + + /* buttons that changed that haven't changed before */ + u32 btns_ch = (btns_h ^ btns_p) & ~btns_ev; + btns_p = btns_h; + /* propagate changes in btns_ch to btns_nd */ + btns_nh = (btns_nh & ~btns_ch) | (btns_h & btns_ch); + /* store these new changes to btns_ev */ + btns_ev |= btns_ch; + + count++; + } + if (ret == WPAD_ERR_QUEUE_EMPTY) { + data.btns_h = s_wpaddata[chan].btns_h = btns_nh; + data.btns_l = s_wpaddata[chan].btns_l = btns_l; + data.btns_d = s_wpaddata[chan].btns_d = btns_nh & ~btns_l; + data.btns_u = s_wpaddata[chan].btns_u = ~btns_nh & btns_l; + return count; + } + return ret; +} + +s32 WPAD_SetDataFormat(s32 chan, s32 fmt) +{ + s32 ret; + + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD2_MAX_DEVICES; i++) { + if ((ret = WPAD_SetDataFormat(i, fmt)) < WPAD_ERR_NONE) + return ret; + } + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + switch (fmt) { + case WPAD_FMT_BTNS: + case WPAD_FMT_BTNS_ACC: + case WPAD_FMT_BTNS_ACC_IR: + _wpad2_worker_set_format(chan, fmt); + break; + default: + return WPAD_ERR_BADVALUE; + } + return WPAD_ERR_NONE; +} + +s32 WPAD_SetMotionPlus(s32 chan, u8 enable) +{ + s32 ret; + + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD_MAX_DEVICES; i++) + if ((ret = WPAD_SetMotionPlus(i, enable)) < WPAD_ERR_NONE) + return ret; + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + _wpad2_worker_set_motion_plus(chan, enable); + return WPAD_ERR_NONE; +} + +s32 WPAD_SetVRes(s32 chan, u32 xres, u32 yres) +{ + s32 ret; + + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD2_MAX_DEVICES; i++) + if ((ret = WPAD_SetVRes(i, xres, yres)) < WPAD_ERR_NONE) + return ret; + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + _wpad2_ir_set_vres(&s_wpaddata[chan], xres, yres); + return WPAD_ERR_NONE; +} + +s32 WPAD_GetStatus(void) +{ + return s_bt_status; +} + +s32 WPAD_Probe(s32 chan, u32 *type) +{ + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + + WpadEventQueue *q = &s_event_queue[chan]; + KMutexLock(&q->mutex); + bool connected = q->connected; + bool ready = q->ready; + bool exp_type = q->exp_type; + KMutexUnlock(&q->mutex); + + if (!connected) return WPAD_ERR_NO_CONTROLLER; + if (!ready) return WPAD_ERR_NOT_READY; + + if (type) *type = exp_type; + return WPAD_ERR_NONE; +} + +[[deprecated]] +s32 WPAD_SetEventBufs(s32 chan, WPADData *bufs, u32 cnt) +{ + WPAD2_WARNING("This function is not implemented (and will never be)"); + return WPAD_ERR_UNKNOWN; +} + +[[deprecated]] +void WPAD_SetPowerButtonCallback(WPADShutdownCallback cb) +{ + s_poweroff_cb = cb ? cb : default_poweroff_cb; +} + +[[deprecated]] +void WPAD_SetBatteryDeadCallback(WPADShutdownCallback cb) +{ + s_battery_dead_cb = cb; +} + +void WPAD_SetDisconnectCallback(WPADDisconnectCallback cb) +{ + s_disconnect_cb = cb; +} + +void WPAD_SetHostSyncButtonCallback(WPADHostSyncBtnCallback cb) +{ + s_sync_button_cb = cb ? cb : default_sync_button_cb; +} + +void WPAD_SetStatusCallback(WPADStatusCallback cb) +{ + s_status_cb = cb; +} + +s32 WPAD_Disconnect(s32 chan) +{ + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + if (s_bt_status != WPAD_STATE_ENABLED) return WPAD_ERR_NOT_READY; + + _wpad2_worker_disconnect(chan); + /* Wait for disconnection */ + WpadEventQueue *q = &s_event_queue[chan]; + for (int i = 0; i < 3000; i++) { + KMutexLock(&q->mutex); + bool connected = q->connected; + KMutexUnlock(&q->mutex); + if (!connected) break; + usleep(50); + } + return WPAD_ERR_NONE; +} + +s32 WPAD_Shutdown(void) +{ + s_bt_status = WPAD_STATE_DISABLED; + + KTickTaskStop(&s_idle_task); + + _wpad2_worker_stop(); + _wpad2_ir_sensor_bar_enable(false); + s_disconnect_cb = NULL; + s_poweroff_cb = NULL; + s_battery_dead_cb = NULL; + s_sync_button_cb = NULL; + s_status_cb = NULL; + return WPAD_ERR_NONE; +} + +void WPAD_SetIdleTimeout(u32 seconds) +{ + s_idle_timeout_secs = seconds; +} + +s32 WPAD_ScanPads(void) +{ + return WPAD_ReadPending(WPAD_CHAN_ALL, NULL); +} + +s32 WPAD_Rumble(s32 chan, int status) +{ + s32 ret; + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD_MAX_DEVICES; i++) + if ((ret = WPAD_Rumble(i, status)) < WPAD_ERR_NONE) + return ret; + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) { + return WPAD_ERR_BAD_CHANNEL; + } + + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + _wpad2_worker_set_rumble(chan, status); + return WPAD_ERR_NONE; +} + +s32 WPAD_SetIdleThresholds(s32 chan, s32 btns, s32 ir, s32 accel, s32 js, s32 wb, s32 mp) +{ + s32 ret; + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD_MAX_DEVICES; i++) + if ((ret = WPAD_SetIdleThresholds(i, btns, ir, accel, js, wb, mp)) < WPAD_ERR_NONE) + return ret; + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) { + return WPAD_ERR_BAD_CHANNEL; + } + + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + + s_idle_info[chan].thresholds.btns = btns; + s_idle_info[chan].thresholds.ir = ir; + s_idle_info[chan].thresholds.acc = accel; + s_idle_info[chan].thresholds.js = js; + s_idle_info[chan].thresholds.wb = wb; + s_idle_info[chan].thresholds.mp = mp; + return WPAD_ERR_NONE; +} + +s32 WPAD_ControlSpeaker(s32 chan, s32 enable) +{ + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + + s32 ret; + if (chan == WPAD_CHAN_ALL) { + for (int i = WPAD_CHAN_0; i < WPAD_MAX_DEVICES; i++) + if ((ret = WPAD_ControlSpeaker(i, enable)) < WPAD_ERR_NONE) + return ret; + return WPAD_ERR_NONE; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + _wpad2_worker_set_speaker(chan, enable); + return WPAD_ERR_NONE; +} + +s32 WPAD_IsSpeakerEnabled(s32 chan) +{ + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + WpadEventQueue *q = &s_event_queue[chan]; + KMutexLock(&q->mutex); + bool enabled = q->speaker_enabled; + KMutexUnlock(&q->mutex); + return enabled ? WPAD_ERR_NONE : WPAD_ERR_NOT_READY; +} + +s32 WPAD_SendStreamData(s32 chan, void *buf, u32 len) +{ + if (s_bt_status == WPAD_STATE_DISABLED) { + return WPAD_ERR_NOT_READY; + } + + if (chan < WPAD_CHAN_0 || chan >= WPAD2_MAX_DEVICES) return WPAD_ERR_BAD_CHANNEL; + + WPAD2_DEBUG("Sending buffer %p, len %d", buf, len); + _wpad2_worker_play_sound(chan, buf, (int)len); + return WPAD_ERR_NONE; +} + +void WPAD_EncodeData(WPADEncStatus *info, u32 flag, + const s16 *pcm_samples, s32 num_samples, u8 *out) +{ + _wpad2_speaker_encode((WENCStatus*)info, flag, pcm_samples, num_samples, out); +} + +WPADData *WPAD_Data(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return NULL; + return &s_wpaddata[chan]; +} + +u8 WPAD_BatteryLevel(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return 0; + return s_wpaddata[chan].battery_level; +} + +u32 WPAD_ButtonsUp(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return 0; + return s_wpaddata[chan].btns_u; +} + +u32 WPAD_ButtonsDown(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return 0; + return s_wpaddata[chan].btns_d; +} + +u32 WPAD_ButtonsHeld(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return 0; + return s_wpaddata[chan].btns_h; +} + +void WPAD_IR(int chan, struct ir_t *ir) +{ + if (chan < 0 || chan >= WPAD_MAX_DEVICES || !ir) return; + *ir = s_wpaddata[chan].ir; +} + +void WPAD_Orientation(int chan, struct orient_t *orient) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES || !orient) return; + *orient = s_wpaddata[chan].orient; +} + +void WPAD_GForce(int chan, struct gforce_t *gforce) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES || !gforce) return; + *gforce = s_wpaddata[chan].gforce; +} + +void WPAD_Accel(int chan, struct vec3w_t *accel) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES || !accel) return; + *accel = s_wpaddata[chan].accel; +} + +void WPAD_Expansion(int chan, struct expansion_t *exp) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES || !exp) return; + *exp = s_wpaddata[chan].exp; +} + +void WPAD_PadStatus(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return; + + WpadEventQueue *q = &s_event_queue[chan]; + KMutexLock(&q->mutex); + bool connected = q->connected; + KMutexUnlock(&q->mutex); + + if (connected && s_status_cb) { + s_status_cb(chan); + } +} + +bool WPAD_IsBatteryCritical(int chan) +{ + if (chan < 0 || chan >= WPAD2_MAX_DEVICES) return false; + + WpadEventQueue *q = &s_event_queue[chan]; + KMutexLock(&q->mutex); + bool battery_critical = q->battery_critical; + KMutexUnlock(&q->mutex); + return battery_critical; +} From 9b43ff9ba4cc5ad08c2fbfe54dc0777e8757c365 Mon Sep 17 00:00:00 2001 From: Alberto Mardegan Date: Thu, 12 Mar 2026 19:23:45 +0300 Subject: [PATCH 2/2] wpad2: don't issue status commands if rumble is already set --- wpad2/device.c | 8 +++++--- wpad2/internals.h | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/wpad2/device.c b/wpad2/device.c index fb7cf7121..0c20b6674 100644 --- a/wpad2/device.c +++ b/wpad2/device.c @@ -680,9 +680,11 @@ void _wpad2_device_timer_event(uint64_t now) void _wpad2_device_set_rumble(WpadDevice *device, bool enable) { - WPAD2_DEBUG("enable: %d", enable); - device->rumble = enable; - _wpad2_device_request_status(device); + if (enable != device->rumble) { + WPAD2_DEBUG("enable: %d", enable); + device->rumble = enable; + _wpad2_device_request_status(device); + } } void _wpad2_device_set_speaker(WpadDevice *device, bool enable) diff --git a/wpad2/internals.h b/wpad2/internals.h index 5d86258fa..4fe035af4 100644 --- a/wpad2/internals.h +++ b/wpad2/internals.h @@ -33,7 +33,7 @@ #define absf(x) ((x >= 0) ? (x) : (x * -1.0f)) #define diff_f(x, y) ((x >= y) ? (absf(x - y)) : (absf(y - x))) -//#define WITH_WPAD_DEBUG +#define WITH_WPAD_DEBUG #ifdef WITH_WPAD_DEBUG #define WPAD2_DEBUG(fmt, ...) SYS_Report("[DEBUG] %s:%i: " fmt "\n", __func__, __LINE__, ##__VA_ARGS__)