diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 00000000..e69de29b
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 76ec888f..249320b0 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 00000000..795d9fe5
--- /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 00000000..41e9e6c6
--- /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 00000000..7fe5c894
--- /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 00000000..0c20b667
--- /dev/null
+++ b/wpad2/device.c
@@ -0,0 +1,700 @@
+/*-------------------------------------------------------------
+
+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)
+{
+ 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)
+{
+ 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 00000000..76675a54
--- /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 00000000..424c0771
--- /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 00000000..8341c353
--- /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 00000000..be352523
--- /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 00000000..a4c27bc3
--- /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 00000000..09f2c640
--- /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 00000000..43fd59d5
--- /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 00000000..4fe035af
--- /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 00000000..64ab15a2
--- /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 00000000..8659c2c4
--- /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 00000000..bbf9807b
--- /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 00000000..05c16d09
--- /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 00000000..29158cd4
--- /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 00000000..ed6a6d3b
--- /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 00000000..659cc624
--- /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 00000000..dbb78208
--- /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 00000000..02cb4ada
--- /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 00000000..96248951
--- /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 00000000..7fea5562
--- /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 00000000..16ecf63a
--- /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 00000000..d87ac048
--- /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 00000000..bb8396b0
--- /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;
+}