From a8006820c2aeabeafcdf3012ecfb669707fcc0d9 Mon Sep 17 00:00:00 2001 From: Wolfywolfy Date: Thu, 5 Dec 2024 08:10:08 -0300 Subject: [PATCH] Add DS5 Support. Co-authored-by: Rick Gaiser --- Makefile | 4 +- modules/pademu/Makefile | 5 + modules/pademu/ds3bt.c | 14 +- modules/pademu/ds4bt.c | 6 +- modules/pademu/ds5bt.c | 1339 ++++++++++++++++++++++++++++++++ modules/pademu/ds5bt.h | 59 ++ modules/pademu/ds5usb.c | 552 +++++++++++++ modules/pademu/ds5usb.h | 44 ++ modules/pademu/pademu.c | 15 + modules/pademu/pademu_common.h | 64 ++ 10 files changed, 2090 insertions(+), 12 deletions(-) create mode 100644 modules/pademu/ds5bt.c create mode 100644 modules/pademu/ds5bt.h create mode 100644 modules/pademu/ds5usb.c create mode 100644 modules/pademu/ds5usb.h diff --git a/Makefile b/Makefile index 2ff13f921..9bac9f1e2 100644 --- a/Makefile +++ b/Makefile @@ -328,7 +328,7 @@ clean: download_lwNBD echo " -ds34bt" $(MAKE) -C modules/ds34bt clean echo " -pademu" - $(MAKE) -C modules/pademu USE_DS3=1 USE_DS4=1 VMC=1 clean + $(MAKE) -C modules/pademu USE_DS3=1 USE_DS4=1 USE_DS5=1 VMC=1 clean echo "-pc tools" $(MAKE) -C pc clean @@ -521,7 +521,7 @@ $(EE_ASM_DIR)ds34usb.c: modules/ds34usb/iop/ds34usb.irx | $(EE_ASM_DIR) $(BIN2C) $< $@ $(*F)_irx modules/pademu/pademu.irx: modules/pademu - $(MAKE) USE_DS3=1 USE_DS4=1 VMC=1 -C $< all + $(MAKE) USE_DS3=1 USE_DS4=1 USE_DS5=1 VMC=1 -C $< all $(EE_ASM_DIR)pademu.c: modules/pademu/pademu.irx $(BIN2C) $< $@ $(*F)_irx diff --git a/modules/pademu/Makefile b/modules/pademu/Makefile index 219894a96..4e8f46e05 100644 --- a/modules/pademu/Makefile +++ b/modules/pademu/Makefile @@ -12,6 +12,11 @@ IOP_CFLAGS += -DUSE_DS4 IOP_OBJS += ds4usb.o ds4bt.o endif +ifeq ($(USE_DS5),1) +IOP_CFLAGS += -DUSE_DS5 +IOP_OBJS += ds5usb.o ds5bt.o +endif + ifeq ($(VMC),1) IOP_CFLAGS += -DVMC endif diff --git a/modules/pademu/ds3bt.c b/modules/pademu/ds3bt.c index 2c9280cd5..33e59ed56 100644 --- a/modules/pademu/ds3bt.c +++ b/modules/pademu/ds3bt.c @@ -671,12 +671,13 @@ static void ds3pad_clear(int pad) ds3pad[pad].data[1] = 0xFF; mips_memset(&ds3pad[pad].data[2], 0x7F, 4); mips_memset(&ds3pad[pad].data[6], 0x00, 12); - padf[pad].priv = &ds3pad[pad]; - padf[pad].get_status = ds3bt_get_status; - padf[pad].get_model = ds3bt_get_model; - padf[pad].get_data = ds3bt_get_data; - padf[pad].set_rumble = ds3bt_set_rumble; - padf[pad].set_mode = ds3bt_set_mode; + pademu_connect(&padf[pad]); + padf[pad].priv = &ds4pad[pad]; + padf[pad].get_status = ds4bt_get_status; + padf[pad].get_model = ds4bt_get_model; + padf[pad].get_data = ds4bt_get_data; + padf[pad].set_rumble = ds4bt_set_rumble; + padf[pad].set_mode = ds4bt_set_mode; } static void ds3pad_init() @@ -921,7 +922,6 @@ static int L2CAP_event_task(int result, int bytes) DelayThread(CMD_DELAY); hid_LEDRumbleCommand(ds3pad[pad].oldled, 0, 0, pad); ds3pad_status_set(DS3BT_STATE_RUNNING, pad); - pademu_connect(&padf[pad]); } ds3pad[pad].btn_delay = 0xFF; break; diff --git a/modules/pademu/ds4bt.c b/modules/pademu/ds4bt.c index 048ba7029..36725ea64 100644 --- a/modules/pademu/ds4bt.c +++ b/modules/pademu/ds4bt.c @@ -670,14 +670,15 @@ static void ds4pad_clear(int pad) ds4pad[pad].btn_delay = 0; ds4pad[pad].data[0] = 0xFF; ds4pad[pad].data[1] = 0xFF; + mips_memset(&ds4pad[pad].data[2], 0x7F, 4); + mips_memset(&ds4pad[pad].data[6], 0x00, 12); + pademu_connect(&padf[pad]); padf[pad].priv = &ds4pad[pad]; padf[pad].get_status = ds4bt_get_status; padf[pad].get_model = ds4bt_get_model; padf[pad].get_data = ds4bt_get_data; padf[pad].set_rumble = ds4bt_set_rumble; padf[pad].set_mode = ds4bt_set_mode; - mips_memset(&ds4pad[pad].data[2], 0x7F, 4); - mips_memset(&ds4pad[pad].data[6], 0x00, 12); } static void ds4pad_init() @@ -946,7 +947,6 @@ static int L2CAP_event_task(int result, int bytes) DelayThread(CMD_DELAY); hid_LEDRumbleCommand(ds4pad[pad].oldled, 0, 0, pad); ds4pad_status_set(DS4BT_STATE_RUNNING, pad); - pademu_connect(&padf[pad]); } ds4pad[pad].btn_delay = 0xFF; break; diff --git a/modules/pademu/ds5bt.c b/modules/pademu/ds5bt.c new file mode 100644 index 000000000..3e1538402 --- /dev/null +++ b/modules/pademu/ds5bt.c @@ -0,0 +1,1339 @@ + +/* based on https://github.com/IonAgorria/Arduino-PSRemote */ +/* and https://github.com/felis/USB_Host_Shield_2.0 */ + +#include "types.h" +#include "loadcore.h" +#include "stdio.h" +#include "sifrpc.h" +#include "sysclib.h" +#include "usbd.h" +#include "usbd_macro.h" +#include "thbase.h" +#include "thsemap.h" +#include "sys_utils.h" +#include "pademu.h" +#include "pademu_common.h" +#include "ds5bt.h" +#include "padmacro.h" + +#define MODNAME "DS5BT" + +#ifdef DEBUG +#define DPRINTF(format, args...) \ + printf(MODNAME ": " format, ##args) +#else +#define DPRINTF(args...) +#endif + +static int bt_probe(int devId); +static int bt_connect(int devId); +static int bt_disconnect(int devId); +static void bt_config_set(int result, int count, void *arg); + +static UsbDriver bt_driver = {NULL, NULL, "ds5bt", bt_probe, bt_connect, bt_disconnect}; +static bt_device bt_dev = {-1, -1, -1, -1, -1, -1, DS5BT_STATE_USB_DISCONNECTED}; + +static void ds5pad_clear(int pad); +static void ds5pad_init(); +static int ds5bt_get_status(struct pad_funcs *pf); +static int ds5bt_get_model(struct pad_funcs *pf); +static int ds5bt_get_data(struct pad_funcs *pf, u8 *dst, int size, int port); +static void ds5bt_set_rumble(struct pad_funcs *pf, u8 lrum, u8 rrum); +static void ds5bt_set_mode(struct pad_funcs *pf, int mode, int lock); + + +static int bt_probe(int devId) +{ + UsbDeviceDescriptor *device = NULL; + UsbConfigDescriptor *config = NULL; + UsbInterfaceDescriptor *intf = NULL; + + DPRINTF("probe: devId=%i\n", devId); + + if ((bt_dev.devId > 0) && (bt_dev.status & DS5BT_STATE_USB_AUTHORIZED)) { + DPRINTF("Error - only one device allowed !\n"); + return 0; + } + + device = (UsbDeviceDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_DEVICE); + if (device == NULL) { + DPRINTF("Error - Couldn't get device descriptor\n"); + return 0; + } + + if (device->bNumConfigurations < 1) + return 0; + + config = (UsbConfigDescriptor *)sceUsbdScanStaticDescriptor(devId, device, USB_DT_CONFIG); + if (config == NULL) { + DPRINTF("Error - Couldn't get configuration descriptor\n"); + return 0; + } + + if ((config->bNumInterfaces < 1) || (config->wTotalLength < (sizeof(UsbConfigDescriptor) + sizeof(UsbInterfaceDescriptor)))) { + DPRINTF("Error - No interfaces available\n"); + return 0; + } + + intf = (UsbInterfaceDescriptor *)((char *)config + config->bLength); + + DPRINTF("bInterfaceClass %X bInterfaceSubClass %X bInterfaceProtocol %X\n", intf->bInterfaceClass, intf->bInterfaceSubClass, intf->bInterfaceProtocol); + + if ((intf->bInterfaceClass != USB_CLASS_WIRELESS_CONTROLLER) || + (intf->bInterfaceSubClass != USB_SUBCLASS_RF_CONTROLLER) || + (intf->bInterfaceProtocol != USB_PROTOCOL_BLUETOOTH_PROG) || + (intf->bNumEndpoints < 3)) { + return 0; + } + + if (device->idVendor == DS_VID && (device->idProduct == DS5_PID)) + return 1; + + return 1; +} + +static int bt_connect(int devId) +{ + int epCount; + UsbDeviceDescriptor *device; + UsbConfigDescriptor *config; + UsbInterfaceDescriptor *interface; + UsbEndpointDescriptor *endpoint; + + DPRINTF("connect: devId=%i\n", devId); + + if (bt_dev.devId != -1) { + DPRINTF("Error - only one device allowed !\n"); + return 1; + } + + bt_dev.status = DS5BT_STATE_USB_DISCONNECTED; + + bt_dev.interruptEndp = -1; + bt_dev.inEndp = -1; + bt_dev.outEndp = -1; + + bt_dev.controlEndp = sceUsbdOpenPipe(devId, NULL); + + device = (UsbDeviceDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_DEVICE); + config = (UsbConfigDescriptor *)sceUsbdScanStaticDescriptor(devId, device, USB_DT_CONFIG); + interface = (UsbInterfaceDescriptor *)((char *)config + config->bLength); + + epCount = interface->bNumEndpoints - 1; + + DPRINTF("Endpoint Count %d \n", epCount + 1); + + endpoint = (UsbEndpointDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_ENDPOINT); + + do { + + if (endpoint->bmAttributes == USB_ENDPOINT_XFER_BULK) { + if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT && bt_dev.outEndp < 0) { + bt_dev.outEndp = sceUsbdOpenPipeAligned(devId, endpoint); + DPRINTF("register Output endpoint id =%i addr=%02X packetSize=%i\n", bt_dev.outEndp, endpoint->bEndpointAddress, (unsigned short int)endpoint->wMaxPacketSizeHB << 8 | endpoint->wMaxPacketSizeLB); + } else if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN && bt_dev.inEndp < 0) { + bt_dev.inEndp = sceUsbdOpenPipeAligned(devId, endpoint); + DPRINTF("register Input endpoint id =%i addr=%02X packetSize=%i\n", bt_dev.inEndp, endpoint->bEndpointAddress, (unsigned short int)endpoint->wMaxPacketSizeHB << 8 | endpoint->wMaxPacketSizeLB); + } + } else if (endpoint->bmAttributes == USB_ENDPOINT_XFER_INT) { + if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN && bt_dev.interruptEndp < 0) { + bt_dev.interruptEndp = sceUsbdOpenPipe(devId, endpoint); + DPRINTF("register Interrupt endpoint id =%i addr=%02X packetSize=%i\n", bt_dev.interruptEndp, endpoint->bEndpointAddress, (unsigned short int)endpoint->wMaxPacketSizeHB << 8 | endpoint->wMaxPacketSizeLB); + } + } + + endpoint = (UsbEndpointDescriptor *)((char *)endpoint + endpoint->bLength); + + } while (epCount--); + + if (bt_dev.interruptEndp < 0 || bt_dev.inEndp < 0 || bt_dev.outEndp < 0) { + DPRINTF("Error - connect failed: not enough endpoints! \n"); + return -1; + } + + bt_dev.devId = devId; + bt_dev.status = DS5BT_STATE_USB_AUTHORIZED; + + sceUsbdSetConfiguration(bt_dev.controlEndp, config->bConfigurationValue, bt_config_set, NULL); + + return 0; +} + +static int bt_disconnect(int devId) +{ + DPRINTF("disconnect: devId=%i\n", devId); + + if (bt_dev.status & DS5BT_STATE_USB_AUTHORIZED) { + + if (bt_dev.interruptEndp >= 0) + sceUsbdClosePipe(bt_dev.interruptEndp); + + if (bt_dev.inEndp >= 0) + sceUsbdClosePipe(bt_dev.inEndp); + + if (bt_dev.outEndp >= 0) + sceUsbdClosePipe(bt_dev.outEndp); + + bt_dev.devId = -1; + bt_dev.interruptEndp = -1; + bt_dev.inEndp = -1; + bt_dev.outEndp = -1; + bt_dev.controlEndp = -1; + bt_dev.status = DS5BT_STATE_USB_DISCONNECTED; + + ds5pad_init(); + SignalSema(bt_dev.hid_sema); + } + + return 0; +} + +#define OUTPUT_01_REPORT_SIZE 48 + +static u8 rgbled_patterns[][2][3] = + { + {{0x00, 0x00, 0x10}, {0x00, 0x00, 0x7F}}, // light blue/blue + {{0x00, 0x10, 0x00}, {0x00, 0x7F, 0x00}}, // light green/green/ + {{0x10, 0x10, 0x00}, {0x7F, 0x7F, 0x00}}, // light yellow/yellow + {{0x00, 0x10, 0x10}, {0x00, 0x7F, 0x7F}}, // light cyan/cyan +}; + +static u8 link_key[] = // for ds5 authorisation + { + 0x56, 0xE8, 0x81, 0x38, 0x08, 0x06, 0x51, 0x41, + 0xC0, 0x7F, 0x12, 0xAA, 0xD9, 0x66, 0x3C, 0xCE}; + +// Taken from nefarius' SCPToolkit +// https://github.com/nefarius/ScpToolkit/blob/master/ScpControl/ScpControl.ini +// Valid MAC addresses used by Sony +static u8 GenuineMacAddress[][3] = + { + // Bluetooth chips by ALPS ELECTRIC CO., LTD + {0x00, 0x02, 0xC7}, + {0x00, 0x06, 0xF5}, + {0x00, 0x06, 0xF7}, + {0x00, 0x07, 0x04}, + {0x00, 0x16, 0xFE}, + {0x00, 0x19, 0xC1}, + {0x00, 0x1B, 0xFB}, + {0x00, 0x1E, 0x3D}, + {0x00, 0x21, 0x4F}, + {0x00, 0x23, 0x06}, + {0x00, 0x24, 0x33}, + {0x00, 0x26, 0x43}, + {0x00, 0xA0, 0x79}, + {0x04, 0x76, 0x6E}, + {0x04, 0x98, 0xF3}, + {0x28, 0xA1, 0x83}, + {0x34, 0xC7, 0x31}, + {0x38, 0xC0, 0x96}, + {0x60, 0x38, 0x0E}, + {0x64, 0xD4, 0xBD}, + {0xAC, 0x7A, 0x4D}, + {0xE0, 0x75, 0x0A}, + {0xE0, 0xAE, 0x5E}, + {0xFC, 0x62, 0xB9}, + // Bluetooth chips by AzureWave Technology Inc. + {0xE0, 0xB9, 0xA5}, + {0xDC, 0x85, 0xDE}, + {0xD0, 0xE7, 0x82}, + {0xB0, 0xEE, 0x45}, + {0xAC, 0x89, 0x95}, + {0xA8, 0x1D, 0x16}, + {0x94, 0xDB, 0xC9}, + {0x80, 0xD2, 0x1D}, + {0x80, 0xA5, 0x89}, + {0x78, 0x18, 0x81}, + {0x74, 0xF0, 0x6D}, + {0x74, 0xC6, 0x3B}, + {0x74, 0x2F, 0x68}, + {0x6C, 0xAD, 0xF8}, + {0x6C, 0x71, 0xD9}, + {0x60, 0x5B, 0xB4}, + {0x5C, 0x96, 0x56}, + {0x54, 0x27, 0x1E}, + {0x4C, 0xAA, 0x16}, + {0x48, 0x5D, 0x60}, + {0x44, 0xD8, 0x32}, + {0x40, 0xE2, 0x30}, + {0x38, 0x4F, 0xF0}, + {0x28, 0xC2, 0xDD}, + {0x24, 0x0A, 0x64}, + {0x1C, 0x4B, 0xD6}, + {0x08, 0xA9, 0x5A}, + {0x00, 0x25, 0xD3}, + {0x00, 0x24, 0x23}, + {0x00, 0x22, 0x43}, + {0x00, 0x15, 0xAF}, + // fake with AirohaTechnologyCorp's Chip + {0x0C, 0xFC, 0x83}}; + +#define REQ_HCI_OUT (USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_DEVICE) +#define HCI_COMMAND_REQ 0 + +#define MAX_PADS 4 +#define MAX_DELAY 10 + +static u8 hci_buf[MAX_BUFFER_SIZE] __attribute((aligned(4))) = {0}; +static u8 l2cap_buf[MAX_BUFFER_SIZE + 32] __attribute((aligned(4))) = {0}; +static u8 hci_cmd_buf[MAX_BUFFER_SIZE] __attribute((aligned(4))) = {0}; +static u8 l2cap_cmd_buf[MAX_BUFFER_SIZE + 32] __attribute((aligned(4))) = {0}; + +static u8 identifier = 0; +static u8 g_press_emu = 0; +static u8 enable_fake = 0; + +static ds5bt_pad_t ds5pad[MAX_PADS]; +static struct pad_funcs padf[MAX_PADS]; + +static void hci_event_cb(int resultCode, int bytes, void *arg); +static void l2cap_event_cb(int resultCode, int bytes, void *arg); + +static int hid_initDS34(int pad); +static int hid_LEDRumbleCommand(u8 *led, u8 lrum, u8 rrum, int pad); +static void hid_readReport(u8 *data, int bytes, int pad); + +static int l2cap_connection_request(u16 handle, u8 rxid, u16 scid, u16 psm); +static int hci_reset(); + +static void bt_config_set(int result, int count, void *arg) +{ + PollSema(bt_dev.hid_sema); + + sceUsbdInterruptTransfer(bt_dev.interruptEndp, hci_buf, MAX_BUFFER_SIZE, hci_event_cb, NULL); + sceUsbdBulkTransfer(bt_dev.inEndp, l2cap_buf, MAX_BUFFER_SIZE, l2cap_event_cb, NULL); + + ds5pad_init(); + hci_reset(); + + SignalSema(bt_dev.hid_sema); +} + +/************************************************************/ +/* HCI Commands */ +/************************************************************/ + +static int HCI_Command(int nbytes, u8 *dataptr) +{ + return sceUsbdControlTransfer(bt_dev.controlEndp, REQ_HCI_OUT, HCI_COMMAND_REQ, 0, 0, nbytes, dataptr, NULL, NULL); +} + +static int hci_reset() +{ + hci_cmd_buf[0] = HCI_OCF_RESET; + hci_cmd_buf[1] = HCI_OGF_CTRL_BBAND; + hci_cmd_buf[2] = 0x00; // Parameter Total Length = 0 + + return HCI_Command(3, hci_cmd_buf); +} + +static int hci_write_scan_enable(u8 conf) +{ + hci_cmd_buf[0] = HCI_OCF_WRITE_SCAN_ENABLE; + hci_cmd_buf[1] = HCI_OGF_CTRL_BBAND; + hci_cmd_buf[2] = 0x01; + hci_cmd_buf[3] = conf; + + return HCI_Command(4, hci_cmd_buf); +} + +static int hci_accept_connection(u8 *bdaddr) +{ + hci_cmd_buf[0] = HCI_OCF_ACCEPT_CONNECTION; // HCI OCF = 9 + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; // HCI OGF = 1 + hci_cmd_buf[2] = 0x07; // parameter length 7 + hci_cmd_buf[3] = *bdaddr; // 6 octet bluetooth address + hci_cmd_buf[4] = *(bdaddr + 1); + hci_cmd_buf[5] = *(bdaddr + 2); + hci_cmd_buf[6] = *(bdaddr + 3); + hci_cmd_buf[7] = *(bdaddr + 4); + hci_cmd_buf[8] = *(bdaddr + 5); + hci_cmd_buf[9] = 0x01; // switch role to (slave = 1 / master = 0) + + return HCI_Command(10, hci_cmd_buf); +} + +static int hci_remote_name(u8 *bdaddr) +{ + hci_cmd_buf[0] = HCI_OCF_REMOTE_NAME; // HCI OCF = 19 + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; // HCI OGF = 1 + hci_cmd_buf[2] = 0x0A; // parameter length = 10 + hci_cmd_buf[3] = *bdaddr; // 6 octet bluetooth address + hci_cmd_buf[4] = *(bdaddr + 1); + hci_cmd_buf[5] = *(bdaddr + 2); + hci_cmd_buf[6] = *(bdaddr + 3); + hci_cmd_buf[7] = *(bdaddr + 4); + hci_cmd_buf[8] = *(bdaddr + 5); + hci_cmd_buf[9] = 0x01; // Page Scan Repetition Mode + hci_cmd_buf[10] = 0x00; // Reserved + hci_cmd_buf[11] = 0x00; // Clock offset - low byte + hci_cmd_buf[12] = 0x00; // Clock offset - high byte + + return HCI_Command(13, hci_cmd_buf); +} + +static int hci_reject_connection(u8 *bdaddr) +{ + hci_cmd_buf[0] = HCI_OCF_REJECT_CONNECTION; // HCI OCF = A + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; // HCI OGF = 1 + hci_cmd_buf[2] = 0x07; // parameter length 7 + hci_cmd_buf[3] = *bdaddr; // 6 octet bluetooth address + hci_cmd_buf[4] = *(bdaddr + 1); + hci_cmd_buf[5] = *(bdaddr + 2); + hci_cmd_buf[6] = *(bdaddr + 3); + hci_cmd_buf[7] = *(bdaddr + 4); + hci_cmd_buf[8] = *(bdaddr + 5); + hci_cmd_buf[9] = 0x09; // reason max connection + + return HCI_Command(10, hci_cmd_buf); +} + +static int hci_disconnect(u16 handle) +{ + hci_cmd_buf[0] = HCI_OCF_DISCONNECT; // HCI OCF = 6 + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; // HCI OGF = 1 + hci_cmd_buf[2] = 0x03; // parameter length = 3 + hci_cmd_buf[3] = (u8)(handle & 0xFF); // connection handle - low byte + hci_cmd_buf[4] = (u8)((handle >> 8) & 0x0F); // connection handle - high byte + hci_cmd_buf[5] = 0x13; // reason + + return HCI_Command(6, hci_cmd_buf); +} + +static int hci_change_connection_type(u16 handle) +{ + hci_cmd_buf[0] = HCI_OCF_CHANGE_CONNECTION_TYPE; + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; + hci_cmd_buf[2] = 0x04; // parameter length 4 + hci_cmd_buf[3] = (u8)(handle & 0xFF); // connection handle - low byte + hci_cmd_buf[4] = (u8)((handle >> 8) & 0x0F); // connection handle - high byte + hci_cmd_buf[5] = 0x18; // Packet Type: 0xcc18 + hci_cmd_buf[6] = 0xcc; + + return HCI_Command(7, hci_cmd_buf); +} + +static int hci_link_key_request_reply(u8 *bdaddr) +{ + hci_cmd_buf[0] = HCI_OCF_LINK_KEY_REQUEST_REPLY; // HCI OCF = 0E + hci_cmd_buf[1] = HCI_OGF_LINK_CNTRL; // HCI OGF = 1 + hci_cmd_buf[2] = 0x06 + sizeof(link_key); // parameter length 6 + hci_cmd_buf[3] = *bdaddr; // 6 octet bluetooth address + hci_cmd_buf[4] = *(bdaddr + 1); + hci_cmd_buf[5] = *(bdaddr + 2); + hci_cmd_buf[6] = *(bdaddr + 3); + hci_cmd_buf[7] = *(bdaddr + 4); + hci_cmd_buf[8] = *(bdaddr + 5); + + mips_memcpy(&hci_cmd_buf[9], link_key, sizeof(link_key)); + + return HCI_Command(9 + sizeof(link_key), hci_cmd_buf); +} + +static void print_bd_addr(const u8 *addr) +{ + int i; + for (i = 0; i < 6; i++) { + // DPRINTF("0x%02X", addr[i]); + DPRINTF("%02X", addr[i]); + if (i < 5) + DPRINTF(":"); + } +} + +static void HCI_event_task(int result) +{ + int i, pad; + + if (!result) { + /* buf[0] = Event Code */ + /* buf[1] = Parameter Total Length */ + /* buf[n] = Event Parameters based on each event */ + + // Ignore this packet + if (hci_buf[0] == HCI_EVENT_NUM_COMPLETED_PKT) + return; + + DPRINTF("HCI event = 0x%02X \n", hci_buf[0]); + switch (hci_buf[0]) { // switch on event type + case HCI_EVENT_COMMAND_COMPLETE: + DPRINTF("HCI Command Complete = 0x%02X \n", hci_buf[3]); + DPRINTF("\tReturned = 0x%02X \n", hci_buf[5]); + if ((hci_buf[3] == HCI_OCF_RESET) && (hci_buf[4] == HCI_OGF_CTRL_BBAND)) { + if (hci_buf[5] == 0) { + hci_write_scan_enable(SCAN_ENABLE_NOINQ_ENPAG); + } else { + DelayThread(500); + hci_reset(); + } + } else if ((hci_buf[3] == HCI_OCF_WRITE_SCAN_ENABLE) && (hci_buf[4] == HCI_OGF_CTRL_BBAND)) { + if (hci_buf[5] == 0) { + bt_dev.status |= DS5BT_STATE_USB_CONFIGURED; + } else { + DelayThread(500); + hci_reset(); + } + } + break; + + case HCI_EVENT_COMMAND_STATUS: + if (hci_buf[2]) { // show status on serial if not OK + DPRINTF("HCI Command Failed: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + DPRINTF("\t Command_OpCode(OGF) = 0x%02X \n", ((hci_buf[5] & 0xFC) >> 2)); + DPRINTF("\t Command_OpCode(OCF) = 0x%02X 0x%02X \n", (hci_buf[5] & 0x03), hci_buf[4]); + } + break; + + case HCI_EVENT_CONNECT_COMPLETE: + DPRINTF("HCI event Connect Complete: \n"); + if (!hci_buf[2]) { // check if connected OK + DPRINTF("\t Connection_Handle 0x%02X \n", hci_buf[3] | ((hci_buf[4] & 0x0F) << 8)); + DPRINTF("\t Requested by BD_ADDR: \n\t"); + print_bd_addr(&hci_buf[5]); + DPRINTF("\n"); + for (i = 0; i < MAX_PADS; i++) { + if (memcmp(ds5pad[i].bdaddr, hci_buf + 5, 6) == 0) { + // store the handle for the ACL connection + ds5pad[i].hci_handle = hci_buf[3] | ((hci_buf[4] & 0x0F) << 8); + break; + } + } + if (i >= MAX_PADS) { + break; + } + ds5pad_status_set(DS5BT_STATE_CONNECTED, i); + hci_remote_name(ds5pad[i].bdaddr); + } else { + DPRINTF("\t Error 0x%02X \n", hci_buf[2]); + } + break; + + case HCI_EVENT_NUM_COMPLETED_PKT: + DPRINTF("HCI Number Of Completed Packets Event: \n"); + DPRINTF("\t Number_of_Handles = 0x%02X \n", hci_buf[2]); + for (i = 0; i < hci_buf[2]; i++) { + DPRINTF("\t Connection_Handle = 0x%04X \n", (hci_buf[3 + i] | ((hci_buf[4 + i] & 0x0F) << 8))); + } + break; + + case HCI_EVENT_QOS_SETUP_COMPLETE: + break; + + case HCI_EVENT_DISCONN_COMPLETE: + DPRINTF("HCI Disconnection Complete Event: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + DPRINTF("\t Connection_Handle = 0x%04X \n", (hci_buf[3] | ((hci_buf[4] & 0x0F) << 8))); + DPRINTF("\t Reason = 0x%02X \n", hci_buf[5]); + for (i = 0; i < MAX_PADS; i++) { // detect pad + if (ds5pad[i].hci_handle == (hci_buf[3] | ((hci_buf[4] & 0x0F) << 8))) { + break; + } + } + pademu_disconnect(&padf[i]); + ds5pad_clear(i); + break; + + case HCI_EVENT_AUTHENTICATION_COMPLETE: + DPRINTF("HCI Authentication Complete Event: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + DPRINTF("\t Connection_Handle = 0x%04X \n", (hci_buf[3] | ((hci_buf[4] & 0x0F) << 8))); + if (!hci_buf[2]) { + DPRINTF("\t Success \n"); + } else { + DPRINTF("\t Failed \n"); + } + break; + + case HCI_EVENT_REMOTE_NAME_COMPLETE: + DPRINTF("HCI Remote Name Requested Complete Event: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + if (!hci_buf[2]) { + for (i = 0; i < MAX_PADS; i++) { + if (memcmp(ds5pad[i].bdaddr, hci_buf + 3, 6) == 0) { + break; + } + } + if (i >= MAX_PADS) { + break; + } + ds5pad[i].type = DS5; + ds5pad[i].isfake = 0; + DPRINTF("\t Type: Dualshock 4 \n"); + hci_change_connection_type(ds5pad[i].hci_handle); + identifier++; + l2cap_connection_request(ds5pad[i].hci_handle, identifier, 0x0070, L2CAP_PSM_CTRL); + } + break; + + case HCI_EVENT_ENCRYPTION_CHANGE: + DPRINTF("HCI Encryption Change Event: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + DPRINTF("\t Connection_Handle = 0x%04X \n", (hci_buf[3] | ((hci_buf[4] & 0x0F) << 8))); + DPRINTF("\t Encryption_Enabled = 0x%02X \n", hci_buf[5]); + break; + + case HCI_EVENT_CONNECT_REQUEST: + DPRINTF("HCI Connection Requested by BD_ADDR: \n\t"); + print_bd_addr(&hci_buf[2]); + DPRINTF("\n\t Link = 0x%02X \n", hci_buf[11]); + DPRINTF("\t Class = 0x%02X 0x%02X 0x%02X \n", hci_buf[8], hci_buf[9], hci_buf[10]); + for (i = 0; i < MAX_PADS; i++) { // find free slot + if (!ds5pad_status_check(DS5BT_STATE_RUNNING, i) && ds5pad[i].enabled) { + if (ds5pad_status_check(DS5BT_STATE_CONNECTED, i)) { + if (ds5pad_status_check(DS5BT_STATE_DISCONNECTING, i)) // if we're waiting for hci disconnect event + continue; + else + hci_disconnect(ds5pad[i].hci_handle); // try to disconnect + } + break; + } + } + if (i >= MAX_PADS) { // no free slot + hci_reject_connection(hci_buf + 2); + break; + } + pad = i; + mips_memcpy(ds5pad[pad].bdaddr, hci_buf + 2, 6); + ds5pad[pad].isfake = 0; + if (enable_fake) { + ds5pad[pad].isfake = 1; // fake ds3 + for (i = 0; i < sizeof(GenuineMacAddress) / 3; i++) { // check if ds3 is genuine + if (ds5pad[pad].bdaddr[5] == GenuineMacAddress[i][0] && + ds5pad[pad].bdaddr[4] == GenuineMacAddress[i][1] && + ds5pad[pad].bdaddr[3] == GenuineMacAddress[i][2]) { + ds5pad[pad].isfake = 0; + break; + } + } + } + ds5pad_status_clear(DS5BT_STATE_CONNECTED, pad); + ds5pad_status_clear(DS5BT_STATE_RUNNING, pad); + ds5pad_status_clear(DS5BT_STATE_DISCONNECTING, pad); + hci_accept_connection(ds5pad[pad].bdaddr); + break; + + case HCI_EVENT_ROLE_CHANGED: + DPRINTF("HCI Role Change Event: \n"); + DPRINTF("\t Status = 0x%02X \n", hci_buf[2]); + DPRINTF("\t BD_ADDR: "); + print_bd_addr(&hci_buf[3]); + DPRINTF("\n\t Role 0x%02X \n", hci_buf[9]); + break; + + case HCI_EVENT_MAX_SLOT_CHANGE: + DPRINTF("HCI Max Slot Change Event: \n"); + DPRINTF("\t Connection_Handle = 0x%x \n", (hci_buf[2] | ((hci_buf[3] & 0x0F) << 8))); + DPRINTF("\t LMP Max Slots = 0x%x \n", hci_buf[5]); + break; + + case HCI_EVENT_PIN_CODE_REQUEST: + DPRINTF("HCI Pin Code Request Event \n"); + break; + + case HCI_EVENT_LINK_KEY_REQUEST: + DPRINTF("HCI Link Key Request Event by BD_ADDR: \n\t"); + print_bd_addr(&hci_buf[2]); + DPRINTF("\n"); + hci_link_key_request_reply(hci_buf + 2); + break; + + case HCI_EVENT_CHANGED_CONNECTION_TYPE: + DPRINTF("Packet Type Changed STATUS: 0x%x \n", hci_buf[2]); + DPRINTF("\t Type = %x \n", (hci_buf[5] | (hci_buf[6] << 8))); + break; + + case HCI_EVENT_PAGE_SR_CHANGED: + break; + + default: + DPRINTF("Unmanaged Event: 0x%02X \n", hci_buf[0]); + break; + } // switch (buf[0]) + } +} + +static void ds5pad_clear(int pad) +{ + if (pad >= MAX_PADS) + return; + + ds5pad[pad].hci_handle = 0x0FFF; + ds5pad[pad].control_scid = 0; + ds5pad[pad].interrupt_scid = 0; + mips_memset(ds5pad[pad].bdaddr, 0, 6); + ds5pad[pad].status = bt_dev.status; + ds5pad[pad].status |= DS5BT_STATE_USB_CONFIGURED; + ds5pad[pad].isfake = 0; + ds5pad[pad].type = 0; + ds5pad[pad].btn_delay = 0; + ds5pad[pad].data[0] = 0xFF; + ds5pad[pad].data[1] = 0xFF; + padf[pad].priv = &ds5pad[pad]; + padf[pad].get_status = ds5bt_get_status; + padf[pad].get_model = ds5bt_get_model; + padf[pad].get_data = ds5bt_get_data; + padf[pad].set_rumble = ds5bt_set_rumble; + padf[pad].set_mode = ds5bt_set_mode; + mips_memset(&ds5pad[pad].data[2], 0x7F, 4); + mips_memset(&ds5pad[pad].data[6], 0x00, 12); +} + +static void ds5pad_init() +{ + int i; + + for (i = 0; i < MAX_PADS; i++) { + ds5pad_clear(i); + } + + g_press_emu = 0; + identifier = 0; +} + +static void hci_event_cb(int resultCode, int bytes, void *arg) +{ + PollSema(bt_dev.hid_sema); + HCI_event_task(resultCode); + sceUsbdInterruptTransfer(bt_dev.interruptEndp, hci_buf, MAX_BUFFER_SIZE, hci_event_cb, NULL); + SignalSema(bt_dev.hid_sema); +} + +/************************************************************/ +/* L2CAP Commands */ +/************************************************************/ + +static int L2CAP_Command(u16 handle, u8 *data, u8 length) +{ + l2cap_cmd_buf[0] = (u8)(handle & 0xff); // HCI handle with PB,BC flag + l2cap_cmd_buf[1] = (u8)(((handle >> 8) & 0x0f) | 0x20); + l2cap_cmd_buf[2] = (u8)((4 + length) & 0xff); // HCI ACL total data length + l2cap_cmd_buf[3] = (u8)((4 + length) >> 8); + l2cap_cmd_buf[4] = (u8)(length & 0xff); // L2CAP header: Length + l2cap_cmd_buf[5] = (u8)(length >> 8); + l2cap_cmd_buf[6] = 0x01; // L2CAP header: Channel ID + l2cap_cmd_buf[7] = 0x00; // L2CAP Signalling channel over ACL-U logical link + + mips_memcpy(&l2cap_cmd_buf[8], data, length); + + // output on endpoint 2 + return sceUsbdBulkTransfer(bt_dev.outEndp, l2cap_cmd_buf, (8 + length), NULL, NULL); +} + +static int l2cap_connection_request(u16 handle, u8 rxid, u16 scid, u16 psm) +{ + u8 cmd_buf[8]; + + cmd_buf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x04; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(psm & 0xff); // PSM + cmd_buf[5] = (u8)(psm >> 8); + cmd_buf[6] = (u8)(scid & 0xff); // Source CID (PS Remote) + cmd_buf[7] = (u8)(scid >> 8); + + return L2CAP_Command(handle, cmd_buf, 8); +} + +static int l2cap_connection_response(u16 handle, u8 rxid, u16 dcid, u16 scid, u8 result) +{ + u8 cmd_buf[12]; + + cmd_buf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x08; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(dcid & 0xff); // Destination CID (Our) + cmd_buf[5] = (u8)(dcid >> 8); + cmd_buf[6] = (u8)(scid & 0xff); // Source CID (PS Remote) + cmd_buf[7] = (u8)(scid >> 8); + cmd_buf[8] = result; // Result + cmd_buf[9] = 0x00; + cmd_buf[10] = 0x00; // Status + cmd_buf[11] = 0x00; + + if (result != 0) + cmd_buf[10] = 0x01; // Authentication pending + + return L2CAP_Command(handle, cmd_buf, 12); +} + +static int l2cap_config_request(u16 handle, u8 rxid, u16 dcid) +{ + u8 cmd_buf[12]; + + cmd_buf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x08; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(dcid & 0xff); // Destination CID + cmd_buf[5] = (u8)(dcid >> 8); + cmd_buf[6] = 0x00; // Flags + cmd_buf[7] = 0x00; + cmd_buf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) + cmd_buf[9] = 0x02; // Config Opt: length + // cmd_buf[10] = 0x96; // Config Opt: data + // cmd_buf[11] = 0x00; + + // this setting disable hid cmd reports from ds3 + cmd_buf[10] = 0xFF; // Config Opt: data + cmd_buf[11] = 0xFF; + + return L2CAP_Command(handle, cmd_buf, 12); +} + +static int l2cap_config_response(u16 handle, u8 rxid, u16 scid) +{ + u8 cmd_buf[14]; + + cmd_buf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x0A; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(scid & 0xff); // Source CID + cmd_buf[5] = (u8)(scid >> 8); + cmd_buf[6] = 0x00; // Result + cmd_buf[7] = 0x00; + cmd_buf[8] = 0x00; // Config + cmd_buf[9] = 0x00; + cmd_buf[10] = 0x01; // Config + cmd_buf[11] = 0x02; + cmd_buf[12] = 0xA0; + cmd_buf[13] = 0x02; + + return L2CAP_Command(handle, cmd_buf, 14); +} + +static int l2cap_disconnection_request(u16 handle, u8 rxid, u16 dcid, u16 scid) +{ + u8 cmd_buf[8]; + + cmd_buf[0] = L2CAP_CMD_DISCONNECT_REQUEST; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x04; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(dcid & 0xff); // Destination CID + cmd_buf[5] = (u8)(dcid >> 8); + cmd_buf[6] = (u8)(scid & 0xff); // Source CID + cmd_buf[7] = (u8)(scid >> 8); + + return L2CAP_Command(handle, cmd_buf, 8); +} + +static int l2cap_disconnection_response(u16 handle, u8 rxid, u16 scid, u16 dcid) +{ + u8 cmd_buf[8]; + + cmd_buf[0] = L2CAP_CMD_DISCONNECT_RESPONSE; // Code + cmd_buf[1] = rxid; // Identifier + cmd_buf[2] = 0x04; // Length + cmd_buf[3] = 0x00; + cmd_buf[4] = (u8)(dcid & 0xff); // Destination CID + cmd_buf[5] = (u8)(dcid >> 8); + cmd_buf[6] = (u8)(scid & 0xff); // Source CID + cmd_buf[7] = (u8)(scid >> 8); + + return L2CAP_Command(handle, cmd_buf, 8); +} + +#define CMD_DELAY 2 + +static int L2CAP_event_task(int result, int bytes) +{ + int pad = -1; + u16 control_dcid = 0x0040; // Channel endpoint on command source + u16 interrupt_dcid = 0x0041; // Channel endpoint on interrupt source + + if (!result) { + for (pad = 0; pad < MAX_PADS; pad++) { + if (l2cap_handle_ok(ds5pad[pad].hci_handle)) + break; + } + + if (pad >= MAX_PADS) { + DPRINTF("L2CAP Wrong Handle = 0x%04X\n", ((l2cap_buf[0] | (l2cap_buf[1] << 8)))); + return pad; + } + + if (l2cap_handle_ok(ds5pad[pad].hci_handle)) { + control_dcid = 0x0070; + interrupt_dcid = 0x0071; + + if (l2cap_control_channel) { + DPRINTF("L2CAP Signaling Command = 0x%02X, pad %d \n", l2cap_buf[8], pad); + + switch (l2cap_buf[8]) { + case L2CAP_CMD_COMMAND_REJECT: + DPRINTF("Command Reject ID = 0x%02X \n", l2cap_buf[9]); + DPRINTF("\t Reason = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + DPRINTF("\t DATA = 0x%04X \n", (l2cap_buf[14] | (l2cap_buf[15] << 8))); + break; + + case L2CAP_CMD_CONNECTION_REQUEST: + DPRINTF("Connection Request ID = 0x%02X \n", l2cap_buf[9]); + DPRINTF("\t PSM = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + DPRINTF("\t SCID = 0x%04X \n", (l2cap_buf[14] | (l2cap_buf[15] << 8))); + + if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == L2CAP_PSM_CTRL) { + ds5pad[pad].control_scid = l2cap_buf[14] | (l2cap_buf[15] << 8); + l2cap_connection_response(ds5pad[pad].hci_handle, l2cap_buf[9], control_dcid, ds5pad[pad].control_scid, PENDING); + DelayThread(CMD_DELAY); + l2cap_connection_response(ds5pad[pad].hci_handle, l2cap_buf[9], control_dcid, ds5pad[pad].control_scid, SUCCESSFUL); + DelayThread(CMD_DELAY); + identifier++; + l2cap_config_request(ds5pad[pad].hci_handle, identifier, ds5pad[pad].control_scid); + } else if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == L2CAP_PSM_INTR) { + ds5pad[pad].interrupt_scid = l2cap_buf[14] | (l2cap_buf[15] << 8); + l2cap_connection_response(ds5pad[pad].hci_handle, l2cap_buf[9], interrupt_dcid, ds5pad[pad].interrupt_scid, PENDING); + DelayThread(CMD_DELAY); + l2cap_connection_response(ds5pad[pad].hci_handle, l2cap_buf[9], interrupt_dcid, ds5pad[pad].interrupt_scid, SUCCESSFUL); + DelayThread(CMD_DELAY); + identifier++; + l2cap_config_request(ds5pad[pad].hci_handle, identifier, ds5pad[pad].interrupt_scid); + } + break; + + case L2CAP_CMD_CONNECTION_RESPONSE: + DPRINTF("Connection Response ID = 0x%02X \n", l2cap_buf[9]); + DPRINTF("\t PSM = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + DPRINTF("\t SCID = 0x%04X \n", (l2cap_buf[14] | (l2cap_buf[15] << 8))); + DPRINTF("\t RESULT = 0x%04X \n", (l2cap_buf[16] | (l2cap_buf[17] << 8))); + + if (((l2cap_buf[16] | (l2cap_buf[17] << 8)) == 0x0000) && ((l2cap_buf[18] | (l2cap_buf[19] << 8)) == 0x0000)) { + if ((l2cap_buf[14] | (l2cap_buf[15] << 8)) == control_dcid) { + ds5pad[pad].control_scid = l2cap_buf[12] | (l2cap_buf[13] << 8); + identifier++; + l2cap_config_request(ds5pad[pad].hci_handle, identifier, ds5pad[pad].control_scid); + } else if ((l2cap_buf[14] | (l2cap_buf[15] << 8)) == interrupt_dcid) { + ds5pad[pad].interrupt_scid = l2cap_buf[12] | (l2cap_buf[13] << 8); + identifier++; + l2cap_config_request(ds5pad[pad].hci_handle, identifier, ds5pad[pad].interrupt_scid); + } + } + break; + + case L2CAP_CMD_CONFIG_REQUEST: + DPRINTF("Configuration Request ID = 0x%02X \n", l2cap_buf[9]); + DPRINTF("\t LEN = 0x%04X \n", (l2cap_buf[10] | (l2cap_buf[11] << 8))); + DPRINTF("\t SCID = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + DPRINTF("\t FLAG = 0x%04X \n", (l2cap_buf[14] | (l2cap_buf[15] << 8))); + + if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == control_dcid) { + l2cap_config_response(ds5pad[pad].hci_handle, l2cap_buf[9], ds5pad[pad].control_scid); + } else if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == interrupt_dcid) { + l2cap_config_response(ds5pad[pad].hci_handle, l2cap_buf[9], ds5pad[pad].interrupt_scid); + } + break; + + case L2CAP_CMD_CONFIG_RESPONSE: + DPRINTF("Configuration Response ID = 0x%02X \n", l2cap_buf[9]); + DPRINTF("\t LEN = 0x%04X \n", (l2cap_buf[10] | (l2cap_buf[11] << 8))); + DPRINTF("\t SCID = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + DPRINTF("\t FLAG = 0x%04X \n", (l2cap_buf[14] | (l2cap_buf[15] << 8))); + DPRINTF("\t RESULT = 0x%04X \n", (l2cap_buf[16] | (l2cap_buf[17] << 8))); + + if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == control_dcid) { + identifier++; + l2cap_connection_request(ds5pad[pad].hci_handle, identifier, interrupt_dcid, L2CAP_PSM_INTR); + } else if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == interrupt_dcid) { + hid_initDS34(pad); + ds5pad[pad].oldled[0] = rgbled_patterns[pad][1][0]; + ds5pad[pad].oldled[1] = rgbled_patterns[pad][1][1]; + ds5pad[pad].oldled[2] = rgbled_patterns[pad][1][2]; + ds5pad[pad].oldled[3] = 0; + DelayThread(CMD_DELAY); + hid_LEDRumbleCommand(ds5pad[pad].oldled, 0, 0, pad); + ds5pad_status_set(DS5BT_STATE_RUNNING, pad); + pademu_connect(&padf[pad]); + } + ds5pad[pad].btn_delay = 0xFF; + break; + + case L2CAP_CMD_DISCONNECT_REQUEST: + DPRINTF("Disconnect Request SCID = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + + if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == control_dcid) { + ds5pad_status_set(DS5BT_STATE_DISCONNECTING, pad); + l2cap_disconnection_response(ds5pad[pad].hci_handle, l2cap_buf[9], control_dcid, ds5pad[pad].control_scid); + } else if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == interrupt_dcid) { + ds5pad_status_set(DS5BT_STATE_DISCONNECTING, pad); + l2cap_disconnection_response(ds5pad[pad].hci_handle, l2cap_buf[9], interrupt_dcid, ds5pad[pad].interrupt_scid); + } + break; + + case L2CAP_CMD_DISCONNECT_RESPONSE: + DPRINTF("Disconnect Response SCID = 0x%04X \n", (l2cap_buf[12] | (l2cap_buf[13] << 8))); + + if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == ds5pad[pad].control_scid) { + ds5pad_status_set(DS5BT_STATE_DISCONNECTING, pad); + hci_disconnect(ds5pad[pad].hci_handle); + } else if ((l2cap_buf[12] | (l2cap_buf[13] << 8)) == ds5pad[pad].interrupt_scid) { + ds5pad_status_set(DS5BT_STATE_DISCONNECTING, pad); + identifier++; + l2cap_disconnection_request(ds5pad[pad].hci_handle, identifier, ds5pad[pad].control_scid, control_dcid); + } + break; + + default: + break; + } + } else if (l2cap_interrupt_channel) { + hid_readReport(l2cap_buf, bytes, pad); + } else if (l2cap_command_channel) { + DPRINTF("HID command status 0x%02X \n", l2cap_buf[8]); + pad = MAX_PADS; + } + } // acl_handle_ok + } // !rcode + + return pad; +} + +static void l2cap_event_cb(int resultCode, int bytes, void *arg) +{ + int ret; + u16 interrupt_dcid = 0x0041; + + PollSema(bt_dev.hid_sema); + + ret = L2CAP_event_task(resultCode, bytes); + + if (ret < MAX_PADS) { + if (ds5pad_status_check(DS5BT_STATE_RUNNING, ret)) { + if (ds5pad_status_check(DS5BT_STATE_DISCONNECT_REQUEST, ret)) { + if (!ds5pad[ret].isfake) { + interrupt_dcid = 0x0071; + identifier++; + l2cap_disconnection_request(ds5pad[ret].hci_handle, identifier, ds5pad[ret].interrupt_scid, interrupt_dcid); + } else { + hci_disconnect(ds5pad[ret].hci_handle); + } + ds5pad_status_clear(DS5BT_STATE_DISCONNECT_REQUEST, ret); + } else if (ds5pad[ret].update_rum) { + hid_LEDRumbleCommand(ds5pad[ret].oldled, ds5pad[ret].lrum, ds5pad[ret].rrum, ret); + ds5pad[ret].update_rum = 0; + } + } + } + + sceUsbdBulkTransfer(bt_dev.inEndp, l2cap_buf, MAX_BUFFER_SIZE + 23, l2cap_event_cb, arg); + SignalSema(bt_dev.hid_sema); +} + +/************************************************************/ +/* HID Commands */ +/************************************************************/ +static int HID_command(u16 handle, u16 scid, u8 *data, u8 length) +{ + l2cap_cmd_buf[0] = (u8)(handle & 0xff); // HCI handle with PB,BC flag + l2cap_cmd_buf[1] = (u8)(((handle >> 8) & 0x0f) | 0x20); + l2cap_cmd_buf[2] = (u8)((4 + length) & 0xff); // HCI ACL total data length + l2cap_cmd_buf[3] = (u8)((4 + length) >> 8); + l2cap_cmd_buf[4] = (u8)(length & 0xff); // L2CAP header: Length + l2cap_cmd_buf[5] = (u8)(length >> 8); + l2cap_cmd_buf[6] = (u8)(scid & 0xff); // L2CAP header: Channel ID + l2cap_cmd_buf[7] = (u8)(scid >> 8); + + mips_memcpy(&l2cap_cmd_buf[8], data, length); + + // output on endpoint 2 + return sceUsbdBulkTransfer(bt_dev.outEndp, l2cap_cmd_buf, (8 + length), NULL, NULL); +} + +static int hid_initDS34(int pad) +{ + u8 init_buf[PS3_F4_REPORT_LEN + 2]; + u8 size = 2; + + init_buf[0] = HID_THDR_GET_REPORT_FEATURE; // THdr + init_buf[1] = PS4_02_REPORT_ID; // Report I + + return HID_command(ds5pad[pad].hci_handle, ds5pad[pad].control_scid, init_buf, size); +} + +/** + * Send a HID command with LED and rumble status + * @param led 4 bytes describing LED state. + * For DS5, first three bytes are R, G, B values of the front light + * Fourth byte indicates whether light should be blinking (1) or solid (0). + * @param lrum Strength of left rumble motor + * @param lrum Strength of right rumble motor + * @param pad Which pad the command should be sent to + */ +static int hid_LEDRumbleCommand(u8 *led, u8 lrum, u8 rrum, int pad) +{ + u8 led_buf[PS4_11_REPORT_LEN + 2]; + u8 size = 2; + + mips_memset(led_buf, 0, PS3_01_REPORT_LEN + 2); + + led_buf[0] = HID_THDR_SET_REPORT_OUTPUT; // THdr + led_buf[1] = PS4_11_REPORT_ID; // Report ID + led_buf[2] = 0x80; // update rate 1000Hz + led_buf[4] = 0xFF; + + led_buf[7] = rrum * 255; + led_buf[8] = lrum; + + led_buf[9] = led[0]; // r + led_buf[10] = led[1]; // g + led_buf[11] = led[2]; // b + + if (led[3]) { // means charging, so blink + led_buf[12] = 0x80; // Time to flash bright (255 = 2.5 seconds) + led_buf[13] = 0x80; // Time to flash dark (255 = 2.5 seconds) + } + + size += PS4_11_REPORT_LEN; + + ds5pad[pad].oldled[0] = led[0]; + ds5pad[pad].oldled[1] = led[1]; + ds5pad[pad].oldled[2] = led[2]; + ds5pad[pad].oldled[3] = led[3]; + + return HID_command(ds5pad[pad].hci_handle, ds5pad[pad].control_scid, led_buf, size); +} + +static void hid_readReport(u8 *data, int bytes, int pad_idx) +{ + ds5bt_pad_t *pad = &ds5pad[pad_idx]; + struct ds5report *r = (struct ds5report *)data; + uint8_t up = 0, down = 0, left = 0, right = 0; + + /* + * By default we get report 0x01 with length 64 + * But we can also get report 0x31 with length 78 + */ + if (data[0] != 0x01) { + DPRINTF("ds5: Unexpected report_id, got: 0x%02x, want: 0x01\n", data[0]); + return; + } + + switch (r->Dpad) { + case 0: + up = 1; + break; + case 1: + up = 1; + right = 1; + break; + case 2: + right = 1; + break; + case 3: + down = 1; + right = 1; + break; + case 4: + down = 1; + break; + case 5: + down = 1; + left = 1; + break; + case 6: + left = 1; + break; + case 7: + up = 1; + left = 1; + break; + } + + pad->data[0] = ~(r->Share | r->L3 << 1 | r->R3 << 2 | r->Option << 3 | up << 4 | right << 5 | down << 6 | left << 7); + pad->data[1] = ~(r->L2 | r->R2 << 1 | r->L1 << 2 | r->R1 << 3 | r->Triangle << 4 | r->Circle << 5 | r->Cross << 6 | r->Square << 7); + + pad->data[2] = r->RightStickX; // rx + pad->data[3] = r->RightStickY; // ry + pad->data[4] = r->LeftStickX; // lx + pad->data[5] = r->LeftStickY; // ly + + pad->data[6] = right * 255; // right + pad->data[7] = left * 255; // left + pad->data[8] = up * 255; // up + pad->data[9] = down * 255; // down + + pad->data[10] = r->Triangle * 255; // triangle + pad->data[11] = r->Circle * 255; // circle + pad->data[12] = r->Cross * 255; // cross + pad->data[13] = r->Square * 255; // square + + pad->data[14] = r->L1 * 255; // L1 + pad->data[15] = r->R1 * 255; // R1 + pad->data[16] = r->PressureL2; // L2 + pad->data[17] = r->PressureR2; // R2 + /* + enum power_state power = normal; + if (r->Power <= 2) + power = empty; + else if (r->Power == 10) + power = charging; + else if (r->Power == 11) + power = full; + + int update = pad->update_num + pad->update_rum + (power != pad->power); + if (update != 0) { + printf("ds5: update led/rumble, power=%d\n", r->Power); + memset(usb_led_buf, 0, sizeof(usb_led_buf)); + + usb_led_buf[0] = 0x05; + usb_led_buf[1] = 0xFF; + + usb_led_buf[4] = pad->rrum * 255; // ds5 has full control + usb_led_buf[5] = pad->lrum; + + if (pad->num < 0) { + // Set default color + printf("ds5: set default color\n"); + usb_led_buf[6] = 0x7f; // r + usb_led_buf[7] = 0x7f; // g + usb_led_buf[8] = 0x7f; // b + } else { + // Set controller color + printf("ds5: set controller color %d\n", pad->num); + usb_led_buf[6] = rgbled_patterns[pad->num][1][0]; // r + usb_led_buf[7] = rgbled_patterns[pad->num][1][1]; // g + usb_led_buf[8] = rgbled_patterns[pad->num][1][2]; // b + } + + if (power == charging) { + printf("ds5: blink\n"); + // Blink when charging + usb_led_buf[9] = 0x80; // Time to flash bright (255 = 2.5 seconds) + usb_led_buf[10] = 0x80; // Time to flash dark (255 = 2.5 seconds) + } + + pad->interrupt_write(pad, usb_led_buf, 32, NULL, NULL); + + pad->update_num = 0; + pad->update_rum = 0; + pad->power = power; + } + */ + if (pad->num < 0 && r->PSButton == 1) { + DPRINTF("ds5: pademu_connect\n"); + pademu_connect(&padf[pad_idx]); + } +} + +/************************************************************/ +/* DS5BT Commands */ +/************************************************************/ + +static void ds5bt_set_rumble(struct pad_funcs *pf, u8 lrum, u8 rrum) +{ + ds5bt_pad_t *pad = pf->priv; + + WaitSema(bt_dev.hid_sema); + + if ((pad->lrum != lrum) || (pad->rrum != rrum)) { + pad->lrum = lrum; + pad->rrum = rrum; + pad->update_rum = 1; + } + + SignalSema(bt_dev.hid_sema); +} + +static int ds5bt_get_data(struct pad_funcs *pf, u8 *dst, int size, int port) +{ + ds5bt_pad_t *pad = pf->priv; + int ret; + + WaitSema(bt_dev.hid_sema); + + mips_memcpy(dst, pad->data, size); + ret = pad->analog_btn & 1; + + SignalSema(bt_dev.hid_sema); + + return ret; +} + +static void ds5bt_set_mode(struct pad_funcs *pf, int mode, int lock) +{ + ds5bt_pad_t *pad = pf->priv; + + WaitSema(bt_dev.hid_sema); + + if (lock == 3) + pad->analog_btn = 3; + else + pad->analog_btn = mode; + + SignalSema(bt_dev.hid_sema); +} + +static int ds5bt_get_status(struct pad_funcs *pf) +{ + ds5bt_pad_t *pad = pf->priv; + int ret; + + WaitSema(bt_dev.hid_sema); + + ret = pad->status; + + SignalSema(bt_dev.hid_sema); + + return ret; +} + +static int ds5bt_get_model(struct pad_funcs *pf) +{ + (void)pf; + return 3; +} + +int ds5bt_init(u8 pads, u8 options) +{ + int ret, i; + + for (i = 0; i < MAX_PADS; i++) + ds5pad[i].enabled = (pads >> i) & 1; + + ds5pad_init(); + + enable_fake = options & 1; + + bt_dev.hid_sema = CreateMutex(IOP_MUTEX_UNLOCKED); + + if (bt_dev.hid_sema < 0) { + DPRINTF("Failed to allocate semaphore.\n"); + return 0; + } + + ret = sceUsbdRegisterLdd(&bt_driver); + + if (ret != USB_RC_OK) { + DPRINTF("Error registering USB devices: %02X\n", ret); + return 0; + } + + return 1; +} + +void ds5bt_reset() +{ + int pad; + + if (bt_dev.status & DS5BT_STATE_USB_AUTHORIZED) { + for (pad = 0; pad < MAX_PADS; pad++) { + WaitSema(bt_dev.hid_sema); + ds5pad_status_set(DS5BT_STATE_DISCONNECT_REQUEST, pad); + SignalSema(bt_dev.hid_sema); + while (1) { + DelayThread(500); + WaitSema(bt_dev.hid_sema); + if (!ds5pad_status_check(DS5BT_STATE_RUNNING, pad)) { + SignalSema(bt_dev.hid_sema); + break; + } + SignalSema(bt_dev.hid_sema); + } + } + DelayThread(1000000); + bt_disconnect(bt_dev.devId); + } +} \ No newline at end of file diff --git a/modules/pademu/ds5bt.h b/modules/pademu/ds5bt.h new file mode 100644 index 000000000..dc2e1fd74 --- /dev/null +++ b/modules/pademu/ds5bt.h @@ -0,0 +1,59 @@ +#ifndef _DS5BT_H_ +#define _DS5BT_H_ + +#include "irx.h" + +#define PENDING 1 +#define SUCCESSFUL 0 + +typedef struct +{ + u16 hci_handle; // hci connection handle + u16 control_scid; // Channel endpoint on command destination + u16 interrupt_scid; // Channel endpoint on interrupt destination + u8 bdaddr[6]; + u8 enabled; + u8 status; + u8 isfake; + u8 type; // 0 - ds3, 1 - ds4 + u8 oldled[4]; // rgb for ds4 and blink + u8 lrum; + u8 rrum; + u8 update_rum; + union + { + struct ds2report ds2; + u8 data[18]; + }; + u8 num; + u8 analog_btn; + u8 btn_delay; +} ds5bt_pad_t; + +enum eDS5BTStatus { + DS5BT_STATE_USB_DISCONNECTED = 0x00, + DS5BT_STATE_USB_AUTHORIZED = 0x01, + DS5BT_STATE_USB_CONFIGURED = 0x02, + DS5BT_STATE_CONNECTED = 0x04, + DS5BT_STATE_RUNNING = 0x08, + DS5BT_STATE_DISCONNECTING = 0x10, + DS5BT_STATE_DISCONNECT_REQUEST = 0x20, +}; + +#define ds5pad_status_clear(flag, pad) ds5pad[pad].status &= ~flag +#define ds5pad_status_set(flag, pad) ds5pad[pad].status |= flag +#define ds5pad_status_check(flag, pad) (ds5pad[pad].status & flag) + +#define hci_event_flag_clear(flag) hci_event_flag &= ~flag +#define hci_event_flag_set(flag) hci_event_flag |= flag +#define hci_event_flag_check(flag) (hci_event_flag & flag) + +#define l2cap_handle_ok(handle) (((u16)(l2cap_buf[0] | (l2cap_buf[1] << 8)) & 0x0FFF) == (u16)(handle & 0x0FFF)) +#define l2cap_control_channel ((l2cap_buf[6] | (l2cap_buf[7] << 8)) == 0x0001) // Channel ID for ACL-U +#define l2cap_interrupt_channel ((l2cap_buf[6] | (l2cap_buf[7] << 8)) == interrupt_dcid) +#define l2cap_command_channel ((l2cap_buf[6] | (l2cap_buf[7] << 8)) == control_dcid) + +int ds5bt_init(u8 pads, u8 options); +void ds5bt_reset(); + +#endif diff --git a/modules/pademu/ds5usb.c b/modules/pademu/ds5usb.c new file mode 100644 index 000000000..2a262cb79 --- /dev/null +++ b/modules/pademu/ds5usb.c @@ -0,0 +1,552 @@ +/* based on https://github.com/IonAgorria/Arduino-PSRemote */ +/* and https://github.com/felis/USB_Host_Shield_2.0 */ + +#include "types.h" +#include "loadcore.h" +#include "stdio.h" +#include "sifrpc.h" +#include "sysclib.h" +#include "usbd.h" +#include "usbd_macro.h" +#include "thbase.h" +#include "thsemap.h" +#include "sys_utils.h" +#include "pademu.h" +#include "pademu_common.h" +#include "ds5usb.h" +#include "padmacro.h" + +#define MODNAME "DS4USB" + +#ifdef DEBUG +#define DPRINTF(format, args...) \ + printf(MODNAME ": " format, ##args) +#else +#define DPRINTF(args...) +#endif + +#define REQ_USB_OUT (USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) +#define REQ_USB_IN (USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) + +#define MAX_PADS 4 + +static u8 rgbled_patterns[][2][3] = + { + {{0x00, 0x00, 0x10}, {0x00, 0x00, 0x7F}}, // light blue/blue + {{0x00, 0x10, 0x00}, {0x00, 0x7F, 0x00}}, // light green/green + {{0x10, 0x10, 0x00}, {0x7F, 0x7F, 0x00}}, // light yellow/yellow + {{0x00, 0x10, 0x10}, {0x00, 0x7F, 0x7F}}, // light cyan/cyan +}; + +static u8 usb_buf[MAX_BUFFER_SIZE + 32] __attribute((aligned(4))) = {0}; + +static int usb_probe(int devId); +static int usb_connect(int devId); +static int usb_disconnect(int devId); + +static void usb_release(int pad); +static void usb_config_set(int result, int count, void *arg); + +static UsbDriver usb_driver = {NULL, NULL, "ds4usb", usb_probe, usb_connect, usb_disconnect}; + +static void readReport(u8 *data, int pad); +static int LEDRumble(u8 *led, u8 lrum, u8 rrum, int pad); + +static int ds5usb_get_model(struct pad_funcs *pf); +static int ds5usb_get_data(struct pad_funcs *pf, u8 *dst, int size, int port); +static void ds5usb_set_rumble(struct pad_funcs *pf, u8 lrum, u8 rrum); +static void ds5usb_set_mode(struct pad_funcs *pf, int mode, int lock); + +static ds5usb_device ds5pad[MAX_PADS]; +static struct pad_funcs padf[MAX_PADS]; + +static int usb_probe(int devId) +{ + UsbDeviceDescriptor *device = NULL; + + DPRINTF("probe: devId=%i\n", devId); + + device = (UsbDeviceDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_DEVICE); + if (device == NULL) { + DPRINTF("Error - Couldn't get device descriptor\n"); + return 0; + } + + if (device->idVendor == SONY_VID && (device->idProduct == GUITAR_HERO_PS3_PID || device->idProduct == ROCK_BAND_PS3_PID)) { + return 1; + } + + if (device->idVendor == DS_VID && (device->idProduct == DS5_PID)) + return 1; + + return 0; +} + +static int usb_connect(int devId) +{ + int pad, epCount; + UsbDeviceDescriptor *device; + UsbConfigDescriptor *config; + UsbEndpointDescriptor *endpoint; + + DPRINTF("connect: devId=%i\n", devId); + + for (pad = 0; pad < MAX_PADS; pad++) { + if (ds5pad[pad].devId == -1 && ds5pad[pad].enabled) + break; + } + + if (pad >= MAX_PADS) { + DPRINTF("Error - only %d device allowed !\n", MAX_PADS); + return 1; + } + + PollSema(ds5pad[pad].sema); + + ds5pad[pad].devId = devId; + + ds5pad[pad].status = DS5USB_STATE_AUTHORIZED; + + ds5pad[pad].controlEndp = sceUsbdOpenPipe(devId, NULL); + + device = (UsbDeviceDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_DEVICE); + config = (UsbConfigDescriptor *)sceUsbdScanStaticDescriptor(devId, device, USB_DT_CONFIG); + + ds5pad[pad].type = DS5; + epCount = 20; // ds4 v2 returns interface->bNumEndpoints as 0 + + endpoint = (UsbEndpointDescriptor *)sceUsbdScanStaticDescriptor(devId, NULL, USB_DT_ENDPOINT); + + do { + if (endpoint->bmAttributes == USB_ENDPOINT_XFER_INT) { + if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN && ds5pad[pad].interruptEndp < 0) { + ds5pad[pad].interruptEndp = sceUsbdOpenPipe(devId, endpoint); + DPRINTF("register Event endpoint id =%i addr=%02X packetSize=%i\n", ds5pad[pad].interruptEndp, endpoint->bEndpointAddress, (unsigned short int)endpoint->wMaxPacketSizeHB << 8 | endpoint->wMaxPacketSizeLB); + } + if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT && ds5pad[pad].outEndp < 0) { + ds5pad[pad].outEndp = sceUsbdOpenPipe(devId, endpoint); + DPRINTF("register Output endpoint id =%i addr=%02X packetSize=%i\n", ds5pad[pad].outEndp, endpoint->bEndpointAddress, (unsigned short int)endpoint->wMaxPacketSizeHB << 8 | endpoint->wMaxPacketSizeLB); + } + } + + endpoint = (UsbEndpointDescriptor *)((char *)endpoint + endpoint->bLength); + + } while (epCount--); + + if (ds5pad[pad].interruptEndp < 0 || ds5pad[pad].outEndp < 0) { + usb_release(pad); + return 1; + } + + ds5pad[pad].status |= DS5USB_STATE_CONNECTED; + + sceUsbdSetConfiguration(ds5pad[pad].controlEndp, config->bConfigurationValue, usb_config_set, (void *)pad); + SignalSema(ds5pad[pad].sema); + + return 0; +} + +static int usb_disconnect(int devId) +{ + u8 pad; + + DPRINTF("disconnect: devId=%i\n", devId); + + for (pad = 0; pad < MAX_PADS; pad++) { + if (ds5pad[pad].devId == devId) + break; + } + + if (pad < MAX_PADS) { + usb_release(pad); + pademu_disconnect(&padf[pad]); + } + + return 0; +} + +static void usb_release(int pad) +{ + PollSema(ds5pad[pad].sema); + + if (ds5pad[pad].interruptEndp >= 0) + sceUsbdClosePipe(ds5pad[pad].interruptEndp); + + if (ds5pad[pad].outEndp >= 0) + sceUsbdClosePipe(ds5pad[pad].outEndp); + + ds5pad[pad].controlEndp = -1; + ds5pad[pad].interruptEndp = -1; + ds5pad[pad].outEndp = -1; + ds5pad[pad].devId = -1; + ds5pad[pad].status = DS5USB_STATE_DISCONNECTED; + + SignalSema(ds5pad[pad].sema); +} + +static int usb_resulCode; + +static void usb_data_cb(int resultCode, int bytes, void *arg) +{ + int pad = (int)arg; + + DPRINTF("usb_data_cb: res %d, bytes %d, arg %p \n", resultCode, bytes, arg); + + usb_resulCode = resultCode; + + SignalSema(ds5pad[pad].sema); +} + +static void usb_cmd_cb(int resultCode, int bytes, void *arg) +{ + int pad = (int)arg; + + DPRINTF("usb_cmd_cb: res %d, bytes %d, arg %p \n", resultCode, bytes, arg); + + SignalSema(ds5pad[pad].cmd_sema); +} + +static void usb_config_set(int result, int count, void *arg) +{ + int pad = (int)arg; + u8 led[4]; + + PollSema(ds5pad[pad].sema); + + ds5pad[pad].status |= DS5USB_STATE_CONFIGURED; + + led[0] = rgbled_patterns[pad][1][0]; + led[1] = rgbled_patterns[pad][1][1]; + led[2] = rgbled_patterns[pad][1][2]; + led[3] = 0; + + LEDRumble(led, 0, 0, pad); + + ds5pad[pad].status |= DS5USB_STATE_RUNNING; + + SignalSema(ds5pad[pad].sema); + + pademu_connect(&padf[pad]); +} + +#define MAX_DELAY 10 + +static void readReport(u8 *data, int pad_idx) +{ + ds5usb_device *pad = &ds5pad[pad_idx]; + struct ds5report *r = (struct ds5report *)data; + uint8_t up = 0, down = 0, left = 0, right = 0; + + /* + * By default we get report 0x01 with length 64 + * But we can also get report 0x31 with length 78 + */ + if (data[0] != 0x01) { + DPRINTF("ds5: Unexpected report_id, got: 0x%02x, want: 0x01\n", data[0]); + return; + } + + switch (r->Dpad) { + case 0: + up = 1; + break; + case 1: + up = 1; + right = 1; + break; + case 2: + right = 1; + break; + case 3: + down = 1; + right = 1; + break; + case 4: + down = 1; + break; + case 5: + down = 1; + left = 1; + break; + case 6: + left = 1; + break; + case 7: + up = 1; + left = 1; + break; + } + + pad->data[0] = ~(r->Share | r->L3 << 1 | r->R3 << 2 | r->Option << 3 | up << 4 | right << 5 | down << 6 | left << 7); + pad->data[1] = ~(r->L2 | r->R2 << 1 | r->L1 << 2 | r->R1 << 3 | r->Triangle << 4 | r->Circle << 5 | r->Cross << 6 | r->Square << 7); + + pad->data[2] = r->RightStickX; // rx + pad->data[3] = r->RightStickY; // ry + pad->data[4] = r->LeftStickX; // lx + pad->data[5] = r->LeftStickY; // ly + + pad->data[6] = right * 255; // right + pad->data[7] = left * 255; // left + pad->data[8] = up * 255; // up + pad->data[9] = down * 255; // down + + pad->data[10] = r->Triangle * 255; // triangle + pad->data[11] = r->Circle * 255; // circle + pad->data[12] = r->Cross * 255; // cross + pad->data[13] = r->Square * 255; // square + + pad->data[14] = r->L1 * 255; // L1 + pad->data[15] = r->R1 * 255; // R1 + pad->data[16] = r->PressureL2; // L2 + pad->data[17] = r->PressureR2; // R2 + /* + enum power_state power = normal; + if (r->Power <= 2) + power = empty; + else if (r->Power == 10) + power = charging; + else if (r->Power == 11) + power = full; + + int update = pad->update_num + pad->update_rum + (power != pad->power); + if (update != 0) { + printf("ds5: update led/rumble, power=%d\n", r->Power); + memset(usb_led_buf, 0, sizeof(usb_led_buf)); + + usb_led_buf[0] = 0x05; + usb_led_buf[1] = 0xFF; + + usb_led_buf[4] = pad->rrum * 255; // ds5 has full control + usb_led_buf[5] = pad->lrum; + + if (pad->num < 0) { + // Set default color + printf("ds5: set default color\n"); + usb_led_buf[6] = 0x7f; // r + usb_led_buf[7] = 0x7f; // g + usb_led_buf[8] = 0x7f; // b + } else { + // Set controller color + printf("ds5: set controller color %d\n", pad->num); + usb_led_buf[6] = rgbled_patterns[pad->num][1][0]; // r + usb_led_buf[7] = rgbled_patterns[pad->num][1][1]; // g + usb_led_buf[8] = rgbled_patterns[pad->num][1][2]; // b + } + + if (power == charging) { + printf("ds5: blink\n"); + // Blink when charging + usb_led_buf[9] = 0x80; // Time to flash bright (255 = 2.5 seconds) + usb_led_buf[10] = 0x80; // Time to flash dark (255 = 2.5 seconds) + } + + pad->interrupt_write(pad, usb_led_buf, 32, NULL, NULL); + + pad->update_num = 0; + pad->update_rum = 0; + pad->power = power; + } + */ + if (pad->num < 0 && r->PSButton == 1) { + DPRINTF("ds5: pademu_connect\n"); + pademu_connect(&padf[pad_idx]); + } +} + +static int LEDRumble(u8 *led, u8 lrum, u8 rrum, int pad) +{ + int ret = 0; + + PollSema(ds5pad[pad].cmd_sema); + + mips_memset(usb_buf, 0, sizeof(usb_buf)); + + usb_buf[0] = 0x05; + usb_buf[1] = 0xFF; + + usb_buf[4] = rrum * 255; // ds4 has full control + usb_buf[5] = lrum; + + usb_buf[6] = led[0]; // r + usb_buf[7] = led[1]; // g + usb_buf[8] = led[2]; // b + + if (led[3]) // means charging, so blink + { + usb_buf[9] = 0x80; // Time to flash bright (255 = 2.5 seconds) + usb_buf[10] = 0x80; // Time to flash dark (255 = 2.5 seconds) + } + + ret = sceUsbdInterruptTransfer(ds5pad[pad].outEndp, usb_buf, 32, usb_cmd_cb, (void *)pad); + + ds5pad[pad].oldled[0] = led[0]; + ds5pad[pad].oldled[1] = led[1]; + ds5pad[pad].oldled[2] = led[2]; + ds5pad[pad].oldled[3] = led[3]; + + return ret; +} + +static unsigned int timeout(void *arg) +{ + int sema = (int)arg; + iSignalSema(sema); + return 0; +} + +static void TransferWait(int sema) +{ + iop_sys_clock_t cmd_timeout; + + cmd_timeout.lo = 200000; + cmd_timeout.hi = 0; + + if (SetAlarm(&cmd_timeout, timeout, (void *)sema) == 0) { + WaitSema(sema); + CancelAlarm(timeout, NULL); + } +} + +static void ds5usb_set_rumble(struct pad_funcs *pf, u8 lrum, u8 rrum) +{ + ds5usb_device *pad = pf->priv; + WaitSema(pad->sema); + + if ((pad->lrum != lrum) || (pad->rrum != rrum)) { + pad->lrum = lrum; + pad->rrum = rrum; + pad->update_rum = 1; + } + + SignalSema(pad->sema); +} + +static int ds5usb_get_data(struct pad_funcs *pf, u8 *dst, int size, int port) +{ + ds5usb_device *pad = pf->priv; + int ret = 0; + + WaitSema(pad->sema); + + PollSema(pad->sema); + + ret = sceUsbdInterruptTransfer(pad->interruptEndp, usb_buf, MAX_BUFFER_SIZE, usb_data_cb, (void *)port); + + if (ret == USB_RC_OK) { + TransferWait(pad->sema); + if (!usb_resulCode) + readReport(usb_buf, port); + + usb_resulCode = 1; + } else { + DPRINTF("ds5usb_get_data usb transfer error %d\n", ret); + } + + mips_memcpy(dst, pad->data, size); + ret = pad->analog_btn & 1; + + if (pad->update_rum) { + ret = LEDRumble(pad->oldled, pad->lrum, pad->rrum, port); + if (ret == USB_RC_OK) + TransferWait(pad->cmd_sema); + else + DPRINTF("LEDRumble usb transfer error %d\n", ret); + + pad->update_rum = 0; + } + + SignalSema(pad->sema); + + return ret; +} + +static void ds5usb_set_mode(struct pad_funcs *pf, int mode, int lock) +{ + ds5usb_device *pad = pf->priv; + WaitSema(pad->sema); + if (lock == 3) + pad->analog_btn = 3; + else + pad->analog_btn = mode; + SignalSema(pad->sema); +} + +void ds5usb_reset() +{ + int pad; + + for (pad = 0; pad < MAX_PADS; pad++) + usb_release(pad); +} + +static int ds5usb_get_status(struct pad_funcs *pf) +{ + ds5usb_device *pad = pf->priv; + int ret; + + WaitSema(pad->sema); + + ret = pad->status; + + SignalSema(pad->sema); + + return ret; +} + +static int ds5usb_get_model(struct pad_funcs *pf) +{ + (void)pf; + + return 3; +} + +int ds5usb_init(u8 pads, u8 options) +{ + int pad; + + for (pad = 0; pad < MAX_PADS; pad++) { + ds5pad[pad].status = 0; + ds5pad[pad].devId = -1; + ds5pad[pad].oldled[0] = 0; + ds5pad[pad].oldled[1] = 0; + ds5pad[pad].oldled[2] = 0; + ds5pad[pad].oldled[3] = 0; + ds5pad[pad].lrum = 0; + ds5pad[pad].rrum = 0; + ds5pad[pad].update_rum = 1; + ds5pad[pad].sema = -1; + ds5pad[pad].cmd_sema = -1; + ds5pad[pad].controlEndp = -1; + ds5pad[pad].interruptEndp = -1; + ds5pad[pad].enabled = (pads >> pad) & 1; + ds5pad[pad].type = 0; + + ds5pad[pad].data[0] = 0xFF; + ds5pad[pad].data[1] = 0xFF; + ds5pad[pad].analog_btn = 0; + + mips_memset(&ds5pad[pad].data[2], 0x7F, 4); + mips_memset(&ds5pad[pad].data[6], 0x00, 12); + + ds5pad[pad].sema = CreateMutex(IOP_MUTEX_UNLOCKED); + ds5pad[pad].cmd_sema = CreateMutex(IOP_MUTEX_UNLOCKED); + + if (ds5pad[pad].sema < 0 || ds5pad[pad].cmd_sema < 0) { + DPRINTF("Failed to allocate I/O semaphore.\n"); + return 0; + } + padf[pad].priv = &ds5pad[pad]; + padf[pad].get_status = ds5usb_get_status; + padf[pad].get_model = ds5usb_get_model; + padf[pad].get_data = ds5usb_get_data; + padf[pad].set_rumble = ds5usb_set_rumble; + padf[pad].set_mode = ds5usb_set_mode; + } + + if (sceUsbdRegisterLdd(&usb_driver) != USB_RC_OK) { + DPRINTF("Error registering USB devices\n"); + return 0; + } + + return 1; +} diff --git a/modules/pademu/ds5usb.h b/modules/pademu/ds5usb.h new file mode 100644 index 000000000..31b1de7a3 --- /dev/null +++ b/modules/pademu/ds5usb.h @@ -0,0 +1,44 @@ +#ifndef _DS5USB_H_ +#define _DS5USB_H_ + +#include "irx.h" + +#define MAX_BUFFER_SIZE 64 // Size of general purpose data buffer + +typedef struct _usb_ds5 +{ + int devId; + int sema; + int cmd_sema; + int controlEndp; + int interruptEndp; + int outEndp; + uint8_t enabled; + uint8_t status; + uint8_t type; // 2 - ds5, 3 - rock band guitar + uint8_t oldled[4]; // rgb for ds4 and blink + uint8_t lrum; + uint8_t rrum; + uint8_t update_rum; + union + { + struct ds2report ds2; + uint8_t data[18]; + }; + u8 num; + uint8_t analog_btn; + uint8_t btn_delay; +} ds5usb_device; + +enum eDS5USBStatus { + DS5USB_STATE_DISCONNECTED = 0x00, + DS5USB_STATE_AUTHORIZED = 0x01, + DS5USB_STATE_CONFIGURED = 0x02, + DS5USB_STATE_CONNECTED = 0x04, + DS5USB_STATE_RUNNING = 0x08, +}; + +int ds5usb_init(u8 pads, u8 options); +void ds5usb_reset(); + +#endif \ No newline at end of file diff --git a/modules/pademu/pademu.c b/modules/pademu/pademu.c index 3a6390bf3..ab757188a 100644 --- a/modules/pademu/pademu.c +++ b/modules/pademu/pademu.c @@ -26,6 +26,13 @@ static struct pad_funcs *padf[MAX_PORTS]; #endif +#ifdef USE_DS5 + +#include "ds5bt.h" +#include "ds5usb.h" + +#endif + #define MODNAME "pademu" IRX_ID(MODNAME, 1, 1); @@ -187,6 +194,10 @@ void _exit(int mode) ds4bt_reset(); ds4usb_reset(); #endif +#ifdef USE_DS5 + ds5bt_reset(); + ds5usb_reset(); +#endif } int install_sio2hook() @@ -371,6 +382,10 @@ void pademu(sio2_transfer_data_t *td) #ifdef USE_DS4 pad_inited = ds4bt_init(pad_enable, pad_options); pad_inited = ds4usb_init(pad_enable, pad_options); +#endif +#ifdef USE_DS5 + pad_inited = ds5bt_init(pad_enable, pad_options); + pad_inited = ds5usb_init(pad_enable, pad_options); #endif } diff --git a/modules/pademu/pademu_common.h b/modules/pademu/pademu_common.h index e25dd4d3c..d92498e0b 100644 --- a/modules/pademu/pademu_common.h +++ b/modules/pademu/pademu_common.h @@ -11,10 +11,12 @@ #define DS3_PID 0x0268 // PS3 Controller #define DS4_PID 0x05C4 // PS4 Controller #define DS4_PID_SLIM 0x09CC // PS4 Slim Controller +#define DS5_PID 0x0CE6 // PS5 Controller #define GUITAR_HERO_PS3_PID 0x0100 // PS3 Guitar Hero Guitar #define ROCK_BAND_PS3_PID 0x0200 // PS3 Rock Band Guitar #define DS3 0 #define DS4 1 +#define DS5 2 #define MAX_BUFFER_SIZE 64 // Size of general purpose data buffer #define MAX_PORTS 4 @@ -302,6 +304,68 @@ typedef struct _usb_ds34 uint8_t btn_delay; } ds34usb_device; +struct ds5report +{ + uint8_t ReportID; + uint8_t LeftStickX; // left Joystick X axis 0 - 255, 128 is mid + uint8_t LeftStickY; // left Joystick Y axis 0 - 255, 128 is mid + uint8_t RightStickX; // right Joystick X axis 0 - 255, 128 is mid + uint8_t RightStickY; // right Joystick Y axis 0 - 255, 128 is mid + + uint8_t PressureL2; // digital Pad L2 button Pressure 0 - 255 + uint8_t PressureR2; // digital Pad R2 button Pressure 0 - 255 + + uint8_t Unknown0; + + uint8_t Dpad : 4; // hat format, 0x08 is released, 0=N, 1=NE, 2=E, 3=SE, 4=S, 5=SW, 6=W, 7=NW + uint8_t Square : 1; + uint8_t Cross : 1; + uint8_t Circle : 1; + uint8_t Triangle : 1; + + uint8_t L1 : 1; + uint8_t R1 : 1; + uint8_t L2 : 1; + uint8_t R2 : 1; + uint8_t Share : 1; + uint8_t Option : 1; + uint8_t L3 : 1; + uint8_t R3 : 1; + + uint8_t PSButton : 1; + uint8_t TPad : 1; + uint8_t Mute : 1; + uint8_t Unknown1 : 5; + + uint8_t Counter2; + uint8_t Counter3; + uint8_t Battery; // battery level from 0x00 to 0xff + int16_t AccelX; + int16_t AccelY; + int16_t AccelZ; + int16_t GyroZ; + int16_t GyroY; + int16_t GyroX; + uint8_t Reserved1[5]; // Unknown + uint8_t Power : 4; // 0 - 9 while unplugged, 0 - 10 while plugged in, 11 charge complete + uint8_t Usb_plugged : 1; + uint8_t Headphones : 1; + uint8_t Microphone : 1; + uint8_t Padding : 1; + uint8_t Reserved2[2]; // Unknown + uint8_t TPpack; // number of trackpad packets (0x00 to 0x04) + uint8_t PackCounter; // packet counter + uint8_t Finger1ID : 7; // counter + uint8_t Finger1Active : 1; // 0 - active, 1 - unactive + uint16_t Finger1X : 12; // finger 1 coordinates resolution 1920x943 + uint16_t Finger1Y : 12; + uint8_t Finger2ID : 7; + uint8_t Finger2Active : 1; + uint16_t Finger2X : 12; // finger 2 coordinates resolution 1920x943 + uint16_t Finger2Y : 12; + +} __attribute__((packed)); + enum eHID { // {{{ /* HID event flag */