visus
/
teensy-usbhost
关注 1
点赞 0
派生 0
代码 工单 0 合并请求 0 版本发布 0 百科 动态
您最多选择25个主题 主题必须以字母或数字开头，可以包含连字符 (-)，并且长度不得超过35个字符
291 提交
1 分支
目录树: 5b0d5dee30
分支列表 标签列表
${ item.name }
创建分支 ${ searchTerm }
从 '5b0d5dee30'
${ noResults }
teensy-usbhost/bluetooth.cpp

1558 行
61KB
原始文件 Blame 文件历史 

  1. /* USB EHCI Host for Teensy 3.6

  2.  * Copyright 2017 Paul Stoffregen (paul@pjrc.com)

  3.  *

  4.  * Permission is hereby granted, free of charge, to any person obtaining a

  5.  * copy of this software and associated documentation files (the

  6.  * "Software"), to deal in the Software without restriction, including

  7.  * without limitation the rights to use, copy, modify, merge, publish,

  8.  * distribute, sublicense, and/or sell copies of the Software, and to

  9.  * permit persons to whom the Software is furnished to do so, subject to

  10.  * the following conditions:

  11.  *

  12.  * The above copyright notice and this permission notice shall be included

  13.  * in all copies or substantial portions of the Software.

  14.  *

  15.  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS

  16.  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF

  17.  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.

  18.  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY

  19.  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,

  20.  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE

  21.  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

  22.  *

  23.  * information about the BlueTooth HCI comes from logic analyzer captures

  24.  * plus... http://affon.narod.ru/BT/bluetooth_app_c10.pdf

  25.  */

  26. 

  27. #include <Arduino.h>

  28. #include "USBHost_t36.h"  // Read this header first for key info

  29. 

  30. #define print   USBHost::print_

  31. #define println USBHost::println_//#define DEBUG_BT

  32. 

  33. #define DEBUG_BT

  34. //#define DEBUG_BT_VERBOSE

  35. 

  36. #ifndef DEBUG_BT

  37. #undef DEBUG_BT_VERBOSE

  38. void inline DBGPrintf(...) {};

  39. #else

  40. #define DBGPrintf USBHDBGSerial.printf

  41. #endif

  42. 

  43. #ifndef DEBUG_BT_VERBOSE

  44. void inline VDBGPrintf(...) {};

  45. #else

  46. #define VDBGPrintf USBHDBGSerial.printf

  47. #endif

  48. 

  49. 

  50. /************************************************************/

  51. //  Define HCI Commands OGF HIgh byte OCF is low byte... 

  52. //     Actually shifted values...

  53. /************************************************************/

  54. #define HCI_INQUIRY 						0x0401

  55. #define HCI_INQUIRY_CANCEL					0x0402

  56. #define HCI_CREATE_CONNECTION				0x0405

  57. #define HCI_OP_ACCEPT_CONN_REQ				0x0409

  58. #define HCI_LINK_KEY_NEG_REPLY				0x040C

  59. #define HCI_PIN_CODE_REPLY					0x040D

  60. #define HCI_AUTH_REQUESTED					0x0411

  61. #define HCI_OP_REMOTE_NAME_REQ				0x0419

  62. #define HCI_OP_REMOTE_NAME_REQ_CANCEL		0x041a

  63. #define HCI_OP_READ_REMOTE_FEATURES			0x041b

  64. #define HCI_OP_READ_REMOTE_VERSION_INFORMATION 0x041D

  65. 

  66. 

  67. #define HCI_Write_Default_Link_Policy_Settings	0x080f

  68. #define HCI_Set_Event_Mask					0x0c01

  69. #define HCI_RESET 							0x0c03

  70. #define HCI_Set_Event_Filter_Clear			0x0c05

  71. #define HCI_Read_Local_Name					0x0c14

  72. #define HCI_Read_Stored_Link_Key			0x0c0d

  73. #define HCI_DELETE_STORED_LINK_KEY			0x0c12

  74. #define HCI_WRITE_LOCAL_NAME				0x0c13

  75. #define Write_Connection_Accept_Timeout		0x0c16

  76. #define HCI_WRITE_SCAN_ENABLE				0x0c1a

  77. #define HCI_Read_Page_Scan_Activity			0x0c1b

  78. #define HCI_READ_CLASS_OF_DEVICE			0x0c23

  79. #define HCI_WRITE_CLASS_OF_DEV				0x0C24

  80. #define HCI_Read_Voice_Setting				0x0c25

  81. #define HCI_Read_Number_Of_Supported_IAC	0x0c38

  82. #define HCI_Read_Current_IAC_LAP			0x0c39

  83. #define HCI_WRITE_INQUIRY_MODE				0x0c45

  84. #define HCI_Read_Page_Scan_Type				0x0c46

  85. #define HCI_WRITE_EIR						0x0c52

  86. #define HCI_WRITE_SSP_MODE					0x0c56

  87. #define HCI_Read_Inquiry_Response_Transmit_Power_Level 0x0c58

  88. #define HCI_WRITE_LE_HOST_SUPPORTED			0x0c6d

  89. 

  90. #define HCI_Read_Local_Supported_Features	0x1003

  91. #define HCI_Read_Local_Extended_Features	0x1004

  92. #define HCI_Read_Buffer_Size				0x1005

  93. #define HCI_Read_BD_ADDR					0x1009

  94. #define HCI_Read_Local_Version_Information	0x1001

  95. #define HCI_Read_Local_Supported_Commands	0x1002

  96. 

  97. #define HCI_LE_SET_EVENT_MASK				0x2001

  98. #define HCI_LE_Read_Buffer_Size				0x2002

  99. #define HCI_LE_Read_Local_supported_Features 0x2003

  100. #define HCI_LE_READ_ADV_TX_POWER			0x2007

  101. #define HCI_LE_SET_ADV_DATA					0x2008

  102. #define HCI_LE_SET_SCAN_RSP_DATA			0x2009

  103. #define HCI_LE_READ_WHITE_LIST_SIZE			0x200f

  104. #define HCI_LE_CLEAR_WHITE_LIST				0x2010

  105. #define HCI_LE_Supported_States				0x201c

  106. 

  107. /* Bluetooth L2CAP PSM - see http://www.bluetooth.org/Technical/AssignedNumbers/logical_link.htm */

  108. #define HID_CTRL_PSM    0x11 // HID_Control PSM Value

  109. #define HID_INTR_PSM    0x13 // HID_Interrupt PSM Value

  110. 

  111. // Used For Connection Response 

  112. #define PENDING     0x01

  113. #define SUCCESSFUL  0x00

  114. 

  115. 

  116. /* L2CAP signaling commands */

  117. #define L2CAP_CMD_COMMAND_REJECT        0x01

  118. #define L2CAP_CMD_CONNECTION_REQUEST    0x02

  119. #define L2CAP_CMD_CONNECTION_RESPONSE   0x03

  120. #define L2CAP_CMD_CONFIG_REQUEST        0x04

  121. #define L2CAP_CMD_CONFIG_RESPONSE       0x05

  122. #define L2CAP_CMD_DISCONNECT_REQUEST    0x06

  123. #define L2CAP_CMD_DISCONNECT_RESPONSE   0x07

  124. #define L2CAP_CMD_INFORMATION_REQUEST   0x0A

  125. #define L2CAP_CMD_INFORMATION_RESPONSE  0x0B

  126. 

  127. #define HID_THDR_DATA_INPUT				0xa1 

  128. // HID stuff

  129. #define HID_BOOT_PROTOCOL           	0x00

  130. #define HID_RPT_PROTOCOL                0x01

  131. 

  132. /* HCI Events  */

  133. 	enum {EV_INQUIRY_COMPLETE= 0x01,EV_INQUIRY_RESULT= 0x02,EV_CONNECT_COMPLETE= 0x03,EV_INCOMING_CONNECT= 0x04,EV_DISCONNECT_COMPLETE= 0x05

  134. 		,EV_AUTHENTICATION_COMPLETE= 0x06,EV_REMOTE_NAME_COMPLETE= 0x07,EV_ENCRYPTION_CHANGE= 0x08,EV_CHANGE_CONNECTION_LINK= 0x09,EV_ROLE_CHANGED= 0x12

  135. 		,EV_NUM_COMPLETE_PKT= 0x13,EV_PIN_CODE_REQUEST= 0x16,EV_LINK_KEY_REQUEST= 0x17,EV_LINK_KEY_NOTIFICATION= 0x18,EV_DATA_BUFFER_OVERFLOW= 0x1A

  136. 		,EV_MAX_SLOTS_CHANGE= 0x1B,EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE= 0x0C,EV_QOS_SETUP_COMPLETE= 0x0D,EV_COMMAND_COMPLETE= 0x0E,EV_COMMAND_STATUS= 0x0F

  137. 		,EV_LOOPBACK_COMMAND= 0x19,EV_PAGE_SCAN_REP_MODE= 0x20, EV_INQUIRY_RESULTS_WITH_RSSI=0x22, EV_EXTENDED_INQUIRY_RESULT=0x2F };

  138. 

  139. 

  140. 

  141. 

  142. // different modes

  143. enum {PC_RESET = 1, PC_WRITE_CLASS_DEVICE, PC_READ_BDADDR, PC_READ_LOCAL_VERSION, 

  144. 			PC_SEND_WRITE_INQUIRE_MODE, PC_SEND_SET_EVENT_MASK, PC_SEND_INQUIRE, 

  145. 			PC_INQUIRE_CANCEL=100, PC_AUTHENTICATION_REQUESTED=110, PC_LINK_KEY_NEGATIVE=120, PC_PIN_CODE_REPLY=130,

  146. 			PC_WRITE_SCAN_PAGE=200};

  147. //////////////

  148. 

  149. //////////////

  150. 

  151. 

  152. // Setup some states for the TX pipe where we need to chain messages

  153. enum {STATE_TX_SEND_CONNECT_INT=200, STATE_TX_SEND_CONECT_RSP_SUCCESS, STATE_TX_SEND_CONFIG_REQ, STATE_TX_SEND_CONECT_ISR_RSP_SUCCESS, STATE_TX_SEND_CONFIG_ISR_REQ};

  154. 

  155. // This is a list of all the drivers inherited from the BTHIDInput class.

  156. // Unlike the list of USBDriver (managed in enumeration.cpp), drivers stay

  157. // on this list even when they have claimed a top level collection.

  158. BTHIDInput * BluetoothController::available_bthid_drivers_list = NULL;

  159. 

  160. void BluetoothController::driver_ready_for_bluetooth(BTHIDInput *driver)

  161. {

  162. 	driver->next = NULL;

  163. 	if (available_bthid_drivers_list == NULL) {

  164. 		available_bthid_drivers_list = driver;

  165. 	} else {

  166. 		BTHIDInput *last = available_bthid_drivers_list;

  167. 		while (last->next) last = last->next;

  168. 		last->next = driver;

  169. 	}

  170. }

  171. 

  172. // When a new top level collection is found, this function asks drivers

  173. // if they wish to claim it.  The driver taking ownership of the

  174. // collection is returned, or NULL if no driver wants it.

  175. BTHIDInput * BluetoothController::find_driver(uint32_t device_type, uint8_t *remoteName)

  176. {

  177. 	USBHDBGSerial.printf("BluetoothController::find_driver");

  178. 	BTHIDInput *driver = available_bthid_drivers_list;

  179. 	while (driver) {

  180. 		USBHDBGSerial.printf("  driver %x\n", (uint32_t)driver);

  181. 		if (driver->claim_bluetooth(this, device_type, remoteName)) {

  182. 			USBHDBGSerial.printf("    *** Claimed ***\n");

  183. 			return driver;

  184. 		}

  185. 		driver = driver->next;

  186. 	}

  187. 	return NULL;

  188. }

  189. //12 01 00 02 FF 01 01 40 5C 0A E8 21 12 01 01 02 03 01

  190. //VendorID = 0A5C, ProductID = 21E8, Version = 0112

  191. //Class/Subclass/Protocol = 255 / 1 / 1

  192. BluetoothController::product_vendor_mapping_t BluetoothController::pid_vid_mapping[] = {

  193. 	{ 0xA5C, 0x21E8 }};

  194. 

  195. /************************************************************/

  196. //  Initialization and claiming of devices & interfaces

  197. /************************************************************/

  198. 

  199. void BluetoothController::init()

  200. {

  201. 	contribute_Pipes(mypipes, sizeof(mypipes)/sizeof(Pipe_t));

  202. 	contribute_Transfers(mytransfers, sizeof(mytransfers)/sizeof(Transfer_t));

  203. 	contribute_String_Buffers(mystring_bufs, sizeof(mystring_bufs)/sizeof(strbuf_t));

  204. 	driver_ready_for_device(this);

  205. }

  206. 

  207. bool BluetoothController::claim(Device_t *dev, int type, const uint8_t *descriptors, uint32_t len)

  208. {

  209. 	// only claim at device level 

  210. 	println("BluetoothController claim this=", (uint32_t)this, HEX);

  211. 

  212. 	if (type != 0) return false; // claim at the device level

  213. 

  214. 	// Lets try to support the main USB Bluetooth class...

  215. 	// http://www.usb.org/developers/defined_class/#BaseClassE0h

  216. 	if (dev->bDeviceClass != 0xe0)  {

  217. 		bool special_case_device = false;

  218. 		for (uint8_t i=0; i < (sizeof(pid_vid_mapping)/sizeof(pid_vid_mapping[0])); i++) {

  219. 			if ((pid_vid_mapping[i].idVendor == dev->idVendor) && (pid_vid_mapping[i].idProduct == dev->idProduct)) {

  220. 				special_case_device = true;

  221. 				break;

  222. 			}

  223. 		}

  224. 		if (!special_case_device) return false;

  225. 	}

  226. 	if ((dev->bDeviceSubClass != 1) || (dev->bDeviceProtocol != 1)) return false; // Bluetooth Programming Interface

  227. 

  228. 	DBGPrintf("BluetoothController claim this=%x vid:pid=%x:%x\n    ", (uint32_t)this, dev->idVendor,  dev->idProduct);

  229. 	if (len > 512) {

  230. 		DBGPrintf("  Descriptor length %d only showing first 512\n    ");

  231. 		len = 512;

  232. 	}	

  233. 	for (uint16_t i=0; i < len; i++) {

  234. 		DBGPrintf("%x ", descriptors[i]);

  235. 		if ((i & 0x3f) == 0x3f) DBGPrintf("\n    ");

  236. 	}

  237. 	DBGPrintf("\n  ");

  238. 

  239. 	// Lets try to process the first Interface and get the end points...

  240. 	// Some common stuff for both XBoxs

  241. 	uint32_t count_end_points = descriptors[4];

  242. 	if (count_end_points < 2) return false;

  243. 	uint32_t rxep = 0;

  244. 	uint32_t rx2ep = 0;

  245. 	uint32_t txep = 0;

  246. 	uint8_t rx_interval = 0;

  247. 	uint8_t rx2_interval = 0;

  248. 	uint8_t tx_interval = 0;

  249. 	rx_size_ = 0;

  250. 	rx2_size_ = 0;

  251. 	tx_size_ = 0;

  252. 	uint32_t descriptor_index = 9; 

  253. 	while (count_end_points-- /*&& ((rxep == 0) || txep == 0) */) {

  254. 		if (descriptors[descriptor_index] != 7) return false; // length 7

  255. 		if (descriptors[descriptor_index+1] != 5) return false; // ep desc

  256. 		if ((descriptors[descriptor_index+4] <= 64)

  257. 			&& (descriptors[descriptor_index+5] == 0)) {

  258. 			// have a bulk EP size 

  259. 			if (descriptors[descriptor_index+2] & 0x80 ) {

  260. 				if (descriptors[descriptor_index+3] == 3) 	{ // Interrupt

  261. 					rxep = descriptors[descriptor_index+2];

  262. 					rx_size_ = descriptors[descriptor_index+4];

  263. 					rx_interval = descriptors[descriptor_index+6];

  264. 				} else if  (descriptors[descriptor_index+3] == 2) 	{ // bulk

  265. 					rx2ep = descriptors[descriptor_index+2];

  266. 					rx2_size_ = descriptors[descriptor_index+4];

  267. 					rx2_interval = descriptors[descriptor_index+6];

  268. 				}

  269. 			} else {

  270. 				txep = descriptors[descriptor_index+2]; 

  271. 				tx_size_ = descriptors[descriptor_index+4];

  272. 				tx_interval = descriptors[descriptor_index+6];

  273. 			}

  274. 		}

  275. 		descriptor_index += 7;  // setup to look at next one...

  276. 	}

  277. 	if ((rxep == 0) || (txep == 0)) {

  278. 		USBHDBGSerial.printf("Bluetooth end points not found: %d %d\n", rxep, txep);

  279. 				return false; // did not find two end points.

  280. 	}

  281. 	DBGPrintf("    rxep=%d(%d) txep=%d(%d) rx2ep=%d(%d)\n", rxep&15, rx_size_, txep, tx_size_, 

  282. 		rx2ep&15, rx2_size_);

  283. 

  284. 	print("BluetoothController, rxep=", rxep & 15);

  285. 	print("(", rx_size_);

  286. 	print("), txep=", txep);

  287. 	print("(", tx_size_);

  288. 	println(")");

  289. 	rxpipe_ = new_Pipe(dev, 3, rxep & 15, 1, rx_size_, rx_interval);

  290. 	if (!rxpipe_) return false;

  291. 	txpipe_ = new_Pipe(dev, 3, txep, 0, tx_size_, tx_interval);

  292. 	if (!txpipe_) {

  293. 		//free_Pipe(rxpipe_);

  294. 		return false;

  295. 	}

  296. 	rx2pipe_ = new_Pipe(dev, 2, rx2ep & 15, 1, rx2_size_, rx2_interval);

  297. 	if (!rx2pipe_)  {

  298. 		// Free other pipes...

  299. 		return false;

  300. 	}

  301. 

  302. 	rxpipe_->callback_function = rx_callback;

  303. 	queue_Data_Transfer(rxpipe_, rxbuf_, rx_size_, this);

  304. 

  305. 	rx2pipe_->callback_function = rx2_callback;

  306. 	queue_Data_Transfer(rx2pipe_, rx2buf_, rx2_size_, this);

  307. 

  308. 	txpipe_->callback_function = tx_callback;

  309. 

  310. 	// Send out the reset

  311. 	device = dev; // yes this is normally done on return from this but should not hurt if we do it here.

  312. 	sendResetHCI();

  313. 	pending_control_ = PC_RESET; 

  314. 	pending_control_tx_ = 0;	// 

  315. 

  316. 	return true;

  317. }

  318. 

  319. 

  320. void BluetoothController::disconnect()

  321. {

  322. 	USBHDBGSerial.printf("Bluetooth Disconnect");

  323. 	if (connections_[current_connection_].device_driver_) {

  324. 		connections_[current_connection_].device_driver_->release_bluetooth();

  325. 		connections_[current_connection_].device_driver_->remote_name_[0] = 0;

  326. 		connections_[current_connection_].device_driver_ = nullptr;

  327. 	}

  328. 	connections_[current_connection_].connection_complete_ = false;

  329. }

  330. 

  331. void BluetoothController::timer_event(USBDriverTimer *whichTimer)

  332. {

  333. 	

  334. }

  335. 

  336. 

  337. void BluetoothController::control(const Transfer_t *transfer)

  338. {

  339. 	println("    control callback (bluetooth) ", pending_control_, HEX);

  340. #ifdef DEBUG_BT_VERBOSE

  341. 	DBGPrintf("    Control callback (bluetooth): %d : ", pending_control_);

  342. 	uint8_t *buffer = (uint8_t*)transfer->buffer;

  343. 	for (uint8_t i=0; i < transfer->length; i++) DBGPrintf("%x ", buffer[i]);

  344. 	DBGPrintf("\n");

  345. #endif

  346. }

  347. 

  348. /************************************************************/

  349. //  Interrupt-based Data Movement

  350. /************************************************************/

  351. 

  352. void BluetoothController::rx_callback(const Transfer_t *transfer)

  353. {

  354. 	if (!transfer->driver) return;

  355. 	((BluetoothController *)(transfer->driver))->rx_data(transfer);

  356. }

  357. 

  358. void BluetoothController::rx2_callback(const Transfer_t *transfer)

  359. {

  360. 	if (!transfer->driver) return;

  361. 	((BluetoothController *)(transfer->driver))->rx2_data(transfer);

  362. }

  363. 

  364. void BluetoothController::tx_callback(const Transfer_t *transfer)

  365. {

  366. 	if (!transfer->driver) return;

  367. 	((BluetoothController *)(transfer->driver))->tx_data(transfer);

  368. }

  369. 

  370. 

  371. void BluetoothController::rx_data(const Transfer_t *transfer)

  372. {

  373. 	uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);

  374. 	print_hexbytes((uint8_t*)transfer->buffer, len);

  375. 	DBGPrintf("BT rx_data(%d): ", len);

  376. 	uint8_t *buffer = (uint8_t*)transfer->buffer;

  377. 	for (uint8_t i=0; i < len; i++) DBGPrintf("%x ", buffer[i]);

  378. 	DBGPrintf("\n");

  379. 

  380. 	// Note the logical packets returned from the device may be larger

  381. 	// than can fit in one of our packets, so we will detect this and

  382. 	// the next read will be continue in or rx_buf_ in the next logical 

  383. 	// location.  We will only go into process the next logical state

  384. 	// when we have the full response read in... 

  385. 	if (rx_packet_data_remaining == 0) {	// Previous command was fully handled

  386. 		rx_packet_data_remaining = rxbuf_[1] + 2;	// length of data plus the two bytes at start...

  387. 	}		

  388.     // Now see if the data 

  389.     rx_packet_data_remaining -= len;	// remove the length of this packet from length

  390. 

  391. 	if (rx_packet_data_remaining == 0) {	// read started at beginning of packet so get the total length of packet

  392. 	    switch(rxbuf_[0]) { // Switch on event type

  393. 		    case EV_COMMAND_COMPLETE: //0x0e

  394. 	        	handle_hci_command_complete();// Check if command succeeded

  395. 	            break;

  396.             case EV_COMMAND_STATUS:	//0x0f

  397.             	handle_hci_command_status();

  398.             	break;

  399. 			case EV_INQUIRY_COMPLETE: // 0x01

  400. 				handle_hci_inquiry_complete();

  401. 				break;

  402. 			case EV_INQUIRY_RESULT:	  // 0x02

  403. 				handle_hci_inquiry_result(false);

  404. 				break;

  405. 			case EV_CONNECT_COMPLETE:	// 0x03

  406. 				handle_hci_connection_complete();

  407. 				break;

  408. 			case EV_INCOMING_CONNECT:	// 0x04

  409. 				handle_hci_incoming_connect();

  410. 				break;

  411. 			case EV_DISCONNECT_COMPLETE: // 0x05

  412. 				handle_hci_disconnect_complete();

  413. 				break;

  414. 			case EV_AUTHENTICATION_COMPLETE:// 0x06

  415. 				handle_hci_authentication_complete();

  416. 				break;

  417. 			case EV_REMOTE_NAME_COMPLETE: // 0x07

  418. 				handle_hci_remote_name_complete();

  419. 				break;	

  420. 			case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE:

  421. 				handle_hci_remote_version_information_complete();

  422. 				break;

  423. 			case EV_PIN_CODE_REQUEST: // 0x16

  424. 				handle_hci_pin_code_request();

  425. 				break;

  426. 

  427. 			case EV_LINK_KEY_REQUEST:	// 0x17

  428. 				handle_hci_link_key_request();

  429. 				break;

  430. 			case EV_LINK_KEY_NOTIFICATION: // 0x18

  431. 				handle_hci_link_key_notification();	

  432. 				break;

  433. 			case EV_INQUIRY_RESULTS_WITH_RSSI:

  434. 				handle_hci_inquiry_result(true);

  435. 				break;

  436. 			case EV_EXTENDED_INQUIRY_RESULT:

  437. 				handle_hci_extended_inquiry_result();

  438. 				break;

  439. 			default:

  440. 				break;

  441. 	    }

  442. 		// Start read at start of buffer. 

  443. 		queue_Data_Transfer(rxpipe_, rxbuf_, rx_size_, this);

  444. 	} else {

  445. 		// Continue the read - Todo - maybe verify len == rx_size_

  446. 		queue_Data_Transfer(rxpipe_, buffer + rx_size_, rx_size_, this);

  447. 		return;		// Don't process the message yet as we still have data to receive. 

  448. 	}

  449. }

  450. 

  451. //===================================================================

  452. // Called when an HCI command completes.

  453. void BluetoothController::handle_hci_command_complete() 

  454. {

  455. 	uint16_t hci_command = rxbuf_[3] + (rxbuf_[4] << 8);

  456. 	uint8_t buffer_index;

  457.     if(!rxbuf_[5]) { 

  458.     	VDBGPrintf("    Command Completed! \n");

  459.     } else {

  460.     	VDBGPrintf("    Command(%x) Completed - Error: %d! \n", hci_command, rxbuf_[5]);

  461.     	// BUGBUG:: probably need to queue something? 

  462.     }

  463. 

  464. 	switch (hci_command) {

  465. 		case HCI_OP_REMOTE_NAME_REQ:

  466. 			break;

  467. 		case HCI_RESET:	//0x0c03

  468. 			if (!rxbuf_[5]) pending_control_ = PC_WRITE_CLASS_DEVICE;

  469. 			//  If it fails, will retry. maybe should have repeat max...  

  470. 			break;

  471. 		case HCI_Set_Event_Filter_Clear:	//0x0c05

  472. 			break;

  473. 		case HCI_Read_Local_Name:	//0x0c14

  474. 			// received name back... 

  475. 			{

  476. 				//BUGBUG:: probably want to grab string object and copy to 

  477. 				USBHDBGSerial.printf("    Local name: %s\n", &rxbuf_[6]);

  478. /*

  479. 				uint8_t len = rxbuf_[1]+2;	// Length field +2 for total bytes read

  480. 				for (uint8_t i=6; i < len; i++) {

  481. 					if (rxbuf_[i] == 0) {

  482. 						break;

  483. 					}

  484. 					USBHDBGSerial.printf("%c", rxbuf_[i]);

  485. 				}

  486. 				USBHDBGSerial.printf("\n"); */

  487. 			}

  488. 			break;

  489. 		case Write_Connection_Accept_Timeout:	//0x0c16

  490. 			break;

  491. 		case HCI_READ_CLASS_OF_DEVICE:	// 0x0c23

  492. 			break;

  493. 		case HCI_Read_Voice_Setting: 	//0x0c25

  494. 			break;

  495. 		case HCI_Read_Number_Of_Supported_IAC:	//0x0c38

  496. 			break;

  497. 		case HCI_Read_Current_IAC_LAP:	//0x0c39

  498. 			break;

  499. 		case HCI_WRITE_INQUIRY_MODE:	//0x0c45

  500. 			break;

  501. 		case HCI_Read_Inquiry_Response_Transmit_Power_Level: //0x0c58

  502. 			break;

  503. 		case HCI_Read_Local_Supported_Features:	//0x1003

  504. 			// Remember the features supported by local... 

  505. 			for (buffer_index = 0; buffer_index < 8; buffer_index++) {

  506. 				features[buffer_index] = rxbuf_[buffer_index+6];

  507. 			}

  508. 			break;

  509. 		case HCI_Read_Buffer_Size:	// 0x1005

  510. 			break;

  511. 		case HCI_Read_BD_ADDR:	//0x1009

  512. 			{

  513. 	            for(uint8_t i = 0; i < 6; i++) my_bdaddr_[i] = rxbuf_[6 + i];

  514. 		      	DBGPrintf("   BD Addr %x:%x:%x:%x:%x:%x\n", my_bdaddr_[5], my_bdaddr_[4], my_bdaddr_[3], my_bdaddr_[2], my_bdaddr_[1], my_bdaddr_[0]);

  515. 			}

  516. 			break;

  517. 		case HCI_Read_Local_Version_Information:	//0x1001

  518. 			hciVersion = rxbuf_[6];		// Should do error checking above... 

  519. 			DBGPrintf("    Local Version: %x\n", hciVersion);

  520. 			pending_control_ = (do_pair_device_)? PC_SEND_WRITE_INQUIRE_MODE : PC_WRITE_SCAN_PAGE;

  521. 			break;

  522. 		case HCI_Read_Local_Supported_Commands:	//0x1002

  523. 			break;

  524. 		case HCI_LE_Read_Buffer_Size:	//0x2002

  525. 			break;

  526. 		case HCI_LE_Read_Local_supported_Features:	//0x2003

  527. 			break;

  528. 		case HCI_LE_Supported_States:	//0x201c

  529. 			break;

  530. 

  531. 		case HCI_Read_Local_Extended_Features:	//0x1004

  532. 			break;

  533. 		case HCI_Set_Event_Mask:					//0x0c01

  534. 			break;

  535. 		case HCI_Read_Stored_Link_Key:			//0x0c0d

  536. 			break;

  537. 		case HCI_Write_Default_Link_Policy_Settings:	//0x080f

  538. 			break;

  539. 		case HCI_Read_Page_Scan_Activity:			//0x0c1b

  540. 			break;

  541. 		case HCI_Read_Page_Scan_Type:				//0x0c46

  542. 			break;

  543. 		case HCI_LE_SET_EVENT_MASK:				//0x2001

  544. 			break;

  545. 		case HCI_LE_READ_ADV_TX_POWER:			//0x2007

  546. 			break;

  547. 		case HCI_LE_READ_WHITE_LIST_SIZE:			//0x200f

  548. 			break;

  549. 		case HCI_LE_CLEAR_WHITE_LIST:				//0x2010

  550. 			break;

  551. 		case HCI_DELETE_STORED_LINK_KEY:			//0x0c12

  552. 			break;

  553. 		case HCI_WRITE_LOCAL_NAME:				//0x0c13

  554. 			break;

  555. 		case HCI_WRITE_SCAN_ENABLE:				//0x0c1a

  556. 			DBGPrintf("Write_Scan_enable Completed\n");

  557. 			// See if we have driver and a remote

  558. 			if (connections_[current_connection_].device_driver_ && connections_[current_connection_].connection_complete_) {	// We have a driver call their 

  559. 				connections_[current_connection_].device_driver_->connectionComplete();

  560. 				connections_[current_connection_].connection_complete_ = false;	// only call once

  561. 			}

  562. 			break;

  563. 		case HCI_WRITE_SSP_MODE:					//0x0c56

  564. 			break;

  565. 		case HCI_WRITE_EIR:						//0x0c52

  566. 			break;

  567. 		case HCI_WRITE_LE_HOST_SUPPORTED:			//0x0c6d

  568. 			break;

  569. 		case HCI_LE_SET_SCAN_RSP_DATA:			//0x2009

  570. 			break;

  571. 	}

  572. 	// And queue up the next command

  573. 	queue_next_hci_command();

  574. }

  575. 

  576. void BluetoothController::queue_next_hci_command()

  577. {

  578. 	// Ok We completed a command now see if we need to queue another command

  579. 	// Still probably need to reorganize... 

  580.     switch (pending_control_) {

  581.     		// Initial setup states. 

  582.  		case PC_RESET: 

  583. 			sendResetHCI();

  584. 			break;

  585.     	case PC_WRITE_CLASS_DEVICE:

  586.     		sendHDCWriteClassOfDev();

  587.     		pending_control_++;

  588.     		break;

  589.      	case PC_READ_BDADDR:

  590. 			sendHCIReadBDAddr();

  591. 			pending_control_++;

  592. 			break;

  593. 

  594.      	case PC_READ_LOCAL_VERSION:

  595.      		sendHCIReadLocalVersionInfo();

  596. 			//pending_control_++;

  597. 			break;

  598. 

  599. 		// These are used when we are pairing. 	

  600. 	    case PC_SEND_WRITE_INQUIRE_MODE:

  601. 			sendHCIHCIWriteInquiryMode(2);  // lets set into extended inquire mode

  602. 			pending_control_++;

  603. 			break;

  604. 

  605. 	    case PC_SEND_SET_EVENT_MASK:

  606. 			sendHCISetEventMask();  // Set the event mask to include extend inquire event

  607. 			pending_control_++;

  608. 			break;

  609. 

  610. 		case PC_SEND_INQUIRE:

  611. 			sendHCI_INQUIRY();

  612. 			pending_control_++;

  613. 			break;

  614. 		case PC_INQUIRE_CANCEL:	

  615. 			// lets try to create a connection...

  616. 			sendHCICreateConnection();

  617. 			pending_control_++;

  618. 			break;

  619. 		case PC_AUTHENTICATION_REQUESTED:	

  620. 			break;

  621. 		case PC_LINK_KEY_NEGATIVE:

  622. 			break;

  623. 		case PC_PIN_CODE_REPLY:

  624. 			break;

  625. 

  626. 		// None Pair mode

  627. 		case PC_WRITE_SCAN_PAGE:

  628. 			sendHCIWriteScanEnable(2);

  629. 			pending_control_ = 0;	// 

  630. 			break;

  631. 		default:	

  632. 			break;

  633.     }

  634. 

  635. }

  636. 

  637. 

  638. void BluetoothController::handle_hci_command_status() 

  639. {

  640. 	// <event type><param count><status><num packets allowed to be sent><CMD><CMD>

  641. 	uint16_t hci_command = rxbuf_[4] + (rxbuf_[5] << 8);

  642. 	if (rxbuf_[2]) {

  643. 		#ifdef DEBUG_BT

  644. 		DBGPrintf("    Command %x Status %x - ", hci_command, rxbuf_[2]);

  645. 		switch (rxbuf_[2]) {

  646. 			case 0x01: DBGPrintf("Unknown HCI Command\n"); break;

  647. 			case 0x02: DBGPrintf("Unknown Connection Identifier\n"); break;

  648. 			case 0x03: DBGPrintf("Hardware Failure\n"); break;

  649. 			case 0x04: DBGPrintf("Page Timeout\n"); break;

  650. 			case 0x05: DBGPrintf("Authentication Failure\n"); break;

  651. 			case 0x06: DBGPrintf("PIN or Key Missing\n"); break;

  652. 			case 0x07: DBGPrintf("Memory Capacity Exceeded\n"); break;

  653. 			case 0x08: DBGPrintf("Connection Timeout\n"); break;

  654. 			case 0x09: DBGPrintf("Connection Limit Exceeded\n"); break;

  655. 			case 0x0A: DBGPrintf("Synchronous Connection Limit To A Device Exceeded\n"); break;

  656. 			case 0x0B: DBGPrintf("Connection Already Exists\n"); break;

  657. 			case 0x0C: DBGPrintf("Command Disallowed\n"); break;

  658. 			case 0x0D: DBGPrintf("Connection Rejected due to Limited Resources\n"); break;

  659. 			case 0x0E: DBGPrintf("Connection Rejected Due To Security Reasons\n"); break;

  660. 			case 0x0F: DBGPrintf("Connection Rejected due to Unacceptable BD_ADDR\n"); break;

  661. 			default: DBGPrintf("???\n"); break;

  662. 		}

  663. 		#endif

  664. 		// lets try recovering from some errors...  

  665. 		switch (hci_command) {

  666. 			case HCI_OP_ACCEPT_CONN_REQ:

  667. 				// We assume that the connection failed...

  668. 				DBGPrintf("### Connection Failed ###");

  669. 				if (count_connections_) count_connections_--;

  670. 				break;

  671. 			default:

  672. 				break;

  673. 		}

  674. 

  675. 	} else {

  676. 		#ifdef DEBUG_BT

  677. 		VDBGPrintf("    Command %x Status %x\n", hci_command, rxbuf_[2]);

  678. 		#endif

  679. 	}

  680. }

  681. 

  682. void BluetoothController::handle_hci_inquiry_result(bool fRSSI) 

  683. {

  684. 	// result - versus result with RSSI

  685. 	//                              | Diverge here... 

  686. 	//  2 f 1 79 22 23 a c5 cc 1 2  0 40 25 0 3b 2 

  687. 	// 22 f 1 79 22 23 a c5 cc 1 2 40 25 0 3e 31 d2

  688. 

  689. 	// Wondered if multiple items if all of the BDADDR are first then next field...

  690. 	// looks like it is that way...

  691. 	// Section 7.7.2

  692. 	if (fRSSI) DBGPrintf("    Inquiry Result with RSSI - Count: %d\n", rxbuf_[2]);

  693. 	else DBGPrintf("    Inquiry Result - Count: %d\n", rxbuf_[2]);

  694. 	for (uint8_t i=0; i < rxbuf_[2]; i++) {

  695. 		uint8_t index_bd = 3 + (i*6);

  696. 		uint8_t index_ps = 3 + (6*rxbuf_[2]) + i;

  697. 		uint8_t index_class = 3 + (9*rxbuf_[2]) + i;

  698. 		uint8_t index_clock_offset = 3 + (12*rxbuf_[2]) + i;

  699. 		if (fRSSI) {

  700. 			// Handle the differences in offsets here...

  701. 			index_class = 3 + (8*rxbuf_[2]) + i;

  702. 			index_clock_offset = 3 + (11*rxbuf_[2]) + i;

  703. 		} 

  704. 		uint32_t bluetooth_class = rxbuf_[index_class] + ((uint32_t)rxbuf_[index_class+1] << 8) + ((uint32_t)rxbuf_[index_class+2] << 16);

  705. 		DBGPrintf("      BD:%x:%x:%x:%x:%x:%x, PS:%d, class: %x\n", 

  706. 			rxbuf_[index_bd],rxbuf_[index_bd+1],rxbuf_[index_bd+2],rxbuf_[index_bd+3],rxbuf_[index_bd+4],rxbuf_[index_bd+5],

  707. 			rxbuf_[index_ps], bluetooth_class);

  708. 		// See if we know the class 

  709. 		if (((bluetooth_class & 0xff00) == 0x2500) || ((bluetooth_class & 0xff00) == 0x500)) {

  710. 			DBGPrintf("      Peripheral device\n");

  711. 			if (bluetooth_class & 0x80) DBGPrintf("        Mouse\n");

  712. 			if (bluetooth_class & 0x40) DBGPrintf("        Keyboard\n"); 

  713. 			switch(bluetooth_class & 0x3c) {

  714. 				case 4: DBGPrintf("        Joystick\n"); break;

  715. 				case 8: DBGPrintf("        Gamepad\n"); break;

  716. 				case 0xc: DBGPrintf("        Remote Control\n"); break;

  717. 			}

  718. 

  719. 			// BUGBUG, lets hard code to go to new state...

  720. 			for (uint8_t i = 0; i < 6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[index_bd+i];

  721. 			connections_[current_connection_].device_class_ = bluetooth_class;

  722. 			connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_);

  723. 

  724.     		connections_[current_connection_].device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode

  725.     		connections_[current_connection_].device_clock_offset_[0] = rxbuf_[index_clock_offset];

  726.     		connections_[current_connection_].device_clock_offset_[1] = rxbuf_[index_clock_offset+1];

  727. 

  728. 			// Now we need to bail from inquiry and setup to try to connect...

  729. 			sendHCIInquiryCancel();

  730. 			pending_control_ = PC_INQUIRE_CANCEL;

  731. 			break;

  732. 		}

  733. 	}

  734. }

  735. 

  736. void BluetoothController::handle_hci_extended_inquiry_result()

  737. {	

  738. 	DBGPrintf("    Extended Inquiry Result - Count: %d\n", rxbuf_[2]);

  739. 	// Should always be only one result here. 

  740. 	uint8_t index_bd = 3;

  741. 	uint8_t index_ps = 9;

  742. 	uint8_t index_class = 11;

  743. 	uint8_t index_clock_offset = 14;

  744. 	//uint8_t index_rssi = 16;

  745. 	uint8_t index_eir_data = 17;

  746. 	uint8_t index_local_name = 0;

  747. 	uint8_t size_local_name = 0;

  748. 	uint32_t bluetooth_class = rxbuf_[index_class] + ((uint32_t)rxbuf_[index_class+1] << 8) + ((uint32_t)rxbuf_[index_class+2] << 16);

  749. 	DBGPrintf("      BD:%x:%x:%x:%x:%x:%x, PS:%d, class: %x\n", 

  750. 		rxbuf_[index_bd],rxbuf_[index_bd+1],rxbuf_[index_bd+2],rxbuf_[index_bd+3],rxbuf_[index_bd+4],rxbuf_[index_bd+5],

  751. 		rxbuf_[index_ps], bluetooth_class);

  752. 	// Lets see if we can find a name

  753. 	while (index_eir_data < 256) {

  754. 		if (rxbuf_[index_eir_data] == 0) break; // no more data

  755. 		switch (rxbuf_[index_eir_data+1]) {

  756. 			case 0x08: // Shortened local name

  757. 			case 0x09: // complete local name

  758. 				index_local_name = index_eir_data+2;

  759. 				size_local_name = rxbuf_[index_eir_data]-1;

  760. 				break;

  761. 		}

  762. 		index_eir_data += rxbuf_[index_eir_data] + 1;	// point to the next item

  763. 

  764. 	}

  765. 	if (index_local_name && size_local_name) {

  766. 		// Hack lets null teminate the string

  767. 		rxbuf_[index_local_name+size_local_name] = 0;

  768. 

  769. 		DBGPrintf("      Local Name: %s\n", &rxbuf_[index_local_name]);

  770. 	}

  771. 

  772. 	// See if we know the class 

  773. 	if (((bluetooth_class & 0xff00) == 0x2500) || ((bluetooth_class & 0xff00) == 0x500)) {

  774. 		DBGPrintf("      Peripheral device\n");

  775. 		if (bluetooth_class & 0x80) DBGPrintf("        Mouse\n");

  776. 		if (bluetooth_class & 0x40) DBGPrintf("        Keyboard\n"); 

  777. 		switch(bluetooth_class & 0x3c) {

  778. 			case 4: DBGPrintf("        Joystick\n"); break;

  779. 			case 8: DBGPrintf("        Gamepad\n"); break;

  780. 			case 0xc: DBGPrintf("        Remote Control\n"); break;

  781. 		}

  782. 

  783. 		// BUGBUG, lets hard code to go to new state...

  784. 		for (uint8_t i = 0; i < 6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[index_bd+i];

  785. 		connections_[current_connection_].device_class_ = bluetooth_class;

  786. 		connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_, index_local_name? &rxbuf_[index_local_name] : nullptr);

  787. 

  788. 		connections_[current_connection_].device_ps_repetion_mode_  = rxbuf_[index_ps]; // mode

  789. 		connections_[current_connection_].device_clock_offset_[0] = rxbuf_[index_clock_offset];

  790. 		connections_[current_connection_].device_clock_offset_[1] = rxbuf_[index_clock_offset+1];

  791. 

  792. 		// and if we found a driver, save away the name 

  793. 		if (connections_[current_connection_].device_driver_ && index_local_name && size_local_name) {

  794. 			uint8_t buffer_index;

  795. 			for (buffer_index = 0; size_local_name && (buffer_index < BTHIDInput::REMOTE_NAME_SIZE-1); buffer_index++) {

  796. 				connections_[current_connection_].device_driver_->remote_name_[buffer_index] = rxbuf_[index_local_name+buffer_index];

  797. 				size_local_name--;

  798. 			}

  799. 			connections_[current_connection_].device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated

  800. 		}

  801. 

  802. 		// Now we need to bail from inquiry and setup to try to connect...

  803. 		sendHCIInquiryCancel();

  804. 		pending_control_ = PC_INQUIRE_CANCEL;

  805. 	}

  806. }

  807. 

  808. void BluetoothController::handle_hci_inquiry_complete() {

  809. 	VDBGPrintf("    Inquiry Complete - status: %d\n", rxbuf_[2]);

  810. }

  811. 

  812. void BluetoothController::handle_hci_connection_complete() {

  813. 	//  0  1  2  3  4  5  6  7  8 9  10 11 12

  814. 	//       ST CH CH BD BD BD BD BD BD LT EN

  815. 	// 03 0b 04 00 00 40 25 00 58 4b 00 01 00 

  816. 	connections_[current_connection_].device_connection_handle_ = rxbuf_[3]+ (uint16_t)(rxbuf_[4]<<8);

  817. 	DBGPrintf("    Connection Complete - ST:%x LH:%x\n", rxbuf_[2], connections_[current_connection_].device_connection_handle_);

  818. 	if (do_pair_device_ && !(connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {

  819. 		sendHCIAuthenticationRequested();

  820. 		pending_control_ = PC_AUTHENTICATION_REQUESTED;

  821. 	} else if (connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {

  822. 		DBGPrintf("   Needs connect to device(PS4?)\n");

  823. 		// The PS4 requires a connection request to it. 

  824. 		delay(1);

  825. 		sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, HID_CTRL_PSM);

  826. #if 0		

  827. 		delay(1);

  828. 		

  829. 		uint8_t packet[2];

  830. 	    memset(packet, 0, sizeof(packet));

  831. 	    packet[0] = 0x43; 

  832. 	    packet[1] = 0x02;      // Report ID

  833. 	    USBHDBGSerial.printf("SixAxis Command Issued!\r\n");

  834.      	sendL2CapCommand(packet, sizeof(packet), 0x40);	

  835. #endif     	

  836. 	}

  837. }

  838. 

  839. void BluetoothController::handle_hci_incoming_connect() {

  840. 	//           BD    BD   BD    BD  BD   BD  CL    CL    CL  LT

  841. 	// 0x04 0x0A 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x40 0x05 0x00 0x01

  842. 	uint32_t class_of_device  = rxbuf_[8] + (uint16_t)(rxbuf_[9]<<8) + (uint32_t)(rxbuf_[10]<<16);

  843. 	DBGPrintf("    Event: Incoming Connect -  %x:%x:%x:%x:%x:%x CL:%x LT:%x\n", 

  844. 		rxbuf_[2], rxbuf_[3], rxbuf_[4], rxbuf_[5], rxbuf_[6], rxbuf_[7], class_of_device, rxbuf_[11]);

  845. 	if (((class_of_device & 0xff00) == 0x2500) || ((class_of_device & 0xff00) == 0x500)) {

  846. 		DBGPrintf("      Peripheral device\n");

  847. 		if (class_of_device & 0x80) DBGPrintf("        Mouse\n");

  848. 		if (class_of_device & 0x40) DBGPrintf("        Keyboard\n"); 

  849. 		switch(class_of_device & 0x3c) {

  850. 			case 4: DBGPrintf("        Joystick\n"); break;

  851. 			case 8: DBGPrintf("        Gamepad\n"); break;

  852. 			case 0xc: DBGPrintf("        Remote Control\n"); break;

  853. 		}

  854. 		if (count_connections_ < MAX_CONNECTIONS) current_connection_ = count_connections_++;

  855. 

  856. 		connections_[current_connection_].device_driver_ = find_driver(class_of_device);

  857. 

  858. 		// We need to save away the BDADDR and class link type?

  859. 		for(uint8_t i=0; i<6; i++) connections_[current_connection_].device_bdaddr_[i] = rxbuf_[i+2];

  860. 		connections_[current_connection_].device_class_ = class_of_device;	

  861. 		sendHCIRemoteNameRequest();

  862. 	}

  863. 

  864. //	sendHCIAuthenticationRequested();

  865. //	pending_control_ = PC_AUTHENTICATION_REQUESTED;

  866. }

  867. 

  868. 

  869. void BluetoothController::handle_hci_pin_code_request() {

  870. 	// 0x16 0x06 0x79 0x22 0x23 0x0A 0xC5 0xCC

  871. 	DBGPrintf("    Event: Pin Code Request %x:%x:%x:%x:%x:%x\n", 

  872. 		rxbuf_[2], rxbuf_[3], rxbuf_[4], rxbuf_[5], rxbuf_[6], rxbuf_[7]);

  873. 	sendHCIPinCodeReply();

  874. 	pending_control_ = PC_PIN_CODE_REPLY;

  875. }

  876. 

  877. void BluetoothController::handle_hci_link_key_request() {

  878. 	// 17 6 79 22 23 a c5 cc 

  879. 	DBGPrintf("    Event: Link Key Request %x:%x:%x:%x:%x:%x\n", 

  880. 		rxbuf_[2], rxbuf_[3], rxbuf_[4], rxbuf_[5], rxbuf_[6], rxbuf_[7]);

  881. 

  882. 	// Now here is where we need to decide to say we have key or tell them to 

  883. 	// cancel key...  right now hard code to cancel...

  884. 	sendHCILinkKeyNegativeReply();

  885. 	pending_control_ = PC_LINK_KEY_NEGATIVE;

  886. }

  887. 

  888. void BluetoothController::handle_hci_link_key_notification() {

  889. 	// 0   1  2  3  4 5  6  7  8  9 10  1  2  3  4  5  6  7  8  9 20  1  2  3 4

  890. 	// 18 17 79 22 23 a c5 cc 5e 98 d4 5e bb 15 66 da 67 fe 4f 87 2b 61 46 b4 0 

  891. 	DBGPrintf("    Event: Link Key Notificaton %x:%x:%x:%x:%x:%x Type:%x\n    key:", 

  892. 		rxbuf_[2], rxbuf_[3], rxbuf_[4], rxbuf_[5], rxbuf_[6], rxbuf_[7], rxbuf_[24]);

  893. 	for (uint8_t i = 8; i < 24; i++) DBGPrintf("%02x ", rxbuf_[i]);

  894. 	DBGPrintf("\n");

  895. 

  896. 	// Now here is where we need to decide to say we have key or tell them to 

  897. 	// cancel key...  right now hard code to cancel...

  898. 

  899. }

  900. 

  901. void BluetoothController::handle_hci_disconnect_complete()

  902. {

  903. 	//5 4 0 48 0 13

  904. 	DBGPrintf("    Event: HCI Disconnect complete(%d): handle: %x, reason:%x\n", rxbuf_[2], 

  905. 		rxbuf_[3]+(rxbuf_[4]<<8), rxbuf_[5]);

  906. 	if (connections_[current_connection_].device_driver_) {

  907. 		connections_[current_connection_].device_driver_->release_bluetooth();

  908. 		connections_[current_connection_].device_driver_->remote_name_[0] = 0;

  909. 		connections_[current_connection_].device_driver_ = nullptr;

  910.     	

  911.     	// Restore to normal... 

  912.     	connections_[current_connection_].control_dcid_ = 0x70;

  913.     	connections_[current_connection_].interrupt_dcid_ = 0x71;

  914. 	}

  915. 	// Probably should clear out connection data. 

  916. #if 0

  917. 	connections_[current_connection_].device_connection_handle_ = 0;

  918. 	connections_[current_connection_].device_class_ = 0;	

  919. 	memset(connections_[current_connection_].device_bdaddr_, 0, sizeof(connections_[current_connection_].device_bdaddr_));

  920. #endif

  921. 	// Now we need to remove that item from our list of connections.

  922. 	count_connections_--;  

  923. 	if (count_connections_ == 0) {

  924. 		// reset the next connection counts back to initial states.

  925. 		next_dcid_ = 0x70;		// Lets try not hard coding control and interrupt dcid

  926. 	}

  927. 

  928. 	for (uint8_t i = current_connection_; i < count_connections_; i++) connections_[i] = connections_[i+1];

  929. 	current_connection_ = 0;

  930. 

  931. }

  932. 

  933. void BluetoothController::handle_hci_authentication_complete()

  934. {

  935. 	//  6 3 13 48 0

  936. 	DBGPrintf("    Event: HCI Authentication complete(%d): handle: %x\n", rxbuf_[2], 

  937. 		rxbuf_[3]+(rxbuf_[4]<<8));

  938. 	// Start up lcap connection...

  939. 	connections_[current_connection_].connection_rxid_ = 0;

  940. 	sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, HID_CTRL_PSM);

  941. }

  942. 

  943. 

  944. 

  945. void BluetoothController::handle_hci_remote_name_complete() {

  946. 	//           STAT bd   bd   bd   bd    bd  bd

  947. 	// 0x07 0xFF 0x00 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x42 0x6C 0x75 0x65 0x74 0x6F 0x6F ...

  948. 	DBGPrintf("    Event: handle_hci_remote_name_complete(%d)\n", rxbuf_[2]);

  949. 	if (rxbuf_[2] == 0) {

  950. 		DBGPrintf("    Remote Name: ");

  951. 		for (uint8_t *psz = &rxbuf_[9]; *psz; psz++) DBGPrintf("%c", *psz);

  952. 		DBGPrintf("\n");

  953. 	}

  954. 

  955. 	if (connections_[current_connection_].device_driver_) {

  956. 		if (!connections_[current_connection_].device_driver_->remoteNameComplete(&rxbuf_[9])) {

  957. 			connections_[current_connection_].device_driver_->release_bluetooth();

  958. 			connections_[current_connection_].device_driver_ = nullptr;

  959. 		}

  960. 	}	

  961. 	if (!connections_[current_connection_].device_driver_) {

  962. 		connections_[current_connection_].device_driver_ = find_driver(connections_[current_connection_].device_class_, &rxbuf_[9]);

  963. 		// not sure I should call remote name again, but they already process...

  964. 		if (connections_[current_connection_].device_driver_) {

  965. 			connections_[current_connection_].device_driver_->remoteNameComplete(&rxbuf_[9]);

  966. 		}

  967. 	}

  968. 	if (connections_[current_connection_].device_driver_) {

  969. 		// lets save away the string. 

  970. 		uint8_t buffer_index;

  971. 		for (buffer_index = 0; buffer_index < BTHIDInput::REMOTE_NAME_SIZE-1; buffer_index++) {

  972. 			connections_[current_connection_].device_driver_->remote_name_[buffer_index] = rxbuf_[9+buffer_index];

  973. 			if (!connections_[current_connection_].device_driver_->remote_name_[buffer_index]) break;

  974. 		}

  975. 		connections_[current_connection_].device_driver_->remote_name_[buffer_index] = 0;	// make sure null terminated

  976. 

  977. 		if (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS) {

  978. 			// Real hack see if PS3... 

  979. 	    	connections_[current_connection_].control_dcid_ = 0x40;

  980. 	    	connections_[current_connection_].interrupt_dcid_ = 0x41;

  981. 	    } else {

  982. 	    	connections_[current_connection_].control_dcid_ = next_dcid_++;

  983. 	    	connections_[current_connection_].interrupt_dcid_ = next_dcid_++;

  984. 	    }

  985. 	}

  986. 

  987. 	// If we are in the connection complete mode, then this is a pairing state and needed to call

  988. 	// get remote name later. 

  989. 	if (connections_[current_connection_].connection_complete_) {	

  990. 		if (connections_[current_connection_].device_driver_) {	// We have a driver call their 

  991. 			connections_[current_connection_].device_driver_->connectionComplete();

  992. 			connections_[current_connection_].connection_complete_ = false;	// only call once

  993. 		}

  994. 	} else {

  995. 		sendHCIAcceptConnectionRequest();		

  996. 	} 

  997. }

  998. 

  999. void BluetoothController::handle_hci_remote_version_information_complete() {

  1000. 	//           STAT bd   bd   bd   bd    bd  bd

  1001. 	//c 8 0 48 0 5 45 0 0 0

  1002. 	connections_[current_connection_].remote_ver_ = rxbuf_[6];

  1003. 	connections_[current_connection_].remote_man_ = rxbuf_[7]+((uint16_t)rxbuf_[8]<< 8);

  1004. 	connections_[current_connection_].remote_subv_ = rxbuf_[9];

  1005. 	

  1006. 	DBGPrintf("    Event: handle_hci_remote_version_information_complete(%d): ", rxbuf_[2]);

  1007. 	DBGPrintf(" Handle: %x, Ver:%x, Man: %x, SV: %x\n", 

  1008. 		rxbuf_[3]+((uint16_t)rxbuf_[4]<< 8), connections_[current_connection_].remote_ver_, connections_[current_connection_].remote_man_, connections_[current_connection_].remote_subv_);

  1009. 	// Lets now try to accept the connection. 

  1010. 	sendHCIAcceptConnectionRequest();

  1011. }

  1012. 

  1013. void BluetoothController::rx2_data(const Transfer_t *transfer)

  1014. {

  1015. 	uint32_t len = transfer->length - ((transfer->qtd.token >> 16) & 0x7FFF);

  1016. 	DBGPrintf("\n=====================\nBT rx2_data(%d): ", len);

  1017. 	uint8_t *buffer = (uint8_t*)transfer->buffer;

  1018. 	for (uint8_t i=0; i < len; i++) DBGPrintf("%x ", buffer[i]);

  1019. 	DBGPrintf("\n");

  1020. 

  1021. 	// call backs.  See if this is an L2CAP reply. example

  1022. 	//  HCI      | l2cap 

  1023. 	//48 20 10 0 | c 0 1 0 | 3 0 8 0 44 0 70 0 0 0 0 0

  1024. 	// BUGBUG need to do more verification, like the handle

  1025. 	uint16_t hci_length = buffer[2] + ((uint16_t)buffer[3]<<8);

  1026. 	uint16_t l2cap_length = buffer[4] + ((uint16_t)buffer[5]<<8);

  1027. //	uint16_t rsp_packet_length = buffer[10] + ((uint16_t)buffer[11]<<8);

  1028. 	if ((hci_length == (l2cap_length + 4)) /*&& (hci_length == (rsp_packet_length+8))*/) {

  1029. 		// All the lengths appear to be correct...  need to do more...

  1030. 		// See if we should set the current_connection...

  1031. 		for (uint8_t i = 0; i < count_connections_; i++) {

  1032. 			//if (connections_[i].interrupt_dcid_ == buffer[6]) {

  1033. 			if (connections_[i].device_connection_handle_ == buffer[0]) {

  1034. 				current_connection_ = i;

  1035. 				break;

  1036. 			}

  1037. 		}

  1038. 		switch (buffer[8]) {

  1039. 			case L2CAP_CMD_CONNECTION_REQUEST:

  1040. 				process_l2cap_connection_request(&buffer[8]);

  1041. 				break;

  1042. 			case L2CAP_CMD_CONNECTION_RESPONSE:

  1043. 				process_l2cap_connection_response(&buffer[8]);

  1044. 				break;

  1045. 			case L2CAP_CMD_CONFIG_REQUEST:

  1046. 				process_l2cap_config_request(&buffer[8]);

  1047. 				break;

  1048. 			case L2CAP_CMD_CONFIG_RESPONSE:

  1049. 				process_l2cap_config_response(&buffer[8]);

  1050. 				break;

  1051. 

  1052. 			case HID_THDR_DATA_INPUT:

  1053. 				handleHIDTHDRData(buffer);	// Pass the whole buffer...

  1054. 				break;

  1055. 			case L2CAP_CMD_COMMAND_REJECT:

  1056. 				process_l2cap_command_reject(&buffer[8]);

  1057. 				break;

  1058. 			case L2CAP_CMD_DISCONNECT_REQUEST:

  1059. 				process_l2cap_disconnect_request(&buffer[8]);

  1060. 				break;

  1061. 		}

  1062. 	}

  1063. 

  1064. 

  1065. 	

  1066. 	

  1067. 

  1068. 	// Queue up for next read...

  1069. 	queue_Data_Transfer(rx2pipe_, rx2buf_, rx2_size_, this);

  1070. }

  1071. 

  1072. 

  1073. void BluetoothController::sendHCICommand(uint16_t hciCommand, uint16_t cParams, const uint8_t* data)

  1074. {

  1075. 	txbuf_[0] = hciCommand & 0xff;

  1076. 	txbuf_[1] = (hciCommand >> 8) & 0xff;

  1077. 	txbuf_[2] = cParams;

  1078. 	if (cParams) {

  1079. 		memcpy(&txbuf_[3], data, cParams);	// copy in the commands parameters.

  1080. 	}

  1081. 	uint8_t nbytes = cParams+3;

  1082. 	for (uint8_t i=0; i< nbytes; i++) DBGPrintf("%02x ", txbuf_[i]);

  1083. 	DBGPrintf(")\n");

  1084. 	mk_setup(setup, 0x20, 0x0, 0, 0, nbytes);

  1085. 	queue_Control_Transfer(device, &setup, txbuf_, this);

  1086. }

  1087. 

  1088. //---------------------------------------------

  1089. void  BluetoothController::sendHCIHCIWriteInquiryMode(uint8_t inquiry_mode) {

  1090. 	// Setup Inquiry mode 

  1091. 	DBGPrintf("HCI_WRITE_INQUIRY_MODE called (");

  1092. 	sendHCICommand(HCI_WRITE_INQUIRY_MODE, 1, &inquiry_mode);	

  1093. }

  1094. 

  1095. void BluetoothController::sendHCISetEventMask() {

  1096. 	// Setup Inquiry mode 

  1097. 	DBGPrintf("HCI_Set_Event_Mask called (");

  1098. 	static const uint8_t hci_event_mask_data[8] = {

  1099. 		// Default: 0x0000 1FFF FFFF FFFF 

  1100. 		0xff,0xff, 0xff,0xff, 0xff,0x5f, 0x00,0x00};  // default plus extended inquiry mode

  1101. 	sendHCICommand(HCI_Set_Event_Mask, sizeof(hci_event_mask_data), hci_event_mask_data);	

  1102. }

  1103. 

  1104. //---------------------------------------------

  1105. void BluetoothController::sendHCI_INQUIRY() {

  1106. 	// Start unlimited inqury, set timeout to max and 

  1107. 	DBGPrintf("HCI_INQUIRY called (");

  1108. 	static const uint8_t hci_inquiry_data[ ] = {

  1109. 			0x33, 0x8B, 0x9E, 	// Bluetooth assigned number LAP 0x9E8B33 General/unlimited inquiry Access mode

  1110. 			0x30, 0xa};			// Max inquiry time little over minute and up to 10 responses

  1111. 	sendHCICommand(HCI_INQUIRY, sizeof(hci_inquiry_data), hci_inquiry_data);	

  1112. }

  1113. 

  1114. //---------------------------------------------

  1115. void BluetoothController::sendHCIInquiryCancel() {

  1116. 	DBGPrintf("HCI_INQUIRY_CANCEL called (");

  1117. 	sendHCICommand(HCI_INQUIRY_CANCEL, 0, nullptr);	

  1118. }

  1119. 

  1120. //---------------------------------------------

  1121. void BluetoothController::sendHCICreateConnection() {

  1122. 	DBGPrintf("HCI_CREATE_CONNECTION called (");

  1123. 	uint8_t connection_data[13];

  1124. 	//  0    1    2    3    4    5    6    7    8   9    10   11   12

  1125. 	// BD   BD   BD   BD   BD   BD   PT   PT  PRS   0    CS   CS   ARS

  1126. 	//0x79 0x22 0x23 0x0A 0xC5 0xCC 0x18 0xCC 0x01 0x00 0x00 0x00 0x00

  1127. 	//0x05 0x04 0x0D 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x18 0xCC 0x01 0x00 0x00 0x00 0x00

  1128. 	//  05   04   0d   40   25   00   c4   01   00   18   cc   01   00   00 00     00 

  1129. 

  1130. 	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];

  1131. 	connection_data[6] = 0x18; //DM1/DH1

  1132. 	connection_data[7] = 0xcc; //

  1133. 	connection_data[8] = connections_[current_connection_].device_ps_repetion_mode_;  // from device

  1134. 	connection_data[9] = 0;	//

  1135. 	connection_data[10] = 0;  // clock offset 

  1136. 	connection_data[11] = 0;  // clock offset 

  1137. 	connection_data[12] = 0;  // allow role swith no 

  1138. 	sendHCICommand(HCI_CREATE_CONNECTION, sizeof(connection_data), connection_data);	

  1139. }

  1140. 

  1141. //---------------------------------------------

  1142. void BluetoothController::sendHCIAcceptConnectionRequest() {

  1143. 	DBGPrintf("HCI_OP_ACCEPT_CONN_REQ called (");

  1144. 	uint8_t connection_data[7];

  1145. 

  1146. 	//  0    1    2    3    4    5    6    7    8   9    10   11   12

  1147. 	//  BD   BD   BD   BD   BD   BD  role 

  1148. 	//0x79 0x22 0x23 0x0A 0xC5 0xCC 0x00

  1149. 	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];

  1150. 	connection_data[6] = 0; // Role as master

  1151. 	sendHCICommand(HCI_OP_ACCEPT_CONN_REQ, sizeof(connection_data), connection_data);	

  1152. }

  1153. 

  1154. //---------------------------------------------

  1155. void BluetoothController::sendHCIAuthenticationRequested() {

  1156. 	DBGPrintf("HCI_AUTH_REQUESTED called (");

  1157. 	uint8_t connection_data[2];

  1158. 	connection_data[0] = connections_[current_connection_].device_connection_handle_ & 0xff;

  1159. 	connection_data[1] = (connections_[current_connection_].device_connection_handle_>>8) & 0xff;

  1160. 	sendHCICommand(HCI_AUTH_REQUESTED, sizeof(connection_data), connection_data);	

  1161. }

  1162. 

  1163. //---------------------------------------------

  1164. void BluetoothController::sendHCILinkKeyNegativeReply() {

  1165. 	DBGPrintf("HCI_LINK_KEY_NEG_REPLY called (");

  1166. 	uint8_t connection_data[6];

  1167. 	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];

  1168. 	sendHCICommand(HCI_LINK_KEY_NEG_REPLY, sizeof(connection_data), connection_data);	

  1169. }

  1170. 

  1171. //---------------------------------------------

  1172. // BUGBUG:: hard code string for this pass.

  1173. 

  1174. void BluetoothController::sendHCIPinCodeReply() {

  1175. 	// 0x0D 0x04 0x17 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x04 0x30 0x30 0x30 0x30 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00

  1176. 	DBGPrintf("HCI_PIN_CODE_REPLY called (");

  1177. 	uint8_t connection_data[23];

  1178. 	uint8_t i;

  1179. 

  1180. 	for (i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];

  1181. 

  1182. 	for (i=0; pair_pincode_[i] !=0; i++) connection_data[7+i] = pair_pincode_[i];

  1183. 	connection_data[6] = i; // remember the length	

  1184. 	for (uint8_t i=7+connection_data[6]; i<23; i++) connection_data[i] = 0;

  1185. 	sendHCICommand(HCI_PIN_CODE_REPLY, sizeof(connection_data), connection_data);	

  1186. }

  1187. 

  1188. //---------------------------------------------

  1189. void BluetoothController::sendResetHCI() {

  1190. 	DBGPrintf("HCI_RESET called (");

  1191. 	sendHCICommand(HCI_RESET, 0, nullptr);	

  1192. }

  1193. 

  1194. void BluetoothController::sendHDCWriteClassOfDev() {

  1195. 	// 0x24 0x0C 0x03 0x04 0x08 0x00

  1196. 	const static uint8_t device_class_data[] = {BT_CLASS_DEVICE & 0xff, (BT_CLASS_DEVICE >> 8) & 0xff, (BT_CLASS_DEVICE >> 16) & 0xff};

  1197. 	DBGPrintf("HCI_WRITE_CLASS_OF_DEV called (");

  1198. 	sendHCICommand(HCI_WRITE_CLASS_OF_DEV, sizeof(device_class_data), device_class_data);	

  1199. }

  1200. 

  1201. 

  1202. 

  1203. void BluetoothController::sendHCIReadBDAddr() {

  1204. 	DBGPrintf("HCI_Read_BD_ADDR called (");

  1205. 	sendHCICommand(HCI_Read_BD_ADDR, 0, nullptr);	

  1206. }

  1207. 

  1208. void BluetoothController::sendHCIReadLocalVersionInfo() {

  1209. 	DBGPrintf("HCI_Read_Local_Version_Information called (");

  1210. 	sendHCICommand(HCI_Read_Local_Version_Information, 0, nullptr);	

  1211. }

  1212. 

  1213. 

  1214. void BluetoothController::sendHCIWriteScanEnable(uint8_t scan_op)	{//				0x0c1a

  1215. // 0x1A 0x0C 0x01 0x02

  1216. 	DBGPrintf("HCI_WRITE_SCAN_ENABLE called(");

  1217. 	sendHCICommand(HCI_WRITE_SCAN_ENABLE, 1, &scan_op);

  1218. }

  1219. 

  1220. void BluetoothController::sendHCIRemoteNameRequest() {		// 0x0419

  1221. 	//               BD   BD   BD   BD   BD   BD   PS   0    CLK   CLK

  1222. 	//0x19 0x04 0x0A 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x01 0x00 0x00 0x00

  1223. 

  1224. 	DBGPrintf("HCI_OP_REMOTE_NAME_REQ called (");

  1225. 	uint8_t connection_data[10];

  1226. 	for (uint8_t i=0; i<6; i++) connection_data[i] = connections_[current_connection_].device_bdaddr_[i];

  1227. 	connection_data[6] = 1;	// page scan repeat mode...

  1228. 	connection_data[7] = 0;	 // 0

  1229. 	connection_data[8] = 0;	// Clk offset

  1230. 	connection_data[9] = 0; 

  1231. 	sendHCICommand(HCI_OP_REMOTE_NAME_REQ, sizeof(connection_data), connection_data);	

  1232. }

  1233. 

  1234. void BluetoothController::sendHCIRemoteVersionInfoRequest() {		// 0x041D

  1235. 	//               BD   BD   BD   BD   BD   BD   PS   0    CLK   CLK

  1236. 	//0x19 0x04 0x0A 0x79 0x22 0x23 0x0A 0xC5 0xCC 0x01 0x00 0x00 0x00

  1237. 

  1238. 	DBGPrintf("HCI_OP_READ_REMOTE_VERSION_INFORMATION called (");

  1239. 	uint8_t connection_data[2];

  1240. 	connection_data[0] = connections_[current_connection_].device_connection_handle_ & 0xff;

  1241. 	connection_data[1] = (connections_[current_connection_].device_connection_handle_>>8) & 0xff;

  1242. 	sendHCICommand(HCI_OP_READ_REMOTE_VERSION_INFORMATION, sizeof(connection_data), connection_data);	

  1243. }

  1244. 

  1245. // l2cap support functions. 

  1246. void BluetoothController::sendl2cap_ConnectionResponse(uint16_t handle, uint8_t rxid, uint16_t dcid, uint16_t scid, uint8_t result) {

  1247.  	uint8_t l2capbuf[12];

  1248.     l2capbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE; // Code

  1249.     l2capbuf[1] = rxid; // Identifier

  1250.     l2capbuf[2] = 0x08; // Length

  1251.     l2capbuf[3] = 0x00;

  1252.     l2capbuf[4] = dcid & 0xff; // Destination CID

  1253.     l2capbuf[5] = dcid >> 8;

  1254.     l2capbuf[6] = scid & 0xff; // Source CID

  1255.     l2capbuf[7] = scid >> 8;

  1256.     l2capbuf[8] = result; // Result: Pending or Success

  1257.     l2capbuf[9] = 0x00;

  1258.     l2capbuf[10] = 0x00; // No further information

  1259.     l2capbuf[11] = 0x00;

  1260. 

  1261. 	DBGPrintf("L2CAP_CMD_CONNECTION_RESPONSE called(");

  1262.     sendL2CapCommand(handle, l2capbuf, sizeof(l2capbuf));

  1263. }

  1264. 

  1265. 

  1266. 

  1267. void BluetoothController::sendl2cap_ConnectionRequest(uint16_t handle, uint8_t rxid, uint16_t scid, uint16_t psm) {

  1268.  	uint8_t l2capbuf[8];

  1269.     l2capbuf[0] = L2CAP_CMD_CONNECTION_REQUEST; // Code

  1270.     l2capbuf[1] = rxid; // Identifier

  1271.     l2capbuf[2] = 0x04; // Length

  1272.     l2capbuf[3] = 0x00;

  1273.     l2capbuf[4] = (uint8_t)(psm & 0xff); // PSM

  1274.     l2capbuf[5] = (uint8_t)(psm >> 8);

  1275.     l2capbuf[6] = scid & 0xff; // Source CID

  1276.     l2capbuf[7] = (scid >> 8) & 0xff;

  1277. 

  1278. 	DBGPrintf("ConnectionRequest called(");

  1279.     sendL2CapCommand(handle, l2capbuf, sizeof(l2capbuf));

  1280. }

  1281. 

  1282. void BluetoothController::sendl2cap_ConfigRequest(uint16_t handle, uint8_t rxid, uint16_t dcid) {

  1283. 	 	uint8_t l2capbuf[12];

  1284.         l2capbuf[0] = L2CAP_CMD_CONFIG_REQUEST; // Code

  1285.         l2capbuf[1] = rxid; // Identifier

  1286.         l2capbuf[2] = 0x08; // Length

  1287.         l2capbuf[3] = 0x00;

  1288.         l2capbuf[4] = dcid & 0xff; // Destination CID

  1289.         l2capbuf[5] = (dcid >> 8) & 0xff;

  1290.         l2capbuf[6] = 0x00; // Flags

  1291.         l2capbuf[7] = 0x00;

  1292.         l2capbuf[8] = 0x01; // Config Opt: type = MTU (Maximum Transmission Unit) - Hint

  1293.         l2capbuf[9] = 0x02; // Config Opt: length

  1294.         l2capbuf[10] = 0xFF; // MTU

  1295.         l2capbuf[11] = 0xFF;

  1296. 

  1297. 		DBGPrintf("L2CAP_ConfigRequest called(");

  1298.         sendL2CapCommand(handle, l2capbuf, sizeof(l2capbuf));

  1299. }

  1300. 

  1301. void BluetoothController::sendl2cap_ConfigResponse(uint16_t handle, uint8_t rxid, uint16_t scid) {

  1302. 	 	uint8_t l2capbuf[14];

  1303.         l2capbuf[0] = L2CAP_CMD_CONFIG_RESPONSE; // Code

  1304.         l2capbuf[1] = rxid; // Identifier

  1305.         l2capbuf[2] = 0x0A; // Length

  1306.         l2capbuf[3] = 0x00;

  1307.         l2capbuf[4] = scid & 0xff; // Source CID

  1308.         l2capbuf[5] = (scid >> 8) & 0xff;

  1309.         l2capbuf[6] = 0x00; // Flag

  1310.         l2capbuf[7] = 0x00;

  1311.         l2capbuf[8] = 0x00; // Result

  1312.         l2capbuf[9] = 0x00;

  1313.         l2capbuf[10] = 0x01; // Config

  1314.         l2capbuf[11] = 0x02;

  1315.         l2capbuf[12] = 0xA0;

  1316.         l2capbuf[13] = 0x02;

  1317. 

  1318. 		DBGPrintf("L2CAP_ConfigResponse called(");

  1319.         sendL2CapCommand(handle, l2capbuf, sizeof(l2capbuf));

  1320. }

  1321. 

  1322. 

  1323. //*******************************************************************

  1324. //*******************************************************************

  1325. void BluetoothController::tx_data(const Transfer_t *transfer)

  1326. {

  1327. 	println("    tx_data(bluetooth) ", pending_control_, HEX);

  1328. #ifdef DEBUG_BT_VERBOSE

  1329. 	DBGPrintf("tx_data callback (bluetooth): %d : ", pending_control_tx_);

  1330. 	uint8_t *buffer = (uint8_t*)transfer->buffer;

  1331. 	for (uint8_t i=0; i < transfer->length; i++) DBGPrintf("%x ", buffer[i]);

  1332. 	DBGPrintf("\n");

  1333. #endif

  1334. 	switch (pending_control_tx_) {

  1335. 		case STATE_TX_SEND_CONNECT_INT:

  1336. 		delay(1);

  1337. 		connections_[current_connection_].connection_rxid_++;

  1338. 		sendl2cap_ConnectionRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_dcid_, HID_INTR_PSM);

  1339. 		pending_control_tx_ = 0;

  1340. 		break;

  1341. 	case STATE_TX_SEND_CONECT_RSP_SUCCESS:

  1342. 		delay(1);

  1343. 		// Tell the device we are ready

  1344. 		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_++, connections_[current_connection_].control_dcid_, connections_[current_connection_].control_scid_, SUCCESSFUL);

  1345. 		pending_control_tx_ = STATE_TX_SEND_CONFIG_REQ;

  1346. 		break;

  1347. 	case STATE_TX_SEND_CONFIG_REQ:

  1348. 		delay(1);

  1349. 		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_scid_);

  1350. 		pending_control_tx_ = 0;

  1351. 		break;

  1352. 	case STATE_TX_SEND_CONECT_ISR_RSP_SUCCESS:

  1353. 		delay(1);

  1354. 		// Tell the device we are ready

  1355. 		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_++, connections_[current_connection_].interrupt_dcid_, connections_[current_connection_].interrupt_scid_, SUCCESSFUL);

  1356. 		pending_control_tx_ = STATE_TX_SEND_CONFIG_ISR_REQ;

  1357. 		break;

  1358. 	case STATE_TX_SEND_CONFIG_ISR_REQ:

  1359. 		delay(1);

  1360. 		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_scid_);

  1361. 		pending_control_tx_ = 0;

  1362. 		break;

  1363. 	}

  1364. }

  1365. 

  1366. 

  1367. //*******************************************************************

  1368. //                                                                 

  1369. // HCI ACL Packets

  1370. // HCI Handle Low, HCI_Handle_High (PB, BC), Total length low, TLH  - HCI ACL Data packet

  1371. // length Low, length high, channel id low, channel id high - L2CAP header

  1372. // code, identifier, length, ... - Control-frame 

  1373. 

  1374. /************************************************************/

  1375. /*                    L2CAP Commands                        */

  1376. 

  1377. // Public wrrapper function

  1378. void BluetoothController::sendL2CapCommand(uint8_t* data, uint8_t nbytes, int channel) {

  1379. 	uint16_t channel_out;

  1380. 	switch (channel) {

  1381. 		case CONTROL_SCID:

  1382. 			channel_out = connections_[current_connection_].control_scid_;

  1383. 			break;

  1384. 		case INTERRUPT_SCID:

  1385. 			channel_out = connections_[current_connection_].interrupt_scid_;

  1386. 			break;

  1387. 		default:

  1388. 			channel_out = (uint16_t)channel;

  1389. 	}

  1390. 	DBGPrintf("sendL2CapCommand: %x %d %x\n", (uint32_t)data, nbytes, channel, channel_out);

  1391. 	sendL2CapCommand (connections_[current_connection_].device_connection_handle_, data, nbytes, channel_out & 0xff, (channel_out >> 8) & 0xff);

  1392. }

  1393. 

  1394. 

  1395. void BluetoothController::sendL2CapCommand(uint16_t handle, uint8_t* data, uint8_t nbytes, uint8_t channelLow, uint8_t channelHigh)

  1396. {

  1397.     txbuf_[0] = handle & 0xff; // HCI handle with PB,BC flag

  1398.     txbuf_[1] = (((handle >> 8) & 0x0f) | 0x20);

  1399.     txbuf_[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length

  1400.     txbuf_[3] = (uint8_t)((4 + nbytes) >> 8);

  1401.     txbuf_[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length

  1402.     txbuf_[5] = (uint8_t)(nbytes >> 8);

  1403.     txbuf_[6] = channelLow;

  1404.     txbuf_[7] = channelHigh;

  1405. 	if (nbytes) {

  1406. 		memcpy(&txbuf_[8], data, nbytes);	// copy in the commands parameters.

  1407. 	}

  1408. 	nbytes = nbytes+8;

  1409. 	for (uint8_t i=0; i< nbytes; i++) DBGPrintf("%02x ", txbuf_[i]);

  1410. 	DBGPrintf(")\n");

  1411. 	

  1412. 	if (!queue_Data_Transfer(txpipe_, txbuf_, nbytes, this)) {

  1413. 		println("sendL2CapCommand failed");

  1414. 	}

  1415. }

  1416. 

  1417. void  BluetoothController::process_l2cap_connection_request(uint8_t *data) {

  1418. 	//       ID   LEN  LEN PSM  PSM  SCID SCID        

  1419. 	// 0x02 0x02 0x04 0x00 0x11 0x00 0x43 0x00

  1420. 

  1421. 	uint16_t psm = data[4]+((uint16_t)data[5] << 8); 

  1422. 	uint16_t scid = data[6]+((uint16_t)data[7] << 8);

  1423. 	connections_[current_connection_].connection_rxid_ = data[1];

  1424. 	DBGPrintf("    L2CAP Connection Request: ID: %d, PSM: %x, SCID: %x\n",connections_[current_connection_].connection_rxid_, psm, scid);

  1425. 

  1426. 	// Assuming not pair mode Send response like:

  1427. 	//      RXID Len  LEN  DCID DCID  SCID SCID RES 0     0    0

  1428. 	// 0x03 0x02 0x08 0x00 0x70 0x00 0x43 0x00 0x01 0x00 0x00 0x00

  1429. 	if (psm == HID_CTRL_PSM) {

  1430. 		connections_[current_connection_].control_scid_ = scid;

  1431. 		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].control_dcid_, connections_[current_connection_].control_scid_, PENDING);

  1432. 		pending_control_tx_ = STATE_TX_SEND_CONECT_RSP_SUCCESS;		

  1433. 	} else if (psm == HID_INTR_PSM) {

  1434. 		connections_[current_connection_].interrupt_scid_ = scid;

  1435. 		sendl2cap_ConnectionResponse(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, connections_[current_connection_].interrupt_dcid_, connections_[current_connection_].interrupt_scid_, PENDING);

  1436. 		pending_control_tx_ = STATE_TX_SEND_CONECT_ISR_RSP_SUCCESS;		

  1437. 

  1438. 	}

  1439. }

  1440. 

  1441. // Process the l2cap_connection_response...

  1442. void BluetoothController::process_l2cap_connection_response(uint8_t *data) {

  1443. 

  1444. 	uint16_t scid = data[4]+((uint16_t)data[5] << 8); 

  1445. 	uint16_t dcid = data[6]+((uint16_t)data[7] << 8); 

  1446. 

  1447. 	DBGPrintf("    L2CAP Connection Response: ID: %d, Dest:%x, Source:%x, Result:%x, Status: %x\n",

  1448. 		data[1], scid, dcid,

  1449. 		data[8]+((uint16_t)data[9] << 8), data[10]+((uint16_t)data[11] << 8));

  1450. 

  1451. 	//48 20 10 0 | c 0 1 0 | 3 0 8 0 44 0 70 0 0 0 0 0

  1452. 	if (dcid == connections_[current_connection_].interrupt_dcid_) {

  1453. 		connections_[current_connection_].interrupt_scid_ = scid;

  1454. 		DBGPrintf("      Interrupt Response\n");

  1455. 		connections_[current_connection_].connection_rxid_++;

  1456. 		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, scid);

  1457. 	} else if (dcid == connections_[current_connection_].control_dcid_) {

  1458. 		connections_[current_connection_].control_scid_ = scid;

  1459. 		DBGPrintf("      Control Response\n");

  1460. 		sendl2cap_ConfigRequest(connections_[current_connection_].device_connection_handle_, connections_[current_connection_].connection_rxid_, scid);

  1461. 	}

  1462. }

  1463. 

  1464. void BluetoothController::process_l2cap_config_request(uint8_t *data) {

  1465. 	//48 20 10 0 c 0 1 0 *4 2 8 0 70 0 0 0 1 2 30 0

  1466. 	uint16_t dcid = data[4]+((uint16_t)data[5] << 8); 

  1467. 	DBGPrintf("    L2CAP config Request: ID: %d, Dest:%x, Flags:%x,  Options: %x %x %x %x\n",

  1468. 		data[1], dcid, data[6]+((uint16_t)data[7] << 8),

  1469. 		data[8], data[9], data[10], data[11]);

  1470. 	// Now see which dest was specified

  1471. 	if (dcid == connections_[current_connection_].control_dcid_) {

  1472. 		DBGPrintf("      Control Configuration request\n");

  1473. 		sendl2cap_ConfigResponse(connections_[current_connection_].device_connection_handle_, data[1], connections_[current_connection_].control_scid_);

  1474. 	} else if (dcid == connections_[current_connection_].interrupt_dcid_) {

  1475. 		DBGPrintf("      Interrupt Configuration request\n");

  1476. 		sendl2cap_ConfigResponse(connections_[current_connection_].device_connection_handle_, data[1], connections_[current_connection_].interrupt_scid_);

  1477. 	}

  1478. }

  1479. 

  1480. void BluetoothController::process_l2cap_config_response(uint8_t *data) {

  1481. 	// 48 20 12 0 e 0 1 0 5 0 a 0 70 0 0 0 0 0 1 2 30 0

  1482. 	uint16_t scid = data[4]+((uint16_t)data[5] << 8); 

  1483. 	DBGPrintf("    L2CAP config Response: ID: %d, Source:%x, Flags:%x, Result:%x, Config: %x\n",

  1484. 		data[1], scid, data[6]+((uint16_t)data[7] << 8),

  1485. 		data[8]+((uint16_t)data[9] << 8), data[10]+((uint16_t)data[11] << 8));

  1486. 	if (scid == connections_[current_connection_].control_dcid_) {

  1487. 		// Set HID Boot mode

  1488. 		// Don't do if PS3...

  1489. 		if (!(connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_PS3_IDS)) {

  1490. 			setHIDProtocol(HID_BOOT_PROTOCOL);  //

  1491. 		}

  1492. 		//setHIDProtocol(HID_RPT_PROTOCOL);  //HID_RPT_PROTOCOL

  1493. 		if (do_pair_device_ && !(connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_DONT_NEED_CONNECT))) {

  1494. 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;

  1495. 		} else if (connections_[current_connection_].device_driver_ && (connections_[current_connection_].device_driver_->special_process_required & BTHIDInput::SP_NEED_CONNECT)) {

  1496. 			DBGPrintf("   Needs connect to device INT(PS4?)\n");

  1497. 			// The PS4 requires a connection request to it. 

  1498. 			pending_control_tx_ = STATE_TX_SEND_CONNECT_INT;

  1499. 		} else {

  1500. 			pending_control_ = 0;

  1501. 		}

  1502. 	} else if (scid == connections_[current_connection_].interrupt_dcid_) {

  1503. 		// Enable SCan to page mode

  1504. 		connections_[current_connection_].connection_complete_ = true;

  1505. 		sendHCIWriteScanEnable(2);

  1506. 	}

  1507. }

  1508. 

  1509. void BluetoothController::process_l2cap_command_reject(uint8_t *data) {		

  1510. 	// 48 20 b 0 7 0 70 0 *1 0 0 0 2 0 4 

  1511. 	DBGPrintf("    L2CAP command reject: ID: %d, length:%x, Reason:%x,  Data: %x %x \n",

  1512. 		data[1], data[2] + ((uint16_t)data[3] << 8), data[4], data[5], data[6]);

  1513. 

  1514. }

  1515. 

  1516. void BluetoothController::process_l2cap_disconnect_request(uint8_t *data) {

  1517. 	uint16_t dcid = data[4]+((uint16_t)data[5] << 8); 

  1518. 	uint16_t scid = data[6]+((uint16_t)data[7] << 8); 

  1519. 	DBGPrintf("    L2CAP disconnect request: ID: %d, Length:%x, Dest:%x, Source:%x\n",

  1520. 		data[1], data[2] + ((uint16_t)data[3] << 8), dcid, scid);

  1521. }

  1522. 

  1523. 

  1524. void BluetoothController::setHIDProtocol(uint8_t protocol) {

  1525. 	// Should verify protocol is boot or report

  1526. 	uint8_t l2capbuf[1];

  1527.     l2capbuf[0] = 0x70 | protocol; // Set Protocol, see Bluetooth HID specs page 33

  1528.     DBGPrintf("Set HID Protocol %d (", protocol);

  1529.     sendL2CapCommand(connections_[current_connection_].device_connection_handle_, l2capbuf, sizeof(l2capbuf), connections_[current_connection_].control_scid_ & 0xff, connections_[current_connection_].control_scid_ >> 8);

  1530. }

  1531. 

  1532. void BluetoothController::handleHIDTHDRData(uint8_t *data) {

  1533. 	// Example

  1534. 	//                      T HID data

  1535. 	//48 20 d 0 9 0 71 0 a1 3 8a cc c5 a 23 22 79

  1536. 	uint16_t len = data[4] + ((uint16_t)data[5] << 8);

  1537. 	DBGPrintf("HID HDR Data: len: %d, Type: %d Con:%d\n", len, data[9], current_connection_);

  1538. 

  1539. 	// ??? How to parse??? Use HID object??? 

  1540. 	if (connections_[current_connection_].device_driver_) {

  1541. 		connections_[current_connection_].device_driver_->process_bluetooth_HID_data(&data[9], len-1); // We skip the first byte...

  1542. 	} else {

  1543. 		switch (data[9]) {

  1544. 			case 1:

  1545. 				DBGPrintf("    Keyboard report type\n");

  1546. 				break;

  1547. 			case 2: 

  1548. 				DBGPrintf("    Mouse report type\n");	

  1549. 				break;

  1550. 			case 3:	

  1551. 				DBGPrintf("    Combo keyboard/pointing\n");

  1552. 				break;

  1553. 			default:

  1554. 				DBGPrintf("    Unknown report\n");	

  1555. 		}

  1556. 	}

  1557. }