Skip to content

Commit d9a9873

Browse files
author
ll0s0ll
committed
Modify BTD files
1 parent 4e0657c commit d9a9873

File tree

5 files changed

+452
-161
lines changed

5 files changed

+452
-161
lines changed

‎.gitignore

+2-1
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
*.bak
22
*.zip
3-
*.rar
3+
*.rar
4+
.DS_Store

‎BTD.cpp

+208-16
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,10 @@
1313
Kristian Lauszus, TKJ Electronics
1414
Web : http://www.tkjelectronics.com
1515
e-mail : kristianl@tkjelectronics.com
16-
*/
16+
17+
Modified 9 April 2013 by Shun Ito
18+
Web : http://ll0s0ll.wordpress.com/
19+
*/
1720

1821
#include "BTD.h"
1922
#define DEBUG // Uncomment to print data for debugging
@@ -477,15 +480,30 @@ void BTD::HCI_event_task() {
477480
#endif
478481
hci_link_key_request_negative_reply();
479482
break;
480-
483+
//-- MODIFIED> -----------------------------------------------------------//
481484
case EV_AUTHENTICATION_COMPLETE:
485+
#ifdef DEBUG
486+
Notify(PSTR("\r\n[HCI_EV] AUTHENTICATION_COMPLETE..."), 0x80);
487+
if (!hcibuf[2])
488+
Notify(PSTR("OK"), 0x80);
489+
#endif
490+
if (!hcibuf[2]) { //Status
491+
#ifdef EXTRADEBUG
492+
Notify(PSTR(" Connection_Handle:"), 0x80);
493+
PrintHex<uint8_t>(hcibuf[3], 0x80);
494+
Notify(PSTR(" "), 0x80);
495+
PrintHex<uint8_t>(hcibuf[4], 0x80);
496+
#endif
497+
hci_Set_Connection_Encryption(hci_handle);
498+
}
482499
if (pairWithWii && !connectToWii) {
483500
#ifdef DEBUG
484501
Notify(PSTR("\r\nPairing successful"), 0x80);
485502
#endif
486503
connectToWii = true; // Only send the ACL data to the Wii service
487504
}
488505
break;
506+
//-- <MODIFIED -----------------------------------------------------------//
489507
/* We will just ignore the following events */
490508
case EV_NUM_COMPLETE_PKT:
491509
case EV_ROLE_CHANGED:
@@ -496,9 +514,81 @@ void BTD::HCI_event_task() {
496514
case EV_MAX_SLOTS_CHANGE:
497515
case EV_QOS_SETUP_COMPLETE:
498516
case EV_LINK_KEY_NOTIFICATION:
499-
case EV_ENCRYPTION_CHANGE:
517+
//-- MODIFIED> -----------------------------------------------------------//
518+
// case EV_ENCRYPTION_CHANGE:
519+
//-- <MODIFIED -----------------------------------------------------------//
500520
case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE:
501521
break;
522+
//-- MODIFIED> -----------------------------------------------------------//
523+
case EV_IO_CAPABILITY_REQUEST:
524+
#ifdef EXTRADEBUG
525+
Notify(PSTR("\r\n[HCI_EV] IO_CAPABILITY_REQUEST"), 0x80);
526+
#endif
527+
disc_bdaddr[0] = hcibuf[2];
528+
disc_bdaddr[1] = hcibuf[3];
529+
disc_bdaddr[2] = hcibuf[4];
530+
disc_bdaddr[3] = hcibuf[5];
531+
disc_bdaddr[4] = hcibuf[6];
532+
disc_bdaddr[5] = hcibuf[7];
533+
534+
hci_IO_Capability_Request_Reply();
535+
break;
536+
537+
case EV_IO_CAPABILITY_RESPONSE:
538+
#ifdef EXTRADEBUG
539+
Notify(PSTR("\r\n[HCI_EV] IO_CAPABILITY_RESPONSE - BD_ADDR:"), 0x80);
540+
PrintHex<uint8_t>(hcibuf[2], 0x80);
541+
Notify(PSTR(" "), 0x80);
542+
PrintHex<uint8_t>(hcibuf[3], 0x80);
543+
Notify(PSTR(" "), 0x80);
544+
PrintHex<uint8_t>(hcibuf[4], 0x80);
545+
Notify(PSTR(" "), 0x80);
546+
PrintHex<uint8_t>(hcibuf[5], 0x80);
547+
Notify(PSTR(" "), 0x80);
548+
PrintHex<uint8_t>(hcibuf[6], 0x80);
549+
Notify(PSTR(" "), 0x80);
550+
PrintHex<uint8_t>(hcibuf[7], 0x80);
551+
Notify(PSTR(" IO_Capability:"), 0x80);
552+
PrintHex<uint8_t>(hcibuf[8], 0x80);
553+
Notify(PSTR(" OOB_Data_Present:"), 0x80);
554+
PrintHex<uint8_t>(hcibuf[9], 0x80);
555+
Notify(PSTR(" Authentication_Requirements:"), 0x80);
556+
PrintHex<uint8_t>(hcibuf[10], 0x80);
557+
#endif
558+
break;
559+
560+
case EV_USER_CONFIRMATION_REQUEST:
561+
#ifdef DEBUG
562+
Notify(PSTR("\r\n[HCI_EV] User_Confirmation_Request - Numeric_Value:0x"), 0x80);
563+
PrintHex<uint8_t>(hcibuf[11], 0x80);
564+
PrintHex<uint8_t>(hcibuf[10], 0x80);
565+
PrintHex<uint8_t>(hcibuf[9], 0x80);
566+
PrintHex<uint8_t>(hcibuf[8], 0x80);
567+
#endif
568+
hci_User_Confirmation_Request_Reply();
569+
break;
570+
571+
case EV_SIMPLE_PAIRING_COMPLETE:
572+
#ifdef DEBUG
573+
Notify(PSTR("\r\n[HCI_EV] SIMPLE_PAIRING_COMPLETE... "), 0x80);
574+
if (!hcibuf[2])
575+
Notify(PSTR("OK"), 0x80);
576+
#endif
577+
break;
578+
579+
case EV_ENCRYPTION_CHANGE:
580+
#ifdef DEBUG
581+
Notify(PSTR("\r\n[HCI_EV] ENCRYPTION_CHANGE... "), 0x80);
582+
#endif
583+
if (!hcibuf[2]) { //Status
584+
#ifdef DEBUG
585+
Notify(PSTR("OK"), 0x80);
586+
#endif
587+
m_simple_pairing_completed = true;
588+
}
589+
break;
590+
591+
//-- <MODIFIED -----------------------------------------------------------//
502592
#ifdef EXTRADEBUG
503593
default:
504594
if (hcibuf[0] != 0x00) {
@@ -585,17 +675,47 @@ void BTD::HCI_task() {
585675
hci_state = HCI_CHECK_WII_SERVICE;
586676
}
587677
break;
588-
678+
//-- MODIFIED> -----------------------------------------------------------//
589679
case HCI_SET_NAME_STATE:
590680
if (hci_cmd_complete) {
591681
#ifdef DEBUG
592682
Notify(PSTR("\r\nThe name is set to: "), 0x80);
593683
Serial.print(btdName);
594684
#endif
595-
hci_state = HCI_CHECK_WII_SERVICE;
685+
hci_write_class_of_device();
686+
hci_state = HCI_WRITE_CoD_STATE;
596687
}
597688
break;
598-
689+
690+
case HCI_WRITE_CoD_STATE:
691+
if (hci_cmd_complete) {
692+
#ifdef DEBUG
693+
Notify(PSTR("\r\n[HCI_EV] Write_CoD_CMD COMPLETE"), 0x80);
694+
#endif
695+
hci_Write_Simple_Pairing_Mode();
696+
hci_state = HCI_WRITE_SIMPLE_PAIRING_STATE;
697+
}
698+
break;
699+
700+
case HCI_WRITE_SIMPLE_PAIRING_STATE:
701+
if (hci_cmd_complete) {
702+
#ifdef DEBUG
703+
Notify(PSTR("\r\n[HCI_EV] WRITE_SIMPLE_PAIRING COMPLETE"), 0x80);
704+
#endif
705+
hci_Set_Event_Mask_SPM();
706+
hci_state = HCI_SET_EVENT_MASK_STATE;
707+
}
708+
break;
709+
710+
case HCI_SET_EVENT_MASK_STATE:
711+
if (hci_cmd_complete) {
712+
#ifdef DEBUG
713+
Notify(PSTR("\r\n[HCI_EV] SET_EVENT_MASK COMPLETE"), 0x80);
714+
#endif
715+
hci_state = HCI_CHECK_WII_SERVICE;
716+
}
717+
break;
718+
//-- <MODIFIED -----------------------------------------------------------//
599719
case HCI_CHECK_WII_SERVICE:
600720
if (pairWithWii) { // Check if it should try to connect to a wiimote
601721
#ifdef DEBUG
@@ -1015,17 +1135,89 @@ void BTD::hci_disconnect(uint16_t handle) { // This is called by the different s
10151135

10161136
HCI_Command(hcibuf, 6);
10171137
}
1018-
1019-
void BTD::hci_write_class_of_device() { // See http://bluetooth-pentest.narod.ru/software/bluetooth_class_of_device-service_generator.html
1020-
hcibuf[0] = 0x24; // HCI OCF = 3
1021-
hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1022-
hcibuf[2] = 0x03; // parameter length = 3
1023-
hcibuf[3] = 0x04; // Robot
1024-
hcibuf[4] = 0x08; // Toy
1025-
hcibuf[5] = 0x00;
1026-
1027-
HCI_Command(hcibuf, 6);
1138+
//-- MODIFIED> -----------------------------------------------------------//
1139+
void BTD::hci_write_class_of_device() {
1140+
hcibuf[0] = 0x24; // HCI OCF = 24
1141+
hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1142+
hcibuf[2] = 0x03; // Parameter Length = 3
1143+
hcibuf[3] = 0x40; // Minor Device Class - 0x40=keyboard 0xC0=Combo(keyboard/pointing device)
1144+
hcibuf[4] = 0x05; // Major Device Class - Peripheral
1145+
hcibuf[5] = 0x00; //
1146+
1147+
HCI_Command(hcibuf, 6);
1148+
}
1149+
void BTD::hci_Write_Simple_Pairing_Mode() {
1150+
hcibuf[0] = 0x56; // HCI OCF = 56
1151+
hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1152+
hcibuf[2] = 0x01; // Parameter Length = 3
1153+
hcibuf[3] = 0x01; // Simple_Pairing_Mode - 0x01=enabled
1154+
1155+
HCI_Command(hcibuf, 4);
1156+
}
1157+
void BTD::hci_Set_Event_Mask_SPM() {
1158+
hcibuf[0] = 0x01; // HCI OCF = 1
1159+
hcibuf[1] = 0x03 << 2; // HCI OGF = 3
1160+
hcibuf[2] = 0x08; // Parameter Length = 8
1161+
hcibuf[3] = 0xFF; // Event_Mask:0x00 ff 1f ff ff ff ff ff
1162+
hcibuf[4] = 0xFF; //
1163+
hcibuf[5] = 0xFF; //
1164+
hcibuf[6] = 0xFF; //
1165+
hcibuf[7] = 0xFF; //
1166+
hcibuf[8] = 0x1F; //
1167+
hcibuf[9] = 0xFF; //
1168+
hcibuf[10] = 0x00; //
1169+
1170+
HCI_Command(hcibuf, 11);
1171+
}
1172+
void BTD::hci_Authentication_Requested(uint16_t handle) {
1173+
hcibuf[0] = 0x11; // HCI OCF = 11
1174+
hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1175+
hcibuf[2] = 0x02; // Parameter Length = 8
1176+
hcibuf[3] = (uint8_t)(handle & 0xFF);//connection handle - low byte
1177+
hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F);//connection handle - high byte
1178+
1179+
HCI_Command(hcibuf, 5);
1180+
}
1181+
void BTD::hci_IO_Capability_Request_Reply() {
1182+
hcibuf[0] = 0x2B; // HCI OCF = 2B
1183+
hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1184+
hcibuf[2] = 0x09; // Parameter Length = 9
1185+
hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1186+
hcibuf[4] = disc_bdaddr[1];
1187+
hcibuf[5] = disc_bdaddr[2];
1188+
hcibuf[6] = disc_bdaddr[3];
1189+
hcibuf[7] = disc_bdaddr[4];
1190+
hcibuf[8] = disc_bdaddr[5];
1191+
hcibuf[9] = 0x03; //IO_Capability - 0x03 NoInputNoOutput
1192+
hcibuf[10] = 0x00; //OOB_Data_Present - 0x00 not present
1193+
hcibuf[11] = 0x05; //Authentication_Requirements - 0x00?
1194+
1195+
HCI_Command(hcibuf, 12);
1196+
}
1197+
void BTD::hci_User_Confirmation_Request_Reply() {
1198+
hcibuf[0] = 0x2C; // HCI OCF = 2C
1199+
hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1200+
hcibuf[2] = 0x06; // Parameter Length = 6
1201+
hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr
1202+
hcibuf[4] = disc_bdaddr[1];
1203+
hcibuf[5] = disc_bdaddr[2];
1204+
hcibuf[6] = disc_bdaddr[3];
1205+
hcibuf[7] = disc_bdaddr[4];
1206+
hcibuf[8] = disc_bdaddr[5];
1207+
1208+
HCI_Command(hcibuf, 9);
1209+
}
1210+
void BTD::hci_Set_Connection_Encryption(uint16_t handle) {
1211+
hcibuf[0] = 0x13; // HCI OCF = 13
1212+
hcibuf[1] = 0x01 << 2; // HCI OGF = 1
1213+
hcibuf[2] = 0x03; // Parameter Length = 9
1214+
hcibuf[3] = (uint8_t)(handle & 0xFF); //Connection_Handle - low byte
1215+
hcibuf[4] = (uint8_t)((handle >> 8) & 0x0F); //Connection_Handle - high byte
1216+
hcibuf[5] = 0x01; //Encryption_Enable - 0x01=ON
1217+
1218+
HCI_Command(hcibuf, 6);
10281219
}
1220+
//-- <MODIFIED -----------------------------------------------------------//
10291221
/*******************************************************************
10301222
* *
10311223
* HCI ACL Data Packet *

‎BTD.h

+38-2
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,11 @@
1313
Kristian Lauszus, TKJ Electronics
1414
Web : http://www.tkjelectronics.com
1515
e-mail : kristianl@tkjelectronics.com
16-
*/
16+
17+
Modified 9 April 2013 by Shun Ito
18+
Web : http://ll0s0ll.wordpress.com/
19+
*/
20+
1721

1822
#ifndef _btd_h_
1923
#define _btd_h_
@@ -27,8 +31,10 @@
2731
#define PS3NAVIGATION_PID 0x042F // Navigation controller
2832
#define PS3MOVE_PID 0x03D5 // Motion controller
2933

34+
//-- MODIFIED> -----------------------------------------------------------//
3035
/* Bluetooth dongle data taken from descriptors */
31-
#define BULK_MAXPKTSIZE 64 // max size for ACL data
36+
#define BULK_MAXPKTSIZE 128 // max size for ACL data 64->128 MODIFIED
37+
//-- <MODIFIED -----------------------------------------------------------//
3238

3339
// Used in control endpoint header for HCI Commands
3440
#define bmREQ_HCI_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_DEVICE
@@ -56,6 +62,11 @@
5662
#define HCI_DISABLE_SCAN_STATE 14
5763
#define HCI_DONE_STATE 15
5864
#define HCI_DISCONNECT_STATE 16
65+
//-- MODIFIED> -----------------------------------------------------------//
66+
#define HCI_WRITE_CoD_STATE 17
67+
#define HCI_WRITE_SIMPLE_PAIRING_STATE 18
68+
#define HCI_SET_EVENT_MASK_STATE 19
69+
//-- <MODIFIED -----------------------------------------------------------//
5970

6071
/* HCI event flags*/
6172
#define HCI_FLAG_CMD_COMPLETE 0x01
@@ -102,6 +113,13 @@
102113
#define EV_COMMAND_STATUS 0x0F
103114
#define EV_LOOPBACK_COMMAND 0x19
104115
#define EV_PAGE_SCAN_REP_MODE 0x20
116+
//-- MODIFIED> -----------------------------------------------------------//
117+
//#define EV_SIMPLE_PAIRING_COMPLETE 0x2C
118+
#define EV_IO_CAPABILITY_REQUEST 0x31
119+
#define EV_IO_CAPABILITY_RESPONSE 0x32
120+
#define EV_USER_CONFIRMATION_REQUEST 0x33
121+
#define EV_SIMPLE_PAIRING_COMPLETE 0x36
122+
//-- <MODIFIED -----------------------------------------------------------//
105123

106124
/* L2CAP signaling commands */
107125
#define L2CAP_CMD_COMMAND_REJECT 0x01
@@ -404,6 +422,21 @@ class BTD : public USBDeviceConfig, public UsbConfigXtracter {
404422
uint8_t readPollInterval() {
405423
return pollInterval;
406424
};
425+
//-- MODIFIED> -----------------------------------------------------------//
426+
// void hci_write_class_of_device();
427+
void hci_Write_Simple_Pairing_Mode();
428+
void hci_Set_Event_Mask_SPM();
429+
void hci_Authentication_Requested(uint16_t handle);
430+
void hci_IO_Capability_Request_Reply();
431+
void hci_User_Confirmation_Request_Reply();
432+
void hci_Set_Connection_Encryption(uint16_t handle);
433+
bool isSimplePairingCompleted(){return m_simple_pairing_completed;};
434+
void StartSimplePairingOperation(uint16_t handle)
435+
{
436+
m_simple_pairing_completed = false;
437+
hci_Authentication_Requested(handle);
438+
}
439+
//-- <MODIFIED -----------------------------------------------------------//
407440

408441
protected:
409442
/** Pointer to USB class instance. */
@@ -460,5 +493,8 @@ class BTD : public USBDeviceConfig, public UsbConfigXtracter {
460493
/* Used to set the Bluetooth Address internally to the PS3 Controllers */
461494
void setBdaddr(uint8_t* BDADDR);
462495
void setMoveBdaddr(uint8_t* BDADDR);
496+
//-- MODIFIED> -----------------------------------------------------------//
497+
bool m_simple_pairing_completed;
498+
//-- <MODIFIED -----------------------------------------------------------//
463499
};
464500
#endif

0 commit comments

Comments
 (0)