diff --git a/.gitattributes b/.gitattributes deleted file mode 100644 index bdb0cab..0000000 --- a/.gitattributes +++ /dev/null @@ -1,17 +0,0 @@ -# Auto detect text files and perform LF normalization -* text=auto - -# Custom for Visual Studio -*.cs diff=csharp - -# Standard to msysgit -*.doc diff=astextplain -*.DOC diff=astextplain -*.docx diff=astextplain -*.DOCX diff=astextplain -*.dot diff=astextplain -*.DOT diff=astextplain -*.pdf diff=astextplain -*.PDF diff=astextplain -*.rtf diff=astextplain -*.RTF diff=astextplain diff --git a/.gitignore b/.gitignore index cd2946a..8eaa144 100644 --- a/.gitignore +++ b/.gitignore @@ -1,47 +1,6 @@ -# Windows image file caches -Thumbs.db -ehthumbs.db - -# Folder config file -Desktop.ini - -# Recycle Bin used on file shares -$RECYCLE.BIN/ - -# Windows Installer files -*.cab -*.msi -*.msm -*.msp - -# Windows shortcuts -*.lnk - -# ========================= -# Operating System Files -# ========================= - -# OSX -# ========================= - -.DS_Store -.AppleDouble -.LSOverride - -# Thumbnails -._* - -# Files that might appear in the root of a volume -.DocumentRevisions-V100 -.fseventsd -.Spotlight-V100 -.TemporaryItems -.Trashes -.VolumeIcon.icns - -# Directories potentially created on remote AFP share -.AppleDB -.AppleDesktop -Network Trash Folder -Temporary Items -.apdisk +bluez-4.101 +wmemulator +wmmitm +packedtest +*.o +*.so \ No newline at end of file diff --git a/Makefile b/Makefile index b914fa5..56702e4 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,19 @@ -all: - gcc -g -o wmemulator wmemulator.c wiimote.c input.c wm_crypto.c wm_reports.c wm_print.c -lbluetooth -lSDL -lpthread -lm `pkg-config --cflags dbus-1` -ldbus-1 -Wall - gcc -o bdaddr bdaddr.c oui.c -lbluetooth - gcc -o sdptool sdptool.c -lbluetooth +ifeq ($(origin CUSTOM_BUILD),undefined) +LBLUETOOTH=-lbluetooth +CFLAGS= +else +BLUEZ_DIST=$(shell pwd)/bluez-4.101/dist +LBLUETOOTH=-L"$(BLUEZ_DIST)/lib" -I"$(BLUEZ_DIST)/include" -Wl,-rpath="$(BLUEZ_DIST)/lib" -lbluetooth +CFLAGS=-D SDP_SERVER +endif +LDBUS=`pkg-config --cflags dbus-1` -ldbus-1 + +all: wmemulator packedtest wmmitm +clean: + rm -f wmemulator packedtest wmmitm +wmemulator: wmemulator.c wiimote.c input.c motion.c input_sdl.c input_socket.c wm_crypto.c wm_reports.c wm_print.c sdp.c bdaddr.c adapter.c + gcc $(CFLAGS) -o wmemulator wmemulator.c wiimote.c input.c motion.c input_sdl.c input_socket.c wm_crypto.c wm_reports.c wm_print.c sdp.c bdaddr.c adapter.c $(LBLUETOOTH) -lSDL -lpthread -lm $(LDBUS) -Wall +wmmitm: wmmitm.c wm_print.c sdp.c bdaddr.c adapter.c + gcc $(CFLAGS) -o wmmitm wmmitm.c wm_print.c sdp.c bdaddr.c adapter.c $(LBLUETOOTH) -lpthread -lm $(LDBUS) -Wall +packedtest: packedtest.c gcc -o packedtest packedtest.c diff --git a/README.md b/README.md index aff58af..3867583 100644 --- a/README.md +++ b/README.md @@ -4,123 +4,81 @@ Emulates a Bluetooth Wii controller in software. ![Raspberry Pi 3 running the emulator in Raspbian](rpi_ss.png) -## Why? +### Features - - Useful for building portable console mods with internal controllers - - Could theoretically be used for extra controllers (WIP) - - Can emulate all of the Wiimote's many features and extensions - - Allows use of different input devices (keyboard etc.) - - For the fun/hell of it - -## How? +- Emulate the Wiimote's many features and extensions +- Allows use of different input devices (keyboard etc.) ### Build/Install -The following dependencies/packages are required: - - - libdbus-1-dev - - libglib2.0-dev - - libsdl1.2-dev - -Additionally, bluez-4.101 is required (newer versions currently don't work). - -To build the emulator, run the makefile in the source directory. In order to -connect to the Wii, a plugin must be installed that allows pairing using non -UTF-8 pin codes. Copy the contents of the bluez-plugin folder to the -bluez-4.101/plugins folder of the Bluez source and run the makefile in that -directory. Copy the output wmemu.so file to the Bluez installation's plugins -directory (e.g. /usr/lib/bluetooth/plugins) and restart the bluetooth service. +The following dependencies/packages are required (if not already installed): -TODO: Friendlier directions for different distros/targets, prebuilt binaries +- libdbus-1-dev +- libglib2.0-dev +- libsdl1.2-dev -### Step 1: Determine Wiimote Address +Run the build script (in the project directory): -As of now, it is not possible to emulate an arbitrary Wiimote due to the Wii's -fussiness when connecting. So you'll need to determine both the Bluetooth -addresses of a Wii and a Wiimote in order to connect. +> source ./build-custom.sh -There are lots of ways to find the address of a Wiimote. Below are the steps -for one method. - - use hcitool to search for discoverable devices +For more information on the build script, see [this explainer](docs/CustomBuild.md). - > hcitool scan +### Using the Emulator - - press the red sync button under the battery cover of the Wiimote - - the Wiimote should be listed (Nintendo RVL-CNT-01) with its address +Stop any running Bluetooth service, e.g.: +> sudo service bluetooth stop -### Step 2: Determine Wii Address +Start the custom Bluetooth stack (e.g. from the project directory): -Finding a Wii's address is a bit trickier. There are multiple ways to do this -as well, including using a Bluetooth packet sniffer or using a utility via -custom firmware on the Wii. Below is a quicker (although messier) way. +> sudo ./bluez-4.101/dist/sbin/bluetoothd - - change your Bluetooth address to the Wiimote's (you'll need to do this anyway) +Run the emulator (in the project directory): - > sudo ./bdaddr +> ./wmemulator - - reset your adapter to apply the changes +With no arguments, the emulator will listen for incoming connections (similar to +syncing a real Wiimote). Pressing the sync button on a Wii should cause it to +connect. - > sudo hciconfig hci0 reset +You can also supply the address of a Wii to directly connect to it as long as +you have connected to it before (or you change your device's address to the +address of a trusted Wiimote). - - run the hcidump utility (you may need to download) (any BT activity monitor - can be substituted) +> ./wmemulator XX:XX:XX:XX:XX:XX - > hcidump +You will need to run the custom Bluetooth stack (as described above) whenever +using the emulator (it won't persist after e.g. a device restart). Also, the +custom stack generally won't be useful for anything besides Wiimote emulation. - - press the red sync button on the Wii - - press the red sync button on the Wiimote and after the status LEDs on the - Wiimote blink 1-5 times, disconnect one of the batteries - - if done correctly (with a bit of luck) the Wii will connect to your device, - revealing its address - - this may take few tries -- note that you must press both sync buttons again - each time +To stop the custom stack and restore the original Bluetooth service, e.g.: -### Step 3: Run the Emulator +> sudo killall bluetoothd -Once you have determined the address of a Wii and Wiimote, you can run the -emulator and it will automatically connect to the Wii. As long as the Wiimote -and the Wii are synced, you will no longer need the physical Wiimote. Though -due to using its Bluetooth address, you will not be able to connect it to the -Wii simultaneously with the emulator. Run the following from the WiimoteEmulator -directory and replace the zeros with your Wii's address. +> sudo service bluetooth start - > ./wmemulator 00:00:00:00:00:00 +For more information on bluetooth addresses, see [this explainer](docs/BluetoothAddresses.md). -You will need your Bluetooth address to be the Wiimote's address every time you -run the emulator. Note that your adapter may reset to the original address when -you restart your PC, etc. +### Connecting via UDP sockets -In the event that a mishap or bug in the emulator causes your Wii to block -you from connecting, you will need to sync the original controller again to -regain its trust. You will also need to clear the stored link key using -test-device (included with Bluez). +To connect via sockets it is expected that you know the Wii consoles address. - > (somewhere)/bluez-4.101/test/test-device remove +#### UNIX -### Other Tools +> ./wmemulator XX:XX:XX:XX:XX:XX unix /tmp/some-path-here -**bdaddr** +#### IP -A redist of the bdaddr utility for Bluez. Might build into some tools, but -ideally in a future version address spoofing won't be needed anymore. +> ./wmemulator XX:XX:XX:XX:XX:XX ip {some-port-number-here} -**packedtest** +#### Data Format -Simple utility for verifying the behavior of packed structs in your -environment. Might be useful for portability checking. +Sockets use the format `type status action`. For example, to press and hold the Wiimote + button you would send `button 1 WIIMOTE_PLUS`. To release the + button you would send `button 0 WIIMOTE_PLUS`. -**sdptool** +**`type`** This can be `analog_motion`, `button`, `hotplug`, or `emulator_control`. -Not useful for anything right now. Part of ongoing work to try to get the Wii -to sync directly with the emulator. Running this tool creates the Wiimote's -SDP records, which are vital to the Wii's initial sync process (but not used -after pairing). +**`status`** This is the enabled / disabled state of the action. '0' = turn off, '1' = turn on. If you send a 1 the button will stay "pressed" until a 0 is sent. -**wiimitm** +**`action`** This is the equivalent to the physical control you want to invoke. For example `WIIMOTE_PLUS` or `IR_UP` -Connects to a Wiimote and a Wii at the same time to eavesdrop on their -communications. All packets sent between the console and controller are -printed in readable format. Useful for debugging, reverse engineering, -or entertaining one's curiosity. -(TODO: Add this to the repo) +For more information on available types and actions, see [this explainer](docs/SocketActions.md). diff --git a/adapter.c b/adapter.c new file mode 100644 index 0000000..677a5ca --- /dev/null +++ b/adapter.c @@ -0,0 +1,600 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "adapter.h" +#include "bdaddr.h" + +#define HCI_TIMEOUT 1000 + +static int bdaddr_was_set = 0; +static bdaddr_t original_bdaddr; +static char original_name[HCI_MAX_NAME_LENGTH]; +static uint8_t original_class[3]; +static uint8_t original_scan_enable; +static uint8_t original_iac[MAX_IAC_LAP][3]; +static uint8_t original_iac_num; +static uint8_t original_simple_pairing_mode; + +static bdaddr_t wiimote_baddr; +static const char wiimote_name[] = "Nintendo RVL-CNT-01"; +static const uint32_t wiimote_class = 0x002504; +static const uint8_t wiimote_iac[3] = { 0x00, 0x8B, 0x9E }; + +static const uint32_t nintendo_ouis[66] = +{ + 0xECC40D, 0xE84ECE, 0xE0F6B5, 0xE0E751, 0xE00C7F, 0xDC68EB, 0xD86BF7, 0xD4F057, + 0xCCFB65, 0xCC9E00, 0xB8AE6E, 0xB88AEC, 0xB87826, 0xA4C0E1, 0xA45C27, 0xA438CC, + 0x9CE635, 0x98E8FA, 0x98B6E9, 0x98415C, 0x9458CB, 0x8CCDE8, 0x8C56C5, 0x7CBB8A, + 0x78A2A0, 0x7048F7, 0x64B5C6, 0x606BFF, 0x5C521E, 0x58BDA3, 0x582F40, 0x48A5E7, + 0x40F407, 0x40D28A, 0x34AF2C, 0x342FBD, 0x2C10C1, 0x182A7B, 0x0403D6, 0x002709, + 0x002659, 0x0025A0, 0x0024F3, 0x002444, 0x00241E, 0x0023CC, 0x002331, 0x0022D7, + 0x0022AA, 0x00224C, 0x0021BD, 0x002147, 0x001FC5, 0x001F32, 0x001EA9, 0x001E35, + 0x001DBC, 0x001CBE, 0x001BEA, 0x001B7A, 0x001AE9, 0x0019FD, 0x00191D, 0x0017AB, + 0x001656, 0x0009BF +}; + +int hci_read_scan_enable(int dd, uint8_t * enabled, int to) +{ + struct { + uint8_t status; + uint8_t enabled; + } __attribute__ ((packed)) rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_SCAN_ENABLE; + rq.rparam = &rp; + rq.rlen = sizeof(rp); + + if (hci_send_req(dd, &rq, to) < 0) + { + return -1; + } + + if (rp.status) + { + errno = EIO; + return -1; + } + + *enabled = rp.enabled; + return 0; +} + +int hci_write_scan_enable(int dd, uint8_t enabled, int to) +{ + struct { + uint8_t enabled; + } __attribute__ ((packed)) cp; + struct { + uint8_t status; + } __attribute__ ((packed)) rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.enabled = enabled; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_SCAN_ENABLE; + rq.cparam = &cp; + rq.clen = sizeof(cp); + rq.rparam = &rp; + rq.rlen = sizeof(rp); + + if (hci_send_req(dd, &rq, to) < 0) + { + return -1; + } + + if (rp.status) + { + errno = EIO; + return -1; + } + + return 0; +} + +int set_up_device_address(int dd, int device_id) +{ + int ret, i; + uint32_t uap; + struct hci_dev_info di; + struct hci_version ver; + + ret = hci_devinfo(device_id, &di); + if (ret < 0) + { + fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + + if (!bacmp(&di.bdaddr, BDADDR_ANY)) + { + ret = hci_read_bd_addr(dd, &original_bdaddr, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read address for hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + } + else + { + bacpy(&original_bdaddr, &di.bdaddr); + } + + ret = hci_read_local_version(dd, &ver, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read version info for hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + + //check if bdaddr already has a Nintendo OUI (e.g. it was manually set) + uap = (original_bdaddr.b[5] << 16) | (original_bdaddr.b[4] << 8) | + original_bdaddr.b[3]; + for (i = 0; i < sizeof(nintendo_ouis); i++) + { + if (nintendo_ouis[i] == uap) + { + return 0; + } + } + + bacpy(&wiimote_baddr, &original_bdaddr); + wiimote_baddr.b[5] = (nintendo_ouis[65] >> 16) & 0xFF; + wiimote_baddr.b[4] = (nintendo_ouis[65] >> 8) & 0xFF; + wiimote_baddr.b[3] = (nintendo_ouis[65]) & 0xFF; + + ret = set_device_bdaddr(dd, &ver, &wiimote_baddr); + if (ret < 0) + { + printf("Failed to set device address\n"); + printf("Device manufacturer: %s (%d)\n", + bt_compidtostr(ver.manufacturer), ver.manufacturer); + return -1; + } + + bdaddr_was_set = 1; + + ret = ioctl(dd, HCIDEVDOWN, device_id); + if (ret < 0) + { + fprintf(stderr, "Can't down device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + } + + ret = ioctl(dd, HCIDEVUP, device_id); + if (ret < 0) + { + fprintf(stderr, "Can't init device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + } + + return 0; +} + +int restore_device_address(int dd, int device_id) +{ + int ret; + struct hci_version ver; + + if (bdaddr_was_set == 0) + { + return 0; + } + + ret = hci_read_local_version(dd, &ver, 1000); + if (ret < 0) + { + fprintf(stderr, "Can't read version info: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = set_device_bdaddr(dd, &ver, &original_bdaddr); + if (ret < 0) + { + printf("Failed to restore device address\n"); + printf("Device manufacturer: %s (%d)\n", + bt_compidtostr(ver.manufacturer), ver.manufacturer); + return -1; + } + + ret = ioctl(dd, HCIDEVDOWN, device_id); + if (ret < 0) + { + fprintf(stderr, "Can't down device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + } + + ret = ioctl(dd, HCIDEVUP, device_id); + if (ret < 0) + { + fprintf(stderr, "Can't init device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + } + + return 0; +} + +int set_up_device_name(int dd) +{ + int ret; + + ret = hci_read_local_name(dd, HCI_MAX_NAME_LENGTH, original_name, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read device name: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_local_name(dd, wiimote_name, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't write device name: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int restore_device_name(int dd) +{ + int ret; + + ret = hci_write_local_name(dd, original_name, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't restore device name: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int set_up_device_class(int dd) +{ + int ret; + + ret = hci_read_class_of_dev(dd, original_class, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read device class: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_class_of_dev(dd, wiimote_class, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't write device class: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int restore_device_class(int dd) +{ + int ret; + + uint32_t class_int = 0; + class_int |= original_class[0]; + class_int |= original_class[1] << 8; + class_int |= original_class[2] << 16; + + ret = hci_write_class_of_dev(dd, class_int, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't restore device class: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int set_up_device_inquiry(int dd) +{ + int ret; + + ret = hci_read_scan_enable(dd, &original_scan_enable, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read scan enable: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_scan_enable(dd, SCAN_INQUIRY | SCAN_PAGE, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't write scan enable: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_read_current_iac_lap(dd, &original_iac_num, (uint8_t *)original_iac, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read iac: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_current_iac_lap(dd, 1, (uint8_t *)wiimote_iac, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't write iac: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int restore_device_inquiry(int dd) +{ + int ret; + + ret = hci_write_scan_enable(dd, original_scan_enable, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't restore scan enable: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_current_iac_lap(dd, original_iac_num, (uint8_t *)original_iac, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't restore iac: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int set_up_simple_pairing_mode(int dd) +{ + int ret; + + ret = hci_read_simple_pairing_mode(dd, &original_simple_pairing_mode, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't read simple pairing mode: %s (%d)\n", strerror(errno), errno); + return -1; + } + + ret = hci_write_simple_pairing_mode(dd, 0, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't write simple pairing mode: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int restore_simple_pairing_mode(int dd) +{ + int ret; + + ret = hci_write_simple_pairing_mode(dd, original_simple_pairing_mode, HCI_TIMEOUT); + if (ret < 0) + { + fprintf(stderr, "Can't restore simple pairing mode: %s (%d)\n", strerror(errno), errno); + return -1; + } + + return 0; +} + +int set_up_device(char * dev_str) +{ + int device_id = 0, dd, ret; + + dd = hci_open_dev(device_id); + if (dd < 0) + { + fprintf(stderr, "Can't open device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + + ret = set_up_device_address(dd, device_id); + if (ret < 0) + { + printf("Failed to set device address\n"); + printf("Warning: device address must have a Nintendo OUI\n"); + } + + ret = set_up_device_name(dd); + if (ret < 0) + { + printf("Failed to set device name\n"); + hci_close_dev(dd); + return -1; + } + + ret = set_up_device_class(dd); + if (ret < 0) + { + printf("Failed to set device class\n"); + hci_close_dev(dd); + return -1; + } + + ret = set_up_device_inquiry(dd); + if (ret < 0) + { + printf("Failed to set device inquiry settings\n"); + hci_close_dev(dd); + return -1; + } + + ret = set_up_simple_pairing_mode(dd); + if (ret < 0) + { + printf("Failed to set simple pairing mode\n"); + printf("Warning: make sure secure simple pairing mode is disabled\n"); + } + + hci_close_dev(dd); + return 0; +} + +int restore_device() +{ + int device_id = 0, dd, ret; + + dd = hci_open_dev(device_id); + if (dd < 0) + { + fprintf(stderr, "Can't open device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + + ret = restore_device_address(dd, device_id); + if (ret < 0) + { + printf("Failed to restore device address\n"); + } + + ret = restore_device_name(dd); + if (ret < 0) + { + printf("Failed to restore device name\n"); + hci_close_dev(dd); + return -1; + } + + ret = restore_device_class(dd); + if (ret < 0) + { + printf("Failed to restore device class\n"); + hci_close_dev(dd); + return -1; + } + + ret = restore_device_inquiry(dd); + if (ret < 0) + { + printf("Failed to restore device inquiry settings\n"); + hci_close_dev(dd); + return -1; + } + + ret = restore_simple_pairing_mode(dd); + if (ret < 0) + { + printf("Failed to restore simple pairing mode\n"); + hci_close_dev(dd); + return -1; + } + + hci_close_dev(dd); + return 0; +} + +int power_off_host(const bdaddr_t * host_bdaddr) +{ + int ret, dd; + struct hci_conn_info_req * cr; + + dd = hci_open_dev(hci_get_route((bdaddr_t *)host_bdaddr)); + if (dd < 0) + { + return dd; + } + + cr = (struct hci_conn_info_req *)malloc(sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info)); + bacpy(&cr->bdaddr, host_bdaddr); + cr->type = ACL_LINK; + + ret = ioctl(dd, HCIGETCONNINFO, (unsigned long)cr); + if (ret) + { + hci_close_dev(dd); + free(cr); + return ret; + } + + ret = hci_disconnect(dd, cr->conn_info->handle, HCI_OE_POWER_OFF, HCI_TIMEOUT); + hci_close_dev(dd); + free(cr); + + if (ret) + { + return ret; + } + + return 0; +} + +int get_device_bdaddr(int device_id, bdaddr_t * out_bdaddr) +{ + int ret; + struct hci_dev_info di; + + ret = hci_devinfo(device_id, &di); + if (ret < 0) + { + return ret; + } + + bacpy(out_bdaddr, &di.bdaddr); + + return 0; +} + +int find_wiimote(bdaddr_t * out_bdaddr) +{ + int device_id = 0, dd; + int max_rsp, num_rsp; + inquiry_info *ii = NULL; + int i, len, flags; + char name[248] = { 0 }; + + dd = hci_open_dev(device_id); + if (dd < 0) + { + fprintf(stderr, "Can't open device hci%d: %s (%d)\n", + device_id, strerror(errno), errno); + return -1; + } + + len = 8; + max_rsp = 8; + flags = IREQ_CACHE_FLUSH; + ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info)); + + num_rsp = hci_inquiry(device_id, len, max_rsp, (uint8_t *)wiimote_iac, &ii, flags); + if (num_rsp < 0) + { + fprintf(stderr, "HCI inquiry failed\n"); + return -1; + } + + for (i = 0; i < num_rsp; i++) + { + hci_read_remote_name(dd, &(ii+i)->bdaddr, sizeof(name), name, 0); + + if (!strncmp(name, wiimote_name, sizeof(wiimote_name) - 1)) + { + bacpy(out_bdaddr, &(ii+i)->bdaddr); + break; + } + } + + free(ii); + hci_close_dev(dd); + + return 0; +} \ No newline at end of file diff --git a/adapter.h b/adapter.h new file mode 100644 index 0000000..b31cdff --- /dev/null +++ b/adapter.h @@ -0,0 +1,10 @@ +#ifndef ADAPTER_H +#define ADAPTER_H + +int set_up_device(char * dev_str); +int restore_device(); +int power_off_host(const bdaddr_t * host_bdaddr); +int get_device_bdaddr(int device_id, bdaddr_t * out_bdaddr); +int find_wiimote(bdaddr_t * out_bdaddr); + +#endif /* ADAPTER_H */ diff --git a/bdaddr.c b/bdaddr.c index 8fd034c..ef32b27 100644 --- a/bdaddr.c +++ b/bdaddr.c @@ -21,9 +21,6 @@ * */ -#ifdef HAVE_CONFIG_H -#include -#endif #include #include @@ -33,11 +30,7 @@ #include #include -#include -#include -#include - -#include "oui.h" +#include "bdaddr.h" static int transient = 0; @@ -316,164 +309,42 @@ static struct { { 48, st_write_bd_addr, generic_reset_device }, { 57, ericsson_write_bd_addr, generic_reset_device }, { 72, mrvl_write_bd_addr, generic_reset_device }, + { 305, bcm_write_bd_addr, generic_reset_device }, { 65535, NULL, NULL }, }; -static void usage(void) -{ - printf("bdaddr - Utility for changing the Bluetooth device address\n\n"); - printf("Usage:\n" - "\tbdaddr [-i ] [-r] [-t] [new bdaddr]\n"); -} - -static struct option main_options[] = { - { "device", 1, 0, 'i' }, - { "reset", 0, 0, 'r' }, - { "transient", 0, 0, 't' }, - { "help", 0, 0, 'h' }, - { 0, 0, 0, 0 } -}; - -int main(int argc, char *argv[]) +int set_device_bdaddr(int dd, const struct hci_version * ver, const bdaddr_t * bdaddr) { - struct hci_dev_info di; - struct hci_version ver; - bdaddr_t bdaddr; - char addr[18], *comp; - int i, dd, opt, dev = 0, reset = 0; - - bacpy(&bdaddr, BDADDR_ANY); - - while ((opt=getopt_long(argc, argv, "+i:rth", main_options, NULL)) != -1) { - switch (opt) { - case 'i': - dev = hci_devid(optarg); - if (dev < 0) { - perror("Invalid device"); - exit(1); - } - break; - - case 'r': - reset = 1; - break; - - case 't': - transient = 1; - break; - - case 'h': - default: - usage(); - exit(0); - } - } - - argc -= optind; - argv += optind; - optind = 0; - - dd = hci_open_dev(dev); - if (dd < 0) { - fprintf(stderr, "Can't open device hci%d: %s (%d)\n", - dev, strerror(errno), errno); - exit(1); - } - - if (hci_devinfo(dev, &di) < 0) { - fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n", - dev, strerror(errno), errno); - hci_close_dev(dd); - exit(1); - } - - if (hci_read_local_version(dd, &ver, 1000) < 0) { - fprintf(stderr, "Can't read version info for hci%d: %s (%d)\n", - dev, strerror(errno), errno); - hci_close_dev(dd); - exit(1); - } - - if (!bacmp(&di.bdaddr, BDADDR_ANY)) { - if (hci_read_bd_addr(dd, &bdaddr, 1000) < 0) { - fprintf(stderr, "Can't read address for hci%d: %s (%d)\n", - dev, strerror(errno), errno); - hci_close_dev(dd); - exit(1); - } - } else - bacpy(&bdaddr, &di.bdaddr); - - printf("Manufacturer: %s (%d)\n", - bt_compidtostr(ver.manufacturer), ver.manufacturer); - - comp = batocomp(&bdaddr); - - ba2str(&bdaddr, addr); - printf("Device address: %s", addr); - - if (comp) { - printf(" (%s)\n", comp); - free(comp); - } else - printf("\n"); - - if (argc < 1) { - hci_close_dev(dd); - exit(0); + if (ver == NULL) + { + return -1; } - str2ba(argv[0], &bdaddr); - if (!bacmp(&bdaddr, BDADDR_ANY)) { - hci_close_dev(dd); - exit(0); + if (!bacmp(bdaddr, BDADDR_ANY)) + { + return -1; } - for (i = 0; vendor[i].compid != 65535; i++) - if (ver.manufacturer == vendor[i].compid) { - comp = batocomp(&bdaddr); - - ba2str(&bdaddr, addr); - printf("New BD address: %s", addr); - - if (comp) { - printf(" (%s)\n\n", comp); - free(comp); - } else - printf("\n\n"); - - - if (vendor[i].write_bd_addr(dd, &bdaddr) < 0) { - fprintf(stderr, "Can't write new address\n"); - hci_close_dev(dd); - exit(1); + for (int i = 0; vendor[i].compid != 65535; i++) + { + if (ver->manufacturer == vendor[i].compid) + { + if (!vendor[i].reset_device) + { + fprintf(stderr, "Can't automatically reset device\n"); + return -1; } - printf("Address changed - "); - - if (reset && vendor[i].reset_device) { - if (vendor[i].reset_device(dd) < 0) { - printf("Reset device manually\n"); - } else { - ioctl(dd, HCIDEVRESET, dev); - printf("Device reset successfully\n"); - } - } else { - printf("Reset device now\n"); + if (vendor[i].write_bd_addr(dd, (bdaddr_t *)bdaddr) < 0) { + fprintf(stderr, "Can't write new address\n"); + return -1; } - //ioctl(dd, HCIDEVRESET, dev); - //ioctl(dd, HCIDEVDOWN, dev); - //ioctl(dd, HCIDEVUP, dev); - - hci_close_dev(dd); - exit(0); + return 0; } + } - hci_close_dev(dd); - - printf("\n"); fprintf(stderr, "Unsupported manufacturer\n"); - exit(1); + return -1; } diff --git a/bdaddr.h b/bdaddr.h new file mode 100644 index 0000000..62f18fc --- /dev/null +++ b/bdaddr.h @@ -0,0 +1,10 @@ +#ifndef BDADDR_H +#define BDADDR_H + +#include +#include +#include + +int set_device_bdaddr(int dd, const struct hci_version * ver, const bdaddr_t * bdaddr); + +#endif /* BDADDR_H */ diff --git a/bluez-disable-sdp.patch b/bluez-disable-sdp.patch new file mode 100644 index 0000000..04ab391 --- /dev/null +++ b/bluez-disable-sdp.patch @@ -0,0 +1,20 @@ +--- src/main.c 2021-01-29 18:57:36.564328565 +0000 ++++ src/main-patched.c 2021-01-28 19:01:42.333153461 +0000 +@@ -522,7 +522,7 @@ + } + } + +- start_sdp_server(mtu, SDP_SERVER_COMPAT); ++ //start_sdp_server(mtu, SDP_SERVER_COMPAT); + + /* Loading plugins has to be done after D-Bus has been setup since + * the plugins might wanna expose some paths on the bus. However the +@@ -549,7 +549,7 @@ + + plugin_cleanup(); + +- stop_sdp_server(); ++ //stop_sdp_server(); + + agent_exit(); + diff --git a/bluez-include-uio.patch b/bluez-include-uio.patch new file mode 100644 index 0000000..5d0f2aa --- /dev/null +++ b/bluez-include-uio.patch @@ -0,0 +1,21 @@ +--- tools/hciattach_qualcomm.c 2012-06-13 17:04:20.000000000 +0200 ++++ tools/hciattach_qualcomm-patched.c 2024-11-12 19:40:29.296161803 +0100 +@@ -27,6 +27,7 @@ + #endif + + #include ++#include + #include + #include + #include + +--- tools/hciattach_tialt.c 2012-06-13 17:04:20.000000000 +0200 ++++ tools/hciattach_tialt-patched.c 2024-11-12 19:37:58.389273652 +0100 +@@ -26,6 +26,7 @@ + #endif + + #include ++#include + #include + #include + #include diff --git a/bluez-plugin/Makefile b/bluez-plugin/Makefile index 1368858..2049589 100644 --- a/bluez-plugin/Makefile +++ b/bluez-plugin/Makefile @@ -1,4 +1,9 @@ -all: - gcc -Wall -DHAVE_CONFIG_H -g -c -fvisibility=hidden -fPIC -o wmemu.o wmemu.c -I'..' `pkg-config --cflags --libs glib-2.0` `pkg-config --cflags dbus-1` -ldbus-1 +BLUEZ_DIR=../bluez-4.101 + +all: wmemu.so +clean: + rm -f wmemu.so wmemu.o +wmemu.so: + gcc -Wall -DHAVE_CONFIG_H -g -c -fvisibility=hidden -fPIC -o wmemu.o wmemu.c -I"$(BLUEZ_DIR)" -I"$(BLUEZ_DIR)/dist/include" `pkg-config --cflags --libs glib-2.0` `pkg-config --cflags dbus-1` -ldbus-1 gcc -Wl,-E -shared -o wmemu.so wmemu.o -ldbus-1 diff --git a/bluez-plugin/wmemu.c b/bluez-plugin/wmemu.c index 07ca319..184fd15 100644 --- a/bluez-plugin/wmemu.c +++ b/bluez-plugin/wmemu.c @@ -12,9 +12,8 @@ #include "src/device.h" #include "src/log.h" -static ssize_t wmemu_pincb(struct btd_adapter *adapter, struct btd_device *device, - char *pinbuf, bool *display, - unsigned int attempt) +long int wmemu_pincb(struct btd_adapter *adapter, struct btd_device *device, + char *pinbuf, int *user_io_capability) { //force Wii pin (bdaddr bytes backwards) //very basic, no filtering, does this for all devices diff --git a/build-custom.sh b/build-custom.sh new file mode 100644 index 0000000..87cecb8 --- /dev/null +++ b/build-custom.sh @@ -0,0 +1,27 @@ +#!/bin/sh + +# download bluez-4.101 dist +wget https://www.kernel.org/pub/linux/bluetooth/bluez-4.101.tar.xz +tar -xf ./bluez-4.101.tar.xz +rm ./bluez-4.101.tar.xz + +# apply patch (to disable sdp server) and build bluez +cd bluez-4.101 +mkdir dist +DIST=$(pwd)/dist +cp ../bluez-disable-sdp.patch . +cp ../bluez-include-uio.patch . +patch -p0 < bluez-disable-sdp.patch +patch -p0 < bluez-include-uio.patch +./configure --prefix=$DIST --with-systemdunitdir=$DIST/system --disable-service --disable-audio --disable-input --disable-serial +make && make install + +# build bluez-plugin +cd ../bluez-plugin +make clean && make +cp ./wmemu.so $DIST/lib/bluetooth/plugins +cd .. + +# build emulator +make clean +CUSTOM_BUILD=1 make diff --git a/docs/BluetoothAddresses.md b/docs/BluetoothAddresses.md new file mode 100644 index 0000000..a404185 --- /dev/null +++ b/docs/BluetoothAddresses.md @@ -0,0 +1,55 @@ +# Bluetooth Addresses + +This is a reference for techniques involving Bluetooth addresses. In most cases, +these are no longer needed, but are left here in case they may be useful. + +## Determine Wiimote Address + +There are lots of ways to find the address of a Wiimote. Below are the steps +for one method. + - use hcitool to search for discoverable devices + + > hcitool scan + + - press the red sync button under the battery cover of the Wiimote + - the Wiimote should be listed (Nintendo RVL-CNT-01) with its address + +## Determine Wii Address + +There are multiple ways to do this as well, including using a Bluetooth packet +sniffer or using a utility via custom firmware on the Wii. + +### Sync Emulator + +Unless the emulator is not working, the emulator should automatically connect +to your Wii after pressing the controller sync button on the console. Once +connected, it will print the address of the Wii. + +### Homebrew method + +If you have installed the Homebrew Channel on your console or are willing to do +so, then you can use [identify-mii](https://github.com/ThatsJustCheesy/identify-mii) +to display your console's address on the screen. + +### Battery method (hacky but usually works eventually) + + - change your Bluetooth address to a real Wiimote's using the bdaddr utility: + + > sudo bdaddr (wiimote address) + + - reset your adapter to apply the changes + + > sudo hciconfig hci0 reset + + - run the hcidump utility (any BT activity monitor can be substituted) + + > hcidump + + - press the red sync button on the Wii + - press the red sync button on the Wiimote and after the status LEDs on the + Wiimote blink 1-5 times, disconnect one of the batteries + - if done correctly (with a bit of luck) the Wii will connect to your device, + revealing its address + - this may take few tries -- note that you must press both sync buttons again + each time + diff --git a/docs/CustomBuild.md b/docs/CustomBuild.md new file mode 100644 index 0000000..3b7bf99 --- /dev/null +++ b/docs/CustomBuild.md @@ -0,0 +1,28 @@ +# Custom Build + +This is a reference for what the `build-custom.sh` script does. + +The custom build downloads and builds a patched version of bluez-4.101 with +the following changes: + + - The built-in SDP server is disabled, as a Wii-specific SDP handler is used + in the emulator instead. + - A PIN code handler plugin is installed to respond to PIN code requests from + the Wii. This plugin is typically only needed to connect to a Wii without + doing the sync/discovery process (e.g. spoofing a synced Wiimote's address). + The plugin only works on older versions of bluez, so this is why 4.101 is + used. + +Run the build script (in the project directory): + + > source ./build-custom.sh + +Stop any running Bluetooth service, e.g.: + + > sudo service bluetooth stop + +Start the custom bluetooth stack (e.g. from the project directory): + + > sudo ./bluez-4.101/dist/sbin/bluetoothd + +Then use the emulator normally. \ No newline at end of file diff --git a/docs/SocketActions.md b/docs/SocketActions.md new file mode 100644 index 0000000..51fb17a --- /dev/null +++ b/docs/SocketActions.md @@ -0,0 +1,67 @@ +# Socket Actions + +This is the full list of socket actions that can be sent. + +## Type `button` + +- HOME +- WIIMOTE_UP +- WIIMOTE_DOWN +- WIIMOTE_LEFT +- WIIMOTE_RIGHT +- WIIMOTE_A +- WIIMOTE_B +- WIIMOTE_1 +- WIIMOTE_2 +- WIIMOTE_PLUS +- WIIMOTE_MINUS +- NUNCHUK_C +- NUNCHUK_Z +- CLASSIC_UP +- CLASSIC_DOWN +- CLASSIC_LEFT +- CLASSIC_RIGHT +- CLASSIC_A +- CLASSIC_B +- CLASSIC_X +- CLASSIC_Y +- CLASSIC_L +- CLASSIC_R +- CLASSIC_ZL +- CLASSIC_ZR +- CLASSIC_PLUS +- CLASSIC_MINUS + +## Type `analog_motion` + +- IR_UP +- IR_DOWN +- IR_LEFT +- IR_RIGHT +- STEER_LEFT +- STEER_RIGHT +- NUNCHUK_UP +- NUNCHUK_DOWN +- NUNCHUK_LEFT +- NUNCHUK_RIGHT +- CLASSIC_LEFT_STICK_UP +- CLASSIC_LEFT_STICK_DOWN +- CLASSIC_LEFT_STICK_LEFT +- CLASSIC_LEFT_STICK_RIGHT +- MOTIONPLUS_UP +- MOTIONPLUS_DOWN +- MOTIONPLUS_LEFT +- MOTIONPLUS_RIGHT +- MOTIONPLUS_SLOW + +## Type `emulator_control` + +- quit +- power_off + +## Type `hotplug` + +- nunchuk +- classic +- balance_board +- none diff --git a/eeprom.bin b/eeprom.bin index 68880cc..e413bce 100644 Binary files a/eeprom.bin and b/eeprom.bin differ diff --git a/input.c b/input.c index 68e2b39..d4645d0 100644 --- a/input.c +++ b/input.c @@ -2,322 +2,258 @@ #include "SDL/SDL.h" #include +#include "motion.h" -/* This old file badly needs refactoring. */ +int ir_up, ir_down, ir_left, ir_right, + steer_left, steer_right, + nunchuk_up, nunchuk_down, nunchuk_left, nunchuk_right, + classic_left_stick_up, classic_left_stick_down, classic_left_stick_left, classic_left_stick_right, + motionplus_up, motionplus_down, motionplus_left, motionplus_right, motionplus_slow; +extern int show_reports; -int up, down, left, right; -bool steerright, steerleft; -double steerang = (PI / 2); -int arrow_function = 1; //0 for IR, 1 for nunchuck stick, 2 for classic stick -bool togglekey0 = 0; -bool togglekey9 = 0; -bool shift; +static const double pointer_margin = 0.5; +float pointer_x = 0.5; +float pointer_y = 0.5; -void input_init() +int input_update(struct wiimote_state *state, struct input_source const * source) { - //init SDL - if (SDL_Init(0) < 0) - { - printf("Could not initialize SDL: %s\n", SDL_GetError()); - exit(1); - } - SDL_SetVideoMode(128, 128, 0, 0); -} - -void input_unload() -{ - SDL_Quit(); -} + struct input_event event; -void input_update(struct wiimote_state * state) -{ - SDL_Event event; + float pointer_delta_x = 0, pointer_delta_y = 0; /* Loop through waiting messages and process them */ - while (SDL_PollEvent(&event)) + while (source->poll_event(&event)) { switch (event.type) { + case INPUT_EVENT_TYPE_EMULATOR_CONTROL: + switch (event.emulator_control_event.control) + { + case INPUT_EMULATOR_CONTROL_QUIT: + return -1; + case INPUT_EMULATOR_CONTROL_POWER_OFF: + return -2; + case INPUT_EMULATOR_CONTROL_TOGGLE_REPORTS: + show_reports = (show_reports + 1) % 2; + break; + } + break; + case INPUT_EVENT_TYPE_HOTPLUG: + switch (event.hotplug_event.extension) + { + case Nunchuk: + reset_input_nunchuk(&state->usr.nunchuk); + reset_input_ir(state->usr.ir_object); + break; + case Classic: + reset_input_classic(&state->usr.classic); + reset_input_ir(state->usr.ir_object); + break; + case BalanceBoard: + reset_input_ir(state->usr.ir_object); + break; + case NoExtension: + reset_input_ir(state->usr.ir_object); + pointer_x = 0.5; + pointer_y = 0.5; + break; + default: + goto invalid; + } - case SDL_KEYDOWN: - switch (event.key.keysym.sym) - { - case SDLK_ESCAPE: - exit(0); - break; - - case SDLK_0: - if (togglekey0 == 0) - { - togglekey0 = 1; - arrow_function = (arrow_function + 1) % 4; - printf("arrows (IR, nunchuck, classic, wmp): %d \n", arrow_function); - if (arrow_function != 0) - { - ir_object_clear(state, 0); - ir_object_clear(state, 1); - ir_object_clear(state, 2); - ir_object_clear(state, 3); - } else { - state->usr.ir_object[0].x = 400; - state->usr.ir_object[0].y = 400; - state->usr.ir_object[0].size = 8; - state->usr.ir_object[1].x = 600; - state->usr.ir_object[1].y = 400; - state->usr.ir_object[1].size = 8; - } - } - break; - case SDLK_9: - if (togglekey9 == 0) - { - togglekey9 = 1; - show_reports = (show_reports + 1) % 2; - } - break; - case SDLK_LSHIFT: - shift = 1; break; - case SDLK_a: - state->usr.a = 1; - //state->usr.classic.a = 1; - break; - case SDLK_d: - state->usr.b = 1; - //state->usr.classic.b = 1; - break; - case SDLK_q: - state->usr.nunchuck.c = 1; - state->usr.classic.x = 1; - break; - case SDLK_e: - state->usr.nunchuck.z = 1; - state->usr.classic.y = 1; - break; - case SDLK_1: - state->usr.one = 1; - break; - case SDLK_2: - state->usr.two = 1; - break; - case SDLK_3: - state->usr.minus = 1; - break; - case SDLK_4: - state->usr.plus = 1; - break; - case SDLK_h: - state->usr.home = 1; - break; - case SDLK_KP8: - state->usr.up = 1; - break; - case SDLK_KP2: - state->usr.down = 1; - break; - case SDLK_KP4: - state->usr.left = 1; - break; - case SDLK_KP6: - state->usr.right = 1; - break; - case SDLK_UP: - up = 1; - break; - - case SDLK_DOWN: - down = 1; - break; + state->usr.connected_extension_type = event.hotplug_event.extension; + invalid: + break; + case INPUT_EVENT_TYPE_BUTTON: { + bool pressed = event.button_event.pressed; + switch (event.button_event.button) + { + case INPUT_BUTTON_HOME: + state->usr.home = pressed; + break; - case SDLK_LEFT: - left = 1; - break; + case INPUT_BUTTON_WIIMOTE_UP: + state->usr.up = pressed; + break; + case INPUT_BUTTON_WIIMOTE_DOWN: + state->usr.down = pressed; + break; + case INPUT_BUTTON_WIIMOTE_LEFT: + state->usr.left = pressed; + break; + case INPUT_BUTTON_WIIMOTE_RIGHT: + state->usr.right = pressed; + break; + case INPUT_BUTTON_WIIMOTE_A: + state->usr.a = pressed; + break; + case INPUT_BUTTON_WIIMOTE_B: + state->usr.b = pressed; + break; + case INPUT_BUTTON_WIIMOTE_1: + state->usr.one = pressed; + break; + case INPUT_BUTTON_WIIMOTE_2: + state->usr.two = pressed; + break; + case INPUT_BUTTON_WIIMOTE_PLUS: + state->usr.plus = pressed; + break; + case INPUT_BUTTON_WIIMOTE_MINUS: + state->usr.minus = pressed; + break; - case SDLK_RIGHT: - right = 1; - break; + case INPUT_BUTTON_NUNCHUK_C: + state->usr.nunchuk.c = pressed; + break; + case INPUT_BUTTON_NUNCHUK_Z: + state->usr.nunchuk.z = pressed; + break; - case SDLK_t: - steerleft = 1; - break; - case SDLK_y: - steerright = 1; - break; - default: - break; - } + case INPUT_BUTTON_CLASSIC_UP: + state->usr.classic.up = pressed; + break; + case INPUT_BUTTON_CLASSIC_DOWN: + state->usr.classic.down = pressed; + break; + case INPUT_BUTTON_CLASSIC_LEFT: + state->usr.classic.left = pressed; + break; + case INPUT_BUTTON_CLASSIC_RIGHT: + state->usr.classic.right = pressed; + break; + case INPUT_BUTTON_CLASSIC_A: + state->usr.classic.a = pressed; + break; + case INPUT_BUTTON_CLASSIC_B: + state->usr.classic.b = pressed; + break; + case INPUT_BUTTON_CLASSIC_X: + state->usr.classic.x = pressed; + break; + case INPUT_BUTTON_CLASSIC_Y: + state->usr.classic.y = pressed; + break; + case INPUT_BUTTON_CLASSIC_L: + state->usr.classic.ltrigger = pressed; + break; + case INPUT_BUTTON_CLASSIC_R: + state->usr.classic.rtrigger = pressed; + break; + case INPUT_BUTTON_CLASSIC_ZL: + state->usr.classic.lz = pressed; + break; + case INPUT_BUTTON_CLASSIC_ZR: + state->usr.classic.rz = pressed; + break; + case INPUT_BUTTON_CLASSIC_PLUS: + state->usr.classic.plus = pressed; + break; + case INPUT_BUTTON_CLASSIC_MINUS: + state->usr.classic.minus = pressed; + break; + default: + printf("warning: button %d not handled by input_update\n", event.button_event.button); + break; + } + break; + } + case INPUT_EVENT_TYPE_ANALOG_MOTION: { + bool moving = event.analog_motion_event.moving; + switch (event.analog_motion_event.motion) + { + case INPUT_ANALOG_MOTION_POINTER: + pointer_delta_x = event.analog_motion_event.delta_x; + pointer_delta_y = event.analog_motion_event.delta_y; + break; + case INPUT_ANALOG_MOTION_IR_UP: + ir_up = moving; + break; + case INPUT_ANALOG_MOTION_IR_DOWN: + ir_down = moving; + break; + case INPUT_ANALOG_MOTION_IR_LEFT: + ir_left = moving; + break; + case INPUT_ANALOG_MOTION_IR_RIGHT: + ir_right = moving; break; - case SDL_KEYUP: - switch (event.key.keysym.sym) - { - case SDLK_0: - togglekey0 = 0; - break; - case SDLK_9: - togglekey9 = 0; - break; - case SDLK_LSHIFT: - shift = 0; break; - case SDLK_a: - state->usr.a = 0; - state->usr.classic.a = 0; - break; - case SDLK_d: - state->usr.b = 0; - state->usr.classic.b = 0; - break; - case SDLK_q: - state->usr.nunchuck.c = 0; - state->usr.classic.x = 0; - break; - case SDLK_e: - state->usr.nunchuck.z = 0; - state->usr.classic.y = 0; - break; - case SDLK_1: - state->usr.one = 0; - break; - case SDLK_2: - state->usr.two = 0; - break; - case SDLK_3: - state->usr.minus = 0; - break; - case SDLK_4: - state->usr.plus = 0; - break; - case SDLK_h: - state->usr.home = 0; - break; - case SDLK_KP8: - state->usr.up = 0; - break; - case SDLK_KP2: - state->usr.down = 0; - break; - case SDLK_KP4: - state->usr.left = 0; - break; - case SDLK_KP6: - state->usr.right = 0; - break; - case SDLK_UP: - up = 0; - break; - - case SDLK_DOWN: - down = 0; - break; - - case SDLK_LEFT: - left = 0; - break; - - case SDLK_RIGHT: - right = 0; - break; - - case SDLK_t: - steerleft = 0; - break; - case SDLK_y: - steerright = 0; - break; - default: - break; - } - } - } + case INPUT_ANALOG_MOTION_STEER_LEFT: + steer_left = moving; + break; + case INPUT_ANALOG_MOTION_STEER_RIGHT: + steer_right = moving; + break; - if ((steerleft && steerright) || (!steerleft && !steerright)) - { - steerang = (PI / 2); - } else if (steerleft) - { - steerang = (6 * PI / 8); - } else if (steerright) - { - steerang = (2 * PI / 8); - } + case INPUT_ANALOG_MOTION_NUNCHUK_UP: + nunchuk_up = moving; + break; + case INPUT_ANALOG_MOTION_NUNCHUK_DOWN: + nunchuk_down = moving; + break; + case INPUT_ANALOG_MOTION_NUNCHUK_LEFT: + nunchuk_left = moving; + break; + case INPUT_ANALOG_MOTION_NUNCHUK_RIGHT: + nunchuk_right = moving; + break; - /* + case INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_UP: + classic_left_stick_up = moving; + break; + case INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_DOWN: + classic_left_stick_down = moving; + break; + case INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_LEFT: + classic_left_stick_left = moving; + break; + case INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_RIGHT: + classic_left_stick_right = moving; + break; - if (steerleft) - { - if (steerang < (7 * PI / 8)) - steerang += 0.02; - state->usr.accel_y = -cos(steerang) * (0x19 << 2) + 0x200; - state->usr.accel_x = -sin(steerang) * (0x19 << 2) + 0x200; + case INPUT_ANALOG_MOTION_MOTIONPLUS_UP: + motionplus_up = moving; + break; + case INPUT_ANALOG_MOTION_MOTIONPLUS_DOWN: + motionplus_down = moving; + break; + case INPUT_ANALOG_MOTION_MOTIONPLUS_LEFT: + motionplus_left = moving; + break; + case INPUT_ANALOG_MOTION_MOTIONPLUS_RIGHT: + motionplus_right = moving; + break; + case INPUT_ANALOG_MOTION_MOTIONPLUS_SLOW: + motionplus_slow = moving; + break; + } + break; } - - if (steerright) - { - if (steerang > (1 * PI / 8)) - steerang -= 0.02; - state->usr.accel_y = -cos(steerang) * (0x19 << 2) + 0x200; - state->usr.accel_x = -sin(steerang) * (0x19 << 2) + 0x200; + default: + break; } + } - */ + pointer_delta_x += ir_right * 0.004 - ir_left * 0.004; + pointer_delta_y += ir_up * 0.004 - ir_down * 0.004; - //state->usr.accel_y = -cos(steerang) * (0x19 << 2) + 0x200; - //state->usr.accel_x = -sin(steerang) * (0x19 << 2) + 0x200; + pointer_x = fmax(-pointer_margin, fmin(1.0 + pointer_margin, pointer_x + pointer_delta_x)); + pointer_y = fmax(-pointer_margin, fmin(1.0 + pointer_margin, pointer_y + pointer_delta_y)); - switch (arrow_function) - { - case 0: - if (down) - { - if (state->usr.ir_object[0].y < 764) - { - state->usr.ir_object[0].y += 4; - state->usr.ir_object[1].y += 4; - } - } + set_motion_state(state, pointer_x, pointer_y); - if (up) - { - if (state->usr.ir_object[0].x > 3) - { - state->usr.ir_object[0].y -= 4; - state->usr.ir_object[1].y -= 4; - } + state->usr.nunchuk.x = 128 + nunchuk_right * 100 - nunchuk_left * 100; + state->usr.nunchuk.y = 128 + nunchuk_up * 100 - nunchuk_down * 100; - } + state->usr.classic.ls_x = 32 + classic_left_stick_right * 30 - classic_left_stick_left * 30; + state->usr.classic.ls_y = 32 + classic_left_stick_up * 30 - classic_left_stick_down * 30; - if (left) - { - if (state->usr.ir_object[0].x < 1020) - { - state->usr.ir_object[0].x += 4; - state->usr.ir_object[1].x += 4; - } + state->usr.motionplus.pitch_left = 0x1F7F + motionplus_down * 800 * (1 + !motionplus_slow) - motionplus_up * 800 * (1 + !motionplus_slow); + state->usr.motionplus.yaw_down = 0x1F7F + motionplus_left * 800 * (1 + !motionplus_slow) - motionplus_right * 800 * (1 + !motionplus_slow); + state->usr.motionplus.pitch_slow = motionplus_slow; + state->usr.motionplus.yaw_slow = motionplus_slow; - } - - if (right) - { - if (state->usr.ir_object[0].x > 3) - { - state->usr.ir_object[0].x -= 4; - state->usr.ir_object[1].x -= 4; - } - } - break; - case 1: - state->usr.nunchuck.x = 128 + right * 100 - left * 100; - state->usr.nunchuck.y = 128 + up * 100 - down * 100; - break; - case 2: - state->usr.classic.ls_x = 32 + right * 30 - left * 30; - state->usr.classic.ls_y = 32 + up * 30 - down * 30; - break; - case 3: - state->usr.motionplus.pitch_left = 0x1F7F + down * 800 * (1 + shift) - up * 800 * (1 + shift); - state->usr.motionplus.yaw_down = 0x1F7F + left * 800 * (1 + shift) - right * 800 * (1 + shift); - state->usr.motionplus.pitch_slow = !shift; - state->usr.motionplus.yaw_slow = !shift; - break; - } + return 0; } diff --git a/input.h b/input.h index 192bd3c..c625e39 100644 --- a/input.h +++ b/input.h @@ -6,10 +6,127 @@ #define PI 3.141592654 -extern bool show_reports; +enum input_event_type +{ + INPUT_EVENT_TYPE_EMULATOR_CONTROL, + INPUT_EVENT_TYPE_HOTPLUG, + INPUT_EVENT_TYPE_BUTTON, + INPUT_EVENT_TYPE_ANALOG_MOTION, +}; -void input_init(); -void input_unload(); -void input_update(struct wiimote_state * state); +enum input_emulator_control +{ + INPUT_EMULATOR_CONTROL_QUIT, // Quits the emulator + INPUT_EMULATOR_CONTROL_POWER_OFF, // Powers off host + + INPUT_EMULATOR_CONTROL_TOGGLE_REPORTS, +}; + +struct input_emulator_control_event +{ + enum input_emulator_control control; +}; + +struct input_hotplug_event +{ + enum wiimote_connected_extension_type extension; +}; + +enum input_button +{ + INPUT_BUTTON_HOME, + + INPUT_BUTTON_WIIMOTE_UP, + INPUT_BUTTON_WIIMOTE_DOWN, + INPUT_BUTTON_WIIMOTE_LEFT, + INPUT_BUTTON_WIIMOTE_RIGHT, + INPUT_BUTTON_WIIMOTE_A, + INPUT_BUTTON_WIIMOTE_B, + INPUT_BUTTON_WIIMOTE_1, + INPUT_BUTTON_WIIMOTE_2, + INPUT_BUTTON_WIIMOTE_PLUS, + INPUT_BUTTON_WIIMOTE_MINUS, + + INPUT_BUTTON_NUNCHUK_C, + INPUT_BUTTON_NUNCHUK_Z, + + INPUT_BUTTON_CLASSIC_UP, + INPUT_BUTTON_CLASSIC_DOWN, + INPUT_BUTTON_CLASSIC_LEFT, + INPUT_BUTTON_CLASSIC_RIGHT, + INPUT_BUTTON_CLASSIC_A, + INPUT_BUTTON_CLASSIC_B, + INPUT_BUTTON_CLASSIC_X, + INPUT_BUTTON_CLASSIC_Y, + INPUT_BUTTON_CLASSIC_L, + INPUT_BUTTON_CLASSIC_R, + INPUT_BUTTON_CLASSIC_ZL, + INPUT_BUTTON_CLASSIC_ZR, + INPUT_BUTTON_CLASSIC_PLUS, + INPUT_BUTTON_CLASSIC_MINUS, +}; + +struct input_button_event +{ + bool pressed; + enum input_button button; +}; + +enum input_analog_motion +{ + INPUT_ANALOG_MOTION_IR_UP, + INPUT_ANALOG_MOTION_IR_DOWN, + INPUT_ANALOG_MOTION_IR_LEFT, + INPUT_ANALOG_MOTION_IR_RIGHT, + + INPUT_ANALOG_MOTION_POINTER, + + INPUT_ANALOG_MOTION_STEER_LEFT, + INPUT_ANALOG_MOTION_STEER_RIGHT, + + INPUT_ANALOG_MOTION_NUNCHUK_UP, + INPUT_ANALOG_MOTION_NUNCHUK_DOWN, + INPUT_ANALOG_MOTION_NUNCHUK_LEFT, + INPUT_ANALOG_MOTION_NUNCHUK_RIGHT, + + INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_UP, + INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_DOWN, + INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_LEFT, + INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_RIGHT, + + INPUT_ANALOG_MOTION_MOTIONPLUS_UP, + INPUT_ANALOG_MOTION_MOTIONPLUS_DOWN, + INPUT_ANALOG_MOTION_MOTIONPLUS_LEFT, + INPUT_ANALOG_MOTION_MOTIONPLUS_RIGHT, + INPUT_ANALOG_MOTION_MOTIONPLUS_SLOW, +}; + +struct input_analog_motion_event +{ + bool moving; + float delta_x; + float delta_y; + float delta_z; + enum input_analog_motion motion; +}; + +struct input_event +{ + enum input_event_type type; + union { + struct input_emulator_control_event emulator_control_event; + struct input_hotplug_event hotplug_event; + struct input_button_event button_event; + struct input_analog_motion_event analog_motion_event; + }; +}; + +struct input_source +{ + void (*unload)(void); + bool (*poll_event)(struct input_event *event); +}; + +int input_update(struct wiimote_state * state, struct input_source const * source); #endif diff --git a/input_sdl.c b/input_sdl.c new file mode 100644 index 0000000..6b39d42 --- /dev/null +++ b/input_sdl.c @@ -0,0 +1,393 @@ +#include "input_sdl.h" +#include "SDL/SDL.h" + +void input_sdl_init(void) +{ + if (SDL_Init(0) < 0) + { + printf("Could not initialize SDL: %s\n", SDL_GetError()); + exit(1); + } + SDL_SetVideoMode(128, 128, 0, 0); + printf("Commands overview\n" + "-----------------\n" + " __ arrows use keypad numbers!\n" + " ........ L'\n" + " | . |\\ ,.._\n" + " | 8 | | ,' \\\\\n" + " | -4 6- | | | ^ |\\\n" + " | 2 | | | <-|->/ |q\n" + " | ' | |\\ ` v ' |\n" + " | /-\\ | |d| | | _/e\n" + " | |a| | \\| | //\n" + " | \\-/ | | | |/\n" + " | | | | |\n" + " | 3 h 4 | | \\ /\n" + " | | | --'\n" + " | | | _...______________,...\n" + " | 1 | | ,' `-\n" + " | | | / ^ 3 h 4 q `.\n" + " | 2 | / | <-|-> e a |\n" + " | |-/ \\ v d /\n" + " '`''''''' \\ ,'\n" + " _ _ `-..-'------------`...-'\n" + " ,' `.\n" + " ,' t y '. 0: toggles arrow keys between\n" + " V V IR/nunchuk/classic/motion plus\n" + " ESC: quit\n\n"); +} + +static void input_sdl_unload(void) +{ + SDL_Quit(); +} + +int up, down, left, right; +bool steerright, steerleft; +double steerang = (PI / 2); +int arrow_function = 0; +bool togglekey0 = 0; +bool togglekey9 = 0; +bool shift; + +static const float mouse_sensitivity = 1.0; + +static bool input_sdl_poll_event(struct input_event *out_event) +{ + SDL_Event event; + if (!SDL_PollEvent(&event)) + { + return false; + } + + switch (event.type) + { + case SDL_MOUSEMOTION: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_POINTER; + out_event->analog_motion_event.delta_x = (float)event.motion.xrel / 1024.0 * mouse_sensitivity; + out_event->analog_motion_event.delta_y = -(float)event.motion.yrel / 768.0 * mouse_sensitivity; + out_event->analog_motion_event.delta_z = 0; + return true; + case SDL_MOUSEBUTTONUP: + case SDL_MOUSEBUTTONDOWN: + switch (event.button.button) + { + case SDL_BUTTON_LEFT: + out_event->type = INPUT_EVENT_TYPE_BUTTON; + out_event->button_event.pressed = (event.button.state == SDL_PRESSED); + + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_A; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_A; + } + return true; + case SDL_BUTTON_RIGHT: + out_event->type = INPUT_EVENT_TYPE_BUTTON; + out_event->button_event.pressed = (event.button.state == SDL_PRESSED); + + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_B; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_B; + } + return true; + } + return false; + case SDL_KEYDOWN: + case SDL_KEYUP: + out_event->type = INPUT_EVENT_TYPE_BUTTON; + out_event->button_event.pressed = (event.type == SDL_KEYDOWN); + + switch (event.key.keysym.sym) + { + case SDLK_ESCAPE: + if (event.type != SDL_KEYDOWN) + { + return false; + } + + out_event->type = INPUT_EVENT_TYPE_EMULATOR_CONTROL; + if (shift) + { + out_event->emulator_control_event.control = INPUT_EMULATOR_CONTROL_POWER_OFF; + } + else + { + out_event->emulator_control_event.control = INPUT_EMULATOR_CONTROL_QUIT; + } + break; + case SDLK_0: + if (event.type == SDL_KEYUP) + { + togglekey0 = 0; + return false; + } + else if (togglekey0 == 0) + { + togglekey0 = 1; + arrow_function = (arrow_function + 1) % 4; + printf("arrows (IR, nunchuk, classic, wmp): %d \n", arrow_function); + + out_event->type = INPUT_EVENT_TYPE_HOTPLUG; + if (arrow_function == 1) + { + out_event->hotplug_event.extension = Nunchuk; + } + else if (arrow_function == 2) + { + out_event->hotplug_event.extension = Classic; + } + else if (arrow_function == 3) + { + out_event->hotplug_event.extension = Nunchuk; + } + else + { + out_event->hotplug_event.extension = NoExtension; + } + } + break; + case SDLK_9: + if (event.type == SDL_KEYUP) + { + togglekey9 = 0; + return false; + } + else if (togglekey9 == 0) + { + togglekey9 = 1; + + out_event->type = INPUT_EVENT_TYPE_EMULATOR_CONTROL; + out_event->emulator_control_event.control = INPUT_EMULATOR_CONTROL_TOGGLE_REPORTS; + } + break; + case SDLK_LSHIFT: + shift = (event.type == SDL_KEYDOWN); + + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = !shift; + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_MOTIONPLUS_SLOW; + break; + case SDLK_a: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_A; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_A; + } + break; + case SDLK_d: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_B; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_B; + } + break; + case SDLK_q: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_X; + } + else + { + out_event->button_event.button = INPUT_BUTTON_NUNCHUK_C; + } + break; + case SDLK_e: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_Y; + } + else + { + out_event->button_event.button = INPUT_BUTTON_NUNCHUK_Z; + } + break; + case SDLK_1: + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_1; + break; + case SDLK_2: + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_2; + break; + case SDLK_3: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_MINUS; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_MINUS; + } + break; + case SDLK_4: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_PLUS; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_PLUS; + } + break; + case SDLK_h: + out_event->button_event.button = INPUT_BUTTON_HOME; + break; + case SDLK_KP8: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_UP; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_UP; + } + break; + case SDLK_KP2: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_DOWN; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_DOWN; + } + break; + case SDLK_KP4: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_LEFT; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_LEFT; + } + break; + case SDLK_KP6: + if (arrow_function == 2) + { + out_event->button_event.button = INPUT_BUTTON_CLASSIC_RIGHT; + } + else + { + out_event->button_event.button = INPUT_BUTTON_WIIMOTE_RIGHT; + } + break; + + case SDLK_UP: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + if (arrow_function == 0) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_IR_UP; + } + else if (arrow_function == 1) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_NUNCHUK_UP; + } + else if (arrow_function == 2) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_UP; + } + else + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_MOTIONPLUS_UP; + } + break; + case SDLK_DOWN: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + if (arrow_function == 0) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_IR_DOWN; + } + else if (arrow_function == 1) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_NUNCHUK_DOWN; + } + else if (arrow_function == 2) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_DOWN; + } + else + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_MOTIONPLUS_DOWN; + } + break; + case SDLK_LEFT: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + if (arrow_function == 0) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_IR_LEFT; + } + else if (arrow_function == 1) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_NUNCHUK_LEFT; + } + else if (arrow_function == 2) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_LEFT; + } + else + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_MOTIONPLUS_LEFT; + } + break; + case SDLK_RIGHT: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + if (arrow_function == 0) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_IR_RIGHT; + } + else if (arrow_function == 1) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_NUNCHUK_RIGHT; + } + else if (arrow_function == 2) + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_CLASSIC_LEFT_STICK_RIGHT; + } + else + { + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_MOTIONPLUS_RIGHT; + } + break; + + case SDLK_t: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_STEER_LEFT; + break; + case SDLK_y: + out_event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + out_event->analog_motion_event.moving = (event.type == SDL_KEYDOWN); + out_event->analog_motion_event.motion = INPUT_ANALOG_MOTION_STEER_RIGHT; + break; + + default: + return false; + } + return true; + default: + return false; + } +} + +struct input_source input_source_sdl = { + .unload = input_sdl_unload, + .poll_event = input_sdl_poll_event +}; \ No newline at end of file diff --git a/input_sdl.h b/input_sdl.h new file mode 100644 index 0000000..83fbf3f --- /dev/null +++ b/input_sdl.h @@ -0,0 +1,11 @@ +#ifndef INPUT_SDL_H +#define INPUT_SDL_H + +#include +#include "input.h" + +void input_sdl_init(void); + +extern struct input_source input_source_sdl; + +#endif diff --git a/input_socket.c b/input_socket.c new file mode 100644 index 0000000..0b2f9fc --- /dev/null +++ b/input_socket.c @@ -0,0 +1,256 @@ +#include "input_socket.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define PROGRAM_NAME "wmemulator" + +static bool input_socket_init_from_addrinfo(struct addrinfo *addrinfo); + +static int sock; +static char buf[512]; +static size_t buf_len; + +void input_socket_init_unix_at_path(char const *path) +{ + struct sockaddr_un address = { + .sun_family = AF_UNIX + }; + strncpy(address.sun_path, path, sizeof address.sun_path); + + unlink(path); + input_socket_init((struct sockaddr *)&address, sizeof address); +} + +void input_socket_init_ip_on_port(char const *port) +{ + struct addrinfo hints = { + .ai_family = AF_UNSPEC, + .ai_socktype = SOCK_DGRAM, + .ai_flags = AI_PASSIVE + }; + struct addrinfo *result_info; + int ret = getaddrinfo(NULL, port, &hints, &result_info); + if (ret) + { + printf(PROGRAM_NAME ": getaddrinfo: %s\n", gai_strerror(ret)); + exit(1); + } + + for (struct addrinfo *info = result_info; info; info = info->ai_next) + { + if (input_socket_init_from_addrinfo(info)) + { + freeaddrinfo(result_info); + printf(PROGRAM_NAME ": successfully bound to port %s\n", port); + return; + } + } + + printf(PROGRAM_NAME ": fatal: can't bind to port %s\n", port); + exit(1); +} + +void input_socket_init(struct sockaddr *socket_address, socklen_t socket_address_size) +{ + sock = socket(socket_address->sa_family, SOCK_DGRAM | SOCK_NONBLOCK, 0); + if (sock == -1) + { + perror(PROGRAM_NAME); + exit(1); + } + + if (bind(sock, socket_address, socket_address_size)) + { + perror(PROGRAM_NAME); + exit(1); + } +} + +static bool input_socket_init_from_addrinfo(struct addrinfo *addrinfo) +{ + sock = socket(addrinfo->ai_family, addrinfo->ai_socktype | SOCK_NONBLOCK, addrinfo->ai_protocol); + if (sock == -1) + { + return false; + } + + if (bind(sock, addrinfo->ai_addr, addrinfo->ai_addrlen)) + { + close(sock); + return false; + } + + return true; +} + +static void input_socket_unload(void) +{ + if (close(sock)) + { + perror(PROGRAM_NAME); + } +} + +static bool input_socket_poll_event(struct input_event *event) +{ + if (!buf_len) + { + buf_len = recv(sock, buf, sizeof(buf) - 1, 0); + if (buf_len == -1) + { + buf_len = 0; + if (!(errno == EAGAIN || errno == EWOULDBLOCK)) + { + perror(PROGRAM_NAME); + } + return false; + } + } + buf[buf_len] = '\0'; + //printf("received %d bytes: %s\n", buf_len, buf); + + event->type = INPUT_EVENT_TYPE_BUTTON; + + char event_type_s[32], event_param_s[32]; + int event_status; + if (sscanf(buf, "%32s %d %32s", event_type_s, &event_status, event_param_s) == EOF) + { + printf(PROGRAM_NAME ": received input in invalid format\n"); + buf_len = 0; + return false; + } + + if (strcmp(event_type_s, "emulator_control") == 0) + { + event->type = INPUT_EVENT_TYPE_EMULATOR_CONTROL; + + if (strcmp(event_param_s, "quit") == 0) + { + event->emulator_control_event.control = INPUT_EMULATOR_CONTROL_QUIT; + } + else if (strcmp(event_param_s, "power_off") == 0) + { + event->emulator_control_event.control = INPUT_EMULATOR_CONTROL_POWER_OFF; + } + } + else if (strcmp(event_type_s, "hotplug") == 0) + { + event->type = INPUT_EVENT_TYPE_HOTPLUG; + + if (event_status == 0) + { + event->hotplug_event.extension = NoExtension; + } + else if (strcmp(event_param_s, "nunchuk") == 0) + { + event->hotplug_event.extension = Nunchuk; + } + else if (strcmp(event_param_s, "classic") == 0) + { + event->hotplug_event.extension = Classic; + } + else if (strcmp(event_param_s, "balance_board") == 0) + { + event->hotplug_event.extension = BalanceBoard; + } + else + { + event->hotplug_event.extension = NoExtension; + } + } + else if (strcmp(event_type_s, "button") == 0) + { + event->type = INPUT_EVENT_TYPE_BUTTON; + event->button_event.pressed = event_status; + +#define CHECK(Button) if (strcmp(event_param_s, #Button) == 0) event->button_event.button = INPUT_BUTTON_##Button + CHECK(HOME); + else CHECK(WIIMOTE_UP); + else CHECK(WIIMOTE_DOWN); + else CHECK(WIIMOTE_LEFT); + else CHECK(WIIMOTE_RIGHT); + else CHECK(WIIMOTE_A); + else CHECK(WIIMOTE_B); + else CHECK(WIIMOTE_1); + else CHECK(WIIMOTE_2); + else CHECK(WIIMOTE_PLUS); + else CHECK(WIIMOTE_MINUS); + else CHECK(NUNCHUK_C); + else CHECK(NUNCHUK_Z); + else CHECK(CLASSIC_UP); + else CHECK(CLASSIC_DOWN); + else CHECK(CLASSIC_LEFT); + else CHECK(CLASSIC_RIGHT); + else CHECK(CLASSIC_A); + else CHECK(CLASSIC_B); + else CHECK(CLASSIC_X); + else CHECK(CLASSIC_Y); + else CHECK(CLASSIC_L); + else CHECK(CLASSIC_R); + else CHECK(CLASSIC_ZL); + else CHECK(CLASSIC_ZR); + else CHECK(CLASSIC_PLUS); + else CHECK(CLASSIC_MINUS); +#undef CHECK + else + { + printf(PROGRAM_NAME ": received invalid 'button' parameter: %s\n", event_param_s); + buf_len = 0; + return false; + } + } + else if (strcmp(event_type_s, "analog_motion") == 0) + { + event->type = INPUT_EVENT_TYPE_ANALOG_MOTION; + event->analog_motion_event.moving = event_status; + +#define CHECK(Motion) if (strcmp(event_param_s, #Motion) == 0) event->analog_motion_event.motion = INPUT_ANALOG_MOTION_##Motion + CHECK(IR_UP); + else CHECK(IR_DOWN); + else CHECK(IR_LEFT); + else CHECK(IR_RIGHT); + else CHECK(STEER_LEFT); + else CHECK(STEER_RIGHT); + else CHECK(NUNCHUK_UP); + else CHECK(NUNCHUK_DOWN); + else CHECK(NUNCHUK_LEFT); + else CHECK(NUNCHUK_RIGHT); + else CHECK(CLASSIC_LEFT_STICK_UP); + else CHECK(CLASSIC_LEFT_STICK_DOWN); + else CHECK(CLASSIC_LEFT_STICK_LEFT); + else CHECK(CLASSIC_LEFT_STICK_RIGHT); + else CHECK(MOTIONPLUS_UP); + else CHECK(MOTIONPLUS_DOWN); + else CHECK(MOTIONPLUS_LEFT); + else CHECK(MOTIONPLUS_RIGHT); + else CHECK(MOTIONPLUS_SLOW); +#undef CHECK + else + { + printf(PROGRAM_NAME ": received invalid 'analog_motion' parameter: %s\n", event_param_s); + buf_len = 0; + return false; + } + } + else + { + printf(PROGRAM_NAME ": received invalid event type: %s\n", event_type_s); + buf_len = 0; + return false; + } + + buf_len = 0; + return true; +} + +struct input_source input_source_socket = { + .unload = input_socket_unload, + .poll_event = input_socket_poll_event +}; diff --git a/input_socket.h b/input_socket.h new file mode 100644 index 0000000..2b4c254 --- /dev/null +++ b/input_socket.h @@ -0,0 +1,15 @@ +#ifndef INPUT_SOCKET_H +#define INPUT_SOCKET_H + +#include +#include +#include +#include "input.h" + +void input_socket_init_unix_at_path(char const *path); +void input_socket_init_ip_on_port(char const *port); +void input_socket_init(struct sockaddr *socket_address, socklen_t socket_address_size); + +extern struct input_source input_source_socket; + +#endif diff --git a/motion.c b/motion.c new file mode 100644 index 0000000..254297f --- /dev/null +++ b/motion.c @@ -0,0 +1,155 @@ +#include "motion.h" + +#include "vector_math.h" + +//units in meters +static const double screen_distance = 2; +static const double screen_width = 1.0; +static const double screen_aspect = 4.0 / 3.0; + +static const double sensor_bar_y = screen_width / screen_aspect * 0.5; +static const double sensor_bar_width = 0.20; + +static const double cam_aspect = 1024.0 / 768.0; +static const double cam_fov = 42.0; +static const double cam_far = 4.0; +static const double cam_near = 0.5; + +static const uint16_t accelerometer_zero = 0x85 << 2; +static const uint16_t accelerometer_unit = 0x6C; + +void look_at_pointer(mat4 * wiimote_mat, float pointer_x, float pointer_y) +{ + vec3 pointer_world = { + (pointer_x - 0.5) * screen_width, + (pointer_y - 0.5) * screen_width / screen_aspect, + -screen_distance + }; + + vec3 dir = { pointer_world.x, pointer_world.y, pointer_world.z }; + vec3_normalize(&dir); + + vec3 up = { 0.0, 1.0, 0.0 }; + vec3 z = { -dir.x, -dir.y, -dir.z }; + + vec3 x; + vec3_cross(&x, &up, &z); + vec3_normalize(&x); + + vec3 y; + vec3_cross(&y, &z, &x); + // vec3_normalize(&y); + + wiimote_mat->v0 = (vec4){ x.x, x.y, x.z, 0.0 }; + wiimote_mat->v1 = (vec4){ y.x, y.y, y.z, 0.0 }; + wiimote_mat->v2 = (vec4){ z.x, z.y, z.z, 0.0 }; + wiimote_mat->v3 = (vec4){ 0.0, 0.0, 0.0, 1.0 }; +} + +void make_cam_projection_mat(mat4 * proj_mat) +{ + double near = cam_near; + double far = cam_far; + + double top = near * tan(cam_fov / 180.0 * M_PI * 0.5); + double height = 2.0 * top; + double width = cam_aspect * height; + double left = -0.5 * width; + + double right = left + width; + double bottom = top - height; + + proj_mat->v0 = (vec4){ 2.0 * near / (right - left), 0.0, 0.0, 0.0 }; + proj_mat->v1 = (vec4){ 0.0, 2.0 * near / (top - bottom), 0.0, 0.0 }; + proj_mat->v2 = (vec4){ (right + left) / (right - left), (top + bottom) / (top - bottom), -(far + near) / (far - near), -1.0 }; + proj_mat->v3 = (vec4){ 0.0, 0.0, -2.0 * far * near / (far - near), 0.0 }; +} + +void set_accelerometer(struct wiimote_state * state, const mat4 * wiimote_mat) +{ + vec3 accel = { 0, -1.0, 0 }; + mat3 accel_m; + mat3_from_mat4(&accel_m, wiimote_mat); + mat3_invert(&accel_m); + mat3_transpose(&accel_m); + vec3_apply_mat3(&accel, &accel_m); + + accel.x = fmax(-3.4, fmin(3.4, accel.x)); + accel.y = fmax(-3.4, fmin(3.4, accel.y)); + accel.z = fmax(-3.4, fmin(3.4, accel.z)); + + //transform to wiimote's accelerometer coordinate system + state->usr.accel_x = accelerometer_zero + + (int)round((double)accelerometer_unit * -accel.x); + state->usr.accel_z = accelerometer_zero + + (int)round((double)accelerometer_unit * -accel.y); + state->usr.accel_y = accelerometer_zero + + (int)round((double)accelerometer_unit * accel.z); +} + +void set_motionplus(struct wiimote_state * state, const mat4 * wiimote_mat) +{ + +} + +void set_motion_state(struct wiimote_state * state, float pointer_x, float pointer_y) +{ + mat4 wiimote_mat; + look_at_pointer(&wiimote_mat, pointer_x, pointer_y); + + mat4 view_mat = wiimote_mat; + mat4_invert(&view_mat); + + mat4 model_mat; + vec3 model_pos = (vec3){ 0.0, sensor_bar_y, -screen_distance }; + mat4_make_translation(&model_mat, &model_pos); + + mat4 proj_mat; + make_cam_projection_mat(&proj_mat); + + mat4_mult(&view_mat, &model_mat); + mat4_mult(&proj_mat, &view_mat); + + vec4 sensor_pt0 = { -sensor_bar_width * 0.5, 0.0, 0.0, 1.0 }; + vec4 sensor_pt1 = { sensor_bar_width * 0.5, 0.0, 0.0, 1.0 }; + + vec4_apply_mat4(&sensor_pt0, &proj_mat); + vec4_apply_mat4(&sensor_pt1, &proj_mat); + + vec4_multiply_scalar(&sensor_pt0, 1 / sensor_pt0.w); + vec4_multiply_scalar(&sensor_pt1, 1 / sensor_pt1.w); + + vec4_add_scalar(&sensor_pt0, 1.0); + vec4_multiply_scalar(&sensor_pt0, 0.5); + + vec4_add_scalar(&sensor_pt1, 1.0); + vec4_multiply_scalar(&sensor_pt1, 0.5); + + double min_pt_size = 1.0; + double max_pt_size = 15.0; + + reset_ir_object(&state->usr.ir_object[0]); + reset_ir_object(&state->usr.ir_object[1]); + + if (sensor_pt0.x > 0 && sensor_pt0.x < 1 && + sensor_pt0.y > 0 && sensor_pt0.y < 1 && + sensor_pt0.z > 0 && sensor_pt0.z < 1) + { + state->usr.ir_object[0].x = round(sensor_pt0.x * 1023); + state->usr.ir_object[0].y = round(sensor_pt0.y * 767); + state->usr.ir_object[0].size = round(min_pt_size + + pow(1.0 - sensor_pt0.z, 2.0) * (max_pt_size - min_pt_size)); + } + + if (sensor_pt1.x > 0 && sensor_pt1.x < 1 && + sensor_pt1.y > 0 && sensor_pt1.y < 1 && + sensor_pt1.z > 0 && sensor_pt1.z < 1) + { + state->usr.ir_object[1].x = round(sensor_pt1.x * 1023); + state->usr.ir_object[1].y = round(sensor_pt1.y * 767); + state->usr.ir_object[1].size = round(min_pt_size + + pow(1.0 - sensor_pt1.z, 2.0) * (max_pt_size - min_pt_size)); + } + + set_accelerometer(state, &wiimote_mat); +} \ No newline at end of file diff --git a/motion.h b/motion.h new file mode 100644 index 0000000..266e50a --- /dev/null +++ b/motion.h @@ -0,0 +1,8 @@ +#ifndef MOTION_H +#define MOTION_H + +#include "wiimote.h" + +void set_motion_state(struct wiimote_state * state, float pointer_x, float pointer_y); + +#endif \ No newline at end of file diff --git a/oui.c b/oui.c deleted file mode 100644 index 770c469..0000000 --- a/oui.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2004-2010 Marcel Holtmann - * - * - * 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 2 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, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - -#ifdef HAVE_CONFIG_H -#include -#endif - -#include -#include "oui.h" - -#ifdef HAVE_UDEV_HWDB_NEW -#include - -char *batocomp(const bdaddr_t *ba) -{ - struct udev *udev; - struct udev_hwdb *hwdb; - struct udev_list_entry *head, *entry; - char modalias[11], *comp = NULL; - - sprintf(modalias, "OUI:%2.2X%2.2X%2.2X", ba->b[5], ba->b[4], ba->b[3]); - - udev = udev_new(); - if (!udev) - return NULL; - - hwdb = udev_hwdb_new(udev); - if (!hwdb) - goto done; - - head = udev_hwdb_get_properties_list_entry(hwdb, modalias, 0); - - udev_list_entry_foreach(entry, head) { - const char *name = udev_list_entry_get_name(entry); - - if (name && !strcmp(name, "ID_OUI_FROM_DATABASE")) { - comp = strdup(udev_list_entry_get_value(entry)); - break; - } - } - - hwdb = udev_hwdb_unref(hwdb); - -done: - udev = udev_unref(udev); - - return comp; -} -#else -char *batocomp(const bdaddr_t *ba) -{ - return NULL; -} -#endif diff --git a/oui.h b/oui.h deleted file mode 100644 index 2ddc27f..0000000 --- a/oui.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2004-2010 Marcel Holtmann - * - * - * 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 2 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, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - -char *batocomp(const bdaddr_t *ba); diff --git a/sdp.c b/sdp.c new file mode 100644 index 0000000..e5c9a63 --- /dev/null +++ b/sdp.c @@ -0,0 +1,231 @@ +#include +#include //memcpy +#include "sdp.h" + +#include +#include +#include +#include + +/* + * A real SDP implementation is totally unecessary for this purpose. + * The Wii simply checks for the Wiimote service, verifies its attributes, and + * moves on. This bare minimum implementation provides the expected responses. + */ + +static int state = -1; +static uint32_t sdp_record_handle; +static const uint32_t wiimote_hid_record_handle = 0x10000; + +static const uint8_t resp0[14] = +{ 0x03, 0x00, 0x00, 0x00, 0x09, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00 }; + +static const uint8_t resp1[128] = +{ + 0x05, 0x00, 0x01, 0x00, 0x7B, 0x00, 0x76, 0x36, 0x01, 0xCC, 0x09, 0x00, 0x00, + 0x0A, 0x00, 0x01, 0x00, 0x00, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x24, + 0x09, 0x00, 0x04, 0x35, 0x0D, 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x11, + 0x35, 0x03, 0x19, 0x00, 0x11, 0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02, + 0x09, 0x00, 0x06, 0x35, 0x09, 0x09, 0x65, 0x6E, 0x09, 0x00, 0x6A, 0x09, 0x01, + 0x00, 0x09, 0x00, 0x09, 0x35, 0x08, 0x35, 0x06, 0x19, 0x11, 0x24, 0x09, 0x01, + 0x00, 0x09, 0x00, 0x0D, 0x35, 0x0F, 0x35, 0x0D, 0x35, 0x06, 0x19, 0x01, 0x00, + 0x09, 0x00, 0x13, 0x35, 0x03, 0x19, 0x00, 0x11, 0x09, 0x01, 0x00, 0x25, 0x13, + 0x4E, 0x69, 0x6E, 0x74, 0x65, 0x6E, 0x64, 0x6F, 0x20, 0x52, 0x56, 0x4C, 0x2D, + 0x43, 0x4E, 0x54, 0x2D, 0x30, 0x31, 0x09, 0x01, 0x02, 0x00, 0x76 +}; + +static const uint8_t resp2[128] = +{ + 0x05, 0x00, 0x02, 0x00, 0x7B, 0x00, 0x76, 0x01, 0x25, 0x13, 0x4E, 0x69, 0x6E, + 0x74, 0x65, 0x6E, 0x64, 0x6F, 0x20, 0x52, 0x56, 0x4C, 0x2D, 0x43, 0x4E, 0x54, + 0x2D, 0x30, 0x31, 0x09, 0x01, 0x02, 0x25, 0x08, 0x4E, 0x69, 0x6E, 0x74, 0x65, + 0x6E, 0x64, 0x6F, 0x09, 0x02, 0x00, 0x09, 0x01, 0x00, 0x09, 0x02, 0x01, 0x09, + 0x01, 0x11, 0x09, 0x02, 0x02, 0x08, 0x04, 0x09, 0x02, 0x03, 0x08, 0x33, 0x09, + 0x02, 0x04, 0x28, 0x00, 0x09, 0x02, 0x05, 0x28, 0x01, 0x09, 0x02, 0x06, 0x35, + 0xDF, 0x35, 0xDD, 0x08, 0x22, 0x25, 0xD9, 0x05, 0x01, 0x09, 0x05, 0xA1, 0x01, + 0x85, 0x10, 0x15, 0x00, 0x26, 0xFF, 0x00, 0x75, 0x08, 0x95, 0x01, 0x06, 0x00, + 0xFF, 0x09, 0x01, 0x91, 0x00, 0x85, 0x11, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, + 0x85, 0x12, 0x95, 0x02, 0x09, 0x01, 0x91, 0x00, 0x02, 0x00, 0xEC +}; + +static const uint8_t resp3[128] = +{ + 0x05, 0x00, 0x03, 0x00, 0x7B, 0x00, 0x76, 0x85, 0x13, 0x95, 0x01, 0x09, 0x01, + 0x91, 0x00, 0x85, 0x14, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x15, 0x95, + 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x16, 0x95, 0x15, 0x09, 0x01, 0x91, 0x00, + 0x85, 0x17, 0x95, 0x06, 0x09, 0x01, 0x91, 0x00, 0x85, 0x18, 0x95, 0x15, 0x09, + 0x01, 0x91, 0x00, 0x85, 0x19, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x1A, + 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x20, 0x95, 0x06, 0x09, 0x01, 0x81, + 0x00, 0x85, 0x21, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x22, 0x95, 0x04, + 0x09, 0x01, 0x81, 0x00, 0x85, 0x30, 0x95, 0x02, 0x09, 0x01, 0x81, 0x00, 0x85, + 0x31, 0x95, 0x05, 0x09, 0x01, 0x81, 0x00, 0x85, 0x32, 0x95, 0x0A, 0x09, 0x01, + 0x81, 0x00, 0x85, 0x33, 0x95, 0x11, 0x09, 0x01, 0x02, 0x01, 0x62 +}; + +static const uint8_t resp4[117] = +{ + 0x05, 0x00, 0x04, 0x00, 0x70, 0x00, 0x6D, 0x81, 0x00, 0x85, 0x34, 0x95, 0x15, + 0x09, 0x01, 0x81, 0x00, 0x85, 0x35, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, + 0x36, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x37, 0x95, 0x15, 0x09, 0x01, + 0x81, 0x00, 0x85, 0x3D, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x3E, 0x95, + 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x3F, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, + 0xC0, 0x09, 0x02, 0x07, 0x35, 0x08, 0x35, 0x06, 0x09, 0x04, 0x09, 0x09, 0x01, + 0x00, 0x09, 0x02, 0x08, 0x28, 0x00, 0x09, 0x02, 0x09, 0x28, 0x01, 0x09, 0x02, + 0x0A, 0x28, 0x01, 0x09, 0x02, 0x0B, 0x09, 0x01, 0x00, 0x09, 0x02, 0x0C, 0x09, + 0x0C, 0x80, 0x09, 0x02, 0x0D, 0x28, 0x00, 0x09, 0x02, 0x0E, 0x28, 0x00, 0x00 +}; + +static const uint8_t attributes[463] = +{ + 0x36, 0x01, 0xCC, 0x09, 0x00, 0x00, + 0x0A, 0x00, 0x01, 0x00, 0x00, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x24, + 0x09, 0x00, 0x04, 0x35, 0x0D, 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x11, + 0x35, 0x03, 0x19, 0x00, 0x11, 0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02, + 0x09, 0x00, 0x06, 0x35, 0x09, 0x09, 0x65, 0x6E, 0x09, 0x00, 0x6A, 0x09, 0x01, + 0x00, 0x09, 0x00, 0x09, 0x35, 0x08, 0x35, 0x06, 0x19, 0x11, 0x24, 0x09, 0x01, + 0x00, 0x09, 0x00, 0x0D, 0x35, 0x0F, 0x35, 0x0D, 0x35, 0x06, 0x19, 0x01, 0x00, + 0x09, 0x00, 0x13, 0x35, 0x03, 0x19, 0x00, 0x11, 0x09, 0x01, 0x00, 0x25, 0x13, + 0x4E, 0x69, 0x6E, 0x74, 0x65, 0x6E, 0x64, 0x6F, 0x20, 0x52, 0x56, 0x4C, 0x2D, + 0x43, 0x4E, 0x54, 0x2D, 0x30, 0x31, 0x09, 0x01, + 0x01, 0x25, 0x13, 0x4E, 0x69, 0x6E, + 0x74, 0x65, 0x6E, 0x64, 0x6F, 0x20, 0x52, 0x56, 0x4C, 0x2D, 0x43, 0x4E, 0x54, + 0x2D, 0x30, 0x31, 0x09, 0x01, 0x02, 0x25, 0x08, 0x4E, 0x69, 0x6E, 0x74, 0x65, + 0x6E, 0x64, 0x6F, 0x09, 0x02, 0x00, 0x09, 0x01, 0x00, 0x09, 0x02, 0x01, 0x09, + 0x01, 0x11, 0x09, 0x02, 0x02, 0x08, 0x04, 0x09, 0x02, 0x03, 0x08, 0x33, 0x09, + 0x02, 0x04, 0x28, 0x00, 0x09, 0x02, 0x05, 0x28, 0x01, 0x09, 0x02, 0x06, 0x35, + 0xDF, 0x35, 0xDD, 0x08, 0x22, 0x25, 0xD9, 0x05, 0x01, 0x09, 0x05, 0xA1, 0x01, + 0x85, 0x10, 0x15, 0x00, 0x26, 0xFF, 0x00, 0x75, 0x08, 0x95, 0x01, 0x06, 0x00, + 0xFF, 0x09, 0x01, 0x91, 0x00, 0x85, 0x11, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, + 0x85, 0x12, 0x95, 0x02, 0x09, 0x01, 0x91, 0x00, + 0x85, 0x13, 0x95, 0x01, 0x09, 0x01, + 0x91, 0x00, 0x85, 0x14, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x15, 0x95, + 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x16, 0x95, 0x15, 0x09, 0x01, 0x91, 0x00, + 0x85, 0x17, 0x95, 0x06, 0x09, 0x01, 0x91, 0x00, 0x85, 0x18, 0x95, 0x15, 0x09, + 0x01, 0x91, 0x00, 0x85, 0x19, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x1A, + 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, 0x85, 0x20, 0x95, 0x06, 0x09, 0x01, 0x81, + 0x00, 0x85, 0x21, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x22, 0x95, 0x04, + 0x09, 0x01, 0x81, 0x00, 0x85, 0x30, 0x95, 0x02, 0x09, 0x01, 0x81, 0x00, 0x85, + 0x31, 0x95, 0x05, 0x09, 0x01, 0x81, 0x00, 0x85, 0x32, 0x95, 0x0A, 0x09, 0x01, + 0x81, 0x00, 0x85, 0x33, 0x95, 0x11, 0x09, 0x01, + 0x81, 0x00, 0x85, 0x34, 0x95, 0x15, + 0x09, 0x01, 0x81, 0x00, 0x85, 0x35, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, + 0x36, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x37, 0x95, 0x15, 0x09, 0x01, + 0x81, 0x00, 0x85, 0x3D, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x3E, 0x95, + 0x15, 0x09, 0x01, 0x81, 0x00, 0x85, 0x3F, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, + 0xC0, 0x09, 0x02, 0x07, 0x35, 0x08, 0x35, 0x06, 0x09, 0x04, 0x09, 0x09, 0x01, + 0x00, 0x09, 0x02, 0x08, 0x28, 0x00, 0x09, 0x02, 0x09, 0x28, 0x01, 0x09, 0x02, + 0x0A, 0x28, 0x01, 0x09, 0x02, 0x0B, 0x09, 0x01, 0x00, 0x09, 0x02, 0x0C, 0x09, + 0x0C, 0x80, 0x09, 0x02, 0x0D, 0x28, 0x00, 0x09, 0x02, 0x0E, 0x28, 0x00 +}; + +void sdp_recv_data(uint8_t * buf, int32_t len) +{ + struct sdp_pdu * header = (struct sdp_pdu *)buf; + + //use the transaction id to determine the response to send + state = header->transaction_id >> 8; +} + +int32_t sdp_get_data(uint8_t * buf) +{ + int32_t len = 0; + + if (state >= 0) + { + const uint8_t * arr; + + switch (state) + { + case 0: + arr = resp0; + len = sizeof(resp0); + break; + case 1: + arr = resp1; + len = sizeof(resp1); + break; + case 2: + arr = resp2; + len = sizeof(resp2); + break; + case 3: + arr = resp3; + len = sizeof(resp3); + break; + case 4: + arr = resp4; + len = sizeof(resp4); + break; + } + + memcpy(buf, arr, len); + + state = -1; + } + + return len; +} + +int remove_existing_sdp_records(sdp_session_t * session) +{ + return 0; +} + +int register_wiimote_sdp_record() +{ + int ret; + sdp_session_t * session; + session = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, SDP_RETRY_IF_BUSY); + + if (session == NULL) + { + printf("failed to get sdp server session\n"); + return -1; + } + + ret = sdp_device_record_unregister_binary(session, BDADDR_ANY, wiimote_hid_record_handle); + if (ret < 0 && errno != EINVAL) + { + printf("failed to unregister sdp record %s\n", strerror(errno)); + sdp_close(session); + return -1; + } + + ret = sdp_device_record_register_binary(session, BDADDR_ANY, + (uint8_t *)attributes, sizeof(attributes), SDP_RECORD_PERSIST, &sdp_record_handle); + if (ret < 0) + { + printf("failed to register sdp record %s\n", strerror(errno)); + sdp_close(session); + return -1; + } + + sdp_close(session); + + return 0; +} + +int unregister_wiimote_sdp_record() +{ + int ret; + sdp_session_t * session; + session = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, SDP_RETRY_IF_BUSY); + + if (session == NULL) + { + printf("failed to get sdp server session\n"); + return -1; + } + + ret = sdp_device_record_unregister_binary(session, BDADDR_ANY, sdp_record_handle); + if (ret < 0) + { + printf("failed to unregister sdp record %s\n", strerror(errno)); + sdp_close(session); + return -1; + } + + sdp_close(session); + + return 0; +} \ No newline at end of file diff --git a/sdp.h b/sdp.h new file mode 100644 index 0000000..df66c70 --- /dev/null +++ b/sdp.h @@ -0,0 +1,17 @@ +#ifndef SDP_H +#define SDP_H + +struct sdp_pdu +{ + uint8_t pdu_id; + uint16_t transaction_id; //big endian + uint8_t data[]; +} __attribute__((packed)); + +void sdp_recv_data(uint8_t * buf, int32_t len); +int32_t sdp_get_data(uint8_t * buf); + +int register_wiimote_sdp_record(); +int unregister_wiimote_sdp_record(); + +#endif /* SDP_H */ diff --git a/sdptool.c b/sdptool.c deleted file mode 100644 index d0871c8..0000000 --- a/sdptool.c +++ /dev/null @@ -1,298 +0,0 @@ -/* - * - * BlueZ - Bluetooth protocol stack for Linux - * - * Copyright (C) 2001-2002 Nokia Corporation - * Copyright (C) 2002-2003 Maxim Krasnyansky - * Copyright (C) 2002-2010 Marcel Holtmann - * Copyright (C) 2002-2003 Stephen Crane - * Copyright (C) 2002-2003 Jean Tourrilhes - * - * - * 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 2 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, write to the Free Software - * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - * - */ - -#include -#include -#include -#include -#include - -//signal handler to break out of main loop -static int running = 1; -void sig_handler(int sig) -{ - running = 0; -} - -int register_service_wiimote0(sdp_session_t * session) -{ - sdp_record_t record; - sdp_list_t *svclass_id, *pfseq, *apseq, *root; - uuid_t root_uuid, hid_uuid, l2cap_uuid, hidp_uuid; - sdp_profile_desc_t profile[1]; - sdp_list_t *aproto, *proto[3]; - sdp_data_t *psm, *lang_lst, *lang_lst2, *hid_spec_lst, *hid_spec_lst2; - unsigned int i; - uint8_t dtd = SDP_UINT16; - uint8_t dtd2 = SDP_UINT8; - uint8_t dtd_data = SDP_TEXT_STR8; - void *dtds[2]; - void *values[2]; - void *dtds2[2]; - void *values2[2]; - int leng[2]; - uint8_t hid_spec_type = 0x22; - uint16_t hid_attr_lang[] = { 0x409, 0x100 }; - uint16_t ctrl = 0x11, intr = 0x13; - uint16_t hid_release = 0x0100, parser_version = 0x0111; - uint8_t subclass = 0x04, country = 0x33; - uint8_t virtual_cable = 0, reconnect = 1, sdp_disable = 0; - uint8_t battery = 1, remote_wakeup = 1; - uint16_t profile_version = 0x0100, superv_timeout = 0x0c80; - uint8_t norm_connect = 0, boot_device = 0; - const uint8_t hid_spec[] = { - 0x05, 0x01, 0x09, 0x05, 0xa1, 0x01, 0x85, 0x10, - 0x15, 0x00, 0x26, 0xff, 0x00, 0x75, 0x08, 0x95, - 0x01, 0x06, 0x00, 0xff, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x11, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x12, 0x95, 0x02, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x13, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x14, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x15, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x16, 0x95, 0x15, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x17, 0x95, 0x06, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x18, 0x95, 0x15, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x19, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x1a, 0x95, 0x01, 0x09, 0x01, 0x91, 0x00, - 0x85, 0x20, 0x95, 0x06, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x21, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x22, 0x95, 0x04, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x30, 0x95, 0x02, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x31, 0x95, 0x05, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x32, 0x95, 0x0a, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x33, 0x95, 0x11, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x34, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x35, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x36, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x37, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x3d, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x3e, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0x85, 0x3f, 0x95, 0x15, 0x09, 0x01, 0x81, 0x00, - 0xc0, 0x00 - }; - - memset(&record, 0, sizeof(sdp_record_t)); - record.handle = 0xffffffff; - - sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); - root = sdp_list_append(NULL, &root_uuid); - sdp_set_browse_groups(&record, root); - - sdp_uuid16_create(&hid_uuid, HID_SVCLASS_ID); - svclass_id = sdp_list_append(NULL, &hid_uuid); - sdp_set_service_classes(&record, svclass_id); - - sdp_uuid16_create(&profile[0].uuid, HID_PROFILE_ID); - profile[0].version = 0x0100; - pfseq = sdp_list_append(NULL, profile); - sdp_set_profile_descs(&record, pfseq); - - sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID); - proto[1] = sdp_list_append(0, &l2cap_uuid); - psm = sdp_data_alloc(SDP_UINT16, &ctrl); - proto[1] = sdp_list_append(proto[1], psm); - apseq = sdp_list_append(0, proto[1]); - - sdp_uuid16_create(&hidp_uuid, HIDP_UUID); - proto[2] = sdp_list_append(0, &hidp_uuid); - apseq = sdp_list_append(apseq, proto[2]); - - aproto = sdp_list_append(0, apseq); - sdp_set_access_protos(&record, aproto); - - proto[1] = sdp_list_append(0, &l2cap_uuid); - psm = sdp_data_alloc(SDP_UINT16, &intr); - proto[1] = sdp_list_append(proto[1], psm); - apseq = sdp_list_append(0, proto[1]); - - sdp_uuid16_create(&hidp_uuid, HIDP_UUID); - proto[2] = sdp_list_append(0, &hidp_uuid); - apseq = sdp_list_append(apseq, proto[2]); - - aproto = sdp_list_append(0, apseq); - sdp_set_add_access_protos(&record, aproto); - - sdp_add_lang_attr(&record); - - sdp_set_info_attr(&record, "Nintendo RVL-CNT-01", - "Nintendo", "Nintendo RVL-CNT-01"); - - sdp_attr_add_new(&record, SDP_ATTR_HID_DEVICE_RELEASE_NUMBER, - SDP_UINT16, &hid_release); - - sdp_attr_add_new(&record, SDP_ATTR_HID_PARSER_VERSION, - SDP_UINT16, &parser_version); - - sdp_attr_add_new(&record, SDP_ATTR_HID_DEVICE_SUBCLASS, - SDP_UINT8, &subclass); - - sdp_attr_add_new(&record, SDP_ATTR_HID_COUNTRY_CODE, - SDP_UINT8, &country); - - sdp_attr_add_new(&record, SDP_ATTR_HID_VIRTUAL_CABLE, - SDP_BOOL, &virtual_cable); - - sdp_attr_add_new(&record, SDP_ATTR_HID_RECONNECT_INITIATE, - SDP_BOOL, &reconnect); - - dtds[0] = &dtd2; - values[0] = &hid_spec_type; - dtds[1] = &dtd_data; - values[1] = (uint8_t *) hid_spec; - leng[0] = 0; - leng[1] = sizeof(hid_spec)-1; - hid_spec_lst = sdp_seq_alloc_with_length(dtds, values, leng, 2); - hid_spec_lst2 = sdp_data_alloc(SDP_SEQ8, hid_spec_lst); - sdp_attr_add(&record, SDP_ATTR_HID_DESCRIPTOR_LIST, hid_spec_lst2); - - for (i = 0; i < sizeof(hid_attr_lang) / 2; i++) { - dtds2[i] = &dtd; - values2[i] = &hid_attr_lang[i]; - } - - lang_lst = sdp_seq_alloc(dtds2, values2, sizeof(hid_attr_lang) / 2); - lang_lst2 = sdp_data_alloc(SDP_SEQ8, lang_lst); - sdp_attr_add(&record, SDP_ATTR_HID_LANG_ID_BASE_LIST, lang_lst2); - - sdp_attr_add_new(&record, SDP_ATTR_HID_SDP_DISABLE, - SDP_BOOL, &sdp_disable); - - sdp_attr_add_new(&record, SDP_ATTR_HID_BATTERY_POWER, - SDP_BOOL, &battery); - - sdp_attr_add_new(&record, SDP_ATTR_HID_REMOTE_WAKEUP, - SDP_BOOL, &remote_wakeup); - - sdp_attr_add_new(&record, SDP_ATTR_HID_PROFILE_VERSION, - SDP_UINT16, &profile_version); - - sdp_attr_add_new(&record, SDP_ATTR_HID_SUPERVISION_TIMEOUT, - SDP_UINT16, &superv_timeout); - - sdp_attr_add_new(&record, SDP_ATTR_HID_NORMALLY_CONNECTABLE, - SDP_BOOL, &norm_connect); - - sdp_attr_add_new(&record, SDP_ATTR_HID_BOOT_DEVICE, - SDP_BOOL, &boot_device); - - return sdp_record_register(session, &record, 0); - //SDP_RECORD_PERSIST -} - -int register_service_wiimote1(sdp_session_t * session) -{ - uint16_t l2cap_channel = 1; - sdp_profile_desc_t profile; - uint16_t spec_id = 0x0100, vend_id = 0x057e, prod_id = 0x306; - uint16_t version = 0x0600, id_src = 0x0002; - uint8_t primary_rec = 1; - - uuid_t root_uuid, l2cap_uuid, sdp_uuid, svc_uuid, pnp_uuid; - sdp_list_t *l2cap_list = 0, - *sdp_list = 0, - *root_list = 0, - *proto_list = 0, - *access_proto_list = 0, - *profile_list = 0, - *class_list = 0; - sdp_data_t *channel = 0, *psm = 0; - - sdp_record_t *record = sdp_record_alloc(); - - // service class id list - sdp_uuid16_create(&pnp_uuid, PNP_INFO_SVCLASS_ID); - class_list = sdp_list_append(NULL, &pnp_uuid); - sdp_set_service_classes(record, class_list); - - // set l2cap information - sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID); - channel = sdp_data_alloc(SDP_UINT16, &l2cap_channel); - l2cap_list = sdp_list_append( 0, &l2cap_uuid ); - sdp_list_append( l2cap_list, channel ); - proto_list = sdp_list_append( 0, l2cap_list ); - - // set sdp information - sdp_uuid16_create(&sdp_uuid, SDP_UUID); - sdp_list = sdp_list_append( 0, &sdp_uuid ); - sdp_list_append( proto_list, sdp_list ); - - // attach protocol information to service record - access_proto_list = sdp_list_append( 0, proto_list ); - sdp_set_access_protos( record, access_proto_list ); - - // make the service record publicly browsable - sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); - root_list = sdp_list_append(0, &root_uuid); - sdp_set_browse_groups( record, root_list ); - - // profile descriptor list - sdp_uuid16_create(&profile.uuid, PNP_INFO_PROFILE_ID); - profile.version = 0x0100; - profile_list = sdp_list_append(0, &profile); - sdp_set_profile_descs(record, profile_list); - - //attributes - sdp_attr_add_new(record, SDP_ATTR_SPECIFICATION_ID, SDP_UINT16, &spec_id); - sdp_attr_add_new(record, SDP_ATTR_VENDOR_ID, SDP_UINT16, &vend_id); - sdp_attr_add_new(record, SDP_ATTR_PRODUCT_ID, SDP_UINT16, &prod_id); - sdp_attr_add_new(record, SDP_ATTR_VERSION, SDP_UINT16, &version); - sdp_attr_add_new(record, SDP_ATTR_PRIMARY_RECORD, SDP_BOOL, &primary_rec); - sdp_attr_add_new(record, SDP_ATTR_VENDOR_ID_SOURCE, SDP_UINT16, &id_src); - - int err = 0; - - // connect to the local SDP server, register the service record, and - // disconnect - err = sdp_record_register(session, record, 0); - - // cleanup - sdp_data_free( channel ); - sdp_list_free( l2cap_list, 0 ); - sdp_list_free( sdp_list, 0 ); - sdp_list_free( root_list, 0 ); - sdp_list_free( access_proto_list, 0 ); - - return err; -} - -int main() -{ - sdp_session_t * session; - session = sdp_connect( BDADDR_ANY, BDADDR_LOCAL, SDP_RETRY_IF_BUSY ); - - signal(SIGINT, sig_handler); - - register_service_wiimote0(session); - register_service_wiimote1(session); - - while(running) - { - usleep(1000); - } - - sdp_close(session); -} diff --git a/vector_math.h b/vector_math.h new file mode 100644 index 0000000..87138e3 --- /dev/null +++ b/vector_math.h @@ -0,0 +1,263 @@ +#ifndef VECTOR_MATH_H +#define VECTOR_MATH_H + +#include + +//for debug/printing functions +#include + +typedef struct +{ + double x; + double y; + double z; +} vec3; + +typedef struct +{ + double x; + double y; + double z; + double w; +} vec4; + +typedef struct +{ + vec3 v0; + vec3 v1; + vec3 v2; +} mat3; + +typedef struct +{ + vec4 v0; + vec4 v1; + vec4 v2; + vec4 v3; +} mat4; + +double vec3_len(const vec3 * vec) +{ + return sqrt(vec->x * vec->x + vec->y * vec->y + vec->z * vec->z); +} + +void vec3_normalize(vec3 * vec) +{ + double len = vec3_len(vec); + vec->x /= len; + vec->y /= len; + vec->z /= len; +} + +void vec3_cross(vec3 * out, const vec3 * first, const vec3 * second) +{ + out->x = first->y * second->z - first->z * second->y; + out->y = first->z * second->x - first->x * second->z; + out->z = first->x * second->y - first->y * second->x; +} + +void mat4_make_translation(mat4 * mat, const vec3 * translate) +{ + mat->v0 = (vec4){ 1.0, 0.0, 0.0, 0.0 }; + mat->v1 = (vec4){ 0.0, 1.0, 0.0, 0.0 }; + mat->v2 = (vec4){ 0.0, 0.0, 1.0, 0.0 }; + mat->v3 = (vec4){ translate->x, translate->y, translate->z, 1.0 }; +} + +void mat4_mult(mat4 * a, const mat4 * b) +{ + double a11 = a->v0.x, a12 = a->v1.x, a13 = a->v2.x, a14 = a->v3.x; + double a21 = a->v0.y, a22 = a->v1.y, a23 = a->v2.y, a24 = a->v3.y; + double a31 = a->v0.z, a32 = a->v1.z, a33 = a->v2.z, a34 = a->v3.z; + double a41 = a->v0.w, a42 = a->v1.w, a43 = a->v2.w, a44 = a->v3.w; + + double b11 = b->v0.x, b12 = b->v1.x, b13 = b->v2.x, b14 = b->v3.x; + double b21 = b->v0.y, b22 = b->v1.y, b23 = b->v2.y, b24 = b->v3.y; + double b31 = b->v0.z, b32 = b->v1.z, b33 = b->v2.z, b34 = b->v3.z; + double b41 = b->v0.w, b42 = b->v1.w, b43 = b->v2.w, b44 = b->v3.w; + + a->v0.x = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41; + a->v1.x = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42; + a->v2.x = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43; + a->v3.x = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44; + + a->v0.y = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41; + a->v1.y = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42; + a->v2.y = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43; + a->v3.y = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44; + + a->v0.z = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41; + a->v1.z = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42; + a->v2.z = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43; + a->v3.z = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44; + + a->v0.w = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41; + a->v1.w = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42; + a->v2.w = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43; + a->v3.w = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44; +} + +void mat4_invert(mat4 * m) +{ + double n11 = m->v0.x, n21 = m->v0.y, n31 = m->v0.z, n41 = m->v0.w, + n12 = m->v1.x, n22 = m->v1.y, n32 = m->v1.z, n42 = m->v1.w, + n13 = m->v2.x, n23 = m->v2.y, n33 = m->v2.z, n43 = m->v2.w, + n14 = m->v3.x, n24 = m->v3.y, n34 = m->v3.z, n44 = m->v3.w; + + double t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44; + double t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44; + double t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44; + double t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34; + + double det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14; + + if (det == 0) + { + return; + } + + double detInv = 1 / det; + + m->v0.x = t11 * detInv; + m->v0.y = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv; + m->v0.z = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv; + m->v0.w = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv; + + m->v1.x = t12 * detInv; + m->v1.y = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv; + m->v1.z = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv; + m->v1.w = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv; + + m->v2.x = t13 * detInv; + m->v2.y = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv; + m->v2.z = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv; + m->v2.w = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv; + + m->v3.x = t14 * detInv; + m->v3.y = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv; + m->v3.z = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv; + m->v3.w = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv; +} + +void vec4_apply_mat4(vec4 * vec, const mat4 * mat) +{ + double x = vec->x, y = vec->y, z = vec->z, w = vec->w; + + vec->x = mat->v0.x * x + mat->v1.x * y + mat->v2.x * z + mat->v3.x * w; + vec->y = mat->v0.y * x + mat->v1.y * y + mat->v2.y * z + mat->v3.y * w; + vec->z = mat->v0.z * x + mat->v1.z * y + mat->v2.z * z + mat->v3.z * w; + vec->w = mat->v0.w * x + mat->v1.w * y + mat->v2.w * z + mat->v3.w * w; +} + +void vec3_apply_mat3(vec3 * vec, const mat3 * mat) +{ + double x = vec->x, y = vec->y, z = vec->z; + + vec->x = mat->v0.x * x + mat->v1.x * y + mat->v2.x * z; + vec->y = mat->v0.y * x + mat->v1.y * y + mat->v2.y * z; + vec->z = mat->v0.z * x + mat->v1.z * y + mat->v2.z * z; +} + +void vec4_multiply_scalar(vec4 * vec, double scalar) +{ + vec->x *= scalar; + vec->y *= scalar; + vec->z *= scalar; + vec->w *= scalar; +} + +void vec4_add_scalar(vec4 * vec, double scalar) +{ + vec->x += scalar; + vec->y += scalar; + vec->z += scalar; + vec->w += scalar; +} + +void vec3_multiply_scalar(vec3 * vec, double scalar) +{ + vec->x *= scalar; + vec->y *= scalar; + vec->z *= scalar; +} + +void vec3_add_scalar(vec3 * vec, double scalar) +{ + vec->x += scalar; + vec->y += scalar; + vec->z += scalar; +} + +void mat3_invert(mat3 * m) +{ + double n11 = m->v0.x, n21 = m->v0.y, n31 = m->v0.z, + n12 = m->v1.x, n22 = m->v1.y, n32 = m->v1.z, + n13 = m->v2.x, n23 = m->v2.y, n33 = m->v2.z; + + double t11 = n33 * n22 - n32 * n23; + double t12 = n32 * n13 - n33 * n12; + double t13 = n23 * n12 - n22 * n13; + + double det = n11 * t11 + n21 * t12 + n31 * t13; + + if (det == 0) + { + return; + } + + double detInv = 1 / det; + + m->v0.x = t11 * detInv; + m->v0.y = (n31 * n23 - n33 * n21) * detInv; + m->v0.z = (n32 * n21 - n31 * n22) * detInv; + + m->v1.x = t12 * detInv; + m->v1.y = (n33 * n11 - n31 * n13) * detInv; + m->v1.z = (n31 * n12 - n32 * n11) * detInv; + + m->v2.x = t13 * detInv; + m->v2.y = (n21 * n13 - n23 * n11) * detInv; + m->v2.z = (n22 * n11 - n21 * n12) * detInv; +} + +void mat3_transpose(mat3 * m) +{ + double tmp; + tmp = m->v0.y; m->v0.y = m->v1.x; m->v1.x = tmp; + tmp = m->v0.z; m->v0.z = m->v2.x; m->v2.x = tmp; + tmp = m->v1.z; m->v1.z = m->v2.y; m->v2.y = tmp; +} + +void mat3_from_mat4(mat3 * out, const mat4 * mat) +{ + out->v0 = (vec3){ mat->v0.x, mat->v0.y, mat->v0.z }; + out->v1 = (vec3){ mat->v1.x, mat->v1.y, mat->v1.z }; + out->v2 = (vec3){ mat->v2.x, mat->v2.y, mat->v2.z }; +} + +void vec3_print(const vec3 * vec) +{ + printf("%f %f %f\n", vec->x, vec->y, vec->z); +} + +void vec4_print(const vec4 * vec) +{ + printf("%f %f %f %f\n", vec->x, vec->y, vec->z, vec->w); +} + +void mat3_print(const mat3 * mat) +{ + printf("%f %f %f\n", mat->v0.x, mat->v1.x, mat->v2.x); + printf("%f %f %f\n", mat->v0.y, mat->v1.y, mat->v2.y); + printf("%f %f %f\n", mat->v0.z, mat->v1.z, mat->v2.z); +} + +void mat4_print(const mat4 * mat) +{ + printf("%f %f %f %f\n", mat->v0.x, mat->v1.x, mat->v2.x, mat->v3.x); + printf("%f %f %f %f\n", mat->v0.y, mat->v1.y, mat->v2.y, mat->v3.y); + printf("%f %f %f %f\n", mat->v0.z, mat->v1.z, mat->v2.z, mat->v3.z); + printf("%f %f %f %f\n", mat->v0.w, mat->v1.w, mat->v2.w, mat->v3.w); +} + +#endif diff --git a/wiimote.c b/wiimote.c index 8553ca1..48301ea 100644 --- a/wiimote.c +++ b/wiimote.c @@ -1,7 +1,6 @@ #include "wiimote.h" + #include "wm_reports.h" -#include "wm_crypto.h" -#include "wm_print.h" #include #include @@ -10,12 +9,21 @@ int tries = 0; +static uint8_t classic_calibration[16] = +{ + // 0xF8, 0x04, 0x7A, 0xF8, 0x04, 0x7A, 0xF8, 0x04, 0x7A, 0xF8, 0x04, 0x7A, 0x00, 0x00, 0x00, 0x00 + 0xE1, 0x19, 0x7C, 0xEF, 0x22, 0x7C, 0xE6, 0x1E, 0x85, 0xDE, 0x15, 0x8B, 0x0E, 0x22, 0x8F, 0xE4 +}; + +static uint8_t nunchuk_calibration[16] = +{ + 0x81, 0x80, 0x7F, 0x22, 0xB5, 0xB3, 0xB3, 0x03, 0x00, 0x00, 0x7C, 0x00, 0x00, 0x83, 0x14, 0x69 +}; + int process_report(struct wiimote_state *state, const uint8_t * buf, int len) { struct report_data * data = (struct report_data *)buf; - print_report(buf, len); - //every output report contains rumble info state->sys.rumble = data->buf[0] & 0x01; @@ -108,6 +116,26 @@ int generate_report(struct wiimote_state * state, uint8_t * buf) struct report_data * data = (struct report_data *)buf; uint8_t * contents; + if (state->usr.connected_extension_type != state->sys.connected_extension_type) + { + if (state->sys.extension_connected) + { + state->sys.extension_connected = 0; + state->sys.connected_extension_type = NoExtension; + state->sys.extension_hotplug_timer = 30; + report_queue_push_status(state); + } + + bool extension_connected = (state->usr.connected_extension_type != NoExtension); + if (extension_connected && --state->sys.extension_hotplug_timer <= 0) + { + state->sys.extension_connected = extension_connected; + state->sys.connected_extension_type = state->usr.connected_extension_type; + report_queue_push_status(state); + init_extension(state); + } + } + if (!state->sys.reporting_continuous && !state->sys.report_changed && state->sys.queue == NULL) return 0; @@ -192,15 +220,9 @@ int generate_report(struct wiimote_state * state, uint8_t * buf) break; } - print_report(buf, len); return len; } -void ir_object_clear(struct wiimote_state * state, uint8_t num) -{ - memset(&(state->usr.ir_object[num]), 0xff, sizeof(struct wiimote_ir_object)); -} - void read_eeprom(struct wiimote_state * state, uint32_t offset, uint16_t size) { FILE * file; @@ -216,49 +238,47 @@ void read_eeprom(struct wiimote_state * state, uint32_t offset, uint16_t size) return; } + offset = offset & 0xFFFF; + //addresses greater than 0x16FF cannot be read or written if (offset + size > 0x16FF) { rpt = report_queue_push(state); - report_format_mem_resp(rpt, 0xf, 0x8, offset, NULL); + report_format_mem_resp(state, rpt, 0x10, 0x8, offset, NULL, false); fclose(file); return; } - //start reading at the memory offset (0x70 is the physical address at 0x00 virtual) - fseek(file, offset + 0x70, SEEK_SET); + fseek(file, offset, SEEK_SET); - //allocate memory for the buffer buffer = (uint8_t *)malloc(size); if (!buffer) { - printf("Couldn't allocate eeprom reading buffer"); fclose(file); return; } - //read memory into buffer fread(buffer, 1, size, file); fclose(file); //equivalent to ceil(size / 0x10) - int totalpackets = (size + 0x10 - 1) / 0x10; + int total_packets = (size + 0x10 - 1) / 0x10; //allocate all the needed reports - for (i=0; isys.queue; + struct queued_report * queue_item = state->sys.queue; - for (i=0; irpt; - int psize = (i == totalpackets-1) ? (size % 0x10) : 0xf; - report_format_mem_resp(rpt, psize, 0x0, offset + i*0x10, &buffer[i*0x10]); - currentrpt = currentrpt->next; + rpt = &queue_item->rpt; + int packet_size = (i == total_packets - 1) ? (size - i * 0x10) : 0x10; + report_format_mem_resp(state, rpt, packet_size, 0x0, offset + i*0x10, &buffer[i*0x10], false); + queue_item = queue_item->next; } free(buffer); @@ -269,7 +289,6 @@ void write_eeprom(struct wiimote_state * state, uint32_t offset, uint8_t size, c FILE * file; struct report * rpt; - //Open file file = fopen("eeprom.bin", "rb"); if (!file) { @@ -277,17 +296,18 @@ void write_eeprom(struct wiimote_state * state, uint32_t offset, uint8_t size, c return; } + offset = offset & 0xFFFF; + //addresses greater than 0x16FF cannot be read or written if (offset + size > 0x16FF) { rpt = report_queue_push(state); - report_format_mem_resp(rpt, 0xf, 0x8, offset, NULL); + report_format_mem_resp(state, rpt, 0x10, 0x8, offset, NULL, false); fclose(file); return; } - //start reading at the memory offset (0x70 is the physical address at 0x00 virtual) - fseek(file, offset + 0x70, SEEK_SET); + fseek(file, offset, SEEK_SET); //write buffer into the eeprom file fwrite(buf, 1, size, file); @@ -300,11 +320,12 @@ void read_register(struct wiimote_state *state, uint32_t offset, uint16_t size) uint8_t * buffer; struct report * rpt; int i; + bool encrypt = false; switch ((offset >> 16) & 0xfe) //select register, ignore lsb { case 0xa2: //speaker - buffer = register_a2 + (offset & 0xff); + buffer = state->sys.register_a2 + (offset & 0xff); break; case 0xa4: //extension if (state->sys.wmp_state == 1) @@ -317,73 +338,72 @@ void read_register(struct wiimote_state *state, uint32_t offset, uint16_t size) printf("%d \n", tries); if (tries == 5) { - register_a6[0xf7] = 0x0e; + state->sys.register_a6[0xf7] = 0x0e; } } - buffer = register_a6 + (offset & 0xff); + buffer = state->sys.register_a6 + (offset & 0xff); } else { - buffer = register_a4 + (offset & 0xff); + buffer = state->sys.register_a4 + (offset & 0xff); } + + if (state->sys.extension_encrypted) + { + encrypt = true; + } + break; case 0xa6: //motionplus if (state->sys.wmp_state == 1) { rpt = report_queue_push(state); - report_format_mem_resp(rpt, 0xf, 0x7, offset, NULL); + report_format_mem_resp(state, rpt, 0x10, 0x7, offset, NULL, false); return; } - buffer = register_a6 + (offset & 0xff); + buffer = state->sys.register_a6 + (offset & 0xff); break; case 0xb0: //ir camera - buffer = register_b0 + (offset & 0xff); + buffer = state->sys.register_b0 + (offset & 0xff); break; default: //??? break; } - //equivalent to math.ceil(size / 0x10) - int totalpackets = (size + 0x10 - 1) / 0x10; + //equivalent to ceil(size / 0x10) + int total_packets = (size + 0x10 - 1) / 0x10; //allocate all the needed reports - for (i=0; isys.queue; + struct queued_report * queue_item = state->sys.queue; - for (i=0; irpt; - int psize = (i == totalpackets-1) ? (size % 0x10) : 0xf; - report_format_mem_resp(rpt, psize, 0x0, offset + i*0x10, &buffer[i*0x10]); - currentrpt = currentrpt->next; + rpt = &queue_item->rpt; + int packet_size = (i == total_packets - 1) ? (size - i * 0x10) : 0x10; + report_format_mem_resp(state, rpt, packet_size, 0x0, offset + i*0x10, &buffer[i*0x10], encrypt); + queue_item = queue_item->next; } } void write_register(struct wiimote_state *state, uint32_t offset, uint8_t size, const uint8_t * buf) { uint8_t * reg; + int result = 0x00; switch ((offset >> 16) & 0xfe) //select register, ignore lsb { case 0xa2: //speaker - reg = register_a2; + reg = state->sys.register_a2; memcpy(reg + (offset & 0xff), buf, size); break; case 0xa4: //extension - if (state->sys.wmp_state == 1) - { - reg = register_a6; - } - else - { - reg = register_a4; - } + reg = (state->sys.wmp_state == 1) ? state->sys.register_a6 : state->sys.register_a4; memcpy(reg + (offset & 0xff), buf, size); @@ -416,93 +436,97 @@ void write_register(struct wiimote_state *state, uint32_t offset, uint8_t size, } else if ((offset & 0xff) == 0x4c) //last part of encryption code { - generate_tables(); + ext_generate_tables(&state->sys.extension_crypto_state, ®[0x40]); state->sys.extension_encrypted = 1; } - /* - else if (((offset & 0xff) == 0xf0) && (buf[0] == 0xaa)) + else if ((offset & 0xff) == 0xf0) { - //technically this sets the encryption - //state->sys.extension_encrypted = 1; + if (buf[0] == 0xaa) + { + state->sys.extension_encrypted = 1; + } + else if (buf[0] == 0x55) + { + state->sys.extension_encrypted = 0; + } } - */ else if ((offset & 0xff) == 0xf1) { - register_a6[0xf7] = 0x1a; + state->sys.register_a6[0xf7] = 0x1a; //idk why or how, but sometimes this must be updated - register_a6[0x50] = 0xe7; - register_a6[0x51] = 0x98; - register_a6[0x52] = 0x31; - register_a6[0x53] = 0x8a; - register_a6[0x54] = 0x18; - register_a6[0x55] = 0x82; - register_a6[0x56] = 0x37; - register_a6[0x57] = 0x5e; - register_a6[0x58] = 0x02; - register_a6[0x59] = 0x4f; - register_a6[0x5a] = 0x68; - register_a6[0x5b] = 0x47; - register_a6[0x5c] = 0x78; - register_a6[0x5d] = 0xef; - register_a6[0x5e] = 0xbb; - register_a6[0x5f] = 0xd7; - - register_a6[0x60] = 0x86; - register_a6[0x61] = 0xc8; - register_a6[0x62] = 0x95; - register_a6[0x63] = 0xbd; - register_a6[0x64] = 0x20; - register_a6[0x65] = 0x9b; - register_a6[0x66] = 0xeb; - register_a6[0x67] = 0x8b; - register_a6[0x68] = 0x79; - register_a6[0x69] = 0x81; - register_a6[0x6a] = 0xdc; - register_a6[0x6b] = 0x61; - register_a6[0x6c] = 0x13; - register_a6[0x6d] = 0x54; - register_a6[0x6e] = 0x79; - register_a6[0x6f] = 0x4c; - - register_a6[0x70] = 0xb7; - register_a6[0x71] = 0x26; - register_a6[0x72] = 0x82; - register_a6[0x73] = 0x17; - register_a6[0x74] = 0xe8; - register_a6[0x75] = 0x0f; - register_a6[0x76] = 0xa9; - register_a6[0x77] = 0xb5; - register_a6[0x78] = 0x45; - register_a6[0x79] = 0xa0; - register_a6[0x7a] = 0x38; - register_a6[0x7b] = 0x8e; - register_a6[0x7c] = 0x9e; - register_a6[0x7d] = 0x86; - register_a6[0x7e] = 0x72; - register_a6[0x7f] = 0x55; - - register_a6[0x80] = 0x3d; - register_a6[0x81] = 0x46; - register_a6[0x82] = 0x2e; - register_a6[0x83] = 0x3e; - register_a6[0x84] = 0x10; - register_a6[0x85] = 0x1f; - register_a6[0x86] = 0x8e; - register_a6[0x87] = 0x0c; - register_a6[0x88] = 0xf4; - register_a6[0x89] = 0x04; - register_a6[0x8a] = 0x89; - register_a6[0x8b] = 0x4c; - register_a6[0x8c] = 0xca; - register_a6[0x8d] = 0x3e; - register_a6[0x8e] = 0x9f; - register_a6[0x8f] = 0x36; + state->sys.register_a6[0x50] = 0xe7; + state->sys.register_a6[0x51] = 0x98; + state->sys.register_a6[0x52] = 0x31; + state->sys.register_a6[0x53] = 0x8a; + state->sys.register_a6[0x54] = 0x18; + state->sys.register_a6[0x55] = 0x82; + state->sys.register_a6[0x56] = 0x37; + state->sys.register_a6[0x57] = 0x5e; + state->sys.register_a6[0x58] = 0x02; + state->sys.register_a6[0x59] = 0x4f; + state->sys.register_a6[0x5a] = 0x68; + state->sys.register_a6[0x5b] = 0x47; + state->sys.register_a6[0x5c] = 0x78; + state->sys.register_a6[0x5d] = 0xef; + state->sys.register_a6[0x5e] = 0xbb; + state->sys.register_a6[0x5f] = 0xd7; + + state->sys.register_a6[0x60] = 0x86; + state->sys.register_a6[0x61] = 0xc8; + state->sys.register_a6[0x62] = 0x95; + state->sys.register_a6[0x63] = 0xbd; + state->sys.register_a6[0x64] = 0x20; + state->sys.register_a6[0x65] = 0x9b; + state->sys.register_a6[0x66] = 0xeb; + state->sys.register_a6[0x67] = 0x8b; + state->sys.register_a6[0x68] = 0x79; + state->sys.register_a6[0x69] = 0x81; + state->sys.register_a6[0x6a] = 0xdc; + state->sys.register_a6[0x6b] = 0x61; + state->sys.register_a6[0x6c] = 0x13; + state->sys.register_a6[0x6d] = 0x54; + state->sys.register_a6[0x6e] = 0x79; + state->sys.register_a6[0x6f] = 0x4c; + + state->sys.register_a6[0x70] = 0xb7; + state->sys.register_a6[0x71] = 0x26; + state->sys.register_a6[0x72] = 0x82; + state->sys.register_a6[0x73] = 0x17; + state->sys.register_a6[0x74] = 0xe8; + state->sys.register_a6[0x75] = 0x0f; + state->sys.register_a6[0x76] = 0xa9; + state->sys.register_a6[0x77] = 0xb5; + state->sys.register_a6[0x78] = 0x45; + state->sys.register_a6[0x79] = 0xa0; + state->sys.register_a6[0x7a] = 0x38; + state->sys.register_a6[0x7b] = 0x8e; + state->sys.register_a6[0x7c] = 0x9e; + state->sys.register_a6[0x7d] = 0x86; + state->sys.register_a6[0x7e] = 0x72; + state->sys.register_a6[0x7f] = 0x55; + + state->sys.register_a6[0x80] = 0x3d; + state->sys.register_a6[0x81] = 0x46; + state->sys.register_a6[0x82] = 0x2e; + state->sys.register_a6[0x83] = 0x3e; + state->sys.register_a6[0x84] = 0x10; + state->sys.register_a6[0x85] = 0x1f; + state->sys.register_a6[0x86] = 0x8e; + state->sys.register_a6[0x87] = 0x0c; + state->sys.register_a6[0x88] = 0xf4; + state->sys.register_a6[0x89] = 0x04; + state->sys.register_a6[0x8a] = 0x89; + state->sys.register_a6[0x8b] = 0x4c; + state->sys.register_a6[0x8c] = 0xca; + state->sys.register_a6[0x8d] = 0x3e; + state->sys.register_a6[0x8e] = 0x9f; + state->sys.register_a6[0x8f] = 0x36; } break; case 0xa6: //motionplus - reg = register_a6; + reg = state->sys.register_a6; memcpy(reg + (offset & 0xff), buf, size); if (((offset & 0xff) == 0xfe) && ((buf[0] >> 2) & 0x1)) //activate wmp @@ -523,219 +547,279 @@ void write_register(struct wiimote_state *state, uint32_t offset, uint8_t size, break; case 0xb0: //ir camera - reg = register_b0; + reg = state->sys.register_b0; memcpy(reg + (offset & 0xff), buf, size); break; default: //??? break; } + report_queue_push_ack(state, 0x16, result); +} - report_queue_push_ack(state, 0x16, 0x00); +void reset_ir_object(struct wiimote_ir_object * ir_object) +{ + memset(ir_object, 0xff, sizeof(struct wiimote_ir_object)); } +void reset_input_ir(struct wiimote_ir_object ir_object[4]) +{ + memset(ir_object, 0xff, sizeof(struct wiimote_ir_object) * 4); +} -void init_extension(struct wiimote_state *state) +void reset_input_nunchuk(struct wiimote_nunchuk * nunchuk) { + memset(nunchuk, 0, sizeof(struct wiimote_nunchuk)); - if (state->sys.wmp_state == 1) - { - //register_a6[0xfa] = 0x00; - //register_a6[0xfb] = 0x00; - //register_a6[0xfc] = 0xa4; - //register_a6[0xfd] = 0x20; - //this might not be needed now that memory is proper - //register_a6[0xfe] = state->sys.extension_report_type; - //register_a6[0xff] = 0x05; - register_a6[0xfc] = 0xa4; - - state->sys.extension_encrypted = 0; - - //random guess, pulled from wiimote, not sure what this is for - register_a6[0xf0] = 0x55; - register_a6[0xf1] = 0xff; - register_a6[0xf2] = 0xff; - register_a6[0xf3] = 0xff; - register_a6[0xf4] = 0xff; - register_a6[0xf5] = 0xff; - register_a6[0xf6] = 0x00; - - //a4 40 post init - register_a6[0x40] = 0x81; - register_a6[0x41] = 0x80; - register_a6[0x42] = 0x80; - register_a6[0x43] = 0x28; - register_a6[0x44] = 0xb4; - register_a6[0x45] = 0xb3; - register_a6[0x46] = 0xb3; - register_a6[0x47] = 0x26; - register_a6[0x48] = 0xe3; - register_a6[0x49] = 0x22; - register_a6[0x4a] = 0x7a; - register_a6[0x4b] = 0xd8; - register_a6[0x4c] = 0x1b; - register_a6[0x4d] = 0x81; - register_a6[0x4e] = 0x31; - register_a6[0x4f] = 0x86; - - register_a6[0x20] = 0x7c; - register_a6[0x21] = 0x97; - register_a6[0x22] = 0x7f; - register_a6[0x23] = 0x0a; - register_a6[0x24] = 0x7c; - register_a6[0x25] = 0xa8; - register_a6[0x26] = 0x33; - register_a6[0x27] = 0xb7; - register_a6[0x28] = 0xcc; - register_a6[0x29] = 0x12; - register_a6[0x2a] = 0x33; - register_a6[0x2b] = 0x08; - register_a6[0x2c] = 0xc8; - register_a6[0x2d] = 0x01; - register_a6[0x2e] = 0x72; - register_a6[0x2f] = 0xd4; - - register_a6[0x30] = 0x7c; - register_a6[0x31] = 0x53; - register_a6[0x32] = 0x87; - register_a6[0x33] = 0x58; - register_a6[0x34] = 0x7c; - register_a6[0x35] = 0x9f; - register_a6[0x36] = 0x36; - register_a6[0x37] = 0xb2; - register_a6[0x38] = 0xc9; - register_a6[0x39] = 0x34; - register_a6[0x3a] = 0x35; - register_a6[0x3b] = 0xf8; - register_a6[0x3c] = 0x2d; - register_a6[0x3d] = 0x60; - register_a6[0x3e] = 0xd7; - register_a6[0x3f] = 0xd5; - - //not sure block, this may not be needed - register_a6[0x50] = 0x15; - register_a6[0x51] = 0x6d; - register_a6[0x52] = 0xe0; - register_a6[0x53] = 0x23; - register_a6[0x54] = 0x20; - register_a6[0x55] = 0x79; - register_a6[0x56] = 0xd3; - register_a6[0x57] = 0x73; - register_a6[0x58] = 0x01; - register_a6[0x59] = 0xa9; - register_a6[0x5a] = 0xf0; - register_a6[0x5b] = 0x25; - register_a6[0x5c] = 0xb0; - register_a6[0x5d] = 0xbc; - register_a6[0x5e] = 0xff; - register_a6[0x5f] = 0xe1; - - register_a6[0x60] = 0xd8; - register_a6[0x61] = 0x3f; - register_a6[0x62] = 0x82; - register_a6[0x63] = 0x52; - register_a6[0x64] = 0x75; - register_a6[0x65] = 0x99; - register_a6[0x66] = 0xbe; - register_a6[0x67] = 0xdb; - register_a6[0x68] = 0xcb; - register_a6[0x69] = 0x61; - register_a6[0x6a] = 0x60; - register_a6[0x6b] = 0x0f; - register_a6[0x6c] = 0x35; - register_a6[0x6d] = 0xbd; - register_a6[0x6e] = 0xd4; - register_a6[0x6f] = 0x4d; - - register_a6[0x70] = 0x5c; - register_a6[0x71] = 0x9f; - register_a6[0x72] = 0x5d; - register_a6[0x73] = 0x81; - register_a6[0x74] = 0x71; - register_a6[0x75] = 0xde; - register_a6[0x76] = 0x22; - register_a6[0x77] = 0xe6; - register_a6[0x78] = 0xb9; - register_a6[0x79] = 0x23; - register_a6[0x7a] = 0xa4; - register_a6[0x7b] = 0x58; - register_a6[0x7c] = 0xb7; - register_a6[0x7d] = 0x62; - register_a6[0x7e] = 0x33; - register_a6[0x7f] = 0xa4; - - register_a6[0x80] = 0xcd; - register_a6[0x81] = 0x8b; - register_a6[0x82] = 0x3a; - register_a6[0x83] = 0xfe; - register_a6[0x84] = 0x98; - register_a6[0x85] = 0xf0; - register_a6[0x86] = 0xd9; - register_a6[0x87] = 0x57; - register_a6[0x88] = 0x0c; - register_a6[0x89] = 0xe8; - register_a6[0x8a] = 0x27; - register_a6[0x8b] = 0x51; - register_a6[0x8c] = 0xb6; - register_a6[0x8d] = 0xea; - register_a6[0x8e] = 0xe5; - register_a6[0x8f] = 0x78; - - - //init progress byte, set it to done - register_a6[0xf7] = 0x0c; - register_a6[0xf8] = 0x00; - register_a6[0xf9] = 0x00; + nunchuk->x = 128; + nunchuk->y = 128; + nunchuk->accel_x = 512; + nunchuk->accel_y = 512; + nunchuk->accel_z = 760; +} - } - else +void reset_input_classic(struct wiimote_classic * classic) +{ + memset(classic, 0, sizeof(struct wiimote_classic)); + + classic->ls_x = 32; + classic->ls_y = 32; + classic->rs_x = 15; + classic->rs_y = 15; +} + +void reset_input_motionplus(struct wiimote_motionplus * motionplus) +{ + memset(motionplus, 0, sizeof(struct wiimote_motionplus)); + + motionplus->yaw_down = 0x1F7F; + motionplus->roll_left = 0x1F7F; + motionplus->pitch_left = 0x1F7F; + motionplus->yaw_slow = 1; + motionplus->roll_slow = 1; + motionplus->pitch_slow = 1; +} + +void init_extension(struct wiimote_state * state) +{ + if (state->sys.connected_extension_type == NoExtension) + { + memset(state->sys.register_a4, 0xff, sizeof(state->sys.register_a4)); + } + else + { + memset(state->sys.register_a4, 0, sizeof(state->sys.register_a4)); + } + + if (state->sys.wmp_state == 1) + { + //register_a6[0xfa] = 0x00; + //register_a6[0xfb] = 0x00; + //register_a6[0xfc] = 0xa4; + //register_a6[0xfd] = 0x20; + //this might not be needed now that memory is proper + //register_a6[0xfe] = state->sys.extension_report_type; + //register_a6[0xff] = 0x05; + state->sys.register_a6[0xfc] = 0xa4; + + state->sys.extension_encrypted = 0; + + //random guess, pulled from wiimote, not sure what this is for + state->sys.register_a6[0xf0] = 0x55; + state->sys.register_a6[0xf1] = 0xff; + state->sys.register_a6[0xf2] = 0xff; + state->sys.register_a6[0xf3] = 0xff; + state->sys.register_a6[0xf4] = 0xff; + state->sys.register_a6[0xf5] = 0xff; + state->sys.register_a6[0xf6] = 0x00; + + //a4 40 post init + // state->sys.register_a6[0x40] = 0x81; + // state->sys.register_a6[0x41] = 0x80; + // state->sys.register_a6[0x42] = 0x80; + // state->sys.register_a6[0x43] = 0x28; + // state->sys.register_a6[0x44] = 0xb4; + // state->sys.register_a6[0x45] = 0xb3; + // state->sys.register_a6[0x46] = 0xb3; + // state->sys.register_a6[0x47] = 0x26; + // state->sys.register_a6[0x48] = 0xe3; + // state->sys.register_a6[0x49] = 0x22; + // state->sys.register_a6[0x4a] = 0x7a; + // state->sys.register_a6[0x4b] = 0xd8; + // state->sys.register_a6[0x4c] = 0x1b; + // state->sys.register_a6[0x4d] = 0x81; + // state->sys.register_a6[0x4e] = 0x31; + // state->sys.register_a6[0x4f] = 0x86; + + state->sys.register_a6[0x20] = 0x7c; + state->sys.register_a6[0x21] = 0x97; + state->sys.register_a6[0x22] = 0x7f; + state->sys.register_a6[0x23] = 0x0a; + state->sys.register_a6[0x24] = 0x7c; + state->sys.register_a6[0x25] = 0xa8; + state->sys.register_a6[0x26] = 0x33; + state->sys.register_a6[0x27] = 0xb7; + state->sys.register_a6[0x28] = 0xcc; + state->sys.register_a6[0x29] = 0x12; + state->sys.register_a6[0x2a] = 0x33; + state->sys.register_a6[0x2b] = 0x08; + state->sys.register_a6[0x2c] = 0xc8; + state->sys.register_a6[0x2d] = 0x01; + state->sys.register_a6[0x2e] = 0x72; + state->sys.register_a6[0x2f] = 0xd4; + + state->sys.register_a6[0x30] = 0x7c; + state->sys.register_a6[0x31] = 0x53; + state->sys.register_a6[0x32] = 0x87; + state->sys.register_a6[0x33] = 0x58; + state->sys.register_a6[0x34] = 0x7c; + state->sys.register_a6[0x35] = 0x9f; + state->sys.register_a6[0x36] = 0x36; + state->sys.register_a6[0x37] = 0xb2; + state->sys.register_a6[0x38] = 0xc9; + state->sys.register_a6[0x39] = 0x34; + state->sys.register_a6[0x3a] = 0x35; + state->sys.register_a6[0x3b] = 0xf8; + state->sys.register_a6[0x3c] = 0x2d; + state->sys.register_a6[0x3d] = 0x60; + state->sys.register_a6[0x3e] = 0xd7; + state->sys.register_a6[0x3f] = 0xd5; + + //not sure block, this may not be needed + state->sys.register_a6[0x50] = 0x15; + state->sys.register_a6[0x51] = 0x6d; + state->sys.register_a6[0x52] = 0xe0; + state->sys.register_a6[0x53] = 0x23; + state->sys.register_a6[0x54] = 0x20; + state->sys.register_a6[0x55] = 0x79; + state->sys.register_a6[0x56] = 0xd3; + state->sys.register_a6[0x57] = 0x73; + state->sys.register_a6[0x58] = 0x01; + state->sys.register_a6[0x59] = 0xa9; + state->sys.register_a6[0x5a] = 0xf0; + state->sys.register_a6[0x5b] = 0x25; + state->sys.register_a6[0x5c] = 0xb0; + state->sys.register_a6[0x5d] = 0xbc; + state->sys.register_a6[0x5e] = 0xff; + state->sys.register_a6[0x5f] = 0xe1; + + state->sys.register_a6[0x60] = 0xd8; + state->sys.register_a6[0x61] = 0x3f; + state->sys.register_a6[0x62] = 0x82; + state->sys.register_a6[0x63] = 0x52; + state->sys.register_a6[0x64] = 0x75; + state->sys.register_a6[0x65] = 0x99; + state->sys.register_a6[0x66] = 0xbe; + state->sys.register_a6[0x67] = 0xdb; + state->sys.register_a6[0x68] = 0xcb; + state->sys.register_a6[0x69] = 0x61; + state->sys.register_a6[0x6a] = 0x60; + state->sys.register_a6[0x6b] = 0x0f; + state->sys.register_a6[0x6c] = 0x35; + state->sys.register_a6[0x6d] = 0xbd; + state->sys.register_a6[0x6e] = 0xd4; + state->sys.register_a6[0x6f] = 0x4d; + + state->sys.register_a6[0x70] = 0x5c; + state->sys.register_a6[0x71] = 0x9f; + state->sys.register_a6[0x72] = 0x5d; + state->sys.register_a6[0x73] = 0x81; + state->sys.register_a6[0x74] = 0x71; + state->sys.register_a6[0x75] = 0xde; + state->sys.register_a6[0x76] = 0x22; + state->sys.register_a6[0x77] = 0xe6; + state->sys.register_a6[0x78] = 0xb9; + state->sys.register_a6[0x79] = 0x23; + state->sys.register_a6[0x7a] = 0xa4; + state->sys.register_a6[0x7b] = 0x58; + state->sys.register_a6[0x7c] = 0xb7; + state->sys.register_a6[0x7d] = 0x62; + state->sys.register_a6[0x7e] = 0x33; + state->sys.register_a6[0x7f] = 0xa4; + + state->sys.register_a6[0x80] = 0xcd; + state->sys.register_a6[0x81] = 0x8b; + state->sys.register_a6[0x82] = 0x3a; + state->sys.register_a6[0x83] = 0xfe; + state->sys.register_a6[0x84] = 0x98; + state->sys.register_a6[0x85] = 0xf0; + state->sys.register_a6[0x86] = 0xd9; + state->sys.register_a6[0x87] = 0x57; + state->sys.register_a6[0x88] = 0x0c; + state->sys.register_a6[0x89] = 0xe8; + state->sys.register_a6[0x8a] = 0x27; + state->sys.register_a6[0x8b] = 0x51; + state->sys.register_a6[0x8c] = 0xb6; + state->sys.register_a6[0x8d] = 0xea; + state->sys.register_a6[0x8e] = 0xe5; + state->sys.register_a6[0x8f] = 0x78; + + + //init progress byte, set it to done + state->sys.register_a6[0xf7] = 0x0c; + state->sys.register_a6[0xf8] = 0x00; + state->sys.register_a6[0xf9] = 0x00; + } + else + { + //state->sys.register_a6[0xf7] = 0x0c; + + state->sys.register_a6[0xf0] = 0x55; + state->sys.register_a6[0xf1] = 0xff; + state->sys.register_a6[0xf2] = 0xff; + state->sys.register_a6[0xf3] = 0xff; + state->sys.register_a6[0xf4] = 0xff; + state->sys.register_a6[0xf5] = 0xff; + state->sys.register_a6[0xf6] = 0xff; + state->sys.register_a6[0xf7] = 0x02; + state->sys.register_a6[0xf8] = 0xff; + state->sys.register_a6[0xf9] = 0xff; + state->sys.register_a6[0xfa] = 0x01; + state->sys.register_a6[0xfb] = 0x00; + state->sys.register_a6[0xfc] = 0xa6; + state->sys.register_a6[0xfd] = 0x20; + state->sys.register_a6[0xfe] = 0x00; + state->sys.register_a6[0xff] = 0x05; + + if (state->sys.connected_extension_type == NoExtension) { - register_a6[0xfa] = 0x00; - register_a6[0xfb] = 0x00; - register_a6[0xfc] = 0xa6; - register_a6[0xfd] = 0x20; - //register_a6[0xfe] = 0x00; //leave this be - register_a6[0xff] = 0x05; + return; + } - register_a6[0xf7] = 0x0c; - register_a6[0xf8] = 0xff; - register_a6[0xf9] = 0xff; + memset(&state->sys.register_a4[0xf0], 0x0, 0x10); - state->sys.extension_report_type = state->sys.extension; + state->sys.register_a4[0xf0] = 0x55; + state->sys.register_a4[0xfc] = 0xa4; + state->sys.register_a4[0xfd] = 0x20; - if (state->sys.extension == EXT_NUNCHUCK) - { - register_a4[0xfa] = 0x00; - register_a4[0xfb] = 0x00; - register_a4[0xfc] = 0xa4; - register_a4[0xfd] = 0x20; - register_a4[0xfe] = 0x00; - register_a4[0xff] = 0x00; - } - else if (state->sys.extension == EXT_CLASSIC) - { - register_a4[0xfa] = 0x00; - register_a4[0xfb] = 0x00; - register_a4[0xfc] = 0xa4; - register_a4[0xfd] = 0x20; - register_a4[0xfe] = 0x01; - register_a4[0xff] = 0x01; - } - else - { - register_a4[0xfa] = 0xff; - register_a4[0xfb] = 0xff; - register_a4[0xfc] = 0xff; - register_a4[0xfd] = 0xff; - register_a4[0xfe] = 0xff; - register_a4[0xff] = 0xff; - } + switch (state->sys.connected_extension_type) + { + default: + case Nunchuk: + state->sys.register_a4[0xfe] = 0x00; + state->sys.register_a4[0xff] = 0x00; + memcpy(&state->sys.register_a4[0x20], nunchuk_calibration, 0x10); + memcpy(&state->sys.register_a4[0x30], nunchuk_calibration, 0x10); + break; + case Classic: + state->sys.register_a4[0xfe] = 0x01; + state->sys.register_a4[0xff] = 0x01; + memcpy(&state->sys.register_a4[0x20], classic_calibration, 0x10); + memcpy(&state->sys.register_a4[0x30], classic_calibration, 0x10); + break; + case BalanceBoard: + state->sys.register_a4[0xfe] = 0x04; + state->sys.register_a4[0xff] = 0x02; + break; } + + state->sys.extension_report_type = state->sys.register_a4[0xfe]; + state->sys.extension_type = state->sys.register_a4[0xff]; + } } -void destroy_wiimote(struct wiimote_state *state) +void wiimote_destroy(struct wiimote_state *state) { - //free the queue while (state->sys.queue != NULL) { struct queued_report * rpt = state->sys.queue; @@ -744,60 +828,39 @@ void destroy_wiimote(struct wiimote_state *state) } } -void init_wiimote(struct wiimote_state *state) +void wiimote_init(struct wiimote_state *state) { memset(state, 0, sizeof(struct wiimote_state)); - state->sys.reporting_mode = 0x30; - state->sys.battery_level = 0xff; + //flat + state->usr.accel_x = 0x82 << 2; + state->usr.accel_y = 0x82 << 2; + state->usr.accel_z = 0x9f << 2; - //flat - state->usr.accel_x = 0x80 << 2; - state->usr.accel_y = 0x80 << 2; - state->usr.accel_z = 0x97 << 2; - - /* - //mario kart tilted - state->usr.accel_x = 0x78 << 2; - state->usr.accel_y = 0x80 << 2; - state->usr.accel_z = 0x80 << 2; - */ - - ir_object_clear(state, 0); - ir_object_clear(state, 1); - ir_object_clear(state, 2); - ir_object_clear(state, 3); - - state->usr.nunchuck.x = 128; - state->usr.nunchuck.y = 128; - state->usr.nunchuck.accel_x = 512; - state->usr.nunchuck.accel_y = 512; - state->usr.nunchuck.accel_z = 760; - - state->usr.classic.ls_x = 32; - state->usr.classic.ls_y = 32; - state->usr.classic.rs_x = 15; - state->usr.classic.rs_y = 15; - - state->usr.motionplus.yaw_down = 0x1F7F; - state->usr.motionplus.roll_left = 0x1F7F; - state->usr.motionplus.pitch_left = 0x1F7F; - state->usr.motionplus.yaw_slow = 1; - state->usr.motionplus.roll_slow = 1; - state->usr.motionplus.pitch_slow = 1; - - - state->sys.extension = EXT_CLASSIC; - state->sys.extension_connected = 1; - init_extension(state); + reset_input_ir(state->usr.ir_object); + reset_input_nunchuk(&state->usr.nunchuk); + reset_input_classic(&state->usr.classic); + reset_input_motionplus(&state->usr.motionplus); + state->usr.connected_extension_type = NoExtension; - if (state->sys.extension != 0) - { - report_queue_push_status(state); - } + wiimote_reset(state); + + //power on report + struct report * rpt = report_queue_push(state); + rpt->len = 4; + rpt->data.io = 0xa1; + rpt->data.type = 0x30; +} + +void wiimote_reset(struct wiimote_state *state) +{ + memset(&state->sys, 0, sizeof(struct wiimote_state_sys)); - //state->sys.reporting_enabled = 0; - //state->sys.feature_ef_byte_6 = 0xa0; + state->sys.reporting_mode = 0x30; + state->sys.battery_level = 0xff; + state->sys.connected_extension_type = NoExtension; + + init_extension(state); } diff --git a/wiimote.h b/wiimote.h index eb2de3e..91db7be 100644 --- a/wiimote.h +++ b/wiimote.h @@ -3,9 +3,15 @@ #include #include +#include "wm_crypto.h" -#define EXT_NUNCHUCK 0x1 -#define EXT_CLASSIC 0x2 +enum wiimote_connected_extension_type +{ + Nunchuk = 0x0, + Classic = 0x1, + BalanceBoard = 0x2, + NoExtension = 0xff +}; struct wiimote_ir_object { @@ -19,7 +25,7 @@ struct wiimote_ir_object uint8_t intensity; }; -struct wiimote_nunchuck +struct wiimote_nunchuk { uint16_t accel_x; uint16_t accel_y; @@ -68,7 +74,6 @@ struct wiimote_motionplus struct wiimote_state_usr { - //main controller buttons bool a; bool b; bool minus; @@ -86,23 +91,28 @@ struct wiimote_state_usr bool power; //accelerometer (10 bit range) - //0 acceleration is approximately 0x80 + //0 acceleration is approximately 0x200 uint16_t accel_x; uint16_t accel_y; uint16_t accel_z; - //four ir camera dots - //x, y, size, x min, y min, x max, y max, intensity struct wiimote_ir_object ir_object[4]; - struct wiimote_nunchuck nunchuck; + enum wiimote_connected_extension_type connected_extension_type; + + struct wiimote_nunchuk nunchuk; struct wiimote_classic classic; struct wiimote_motionplus motionplus; }; +void reset_ir_object(struct wiimote_ir_object * object); +void reset_input_ir(struct wiimote_ir_object ir_object[4]); +void reset_input_nunchuk(struct wiimote_nunchuk * nunchuk); +void reset_input_classic(struct wiimote_classic * classic); +void reset_input_motionplus(struct wiimote_motionplus * motionplus); + struct wiimote_state_sys { - //controller led status bool led_1; bool led_2; bool led_3; @@ -116,11 +126,15 @@ struct wiimote_state_sys uint8_t battery_level; bool low_battery; + int extension_hotplug_timer; bool extension_connected; + enum wiimote_connected_extension_type connected_extension_type; + + struct ext_crypto_state extension_crypto_state; bool extension_report; bool extension_encrypted; - uint8_t extension; uint8_t extension_report_type; + uint8_t extension_type; uint8_t wmp_state; //0 inactive, 1 active, 2 deactivated uint8_t reporting_mode; @@ -130,9 +144,10 @@ struct wiimote_state_sys struct queued_report * queue; struct queued_report * queue_end; - //extensions: - //none, nunchuck, classic, wm+, wm+ and nunchuck, wm+ and classic - + uint8_t register_a2[10]; //speaker + uint8_t register_a4[256]; //extension + uint8_t register_a6[256]; //wii motion plus + uint8_t register_b0[52]; //ir camera }; struct wiimote_state @@ -141,21 +156,14 @@ struct wiimote_state struct wiimote_state_usr usr; }; -//574bytes -//TODO: move this to the wiimote struct where it belongs -uint8_t register_a2[0x09 + 1]; //speaker -uint8_t register_a4[0xff + 1]; //extension -uint8_t register_a6[0xff + 1]; //wii motion plus -uint8_t register_b0[0x33 + 1]; //ir camera +void wiimote_init(struct wiimote_state *state); +void wiimote_destroy(struct wiimote_state *state); -void init_wiimote(struct wiimote_state *state); -void destroy_wiimote(struct wiimote_state *state); +void wiimote_reset(struct wiimote_state *state); int process_report(struct wiimote_state *state, const uint8_t *buf, int len); int generate_report(struct wiimote_state * state, uint8_t * buf); -void ir_object_clear(struct wiimote_state * state, uint8_t num); - void read_eeprom(struct wiimote_state * state, uint32_t offset, uint16_t size); void write_eeprom(struct wiimote_state * state, uint32_t offset, uint8_t size, const uint8_t * buf); void read_register(struct wiimote_state *state, uint32_t offset, uint16_t size); @@ -163,4 +171,4 @@ void write_register(struct wiimote_state *state, uint32_t offset, uint8_t size, void init_extension(struct wiimote_state *state); -#endif +#endif //WIIMOTE_H diff --git a/wm_crypto.c b/wm_crypto.c index b6e91db..a425f0a 100644 --- a/wm_crypto.c +++ b/wm_crypto.c @@ -1,20 +1,18 @@ #include "wm_crypto.h" -#include "wiimote.h" - -#include //extension crypto (2602 bytes) -uint8_t ans_tbl[7][6] = { + +static const uint8_t ans_tbl[7][6] = { {0xA8,0x77,0xA6,0xE0,0xF7,0x43}, {0x5A,0x35,0x85,0xE2,0x72,0x97}, {0x8F,0xB7,0x1A,0x62,0x87,0x38}, { 0xD,0x67,0xC7,0xBE,0x4F,0x3E}, {0x20,0x76,0x37,0x8F,0x68,0xB7}, {0xA9,0x26,0x3F,0x2B,0x10,0xE3}, - {0x30,0x7E,0x90, 0xE,0x85, 0xA}, + {0x30,0x7E,0x90, 0xE,0x85, 0xA} }; -uint8_t sboxes[10][256] = { +static const uint8_t sboxes[10][256] = { { 0x70,0x51, 3,0x86,0x40, 0xD,0x4F,0xEB,0x3E,0xCC,0xD1,0x87,0x35,0xBD,0xF5, 0xB, 0x5E,0xD0,0xF8,0xF2,0xD5,0xE2,0x6C,0x31, 0xC,0xAD,0xFC,0x21,0xC3,0x78,0xC1, 6, @@ -184,70 +182,58 @@ static inline uint8_t ror8(uint8_t a, uint8_t b) return (a>>b) | ((a<<(8-b))&0xff); } -void generate_tables() +void ext_generate_tables(struct ext_crypto_state * state, const uint8_t key[16]) { - uint8_t idx; - uint8_t *ans; + int idx, i; + const uint8_t * ans; uint8_t t0[10]; - uint8_t i; - - printf("correct key: %02x %02x %02x %02x %02x %02x\n", - register_a4[0x4f], - register_a4[0x4e], - register_a4[0x4d], - register_a4[0x4c], - register_a4[0x4b], - register_a4[0x4a] - ); //determine idx with simple brute force - for(idx=0;idx<7;idx++) + for (idx = 0; idx < 7; idx++) { ans = ans_tbl[idx]; - for(i=0;i<10;i++) + for(i = 0; i < 10; i++) { - t0[i] = sboxes[0][register_a4[0x49 - i]]; + t0[i] = sboxes[0][key[9 - i]]; } - printf("key %d: %02x %02x %02x %02x %02x %02x\n", idx, - ((ror8((ans[0]^t0[5]),(t0[2]%8)) - t0[9]) ^ t0[4]) & 0xff, - ((ror8((ans[1]^t0[1]),(t0[0]%8)) - t0[5]) ^ t0[7]) & 0xff, - ((ror8((ans[2]^t0[6]),(t0[8]%8)) - t0[2]) ^ t0[0]) & 0xff, - ((ror8((ans[3]^t0[4]),(t0[7]%8)) - t0[3]) ^ t0[2]) & 0xff, - ((ror8((ans[4]^t0[1]),(t0[6]%8)) - t0[3]) ^ t0[4]) & 0xff, - ((ror8((ans[5]^t0[7]),(t0[8]%8)) - t0[5]) ^ t0[9]) & 0xff - ); - //todo: clean up this nasty mess - if ((register_a4[0x4f] == (uint8_t)((ror8(ans[0]^t0[5],t0[2]%8) - t0[9]) ^ t0[4])) && - (register_a4[0x4e] == (uint8_t)((ror8(ans[1]^t0[1],t0[0]%8) - t0[5]) ^ t0[7])) && - (register_a4[0x4d] == (uint8_t)((ror8(ans[2]^t0[6],t0[8]%8) - t0[2]) ^ t0[0])) && - (register_a4[0x4c] == (uint8_t)((ror8(ans[3]^t0[4],t0[7]%8) - t0[3]) ^ t0[2])) && - (register_a4[0x4b] == (uint8_t)((ror8(ans[4]^t0[1],t0[6]%8) - t0[3]) ^ t0[4])) && - (register_a4[0x4a] == (uint8_t)((ror8(ans[5]^t0[7],t0[8]%8) - t0[5]) ^ t0[9]))) + if ((key[0xf] == (uint8_t)((ror8(ans[0]^t0[5],t0[2]%8) - t0[9]) ^ t0[4])) && + (key[0xe] == (uint8_t)((ror8(ans[1]^t0[1],t0[0]%8) - t0[5]) ^ t0[7])) && + (key[0xd] == (uint8_t)((ror8(ans[2]^t0[6],t0[8]%8) - t0[2]) ^ t0[0])) && + (key[0xc] == (uint8_t)((ror8(ans[3]^t0[4],t0[7]%8) - t0[3]) ^ t0[2])) && + (key[0xb] == (uint8_t)((ror8(ans[4]^t0[1],t0[6]%8) - t0[3]) ^ t0[4])) && + (key[0xa] == (uint8_t)((ror8(ans[5]^t0[7],t0[8]%8) - t0[5]) ^ t0[9]))) { - printf("crypto key found %d \n", idx); - break; + break; } } - ft[0] = sboxes[idx+1][register_a4[0x4b]] ^ sboxes[idx+2][register_a4[0x46]]; - ft[1] = sboxes[idx+1][register_a4[0x4d]] ^ sboxes[idx+2][register_a4[0x44]]; - ft[2] = sboxes[idx+1][register_a4[0x4a]] ^ sboxes[idx+2][register_a4[0x42]]; - ft[3] = sboxes[idx+1][register_a4[0x4f]] ^ sboxes[idx+2][register_a4[0x47]]; - ft[4] = sboxes[idx+1][register_a4[0x4e]] ^ sboxes[idx+2][register_a4[0x45]]; - ft[5] = sboxes[idx+1][register_a4[0x4c]] ^ sboxes[idx+2][register_a4[0x40]]; - ft[6] = sboxes[idx+1][register_a4[0x49]] ^ sboxes[idx+2][register_a4[0x43]]; - ft[7] = sboxes[idx+1][register_a4[0x48]] ^ sboxes[idx+2][register_a4[0x41]]; - - sb[0] = sboxes[idx+1][register_a4[0x4f]] ^ sboxes[idx+2][register_a4[0x48]]; - sb[1] = sboxes[idx+1][register_a4[0x4a]] ^ sboxes[idx+2][register_a4[0x45]]; - sb[2] = sboxes[idx+1][register_a4[0x4c]] ^ sboxes[idx+2][register_a4[0x49]]; - sb[3] = sboxes[idx+1][register_a4[0x4d]] ^ sboxes[idx+2][register_a4[0x40]]; - sb[4] = sboxes[idx+1][register_a4[0x4b]] ^ sboxes[idx+2][register_a4[0x42]]; - sb[5] = sboxes[idx+1][register_a4[0x4e]] ^ sboxes[idx+2][register_a4[0x41]]; - sb[6] = sboxes[idx+1][register_a4[0x46]] ^ sboxes[idx+2][register_a4[0x44]]; - sb[7] = sboxes[idx+1][register_a4[0x47]] ^ sboxes[idx+2][register_a4[0x43]]; + state->ft[0] = sboxes[idx+1][key[0xb]] ^ sboxes[idx+2][key[0x6]]; + state->ft[1] = sboxes[idx+1][key[0xd]] ^ sboxes[idx+2][key[0x4]]; + state->ft[2] = sboxes[idx+1][key[0xa]] ^ sboxes[idx+2][key[0x2]]; + state->ft[3] = sboxes[idx+1][key[0xf]] ^ sboxes[idx+2][key[0x7]]; + state->ft[4] = sboxes[idx+1][key[0xe]] ^ sboxes[idx+2][key[0x5]]; + state->ft[5] = sboxes[idx+1][key[0xc]] ^ sboxes[idx+2][key[0x0]]; + state->ft[6] = sboxes[idx+1][key[0x9]] ^ sboxes[idx+2][key[0x3]]; + state->ft[7] = sboxes[idx+1][key[0x8]] ^ sboxes[idx+2][key[0x1]]; + state->sb[0] = sboxes[idx+1][key[0xf]] ^ sboxes[idx+2][key[0x8]]; + state->sb[1] = sboxes[idx+1][key[0xa]] ^ sboxes[idx+2][key[0x5]]; + state->sb[2] = sboxes[idx+1][key[0xc]] ^ sboxes[idx+2][key[0x9]]; + state->sb[3] = sboxes[idx+1][key[0xd]] ^ sboxes[idx+2][key[0x0]]; + state->sb[4] = sboxes[idx+1][key[0xb]] ^ sboxes[idx+2][key[0x2]]; + state->sb[5] = sboxes[idx+1][key[0xe]] ^ sboxes[idx+2][key[0x1]]; + state->sb[6] = sboxes[idx+1][key[0x6]] ^ sboxes[idx+2][key[0x4]]; + state->sb[7] = sboxes[idx+1][key[0x7]] ^ sboxes[idx+2][key[0x3]]; } + +void ext_encrypt_bytes(const struct ext_crypto_state * state, uint8_t * buffer, + int addr_offset, int length) +{ + for (int i = 0; i < length; i++) + { + buffer[i] = (buffer[i] - state->ft[(i + addr_offset) % 8]) ^ state->sb[(i + addr_offset) % 8]; + } +} \ No newline at end of file diff --git a/wm_crypto.h b/wm_crypto.h index baaafb2..4681277 100644 --- a/wm_crypto.h +++ b/wm_crypto.h @@ -3,12 +3,14 @@ #include -uint8_t ans_tbl[7][6]; -uint8_t sboxes[10][256]; +struct ext_crypto_state +{ + uint8_t ft[8]; + uint8_t sb[8]; +}; -uint8_t ft[8]; -uint8_t sb[8]; - -void generate_tables(); +void ext_generate_tables(struct ext_crypto_state * state, const uint8_t key[16]); +void ext_encrypt_bytes(const struct ext_crypto_state * state, uint8_t * buffer, + int addr_offset, int length); #endif diff --git a/wm_print.c b/wm_print.c index 8ad750c..e6005a4 100644 --- a/wm_print.c +++ b/wm_print.c @@ -8,15 +8,22 @@ int show_reports = 0; int reports_truncated = 0; +uint64_t next_report_ts = 0; +uint64_t report_timeout_us = 500000; + +int verbose_reports = 0; + void print_report(const uint8_t * buf, int len) { struct timeval tv; int i; struct report_data * data = (struct report_data *)buf; + uint64_t ts; if (len == 0) return; gettimeofday(&tv, NULL); + ts = tv.tv_sec * (uint64_t)1000000 + tv.tv_usec; if (buf[0] == 0xa2) //report from wii { @@ -36,7 +43,7 @@ void print_report(const uint8_t * buf, int len) { struct report_mode * rpt = (struct report_mode *)data->buf; - printf("(set reporting mode %u, cont: %u)", rpt->mode & 1, rpt->continuous & 1); + printf("(set reporting mode %02x, cont: %u)", rpt->mode, rpt->continuous & 1); break; } @@ -44,7 +51,8 @@ void print_report(const uint8_t * buf, int len) { struct report_leds * rpt = (struct report_leds *)data->buf; - printf("(set player leds %u %u %u %u)", rpt->led_1 & 1, rpt->led_2, rpt->led_3, rpt->led_4); + printf("(set player leds %u %u %u %u)", rpt->led_1 & 1, rpt->led_2 & 1, + rpt->led_3 & 1, rpt->led_4 & 1); break; } case 0x13: @@ -52,6 +60,7 @@ void print_report(const uint8_t * buf, int len) { struct report_ir_enable * rpt = (struct report_ir_enable *)data->buf; + printf("%02x %02x ", buf[2], buf[3]); printf("(set ir cam enable %u)", rpt->enabled & 1); break; @@ -129,41 +138,83 @@ void print_report(const uint8_t * buf, int len) { if (buf[1] < 0x30 || show_reports) { - printf("\e[2;37m%ld.%06ld \e[1;34mWiimote:\e[0m ", tv.tv_sec, tv.tv_usec); - printf("\e[33m%02x\e[0m \e[0;34m%02x %02x\e[0m ", buf[1], buf[2], buf[3]); - - switch(buf[1]) + if (ts >= next_report_ts) { - case 0x22: - printf("\e[0;35m%02x \e[0;31m%02x\e[0m ", buf[4], buf[5]); - printf("(ack report: %02x, res: %02x)", buf[4], buf[5]); - break; - case 0x21: - printf("\e[0;31m%02x \x1B[32m%02x %02x\e[0m ", buf[4], buf[5], buf[6]); - for (i = 7; i < len; i++) - { - printf("%02x ", buf[i]); - } - printf("(memory output)"); - break; - case 0x20: - printf("\e[0;35m%02x\e[0m ", buf[4]); - for (i = 5; i < len; i++) + printf("\e[2;37m%ld.%06ld \e[1;34mWiimote:\e[0m ", tv.tv_sec, tv.tv_usec); + printf("\e[33m%02x\e[0m \e[0;34m%02x %02x\e[0m ", buf[1], buf[2], buf[3]); + + switch(buf[1]) + { + case 0x22: + printf("\e[0;35m%02x \e[0;31m%02x\e[0m ", buf[4], buf[5]); + printf("(ack report: %02x, res: %02x)", buf[4], buf[5]); + break; + case 0x21: + printf("\e[0;31m%02x \x1B[32m%02x %02x\e[0m ", buf[4], buf[5], buf[6]); + for (i = 7; i < len; i++) + { + printf("%02x ", buf[i]); + } + printf("(memory output)"); + break; + case 0x20: + printf("\e[0;35m%02x\e[0m ", buf[4]); + for (i = 5; i < len; i++) + { + printf("%02x ", buf[i]); + } + printf("(status report)"); + break; + default: + for (i = 4; i < len; i++) + { + printf("%02x ", buf[i]); + } + } + + printf("\e[0m\n"); + + if (verbose_reports) + { + struct report_accelerometer * report_accel = (struct report_accelerometer *)(buf + 2); + printf(" accel %02x %02x, %02x %02x, %02x %02x\n", + report_accel->x, report_accel->buttons.accel_0 & 0x3, + report_accel->y, (report_accel->buttons.accel_1 & 0x1) << 1, + report_accel->z, (report_accel->buttons.accel_1 & 0x2)); + + if (buf[1] == 0x33) { - printf("%02x ", buf[i]); + struct report_ir_ext * report_ir = (struct report_ir_ext *)(buf + 2 + 5); + for (int i = 0; i < 4; i++) + { + printf(" object %d: %d, %d [%d]\n", i, + (((unsigned int)report_ir->obj[i].x_hi & 0x3) << 8 | report_ir->obj[i].x_lo), + (((unsigned int)report_ir->obj[i].y_hi & 0x3) << 8 | report_ir->obj[i].y_lo), + report_ir->obj[i].size); + } } - printf("(status report)"); - break; - default: - for (i = 4; i < len; i++) + + if (buf[1] == 0x37) { - printf("%02x ", buf[i]); + struct report_ir_basic * report_ir = (struct report_ir_basic *)(buf + 2 + 5); + printf(" object %d: %d, %d\n", 0, + (((unsigned int)report_ir->x1_hi & 0x3) << 8 | report_ir->x1_lo), + (((unsigned int)report_ir->y1_hi & 0x3) << 8 | report_ir->y1_lo)); + printf(" object %d: %d, %d\n", 1, + (((unsigned int)report_ir->x2_hi & 0x3) << 8 | report_ir->x2_lo), + (((unsigned int)report_ir->y2_hi & 0x3) << 8 | report_ir->y2_lo)); + printf(" object %d: %d, %d\n", 2, + (((unsigned int)report_ir->x3_hi & 0x3) << 8 | report_ir->x3_lo), + (((unsigned int)report_ir->y3_hi & 0x3) << 8 | report_ir->y3_lo)); + printf(" object %d: %d, %d\n", 3, + (((unsigned int)report_ir->x4_hi & 0x3) << 8 | report_ir->x4_lo), + (((unsigned int)report_ir->y4_hi & 0x3) << 8 | report_ir->y4_lo)); } - } - - printf("\e[0m\n"); + } - reports_truncated = 0; + reports_truncated = 0; + next_report_ts = ts + report_timeout_us; + } } else { diff --git a/wm_reports.c b/wm_reports.c index 5f6afbb..c4e76e2 100644 --- a/wm_reports.c +++ b/wm_reports.c @@ -86,7 +86,8 @@ void report_queue_push_status(struct wiimote_state * state) status->battery_level = state->sys.battery_level; } -void report_format_mem_resp(struct report * rpt, int size, int error, uint16_t addr, uint8_t * buf) +void report_format_mem_resp(struct wiimote_state * state, struct report * rpt, + int size, int error, uint16_t addr, uint8_t * buf, bool encrypt) { struct report_mem_resp * resp = (struct report_mem_resp *)rpt->data.buf; @@ -94,12 +95,17 @@ void report_format_mem_resp(struct report * rpt, int size, int error, uint16_t a rpt->data.io = 0xa1; rpt->data.type = 0x21; - resp->size = size-1; + resp->size = size - 1; resp->error = error; resp->addr = htons(addr); if (buf != NULL) //buf will be null for error reports { memcpy(resp->data, buf, size); + + if (encrypt) + { + ext_encrypt_bytes(&state->sys.extension_crypto_state, resp->data, addr & 0x7, size); + } } } @@ -228,45 +234,49 @@ void report_append_interleaved(struct wiimote_state * state, uint8_t * buf) void report_append_extension(struct wiimote_state * state, uint8_t * buf, uint8_t bytes) { - //a600fe = 0x04 activate motionplus, 0x05 activate nunchuk passthrough, 0x07 activate classic passthrough - //if no other extension, send 0x20 + //a600fe = 0x04 activate motionplus, 0x05 activate nunchuk passthrough, 0x07 activate classic passthrough + //if no other extension, send 0x20 - //a600f0 = 0x55 deactivate motionplus - //send report 0x20 twice (once for unplugged, once for plugged in) + //a600f0 = 0x55 deactivate motionplus + //send report 0x20 twice (once for unplugged, once for plugged in) - //0xa400fa contents - //0000 A420 0000 Nunchuk - //0000 A420 0101 Classic - //0000 A420 0405 WMP - //0000 A420 0505 WMP nunchuk - //0000 A420 0705 WMP classic + //0xa400fa contents + //0000 A420 0000 Nunchuk + //0000 A420 0101 Classic + //0000 A420 0405 WMP + //0000 A420 0505 WMP nunchuk + //0000 A420 0705 WMP classic - //0000 A620 0005 Inactive WMP - // ^=6 Deactivated WMP + //0000 A620 0005 Inactive WMP + // ^=6 Deactivated WMP - //memset(buf + 6, 0, sizeof(uint8_t) * (bytes - 6)); - //memset(buf + offset + 6, 0, sizeof(uint8_t) * (bytes - 6)); + //these should be set to the the address offset of the extension data + //and the length in bytes of the extesnion data + //right now, they are always the same in all situations + int addr_offset = 0x08, length = 6; switch (state->sys.extension_report_type) { - case 0x01: //nunchuck + case 0x00: //nunchuk { - struct report_ext_nunchuck * rpt = (struct report_ext_nunchuck *)buf; - - rpt->x = state->usr.nunchuck.x; - rpt->y = state->usr.nunchuck.y; - rpt->accel_x_hi = state->usr.nunchuck.accel_x >> 2; - rpt->accel_y_hi = state->usr.nunchuck.accel_y >> 2; - rpt->accel_z_hi = state->usr.nunchuck.accel_z >> 2; - rpt->accel_x_lo = state->usr.nunchuck.accel_x; - rpt->accel_y_lo = state->usr.nunchuck.accel_y; - rpt->accel_z_lo = state->usr.nunchuck.accel_z; - rpt->c = state->usr.nunchuck.c; - rpt->z = state->usr.nunchuck.z; + struct report_ext_nunchuk * rpt = (struct report_ext_nunchuk *)buf; + + rpt->x = state->usr.nunchuk.x; + rpt->y = state->usr.nunchuk.y; + + rpt->accel_x_hi = state->usr.nunchuk.accel_x >> 2; + rpt->accel_y_hi = state->usr.nunchuk.accel_y >> 2; + rpt->accel_z_hi = state->usr.nunchuk.accel_z >> 2; + rpt->accel_x_lo = state->usr.nunchuk.accel_x; + rpt->accel_y_lo = state->usr.nunchuk.accel_y; + rpt->accel_z_lo = state->usr.nunchuk.accel_z; + + rpt->c = !state->usr.nunchuk.c; + rpt->z = !state->usr.nunchuk.z; break; } - case 0x02: //classic + case 0x01: //classic { struct report_ext_classic * rpt = (struct report_ext_classic *)buf; @@ -321,7 +331,7 @@ void report_append_extension(struct wiimote_state * state, uint8_t * buf, uint8_ break; } - case 0x05: //motionplus + nunchuck + case 0x05: //motionplus + nunchuk if (state->sys.extension_report) { struct report_ext_motionplus * rpt = (struct report_ext_motionplus *)buf; @@ -344,18 +354,20 @@ void report_append_extension(struct wiimote_state * state, uint8_t * buf, uint8_ } else { - struct report_ext_nunchuck_pt * rpt = (struct report_ext_nunchuck_pt *)buf; - - rpt->x = state->usr.nunchuck.x; - rpt->y = state->usr.nunchuck.y; - rpt->accel_x_hi = state->usr.nunchuck.accel_x >> 2; - rpt->accel_y_hi = state->usr.nunchuck.accel_y >> 2; - rpt->accel_z_hi = state->usr.nunchuck.accel_z >> 3; - rpt->accel_x_lo = state->usr.nunchuck.accel_x >> 1; - rpt->accel_y_lo = state->usr.nunchuck.accel_y >> 1; - rpt->accel_z_lo = state->usr.nunchuck.accel_z >> 1; - rpt->c = state->usr.nunchuck.c; - rpt->z = state->usr.nunchuck.z; + struct report_ext_nunchuk_pt * rpt = (struct report_ext_nunchuk_pt *)buf; + + rpt->x = state->usr.nunchuk.x; + rpt->y = state->usr.nunchuk.y; + + rpt->accel_x_hi = state->usr.nunchuk.accel_x >> 2; + rpt->accel_y_hi = state->usr.nunchuk.accel_y >> 2; + rpt->accel_z_hi = state->usr.nunchuk.accel_z >> 3; + rpt->accel_x_lo = state->usr.nunchuk.accel_x >> 1; + rpt->accel_y_lo = state->usr.nunchuk.accel_y >> 1; + rpt->accel_z_lo = state->usr.nunchuk.accel_z >> 1; + + rpt->c = !state->usr.nunchuk.c; + rpt->z = !state->usr.nunchuk.z; rpt->ext = 1; @@ -422,16 +434,8 @@ void report_append_extension(struct wiimote_state * state, uint8_t * buf, uint8_ } - if (state->sys.extension_encrypted) //encryption required + if (state->sys.extension_encrypted) { - int i; - //if crypto problems arise, try encrypting all the bytes - //only the 6 containing data are done now - for (i=0;i<6;i++) - { - //buf[i] = (buf[i] - ft[(0x08 + i)%8]) ^ sb[(0x08 + i)%8]; - //above is technically the full operation, below is equivalent (as of now) - buf[i] = (buf[i] - ft[i]) ^ sb[i]; - } + ext_encrypt_bytes(&state->sys.extension_crypto_state, buf, addr_offset, length); } } diff --git a/wm_reports.h b/wm_reports.h index 7a5c3c7..8d8d1ed 100644 --- a/wm_reports.h +++ b/wm_reports.h @@ -70,13 +70,14 @@ struct report_ir_basic uint8_t x2_lo; uint8_t y2_lo; + uint8_t x3_lo; uint8_t y3_lo; - int x3_hi:2; - int y3_hi:2; int x4_hi:2; int y4_hi:2; + int x3_hi:2; + int y3_hi:2; uint8_t x4_lo; uint8_t y4_lo; @@ -128,7 +129,7 @@ struct report_interleaved struct report_ir_full_obj obj[2]; } __attribute__((packed)); -struct report_ext_nunchuck +struct report_ext_nunchuk { uint8_t x; uint8_t y; @@ -143,7 +144,7 @@ struct report_ext_nunchuck int accel_z_lo:2; } __attribute__((packed)); -struct report_ext_nunchuck_pt +struct report_ext_nunchuk_pt { //different format if in passthrough mode with motionplus uint8_t x; @@ -304,8 +305,8 @@ struct report_leds int unused:3; int led_1:1; int led_2:1; - int led_4:1; int led_3:1; + int led_4:1; } __attribute__((packed)); struct report_mode @@ -321,7 +322,7 @@ struct report_mode struct report_ir_enable { int rumble:1; - int unused1:1; + int ack_requested:1; int enabled:1; int unused0:5; } __attribute__((packed)); @@ -367,7 +368,8 @@ void report_queue_pop(struct wiimote_state * state); void report_queue_push_ack(struct wiimote_state *state, uint8_t report, uint8_t result); void report_queue_push_status(struct wiimote_state * state); -void report_format_mem_resp(struct report * rpt, int size, int error, uint16_t addr, uint8_t * buf); +void report_format_mem_resp(struct wiimote_state * state, struct report * rpt, + int size, int error, uint16_t addr, uint8_t * buf, bool encrypt); void report_append_buttons(struct wiimote_state * state, uint8_t * buf); void report_append_accelerometer(struct wiimote_state * state, uint8_t * buf); diff --git a/wmemulator.c b/wmemulator.c index 94a748d..c01418c 100644 --- a/wmemulator.c +++ b/wmemulator.c @@ -14,15 +14,25 @@ #include #include +#include "sdp.h" #include "wiimote.h" #include "input.h" +#include "input_sdl.h" +#include "input_socket.h" +#include "adapter.h" +#include "wm_print.h" -#define CTRL 17 -#define DATA 19 +#define PSM_SDP 1 +#define PSM_CTRL 0x11 +#define PSM_INT 0x13 -//address, sockets -char * bdaddr = NULL; -int ctrl, data; +bdaddr_t host_bdaddr; +int has_host = 0; + +int sdp_fd, ctrl_fd, int_fd; +int sock_sdp_fd, sock_ctrl_fd, sock_int_fd; + +static int is_connected = 0; //signal handler to break out of main loop static int running = 1; @@ -31,7 +41,7 @@ void sig_handler(int sig) running = 0; } -int createsocket() +int create_socket() { int fd; struct linger l = { .l_onoff = 1, .l_linger = 5 }; @@ -49,6 +59,12 @@ int createsocket() return -1; } + if (setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &opt, sizeof(opt)) < 0) + { + close(fd); + return -1; + } + if (setsockopt(fd, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) { close(fd); @@ -58,12 +74,12 @@ int createsocket() return fd; } -int l2connect(const char *bdaddr, int psm) +int l2cap_connect(bdaddr_t bdaddr, int psm) { int fd; struct sockaddr_l2 addr; - fd = createsocket(); + fd = create_socket(); if (fd < 0) { return -1; @@ -72,9 +88,9 @@ int l2connect(const char *bdaddr, int psm) memset(&addr, 0, sizeof(addr)); addr.l2_family = AF_BLUETOOTH; addr.l2_psm = htobs(psm); - str2ba(bdaddr, &addr.l2_bdaddr); + addr.l2_bdaddr = bdaddr; - if ( connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) + if (connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { close(fd); return -1; @@ -83,14 +99,12 @@ int l2connect(const char *bdaddr, int psm) return fd; } -int l2accept(int psm) +int l2cap_listen(int psm) { - int fd, wiifd; - struct sockaddr_l2 addr, wiiaddr; - socklen_t opt = sizeof(wiiaddr); - char buf[18]; + int fd; + struct sockaddr_l2 addr; - fd = createsocket(); + fd = create_socket(); if (fd < 0) { return -1; @@ -101,225 +115,390 @@ int l2accept(int psm) addr.l2_psm = htobs(psm); addr.l2_bdaddr = *BDADDR_ANY; - if ( bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0 ) + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { close(fd); return -1; } - if ( listen(fd, 1) < 0 ) + if (listen(fd, 1) < 0) { close(fd); return -1; } - wiifd = accept(fd, (struct sockaddr *)&wiiaddr, &opt); - ba2str( &wiiaddr.l2_bdaddr, buf ); - printf("accepted connection from %s.\n", buf); - - close(fd); - - return wiifd; + return fd; } -void waitforwii() +int listen_for_connections() { - - printf("waiting for connection on psm %d...\n", CTRL); - ctrl = l2accept(CTRL); - - if ( ctrl < 0 ) +#ifdef SDP_SERVER + sock_sdp_fd = l2cap_listen(PSM_SDP); + if (sock_sdp_fd < 0) { - printf("can't connect to psm %d: %s\n", CTRL, strerror(errno)); - running = 0; - return; + printf("can't listen on psm %d: %s\n", PSM_SDP, strerror(errno)); + return -1; } +#endif - printf("waiting for connection on psm %d...\n", DATA); - data = l2accept(DATA); + sock_ctrl_fd = l2cap_listen(PSM_CTRL); + if (sock_ctrl_fd < 0) + { + printf("can't listen on psm %d: %s\n", PSM_CTRL, strerror(errno)); + return -1; + } - if (data < 0) + sock_int_fd = l2cap_listen(PSM_INT); + if (sock_int_fd < 0) { - printf("can't connect to psm %d: %s\n", DATA, strerror(errno)); - running = 0; - return; + printf("can't listen on psm %d: %s\n", PSM_INT, strerror(errno)); + return -1; } - printf("connected.\n"); + return 0; } - -void connecttowii() +int accept_connection(int socket_fd, bdaddr_t * bdaddr) { + int fd; + struct sockaddr_l2 addr; + socklen_t opt = sizeof(addr); - printf("connecting to %s psm %d\n", bdaddr, CTRL); - ctrl = l2connect(bdaddr, CTRL); + fd = accept(socket_fd, (struct sockaddr *)&addr, &opt); + if (fd < 0) + { + return -1; + } - if (ctrl < 0) + if (bdaddr != NULL) { - printf("can't connect to %s psm %d: %s\n", bdaddr, CTRL, strerror(errno)); - running = 0; - return; + *bdaddr = addr.l2_bdaddr; } - printf("connecting to %s psm %d\n", bdaddr, DATA); - data = l2connect(bdaddr, DATA); + return fd; +} - if (data < 0) +int connect_to_host() +{ + ctrl_fd = l2cap_connect(host_bdaddr, PSM_CTRL); + if (ctrl_fd < 0) { - printf("can't connect to %s psm %d: %s\n", bdaddr, DATA, strerror(errno)); - running = 0; - return; + printf("can't connect to host psm %d: %s\n", PSM_CTRL, strerror(errno)); + return -1; } - printf("connected.\n"); + int_fd = l2cap_connect(host_bdaddr, PSM_INT); + if (int_fd < 0) + { + printf("can't connect to host psm %d: %s\n", PSM_INT, strerror(errno)); + return -1; + } + return 0; } void disconnect() { - shutdown(ctrl, SHUT_RDWR); - shutdown(data, SHUT_RDWR); - close(ctrl); - close(data); + shutdown(sdp_fd, SHUT_RDWR); + shutdown(ctrl_fd, SHUT_RDWR); + shutdown(int_fd, SHUT_RDWR); + + close(sdp_fd); + close(ctrl_fd); + close(int_fd); + + sdp_fd = 0; + ctrl_fd = 0; + int_fd = 0; +} + +void print_usage(char *argv0) +{ + printf("usage: %s [ [ gui | unix | ip ] ]\n", argv0); } int main(int argc, char *argv[]) { - struct pollfd pfd[2]; - unsigned char buf[32]; - ssize_t len; + struct input_source input_source; - //struct timespec timeout; - //timeout.tv_sec = 0; - //timeout.tv_nsec = 0; + struct pollfd pfd[6]; + unsigned char buf[256]; + ssize_t len; struct wiimote_state state; - int send_report_now = 0; + int send_report_now = 1; + int input_result; int failure = 0; if (argc > 1) { - bdaddr = argv[1]; - - if (bachk(bdaddr) < 0) + if (strcmp(argv[1], "pair") == 0) + { + // Act as if nothing given. + } + else if (bachk(argv[1]) >= 0) + { + str2ba(argv[1], &host_bdaddr); + has_host = 1; + } + else { - printf("usage: %s \n", *argv); - return 1; + print_usage(*argv); + return 1; } } + if (argc <= 2 || strcmp(argv[2], "gui") == 0) + { + input_sdl_init(); + input_source = input_source_sdl; + } + else if (argc > 3 && strcmp(argv[2], "unix") == 0) + { + input_socket_init_unix_at_path(argv[3]); + input_source = input_source_socket; + } + else if (argc > 3 && strcmp(argv[2], "ip") == 0) + { + input_socket_init_ip_on_port(argv[3]); + input_source = input_source_socket; + } + else + { + print_usage(*argv); + return 1; + } //set up unload signals signal(SIGINT, sig_handler); + signal(SIGTERM, sig_handler); signal(SIGHUP, sig_handler); - input_init(); + if (set_up_device(NULL) < 0) + { + printf("failed to set up Bluetooth device\n"); + return 1; + } + +#ifndef SDP_SERVER + if (register_wiimote_sdp_record() < 0) + { + printf("failed to add Wiimote SDP record\n"); + restore_device(); + return 1; + } +#endif - init_wiimote(&state); + wiimote_init(&state); - if (bdaddr == NULL) + if (has_host) { - waitforwii(); + printf("connecting to host...\n"); + if (connect_to_host() < 0) + { + printf("couldn't connect\n"); + running = 0; + } + else + { + char straddr[18]; + ba2str(&host_bdaddr, straddr); + printf("connected to %s\n", straddr); + + is_connected = 1; + } } else { - connecttowii(); + if (listen_for_connections() < 0) + { + printf("couldn't listen\n"); + running = 0; + } + else + { + printf("listening for connections... (press wii's sync button)\n"); + } } - while (running) { - memset(&pfd, 0, sizeof(pfd)); - // Listen for data on either fd - //setting this to zero is not required for every call... - //... also POLLERR has no effect in the events field - pfd[0].fd = ctrl; - pfd[0].events = POLLIN | POLLERR; - pfd[1].fd = data; - pfd[1].events = POLLIN | POLLERR; + pfd[0].fd = sock_sdp_fd; + pfd[1].fd = sock_ctrl_fd; + pfd[2].fd = sock_int_fd; - // Check data PSM for output if it's time to send a report - if (1 || send_report_now) + pfd[3].fd = sdp_fd; + + pfd[4].fd = ctrl_fd; + pfd[5].fd = int_fd; + + if (!is_connected) + { + pfd[0].events = POLLIN; + pfd[1].events = POLLIN; + pfd[2].events = POLLIN; + + pfd[3].events = POLLIN | POLLOUT; + } + else { - pfd[1].events |= POLLOUT; + pfd[4].events = POLLIN; + pfd[5].events = POLLIN; + + pfd[5].events |= POLLOUT; } - if (poll(pfd, 2, 0) < 0) + if (poll(pfd, 6, 20) < 0) { - printf("ppoll\n"); + printf("poll error\n"); break; } - if (pfd[0].revents & POLLERR) + if (pfd[4].revents & POLLERR) { printf("error on ctrl psm\n"); break; } - if (pfd[1].revents & POLLERR) + if (pfd[5].revents & POLLERR) { printf("error on data psm\n"); break; } - + if (pfd[0].revents & POLLIN) + { + sdp_fd = accept_connection(pfd[0].fd, NULL); + if (sdp_fd < 0) + { + printf("error accepting sdp connection\n"); + break; + } + } if (pfd[1].revents & POLLIN) { - len = recv(data, buf, 32, MSG_DONTWAIT); + ctrl_fd = accept_connection(pfd[1].fd, NULL); + if (ctrl_fd < 0) + { + printf("error accepting ctrl connection\n"); + break; + } + } + if (pfd[2].revents & POLLIN) + { + int_fd = accept_connection(pfd[2].fd, &host_bdaddr); + if (int_fd < 0) + { + printf("error accepting int connection\n"); + break; + } + + char straddr[18]; + ba2str(&host_bdaddr, straddr); + printf("connected to %s\n", straddr); + + is_connected = 1; + has_host = 1; + } + if (pfd[3].revents & POLLIN) + { + len = recv(sdp_fd, buf, 32, MSG_DONTWAIT); if (len > 0) { + sdp_recv_data(buf, len); + } + } + if (pfd[3].revents & POLLOUT) + { + len = sdp_get_data(buf); + if (len > 0) + { + send(sdp_fd, buf, len, MSG_DONTWAIT); + } + } + + if (pfd[5].revents & POLLIN) + { + len = recv(int_fd, buf, 32, MSG_DONTWAIT); + if (len > 0) + { + print_report(buf, len); process_report(&state, buf, len); } } - //send report - if (1 || send_report_now) + input_result = input_update(&state, &input_source); + if (input_result) { - //process input - input_update(&state); + running = 0; + if (input_result == -2) + { + power_off_host(&host_bdaddr); + } + else + { + //disconnect(&host_bdaddr); + disconnect(); + } + } - if (pfd[1].revents & POLLOUT) + if (is_connected && send_report_now) + { + if (pfd[5].revents & POLLOUT) { len = generate_report(&state, buf); - send(data, buf, len, MSG_DONTWAIT); - send_report_now = 0; + if (len > 0) + { + print_report(buf, len); + send(int_fd, buf, len, MSG_DONTWAIT); + } failure = 0; } else { - printf("failure \n"); - failure += 1; - if (failure >= 4) + if (++failure > 5) { + printf("connection timed out, attemping to reconnect...\n"); disconnect(); - destroy_wiimote(&state); - init_wiimote(&state); - connecttowii(); + is_connected = 0; } - send_report_now = 0; - - usleep(200*1000); } - - /* Schedule next report in 20000us = 20ms*/ - } - usleep(20*1000); - + if (has_host && !is_connected) + { + if (connect_to_host() < 0) + { + usleep(500*1000); + } + else + { + printf("connected to host\n"); + is_connected = 1; + } + } } printf("cleaning up...\n"); disconnect(); - destroy_wiimote(&state); + close(sock_sdp_fd); + close(sock_ctrl_fd); + close(sock_int_fd); - input_unload(); + restore_device(); + +#ifndef SDP_SERVER + unregister_wiimote_sdp_record(); +#endif + + wiimote_destroy(&state); + input_source.unload(); return 0; } + diff --git a/wmmitm.c b/wmmitm.c new file mode 100644 index 0000000..f514a0a --- /dev/null +++ b/wmmitm.c @@ -0,0 +1,580 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sdp.h" +#include "adapter.h" +#include "wm_print.h" + +#define PSM_SDP 1 +#define PSM_CTRL 0x11 +#define PSM_INT 0x13 + +bdaddr_t host_device_bdaddr; +bdaddr_t wiimote_device_bdaddr; +bdaddr_t host_bdaddr; +bdaddr_t wiimote_bdaddr; + +int sdp_fd, ctrl_fd, int_fd; +int wm_ctrl_fd, wm_int_fd; +int sock_sdp_fd, sock_ctrl_fd, sock_int_fd; + +extern int show_reports; + +static int has_host = 0; +static int is_connected = 0; + +//signal handler to break out of main loop +static int running = 1; +void sig_handler(int sig) +{ + running = 0; +} + +int create_socket() +{ + int fd; + struct linger l = { .l_onoff = 1, .l_linger = 5 }; + int opt = 0; + + fd = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + if (fd < 0) + { + return -1; + } + + if (setsockopt(fd, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) + { + close(fd); + return -1; + } + + if (setsockopt(fd, SOL_SOCKET, SO_SNDBUF, &opt, sizeof(opt)) < 0) + { + close(fd); + return -1; + } + + if (setsockopt(fd, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) + { + close(fd); + return -1; + } + + return fd; +} + +int l2cap_connect(bdaddr_t device_bdaddr, bdaddr_t bdaddr, int psm) +{ + int fd; + struct sockaddr_l2 addr; + + fd = create_socket(); + if (fd < 0) + { + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = htobs(psm); + addr.l2_bdaddr = device_bdaddr; + + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + { + close(fd); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = htobs(psm); + addr.l2_bdaddr = bdaddr; + + if (connect(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + { + close(fd); + return -1; + } + + return fd; +} + +int l2cap_listen(bdaddr_t device_bdaddr, int psm) +{ + int fd; + struct sockaddr_l2 addr; + + fd = create_socket(); + if (fd < 0) + { + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = htobs(psm); + addr.l2_bdaddr = device_bdaddr; + + if (bind(fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) + { + close(fd); + return -1; + } + + if (listen(fd, 1) < 0) + { + close(fd); + return -1; + } + + return fd; +} + +int listen_for_connections() +{ +#ifdef SDP_SERVER + sock_sdp_fd = l2cap_listen(host_device_bdaddr, PSM_SDP); + if (sock_sdp_fd < 0) + { + printf("can't listen on psm %d: %s\n", PSM_SDP, strerror(errno)); + return -1; + } +#endif + + sock_ctrl_fd = l2cap_listen(host_device_bdaddr, PSM_CTRL); + if (sock_ctrl_fd < 0) + { + printf("can't listen on psm %d: %s\n", PSM_CTRL, strerror(errno)); + return -1; + } + + sock_int_fd = l2cap_listen(host_device_bdaddr, PSM_INT); + if (sock_int_fd < 0) + { + printf("can't listen on psm %d: %s\n", PSM_INT, strerror(errno)); + return -1; + } + + return 0; +} + +int accept_connection(int socket_fd, bdaddr_t * bdaddr) +{ + int fd; + struct sockaddr_l2 addr; + socklen_t opt = sizeof(addr); + + fd = accept(socket_fd, (struct sockaddr *)&addr, &opt); + if (fd < 0) + { + return -1; + } + + if (bdaddr != NULL) + { + *bdaddr = addr.l2_bdaddr; + } + + return fd; +} + +int connect_to_host() +{ + ctrl_fd = l2cap_connect(host_device_bdaddr, host_bdaddr, PSM_CTRL); + if (ctrl_fd < 0) + { + printf("can't connect to host psm %d: %s\n", PSM_CTRL, strerror(errno)); + return -1; + } + + int_fd = l2cap_connect(host_device_bdaddr, host_bdaddr, PSM_INT); + if (int_fd < 0) + { + printf("can't connect to host psm %d: %s\n", PSM_INT, strerror(errno)); + return -1; + } + + return 0; +} + +int connect_to_wiimote() +{ + wm_ctrl_fd = l2cap_connect(wiimote_device_bdaddr, wiimote_bdaddr, PSM_CTRL); + if (wm_ctrl_fd < 0) + { + printf("can't connect to wiimote psm %d: %s\n", PSM_CTRL, strerror(errno)); + return -1; + } + + wm_int_fd = l2cap_connect(wiimote_device_bdaddr, wiimote_bdaddr, PSM_INT); + if (wm_int_fd < 0) + { + printf("can't connect to wiimote psm %d: %s\n", PSM_INT, strerror(errno)); + return -1; + } + + return 0; +} + +void disconnect_from_host() +{ + shutdown(sdp_fd, SHUT_RDWR); + shutdown(ctrl_fd, SHUT_RDWR); + shutdown(int_fd, SHUT_RDWR); + + close(sdp_fd); + close(ctrl_fd); + close(int_fd); + + sdp_fd = 0; + ctrl_fd = 0; + int_fd = 0; +} + +void disconnect_from_wiimote() +{ + shutdown(wm_ctrl_fd, SHUT_RDWR); + shutdown(wm_int_fd, SHUT_RDWR); + + close(wm_ctrl_fd); + close(wm_int_fd); + + wm_ctrl_fd = 0; + wm_int_fd = 0; +} + +int main(int argc, char *argv[]) +{ + struct pollfd pfd[8]; + + unsigned char buf[256]; + ssize_t len; + unsigned char in_buf[256]; + ssize_t in_buf_len = 0; + unsigned char out_buf[256]; + ssize_t out_buf_len = 0; + + int failure = 0; + + int enable_report_printing = 0; + show_reports = 1; + + if (argc > 1) + { + if (bachk(argv[1]) < 0) + { + printf("usage: %s \n", *argv); + return 1; + } + + str2ba(argv[1], &wiimote_bdaddr); + + if (argc > 2) + { + if (bachk(argv[2]) < 0) + { + printf("usage: %s \n", *argv); + return 1; + } + + str2ba(argv[2], &host_bdaddr); + has_host = 1; + } + } + + //set up unload signals + signal(SIGINT, sig_handler); + signal(SIGTERM, sig_handler); + signal(SIGHUP, sig_handler); + + if (set_up_device(NULL) < 0) + { + printf("failed to set up Bluetooth device\n"); + return 1; + } + + if (get_device_bdaddr(0, &host_device_bdaddr) < 0) + { + printf("failed to get host Bluetooth adapter address\n"); + restore_device(); + return 1; + } + + if (get_device_bdaddr(1, &wiimote_device_bdaddr) < 0) + { + printf("failed to get Wiimote Bluetooth adapter address\n"); + printf("Warning: two Bluetooth adapters are required for proper functionality\n"); + wiimote_device_bdaddr = host_device_bdaddr; + } + +#ifndef SDP_SERVER + if (register_wiimote_sdp_record() < 0) + { + printf("failed to add Wiimote SDP record\n"); + restore_device(); + return 1; + } +#endif + + printf("connecting to wiimote... (press wiimote's sync button)\n"); + + while (!bacmp(&wiimote_bdaddr, BDADDR_ANY)) + { + if (failure++ > 3) + { + printf("couldn't find a wiimote to connect to\n"); + restore_device(); + return 1; + } + + find_wiimote(&wiimote_bdaddr); + } + + if (connect_to_wiimote() < 0) + { + printf("failed to connect to wiimote\n"); + restore_device(); + return 1; + } + + if (has_host) + { + printf("connecting to host...\n"); + if (connect_to_host() < 0) + { + printf("couldn't connect to host\n"); + running = 0; + } + else + { + char straddr[18]; + ba2str(&host_bdaddr, straddr); + printf("connected to host %s\n", straddr); + + is_connected = 1; + } + } + else + { + if (listen_for_connections() < 0) + { + printf("couldn't listen\n"); + running = 0; + } + else + { + printf("listening for host connections... (press wii's sync button)\n"); + } + } + + while (running) + { + memset(&pfd, 0, sizeof(pfd)); + + pfd[0].fd = sock_sdp_fd; + pfd[1].fd = sock_ctrl_fd; + pfd[2].fd = sock_int_fd; + + pfd[3].fd = sdp_fd; + + pfd[4].fd = ctrl_fd; + pfd[5].fd = int_fd; + + pfd[6].fd = wm_ctrl_fd; + pfd[7].fd = wm_int_fd; + + if (!is_connected) + { + pfd[0].events = POLLIN; + pfd[1].events = POLLIN; + pfd[2].events = POLLIN; + + pfd[3].events = POLLIN | POLLOUT; + } + else + { + pfd[4].events = POLLIN; + pfd[5].events = POLLIN; + pfd[6].events = POLLIN; + pfd[7].events = POLLIN; + + if (in_buf_len > 0) + { + pfd[5].events |= POLLOUT; + } + if (out_buf_len > 0) + { + pfd[7].events |= POLLOUT; + } + } + + if (poll(pfd, 8, 10) < 0) + { + printf("poll error\n"); + break; + } + + if (pfd[4].revents & POLLERR) + { + printf("error on ctrl psm\n"); + break; + } + if (pfd[5].revents & POLLERR) + { + printf("error on data psm\n"); + break; + } + if (pfd[6].revents & POLLERR) + { + printf("error on ctrl psm\n"); + break; + } + if (pfd[7].revents & POLLERR) + { + printf("error on data psm\n"); + break; + } + + if (pfd[0].revents & POLLIN) + { + sdp_fd = accept_connection(pfd[0].fd, NULL); + if (sdp_fd < 0) + { + printf("error accepting sdp connection\n"); + break; + } + } + if (pfd[1].revents & POLLIN) + { + ctrl_fd = accept_connection(pfd[1].fd, NULL); + if (ctrl_fd < 0) + { + printf("error accepting ctrl connection\n"); + break; + } + } + if (pfd[2].revents & POLLIN) + { + int_fd = accept_connection(pfd[2].fd, &host_bdaddr); + if (int_fd < 0) + { + printf("error accepting int connection\n"); + break; + } + + char straddr[18]; + ba2str(&host_bdaddr, straddr); + printf("connected to %s\n", straddr); + + is_connected = 1; + has_host = 1; + } + + if (pfd[3].revents & POLLIN) + { + len = recv(sdp_fd, buf, 32, MSG_DONTWAIT); + if (len > 0) + { + sdp_recv_data(buf, len); + } + } + if (pfd[3].revents & POLLOUT) + { + len = sdp_get_data(buf); + if (len > 0) + { + send(sdp_fd, buf, len, MSG_DONTWAIT); + } + } + + if (is_connected) + { + if (out_buf_len == 0 && (pfd[5].revents & POLLIN)) + { + out_buf_len = recv(int_fd, out_buf, 32, MSG_DONTWAIT); + if (enable_report_printing) + { + print_report(out_buf, out_buf_len); + } + } + if (pfd[5].revents & POLLOUT) + { + if (in_buf_len > 0) + { + send(int_fd, in_buf, in_buf_len, MSG_DONTWAIT); + in_buf_len = 0; + } + + failure = 0; + } + // else + // { + // if (++failure > 3) + // { + // printf("connection timed out, attemping to reconnect...\n"); + // disconnect_from_host(); + // is_connected = 0; + // } + + // usleep(20*1000*1000); + // } + } + + if (in_buf_len == 0 && (pfd[7].revents & POLLIN)) + { + in_buf_len = recv(wm_int_fd, in_buf, 32, MSG_DONTWAIT); + if (enable_report_printing) + { + print_report(in_buf, in_buf_len); + } + } + if (out_buf_len > 0 && (pfd[7].revents & POLLOUT)) + { + send(wm_int_fd, out_buf, out_buf_len, MSG_DONTWAIT); + out_buf_len = 0; + } + + if (has_host && !is_connected) + { + if (connect_to_host() < 0) + { + usleep(500*1000); + } + else + { + printf("connected to host\n"); + is_connected = 1; + } + } + } + + printf("cleaning up...\n"); + + disconnect_from_host(); + disconnect_from_wiimote(); + + close(sock_sdp_fd); + close(sock_ctrl_fd); + close(sock_int_fd); + + restore_device(); + +#ifndef SDP_SERVER + unregister_wiimote_sdp_record(); +#endif + + return 0; +}