diff --git a/RFCOMM.cpp b/RFCOMM.cpp new file mode 100644 index 00000000..f4033bd6 --- /dev/null +++ b/RFCOMM.cpp @@ -0,0 +1,1539 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#include "RFCOMM.h" +#define DEBUG // Uncomment to print data for debugging +//#define EXTRADEBUG // Uncomment to get even more debugging data +//#define PRINTREPORT // Uncomment to print the report sent to the Arduino + +const uint8_t RFCOMM::BTD_EVENT_PIPE = 1; +const uint8_t RFCOMM::BTD_DATAIN_PIPE = 2; +const uint8_t RFCOMM::BTD_DATAOUT_PIPE = 3; + +/* + * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. + */ +const uint8_t rfcomm_crc_table[256] PROGMEM = { /* reversed, 8-bit, poly=0x07 */ + 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, + 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, + 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, + 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, + 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, + 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, + 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, + 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, + 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, + 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, + 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, + 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, + 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, + 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, + 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, + 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF +}; + +RFCOMM::RFCOMM(USB *p, const char* name, const char* pin): +pUsb(p), // pointer to USB class instance - mandatory +bAddress(0), // device address - mandatory +bNumEP(1), // if config descriptor needs to be parsed +qNextPollTime(0), +bPollEnable(false) // don't start polling before dongle is connected +{ + for(uint8_t i=0; iRegisterDeviceClass(this); //set devConfig[] entry + + btdName = name; + btdPin = pin; +} + +uint8_t RFCOMM::Init(uint8_t parent, uint8_t port, bool lowspeed) +{ + uint8_t buf[sizeof(USB_DEVICE_DESCRIPTOR)]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint8_t num_of_conf; // number of configurations + + // get memory address of USB device address pool + AddressPool &addrPool = pUsb->GetAddressPool(); +#ifdef EXTRADEBUG + Notify(PSTR("\r\nRFCOMM Init")); +#endif + // check if address has already been assigned to an instance + if (bAddress) + { +#ifdef DEBUG + Notify(PSTR("\r\nAddress in use")); +#endif + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; + } + + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); + + if (!p) + { +#ifdef DEBUG + Notify(PSTR("\r\nAddress not found")); +#endif + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + } + + if (!p->epinfo) + { +#ifdef DEBUG + Notify(PSTR("\r\nepinfo is null")); +#endif + return USB_ERROR_EPINFO_IS_NULL; + } + + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; + + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; + + p->lowspeed = lowspeed; + + // Get device descriptor + rcode = pUsb->getDevDescr(0, 0, sizeof(USB_DEVICE_DESCRIPTOR), (uint8_t*)buf);// Get device descriptor - addr, ep, nbytes, data + + // Restore p->epinfo + p->epinfo = oldep_ptr; + + if(rcode) + goto FailGetDevDescr; + + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); + + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + + // Extract Max Packet Size from device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + + // Assign new address to the device + rcode = pUsb->setAddr( 0, 0, bAddress ); + if (rcode) + { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; +#ifdef DEBUG + Notify(PSTR("\r\nsetAddr: ")); +#endif + PrintHex(rcode); + return rcode; + } +#ifdef EXTRADEBUG + Notify(PSTR("\r\nAddr: ")); + PrintHex(bAddress); +#endif + p->lowspeed = false; + + //get pointer to assigned address record + p = addrPool.GetUsbDevicePtr(bAddress); + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + + p->lowspeed = lowspeed; + + // Assign epInfo to epinfo pointer - only EP0 is known + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + if (rcode) + goto FailSetDevTblEntry; + + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; + + // check if attached device is a Bluetooth dongle and fill endpoint data structure + // first interface in the configuration must have Bluetooth assigned Class/Subclass/Protocol + // and 3 endpoints - interrupt-IN, bulk-IN, bulk-OUT, + // not necessarily in this order + for (uint8_t i=0; i confDescrParser(this); + rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser); + if(rcode) + goto FailGetConfDescr; + if(bNumEP > 3) //all endpoints extracted + break; + } + + if (bNumEP < BTD_MAX_ENDPOINTS) + goto FailUnknownDevice; + + // Assign epInfo to epinfo pointer - this time all 3 endpoins + rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); + if(rcode) + goto FailSetDevTblEntry; + + delay(200); // Give time for address change + + // Set Configuration Value + rcode = pUsb->setConf(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bConfNum); + if(rcode) + goto FailSetConf; + + /* Set device cid for the SDP and RFCOMM channelse */ + sdp_dcid[0] = 0x50;//0x0050 + sdp_dcid[1] = 0x00; + rfcomm_dcid[0] = 0x51;//0x0051 + rfcomm_dcid[1] = 0x00; + + hci_num_reset_loops = 100; // only loop 100 times before trying to send the hci reset command + + hci_state = HCI_INIT_STATE; + hci_counter = 0; + l2cap_sdp_state = L2CAP_SDP_WAIT; + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; +#ifdef DEBUG + Notify(PSTR("\r\nBluetooth Dongle Initialized")); +#endif + + watingForConnection = false; + bPollEnable = true; + + return 0; //successful configuration + + /* diagnostic messages */ +FailGetDevDescr: +#ifdef DEBUG + Notify(PSTR("\r\ngetDevDescr:")); +#endif + goto Fail; +FailSetDevTblEntry: +#ifdef DEBUG + Notify(PSTR("\r\nsetDevTblEn:")); +#endif + goto Fail; +FailGetConfDescr: +#ifdef DEBUG + Notify(PSTR("\r\ngetConf:")); +#endif + goto Fail; +FailSetConf: +#ifdef DEBUG + Notify(PSTR("\r\nsetConf:")); +#endif + goto Fail; +FailUnknownDevice: +#ifdef DEBUG + Notify(PSTR("\r\nUnknown Device Connected:")); +#endif + goto Fail; +Fail: +#ifdef DEBUG + Notify(PSTR("\r\nBTD Init Failed, error code: ")); + Serial.print(rcode); +#endif + Release(); + return rcode; +} +/* Extracts interrupt-IN, bulk-IN, bulk-OUT endpoint information from config descriptor */ +void RFCOMM::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) { + //ErrorMessage(PSTR("Conf.Val"),conf); + //ErrorMessage(PSTR("Iface Num"),iface); + //ErrorMessage(PSTR("Alt.Set"),alt); + + if(alt) // wrong interface - by BT spec, no alt setting + return; + + bConfNum = conf; + uint8_t index; + + if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) //Interrupt In endpoint found + index = BTD_EVENT_PIPE; + + else { + if ((pep->bmAttributes & 0x02) == 2) //bulk endpoint found + index = ((pep->bEndpointAddress & 0x80) == 0x80) ? BTD_DATAIN_PIPE : BTD_DATAOUT_PIPE; + else + return; + } + + //Fill the rest of endpoint data structure + epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; +#ifdef EXTRADEBUG + PrintEndpointDescriptor(pep); +#endif + if(pollInterval < pep->bInterval) // Set the polling interval as the largest polling interval obtained from endpoints + pollInterval = pep->bInterval; + bNumEP++; + return; +} +void RFCOMM::PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr) { + Notify(PSTR("\r\nEndpoint descriptor:")); + Notify(PSTR("\r\nLength:\t\t")); + PrintHex(ep_ptr->bLength); + Notify(PSTR("\r\nType:\t\t")); + PrintHex(ep_ptr->bDescriptorType); + Notify(PSTR("\r\nAddress:\t")); + PrintHex(ep_ptr->bEndpointAddress); + Notify(PSTR("\r\nAttributes:\t")); + PrintHex(ep_ptr->bmAttributes); + Notify(PSTR("\r\nMaxPktSize:\t")); + PrintHex(ep_ptr->wMaxPacketSize); + Notify(PSTR("\r\nPoll Intrv:\t")); + PrintHex(ep_ptr->bInterval); +} + +/* Performs a cleanup after failed Init() attempt */ +uint8_t RFCOMM::Release() { + connected = false; + pUsb->GetAddressPool().FreeAddress(bAddress); + bAddress = 0; + bPollEnable = false; + bNumEP = 1; // must have to be reset to 1 + return 0; +} +uint8_t RFCOMM::Poll() { + if (!bPollEnable) + return 0; + if (qNextPollTime <= millis()) { // Don't poll if shorter than polling interval + qNextPollTime = millis() + pollInterval; // Set new poll time + HCI_event_task(); // poll the HCI event pipe + ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected + } + return 0; +} + +void RFCOMM::disconnect() { // Use this void to disconnect the RFCOMM Channel + connected = false; + // First the HID interrupt channel has to be disconencted, then the HID control channel and finally the HCI connection + if(RFCOMMConnected) + l2cap_disconnection_request(0x0A, rfcomm_dcid, rfcomm_scid); + if(SDPConnected) + l2cap_disconnection_request(0x0B, sdp_dcid, sdp_scid); + l2cap_sdp_state = L2CAP_DISCONNECT_RESPONSE; +} + +void RFCOMM::HCI_event_task() { + /* check the event pipe*/ + uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; // Request more than 16 bytes anyway, the inTransfer routine will take care of this + uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_EVENT_PIPE ].epAddr, &MAX_BUFFER_SIZE, hcibuf); // input on endpoint 1 + if(!rcode || rcode == hrNAK) // Check for errors + { + switch (hcibuf[0]) //switch on event type + { + case EV_COMMAND_COMPLETE: + hci_event_flag |= HCI_FLAG_CMD_COMPLETE; // set command complete flag + if (!hcibuf[5]) { // check if command succeeded + if((hcibuf[3] == 0x09) && (hcibuf[4] == 0x10)) { // parameters from read local bluetooth address + for (uint8_t i = 0; i < 6; i++) + my_bdaddr[i] = hcibuf[6 + i]; + hci_event_flag |= HCI_FLAG_READ_BDADDR; + } + } + break; + + case EV_COMMAND_STATUS: + if(hcibuf[2]) // show status on serial if not OK + { +#ifdef DEBUG + Notify(PSTR("\r\nHCI Command Failed: ")); + PrintHex(hcibuf[2]); + Notify(PSTR(" ")); + PrintHex(hcibuf[4]); + Notify(PSTR(" ")); + PrintHex(hcibuf[5]); +#endif + } + break; + + case EV_CONNECT_COMPLETE: + if (!hcibuf[2]) { // check if connected OK + hci_handle = hcibuf[3] | hcibuf[4] << 8; //store the handle for the ACL connection + hci_event_flag |= HCI_FLAG_CONN_COMPLETE; // set connection complete flag + } + break; + + case EV_DISCONNECT_COMPLETE: + if (!hcibuf[2]) { // check if disconnected OK + hci_event_flag |= HCI_FLAG_DISCONN_COMPLETE; //set disconnect commend complete flag + hci_event_flag &= ~HCI_FLAG_CONN_COMPLETE; // clear connection complete flag + } + break; + + case EV_REMOTE_NAME_COMPLETE: + if (!hcibuf[2]) { // check if reading is OK + for (uint8_t i = 0; i < 30; i++) + remote_name[i] = hcibuf[9 + i]; //store first 30 bytes + hci_event_flag |= HCI_FLAG_REMOTE_NAME_COMPLETE; + } + break; + + case EV_INCOMING_CONNECT: + disc_bdaddr[0] = hcibuf[2]; + disc_bdaddr[1] = hcibuf[3]; + disc_bdaddr[2] = hcibuf[4]; + disc_bdaddr[3] = hcibuf[5]; + disc_bdaddr[4] = hcibuf[6]; + disc_bdaddr[5] = hcibuf[7]; + hci_event_flag |= HCI_FLAG_INCOMING_REQUEST; + break; + + case EV_PIN_CODE_REQUEST: +#ifdef DEBUG + Notify(PSTR("\r\nBluetooth pin is set too: ")); + Serial.print(btdPin); +#endif + hci_pin_code_request_reply(btdPin); + break; + + case EV_LINK_KEY_REQUEST: +#ifdef DEBUG + Notify(PSTR("\r\nReceived Key Request")); +#endif + hci_link_key_request_negative_reply(); + break; + + /* We will just ignore the following events */ + case EV_NUM_COMPLETE_PKT: + case EV_ROLE_CHANGED: + case EV_PAGE_SCAN_REP_MODE: + case EV_LOOPBACK_COMMAND: + case EV_DATA_BUFFER_OVERFLOW: + case EV_CHANGE_CONNECTION_LINK: + case EV_AUTHENTICATION_COMPLETE: + case EV_MAX_SLOTS_CHANGE: + case EV_QOS_SETUP_COMPLETE: + case EV_LINK_KEY_NOTIFICATION: + case EV_ENCRYPTION_CHANGE: + case EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE: + break; + + default: +#ifdef EXTRADEBUG + if(hcibuf[0] != 0x00) { + Notify(PSTR("\r\nUnmanaged HCI Event: ")); + PrintHex(hcibuf[0]); + } +#endif + break; + } // switch + HCI_task(); + } + else { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nHCI event error: ")); + PrintHex(rcode); +#endif + } +} + +/* Poll Bluetooth and print result */ +void RFCOMM::HCI_task() { + switch (hci_state){ + case HCI_INIT_STATE: + hci_counter++; + if (hci_counter > hci_num_reset_loops) // wait until we have looped x times to clear any old events + { + hci_reset(); + hci_state = HCI_RESET_STATE; + hci_counter = 0; + } + break; + + case HCI_RESET_STATE: + hci_counter++; + if (hci_cmd_complete) + { +#ifdef DEBUG + Notify(PSTR("\r\nHCI Reset complete")); +#endif + hci_state = HCI_BDADDR_STATE; + hci_read_bdaddr(); + } + else if (hci_counter > hci_num_reset_loops) + { + hci_num_reset_loops *= 10; + if(hci_num_reset_loops > 2000) + hci_num_reset_loops = 2000; +#ifdef DEBUG + Notify(PSTR("\r\nNo response to HCI Reset")); +#endif + hci_state = HCI_INIT_STATE; + hci_counter = 0; + } + break; + + case HCI_BDADDR_STATE: + if (hci_read_bdaddr_complete) + { +#ifdef DEBUG + Notify(PSTR("\r\nLocal Bluetooth Address: ")); + for(int8_t i = 5; i > 0;i--) + { + PrintHex(my_bdaddr[i]); + Serial.print(":"); + } + PrintHex(my_bdaddr[0]); +#endif + hci_set_local_name(btdName); + hci_state = HCI_SET_NAME_STATE; + } + break; + + case HCI_SET_NAME_STATE: + if (hci_cmd_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nThe name is set to: ")); + Serial.print(btdName); +#endif + hci_state = HCI_SCANNING_STATE; + } + break; + case HCI_SCANNING_STATE: +#ifdef DEBUG + Notify(PSTR("\r\nWait For Incoming Connection Request")); +#endif + hci_write_scan_enable(); + watingForConnection = true; + hci_state = HCI_CONNECT_IN_STATE; + break; + + case HCI_CONNECT_IN_STATE: + if(hci_incoming_connect_request) { + watingForConnection = false; +#ifdef DEBUG + Notify(PSTR("\r\nIncoming Request")); +#endif + hci_remote_name(); + hci_state = HCI_REMOTE_NAME_STATE; + } + break; + + case HCI_REMOTE_NAME_STATE: + if(hci_remote_name_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nRemote Name: ")); + for (uint8_t i = 0; i < 30; i++) + { + if(remote_name[i] == NULL) + break; + Serial.write(remote_name[i]); + } +#endif + hci_accept_connection(); + hci_state = HCI_CONNECTED_STATE; + } + break; + + case HCI_CONNECTED_STATE: + if (hci_connect_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nConnected to Device: ")); + for(int8_t i = 5; i>0;i--) + { + PrintHex(disc_bdaddr[i]); + Serial.print(":"); + } + PrintHex(disc_bdaddr[0]); +#endif + hci_write_scan_disable(); // Only allow one connection + hci_state = HCI_DISABLE_SCAN; + } + break; + + case HCI_DISABLE_SCAN: + if (hci_cmd_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nScan Disabled")); +#endif + l2cap_event_flag = 0; + hci_state = HCI_DONE_STATE; + } + break; + + case HCI_DONE_STATE: + if (hci_disconnect_complete) + hci_state = HCI_DISCONNECT_STATE; + break; + + case HCI_DISCONNECT_STATE: + if (hci_disconnect_complete) { +#ifdef DEBUG + Notify(PSTR("\r\nDisconnected from Device: ")); + for(int8_t i = 5; i>0;i--) + { + PrintHex(disc_bdaddr[i]); + Serial.print(":"); + } + PrintHex(disc_bdaddr[0]); +#endif + l2cap_event_flag = 0; // Clear all flags + hci_event_flag = 0; // Clear all flags + + //Reset all buffers + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + hcibuf[i] = 0; + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + l2capinbuf[i] = 0; + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + l2capoutbuf[i] = 0; + for (uint8_t i = 0; i < BULK_MAXPKTSIZE; i++) + rfcommbuf[i] = 0; + + l2cap_sdp_state = L2CAP_SDP_WAIT; + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + hci_state = HCI_SCANNING_STATE; + } + break; + default: + break; + } +} + +void RFCOMM::ACL_event_task() +{ + uint16_t MAX_BUFFER_SIZE = BULK_MAXPKTSIZE; + uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[ BTD_DATAIN_PIPE ].epAddr, &MAX_BUFFER_SIZE, l2capinbuf); // input on endpoint 2 + if(!rcode) { // Check for errors + if (((l2capinbuf[0] | (l2capinbuf[1] << 8)) == (hci_handle | 0x2000))) { //acl_handle_ok + if ((l2capinbuf[6] | (l2capinbuf[7] << 8)) == 0x0001) { //l2cap_control - Channel ID for ACL-U + /* + if (l2capinbuf[8] != 0x00) + { + Serial.print("\r\nL2CAP Signaling Command - 0x"); + PrintHex(l2capoutbuf[8]); + } + */ + if (l2capinbuf[8] == L2CAP_CMD_COMMAND_REJECT) { +#ifdef DEBUG + Notify(PSTR("\r\nL2CAP Command Rejected - Reason: ")); + PrintHex(l2capinbuf[13]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[12]); + Notify(PSTR(" Data: ")); + PrintHex(l2capinbuf[17]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[16]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[15]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[14]); +#endif + } + else if (l2capinbuf[8] == L2CAP_CMD_CONNECTION_REQUEST) { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nL2CAP Connection Request - PSM: ")); + PrintHex(l2capinbuf[13]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[12]); + Notify(PSTR(" ")); + + Notify(PSTR(" SCID: ")); + PrintHex(l2capinbuf[15]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[14]); + + Notify(PSTR(" Identifier: ")); + PrintHex(l2capinbuf[9]); +#endif + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == SDP_PSM) { + identifier = l2capinbuf[9]; + sdp_scid[0] = l2capinbuf[14]; + sdp_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_SDP_REQUEST; + + } + if ((l2capinbuf[12] | (l2capinbuf[13] << 8)) == RFCOMM_PSM) { + identifier = l2capinbuf[9]; + rfcomm_scid[0] = l2capinbuf[14]; + rfcomm_scid[1] = l2capinbuf[15]; + l2cap_event_flag |= L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST; + + } + } + else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_RESPONSE) { + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { //Success + //Serial.print("\r\nHID Control Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_SUCCESS; + } + } + else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + if ((l2capinbuf[16] | (l2capinbuf[17] << 8)) == 0x0000) { //Success + //Serial.print("\r\nHID Interrupt Configuration Complete"); + l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS; + } + } + } + else if (l2capinbuf[8] == L2CAP_CMD_CONFIG_REQUEST) { + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + //Serial.print("\r\nHID Control Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_SDP_REQUEST; + } + else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + //Serial.print("\r\nHID Interrupt Configuration Request"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_CONFIG_RFCOMM_REQUEST; + } + } + else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_REQUEST) { + if (l2capinbuf[12] == sdp_dcid[0] && l2capinbuf[13] == sdp_dcid[1]) { + //Notify(PSTR("\r\nDisconnect Request: SDP Channel")); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_SDP_REQUEST; + } + else if (l2capinbuf[12] == rfcomm_dcid[0] && l2capinbuf[13] == rfcomm_dcid[1]) { + //Notify(PSTR("\r\nDisconnect Request: RFCOMM Channel")); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST; + } + } + else if (l2capinbuf[8] == L2CAP_CMD_DISCONNECT_RESPONSE) { + if (l2capinbuf[12] == sdp_scid[0] && l2capinbuf[13] == sdp_scid[1]) { + //Serial.print("\r\nDisconnect Response: SDP Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; + } + else if (l2capinbuf[12] == rfcomm_scid[0] && l2capinbuf[13] == rfcomm_scid[1]) { + //Serial.print("\r\nDisconnect Response: RFCOMM Channel"); + identifier = l2capinbuf[9]; + l2cap_event_flag |= L2CAP_FLAG_DISCONNECT_RESPONSE; + } + } + else if (l2capinbuf[8] == L2CAP_CMD_INFORMATION_REQUEST) { +#ifdef DEBUG + Notify(PSTR("\r\nInformation request")); +#endif + identifier = l2capinbuf[9]; + l2cap_information_response(identifier,l2capinbuf[12],l2capinbuf[13]); + } + else { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nL2CAP Unknown Command: ")); +#endif + PrintHex(l2capinbuf[8]); + } + } + else if (l2capinbuf[6] == sdp_dcid[0] && l2capinbuf[7] == sdp_dcid[1]) { // SDP + if(l2capinbuf[8] == SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU) { + /* + Serial.print("\r\nUUID: 0x"); + Serial.print(l2capinbuf[16],HEX); + Serial.print(" "); + Serial.print(l2capinbuf[17],HEX); + */ + if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == SERIALPORT_UUID) { + if(firstMessage) { + serialPortResponse1(l2capinbuf[9],l2capinbuf[10]); + firstMessage = false; + } else { + serialPortResponse2(l2capinbuf[9],l2capinbuf[10]); // Serialport continuation state + firstMessage = true; + } + } else if ((l2capinbuf[16] << 8 | l2capinbuf[17]) == L2CAP_UUID) { + if(firstMessage) { + l2capResponse1(l2capinbuf[9],l2capinbuf[10]); + firstMessage = false; + } else { + l2capResponse2(l2capinbuf[9],l2capinbuf[10]); // L2CAP continuation state + firstMessage = true; + } + } else + serviceNotSupported(l2capinbuf[9],l2capinbuf[10]); // The service is not supported + } + } + else if (l2capinbuf[6] == rfcomm_dcid[0] && l2capinbuf[7] == rfcomm_dcid[1]) { // RFCOMM + rfcommChannel = l2capinbuf[8] & 0xF8; + rfcommDirection = l2capinbuf[8] & 0x04; + rfcommCommandResponse = l2capinbuf[8] & 0x02; + rfcommChannelType = l2capinbuf[9] & 0xEF; + rfcommPfBit = l2capinbuf[9] & 0x10; + + if(rfcommChannel>>3 != 0x00) + rfcommChannelPermanent = rfcommChannel; + +#ifdef EXTRADEBUG + Notify(PSTR("\r\nRFCOMM Channel: ")); + Serial.print(rfcommChannel>>3,HEX); + Notify(PSTR(" Direction: ")); + Serial.print(rfcommDirection>>2,HEX); + Notify(PSTR(" CommandResponse: ")); + Serial.print(rfcommCommandResponse>>1,HEX); + Notify(PSTR(" ChannelType: ")); + Serial.print(rfcommChannelType,HEX); + Notify(PSTR(" PF_BIT: ")); + Serial.print(rfcommPfBit,HEX); +#endif + + if (rfcommChannelType == RFCOMM_DISC) { +#ifdef DEBUG + Notify(PSTR("\r\nReceived Disconnect RFCOMM Command")); +#endif + connected = false; + l2cap_disconnection_request(0x0A, rfcomm_dcid, rfcomm_scid); + } + if(connected) { + readReport(); +#ifdef PRINTREPORT + printReport(); //Uncomment "#define PRINTREPORT" to print the report send to the Arduino via Bluetooth +#endif + } else { + if(rfcommChannelType == RFCOMM_SABM) { // SABM Command - this is sent twice: once for channel 0 and then for the channel to establish +#ifdef DEBUG + Notify(PSTR("\r\nSABM Command")); +#endif + sendRfcomm(rfcommChannel,rfcommDirection,rfcommCommandResponse,RFCOMM_UA,rfcommPfBit,rfcommbuf,0x00); // UA Command + } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_PN_CMD) { // UIH Parameter Negotiation Command +#ifdef DEBUG + Notify(PSTR("\r\nUIH Parameter Negotiation Command")); +#endif + rfcommbuf[0] = BT_RFCOMM_PN_RSP; // UIH Parameter Negotiation Response + rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 + rfcommbuf[3] = 0xE0; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM + rfcommbuf[4] = 0x00; // Priority + rfcommbuf[5] = 0x00; // Timer + rfcommbuf[6] = 0x40; // Max Fram Size LSB - we will just set this to 64 + rfcommbuf[7] = 0x00; // Max Fram Size MSB + rfcommbuf[8] = 0x00; // MaxRatransm. + rfcommbuf[9] = 0x00; // Number of Frames + sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); + } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_CMD) { // UIH Modem Status Command +#ifdef DEBUG + Notify(PSTR("\r\nUIH Modem Status Response")); +#endif + rfcommbuf[0] = BT_RFCOMM_MSC_RSP; // UIH Modem Status Response + rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) + rfcommbuf[3] = l2capinbuf[14]; + sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); + + delay(1); + +#ifdef DEBUG + Notify(PSTR("\r\nUIH Modem Status Command")); +#endif + rfcommbuf[0] = BT_RFCOMM_MSC_CMD; // UIH Modem Status Command + rfcommbuf[1] = 2 << 1 | 1; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: (1 << 0) | (1 << 1) | (0 << 2) | (channel << 3) + rfcommbuf[3] = 0x8D; // Can receive frames (YES), Ready to Communicate (YES), Ready to Receive (YES), Incomig Call (NO), Data is Value (YES) + + sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x04); + } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_MSC_RSP) { // UIH Modem Status Response + if(!creditSent) { +#ifdef DEBUG + Notify(PSTR("\r\nUIH Command with credit")); +#endif + sendRfcommCredit(rfcommChannelPermanent,rfcommDirection,0,RFCOMM_UIH,0x10,0xFF); // 255 credit + creditSent = true; + } + } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[10] == 0x01) { // UIH Command with credit +#ifdef DEBUG + Notify(PSTR("\r\nReceived credit")); +#endif + timer = millis(); + waitForLastCommand = true; + } else if(rfcommChannelType == RFCOMM_UIH && l2capinbuf[11] == BT_RFCOMM_RPN_CMD) { // UIH Remote Port Negotiation Command +#ifdef DEBUG + Notify(PSTR("\r\nUIH Remote Port Negotiation Command")); +#endif + rfcommbuf[0] = BT_RFCOMM_RPN_RSP; // Command + rfcommbuf[1] = l2capinbuf[12]; // Length and shiftet like so: length << 1 | 1 + rfcommbuf[2] = l2capinbuf[13]; // Channel: channel << 1 | 1 + rfcommbuf[3] = l2capinbuf[14]; // Pre difined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM + rfcommbuf[4] = l2capinbuf[15]; // Priority + rfcommbuf[5] = l2capinbuf[16]; // Timer + rfcommbuf[6] = l2capinbuf[17]; // Max Fram Size LSB + rfcommbuf[7] = l2capinbuf[18]; // Max Fram Size MSB + rfcommbuf[8] = l2capinbuf[19]; // MaxRatransm. + rfcommbuf[9] = l2capinbuf[20]; // Number of Frames + sendRfcomm(rfcommChannel,rfcommDirection,0,RFCOMM_UIH,rfcommPfBit,rfcommbuf,0x0A); // UIH Remote Port Negotiation Response +#ifdef DEBUG + Notify(PSTR("\r\nRFCOMM Connection is now established\r\n")); +#endif + waitForLastCommand = false; + creditSent = false; + connected = true; // The RFCOMM channel is now established + } else if(rfcommChannelType != RFCOMM_DISC) { +#ifdef DEBUG + Notify(PSTR("\r\nUnsupported RFCOMM - ChannelType: ")); + PrintHex(rfcommChannelType); +#endif + } + } + } else { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nUnsupported L2CAP Data: Channel ID: ")); + PrintHex(l2capinbuf[7]); + Notify(PSTR(" ")); + PrintHex(l2capinbuf[6]); +#endif + } + SDP_task(); + RFCOMM_task(); + } + } + else if (rcode != hrNAK) { +#ifdef EXTRADEBUG + Notify(PSTR("\r\nACL data in error: ")); + PrintHex(rcode); +#endif + } +} +void RFCOMM::SDP_task() { + switch (l2cap_sdp_state) + { + case L2CAP_SDP_WAIT: + if (l2cap_connection_request_sdp_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nSDP Incoming Connection Request")); +#endif + l2cap_connection_response(identifier, sdp_dcid, sdp_scid, PENDING); + delay(1); + l2cap_connection_response(identifier, sdp_dcid, sdp_scid, SUCCESSFUL); + identifier++; + delay(1); + l2cap_config_request(identifier, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_REQUEST; + } + break; + case L2CAP_SDP_REQUEST: + if (l2cap_config_request_sdp_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nSDP Configuration Request")); +#endif + l2cap_config_response(identifier, sdp_scid); + l2cap_sdp_state = L2CAP_SDP_SUCCESS; + } + break; + case L2CAP_SDP_SUCCESS: + if (l2cap_config_success_sdp_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nSDP Successfully Configured")); +#endif + firstMessage = true; // Reset bool + SDPConnected = true; + l2cap_sdp_state = L2CAP_SDP_DONE; + } + break; + case L2CAP_SDP_DONE: + if(l2cap_disconnect_request_sdp_flag) { + SDPConnected = false; +#ifdef DEBUG + Notify(PSTR("\r\nDisconnected SDP Channel")); +#endif + l2cap_disconnection_response(identifier,sdp_dcid,sdp_scid); + l2cap_sdp_state = L2CAP_SDP_WAIT; + } + break; + case L2CAP_DISCONNECT_RESPONSE: // This is for both disconnection response from the RFCOMM and SDP channel if they were connected + if (l2cap_disconnect_response_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nDisconnected L2CAP Connection")); +#endif + RFCOMMConnected = false; + SDPConnected = false; + hci_disconnect(); + l2cap_sdp_state = L2CAP_SDP_WAIT; + l2cap_sdp_state = L2CAP_RFCOMM_WAIT; + hci_state = HCI_DISCONNECT_STATE; + } + break; + } +} +void RFCOMM::RFCOMM_task() +{ + if(!connected) { + if((millis() - timer) > 100 && waitForLastCommand) { // We will only wait 100ms and see if the UIH Remote Port Negotiation Command is send, as some deviced don't send it +#ifdef DEBUG + Notify(PSTR("\r\nRFCOMM Connection is now established - Automatic\r\n")); +#endif + creditSent = false; + waitForLastCommand = false; + connected = true; // The RFCOMM channel is now established + + } + } + switch (l2cap_rfcomm_state) + { + case L2CAP_RFCOMM_WAIT: + if(l2cap_connection_request_rfcomm_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nRFCOMM Incoming Connection Request")); +#endif + l2cap_connection_response(identifier, rfcomm_dcid, rfcomm_scid, PENDING); + delay(1); + l2cap_connection_response(identifier, rfcomm_dcid, rfcomm_scid, SUCCESSFUL); + identifier++; + delay(1); + l2cap_config_request(identifier, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_REQUEST; + } + break; + case L2CAP_RFCOMM_REQUEST: + if (l2cap_config_request_rfcomm_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nRFCOMM Configuration Request")); +#endif + l2cap_config_response(identifier, rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_SUCCESS; + } + break; + case L2CAP_RFCOMM_SUCCESS: + if (l2cap_config_success_rfcomm_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nRFCOMM Successfully Configured")); +#endif + rfcommAvailable = 0; // Reset numbers of bytes available + RFCOMMConnected = true; + l2cap_rfcomm_state = L2CAP_RFCOMM_DONE; + } + break; + case L2CAP_RFCOMM_DONE: + if(l2cap_disconnect_request_rfcomm_flag) { + RFCOMMConnected = false; + connected = false; +#ifdef DEBUG + Notify(PSTR("\r\nDisconnected RFCOMM Channel")); +#endif + l2cap_disconnection_response(identifier,rfcomm_dcid,rfcomm_scid); + l2cap_rfcomm_state = L2CAP_RFCOMM_WAIT; + } + break; + } +} + +/************************************************************/ +/* RFCOMM Report */ +/************************************************************/ +void RFCOMM::readReport() { + uint8_t length = l2capinbuf[10] >> 1; + if(rfcommAvailable + length > 256) + return; // Return if the buffer would be full + + uint8_t offset; + if(l2capinbuf[4] == length+4) + offset = 0; + else + offset = 1; // There must be credit + + for(uint8_t i = 0; i < length; i++) + rfcommDataBuffer[rfcommAvailable+i] = l2capinbuf[11+i+offset]; + rfcommAvailable += length; +#ifdef EXTRADEBUG + Notify(PSTR("\r\nRFCOMM Data Available: ")); + Serial.print(rfcommAvailable); +#endif +} +void RFCOMM::printReport() { //Uncomment "#define PRINTREPORT" to print the report send to the Arduino + if(rfcommChannelType == RFCOMM_UIH) { + uint8_t length = l2capinbuf[10] >> 1; // Get length + + uint8_t offset; + if(l2capinbuf[4] == length+4) + offset = 0; + else + offset = 1; // There must be credit + + for(uint8_t i = 0; i < length; i++) + Serial.write(l2capinbuf[i+11+offset]); + } +} + +/************************************************************/ +/* HCI Commands */ +/************************************************************/ + +void RFCOMM::HCI_Command(uint8_t* data, uint16_t nbytes) { + hci_event_flag &= ~HCI_FLAG_CMD_COMPLETE; + pUsb->ctrlReq(bAddress, epInfo[ BTD_CONTROL_PIPE ].epAddr, bmREQ_HCI_OUT, 0x00, 0x00, 0x00 ,0x00, nbytes, nbytes, data, NULL); +} +void RFCOMM::hci_reset() { + hci_event_flag = 0; // clear all the flags + hcibuf[0] = 0x03; // HCI OCF = 3 + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); +} +void RFCOMM::hci_write_scan_enable() { + hci_event_flag &= ~HCI_FLAG_INCOMING_REQUEST; + hcibuf[0] = 0x1A; // HCI OCF = 1A + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x01; // parameter length = 1 + hcibuf[3] = 0x03; // Inquiry Scan enabled. Page Scan enabled. + HCI_Command(hcibuf, 4); +} +void RFCOMM::hci_write_scan_disable() { + hcibuf[0] = 0x1A; // HCI OCF = 1A + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = 0x01; // parameter length = 1 + hcibuf[3] = 0x00; // Inquiry Scan disabled. Page Scan disabled. + HCI_Command(hcibuf, 4); +} +void RFCOMM::hci_read_bdaddr() { + hcibuf[0] = 0x09; // HCI OCF = 9 + hcibuf[1] = 0x04 << 2; // HCI OGF = 4 + hcibuf[2] = 0x00; + HCI_Command(hcibuf, 3); +} +void RFCOMM::hci_accept_connection() { + hcibuf[0] = 0x09; // HCI OCF = 9 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x07; // parameter length 7 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = 0x00; //switch role to master + + HCI_Command(hcibuf, 10); +} +void RFCOMM::hci_remote_name() { + hci_event_flag &= ~HCI_FLAG_REMOTE_NAME_COMPLETE; + hcibuf[0] = 0x19; // HCI OCF = 19 + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x0A; // parameter length = 10 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = 0x01; //Page Scan Repetition Mode + hcibuf[10] = 0x00; //Reserved + hcibuf[11] = 0x00; //Clock offset - low byte + hcibuf[12] = 0x00; //Clock offset - high byte + + HCI_Command(hcibuf, 13); +} +void RFCOMM::hci_set_local_name(const char* name) { + hcibuf[0] = 0x13; // HCI OCF = 13 + hcibuf[1] = 0x03 << 2; // HCI OGF = 3 + hcibuf[2] = strlen(name)+1; // parameter length = the length of the string + uint8_t i; + for(i = 0; i < strlen(name); i++) + hcibuf[i+3] = name[i]; + hcibuf[i+3] = 0x00; // End of string + + HCI_Command(hcibuf, 4+strlen(name)); +} +void RFCOMM::hci_pin_code_request_reply(const char* key) { + hcibuf[0] = 0x0D; // HCI OCF = 0D + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x17; // parameter length 23 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + hcibuf[9] = strlen(key); // Length of key + uint8_t i; + for(i = 0; i < strlen(key); i++) // The maximum size of the key is 16 + hcibuf[i+10] = key[i]; + for(;i < 16; i++) + hcibuf[i+10] = 0x00; // The rest should be 0 + + HCI_Command(hcibuf, 26); +} +void RFCOMM::hci_link_key_request_negative_reply() { + hcibuf[0] = 0x0C; // HCI OCF = 0C + hcibuf[1] = 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x06; // parameter length 7 + hcibuf[3] = disc_bdaddr[0]; // 6 octet bdaddr + hcibuf[4] = disc_bdaddr[1]; + hcibuf[5] = disc_bdaddr[2]; + hcibuf[6] = disc_bdaddr[3]; + hcibuf[7] = disc_bdaddr[4]; + hcibuf[8] = disc_bdaddr[5]; + + HCI_Command(hcibuf, 9); + +} +void RFCOMM::hci_disconnect() { + hci_event_flag &= ~HCI_FLAG_DISCONN_COMPLETE; + hcibuf[0] = 0x06; // HCI OCF = 6 + hcibuf[1]= 0x01 << 2; // HCI OGF = 1 + hcibuf[2] = 0x03; // parameter length =3 + hcibuf[3] = (uint8_t)(hci_handle & 0xFF);//connection handle - low byte + hcibuf[4] = (uint8_t)((hci_handle >> 8) & 0x0F);//connection handle - high byte + hcibuf[5] = 0x13; // reason + + HCI_Command(hcibuf, 6); +} +/******************************************************************* + * * + * HCI ACL Data Packet * + * * + * buf[0] buf[1] buf[2] buf[3] + * 0 4 8 11 12 16 24 31 MSB + * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. + * | HCI Handle |PB |BC | Data Total Length | HCI ACL Data Packet + * .-+-+-+-+-+-+-+-|-+-+-+-|-+-|-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. + * + * buf[4] buf[5] buf[6] buf[7] + * 0 8 16 31 MSB + * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. + * | Length | Channel ID | Basic L2CAP header + * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. + * + * buf[8] buf[9] buf[10] buf[11] + * 0 8 16 31 MSB + * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. + * | Code | Identifier | Length | Control frame (C-frame) + * .-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-|-+-+-+-+-+-+-+-. (signaling packet format) + */ +/************************************************************/ +/* L2CAP Commands */ +/************************************************************/ +void RFCOMM::L2CAP_Command(uint8_t* data, uint16_t nbytes, uint8_t channelLow, uint8_t channelHigh) { + uint8_t buf[64]; + buf[0] = (uint8_t)(hci_handle & 0xff); // HCI handle with PB,BC flag + buf[1] = (uint8_t)(((hci_handle >> 8) & 0x0f) | 0x20); + buf[2] = (uint8_t)((4 + nbytes) & 0xff); // HCI ACL total data length + buf[3] = (uint8_t)((4 + nbytes) >> 8); + buf[4] = (uint8_t)(nbytes & 0xff); // L2CAP header: Length + buf[5] = (uint8_t)(nbytes >> 8); + buf[6] = channelLow; + buf[7] = channelHigh; + + for (uint16_t i = 0; i < nbytes; i++)//L2CAP C-frame + buf[8 + i] = data[i]; + + uint8_t rcode = pUsb->outTransfer(bAddress, epInfo[ BTD_DATAOUT_PIPE ].epAddr, (8 + nbytes), buf); + if(rcode) { +#ifdef DEBUG + Notify(PSTR("\r\nError sending L2CAP message: 0x")); + PrintHex(rcode); +#endif + } +} +void RFCOMM::l2cap_connection_response(uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result) { + l2capoutbuf[0] = L2CAP_CMD_CONNECTION_RESPONSE;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x08;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0];// Destination CID + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = scid[0];// Source CID + l2capoutbuf[7] = scid[1]; + l2capoutbuf[8] = result;// Result: Pending or Success + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x00;// No further information + l2capoutbuf[11] = 0x00; + + L2CAP_Command(l2capoutbuf, 12); +} +void RFCOMM::l2cap_config_request(uint8_t rxid, uint8_t* dcid) { + l2capoutbuf[0] = L2CAP_CMD_CONFIG_REQUEST;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x08;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0];// Destination CID + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = 0x00;// Flags + l2capoutbuf[7] = 0x00; + l2capoutbuf[8] = 0x01;// Config Opt: type = MTU (Maximum Transmission Unit) - Hint + l2capoutbuf[9] = 0x02;// Config Opt: length + l2capoutbuf[10] = 0xFF;// MTU + l2capoutbuf[11] = 0xFF; + + L2CAP_Command(l2capoutbuf, 12); +} +void RFCOMM::l2cap_config_response(uint8_t rxid, uint8_t* scid) { + l2capoutbuf[0] = L2CAP_CMD_CONFIG_RESPONSE;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x0A;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = scid[0];// Source CID + l2capoutbuf[5] = scid[1]; + l2capoutbuf[6] = 0x00;// Flag + l2capoutbuf[7] = 0x00; + l2capoutbuf[8] = 0x00;// Result + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x01;// Config + l2capoutbuf[11] = 0x02; + l2capoutbuf[12] = 0xA0; + l2capoutbuf[13] = 0x02; + + L2CAP_Command(l2capoutbuf, 14); +} +void RFCOMM::l2cap_disconnection_request(uint8_t rxid, uint8_t* dcid, uint8_t* scid) { + l2cap_event_flag = 0; // Reset flags + l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_REQUEST;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x04;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = scid[0];// Really Destination CID + l2capoutbuf[5] = scid[1]; + l2capoutbuf[6] = dcid[0];// Really Source CID + l2capoutbuf[7] = dcid[1]; + L2CAP_Command(l2capoutbuf, 8); +} +void RFCOMM::l2cap_disconnection_response(uint8_t rxid, uint8_t* dcid, uint8_t* scid) { + l2cap_event_flag = 0; // Reset flags + l2capoutbuf[0] = L2CAP_CMD_DISCONNECT_RESPONSE;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x04;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = dcid[0]; + l2capoutbuf[5] = dcid[1]; + l2capoutbuf[6] = scid[0]; + l2capoutbuf[7] = scid[1]; + L2CAP_Command(l2capoutbuf, 8); +} +void RFCOMM::l2cap_information_response(uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh) { + l2capoutbuf[0] = L2CAP_CMD_INFORMATION_RESPONSE;// Code + l2capoutbuf[1] = rxid;// Identifier + l2capoutbuf[2] = 0x08;// Length + l2capoutbuf[3] = 0x00; + l2capoutbuf[4] = infoTypeLow; + l2capoutbuf[5] = infoTypeHigh; + l2capoutbuf[6] = 0x00; // Result = success + l2capoutbuf[7] = 0x00; // Result = success + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x00; + l2capoutbuf[11] = 0x00; + L2CAP_Command(l2capoutbuf, 12); +} + +/************************************************************/ +/* SDP Commands */ +/************************************************************/ +void RFCOMM::SDP_Command(uint8_t* data, uint16_t nbytes) { // See page 223 in the Bluetooth specs + L2CAP_Command(data,nbytes,sdp_scid[0],sdp_scid[1]); +} +void RFCOMM::serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow) { // See page 235 in the Bluetooth specs + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x05; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x02; // AttributeListsByteCount + + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x35; + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x00; + + SDP_Command(l2capoutbuf,10); +} +void RFCOMM::serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x2B; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x26; // AttributeListsByteCount + + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x36; + l2capoutbuf[8] = 0x00; + l2capoutbuf[9] = 0x3C; + l2capoutbuf[10] = 0x36; + l2capoutbuf[11] = 0x00; + + l2capoutbuf[12] = 0x39; + l2capoutbuf[13] = 0x09; + l2capoutbuf[14] = 0x00; + l2capoutbuf[15] = 0x00; + l2capoutbuf[16] = 0x0A; + l2capoutbuf[17] = 0x00; + l2capoutbuf[18] = 0x01; + l2capoutbuf[19] = 0x00; + l2capoutbuf[20] = 0x06; + l2capoutbuf[21] = 0x09; + l2capoutbuf[22] = 0x00; + l2capoutbuf[23] = 0x01; + l2capoutbuf[24] = 0x35; + l2capoutbuf[25] = 0x03; + l2capoutbuf[26] = 0x19; + l2capoutbuf[27] = 0x11; + + l2capoutbuf[28] = 0x01; + l2capoutbuf[29] = 0x09; + l2capoutbuf[30] = 0x00; + l2capoutbuf[31] = 0x04; + l2capoutbuf[32] = 0x35; + l2capoutbuf[33] = 0x0C; + l2capoutbuf[34] = 0x35; + l2capoutbuf[35] = 0x03; + l2capoutbuf[36] = 0x19; + l2capoutbuf[37] = 0x01; + l2capoutbuf[38] = 0x00; + l2capoutbuf[39] = 0x35; + l2capoutbuf[40] = 0x05; + l2capoutbuf[41] = 0x19; + l2capoutbuf[42] = 0x00; + l2capoutbuf[43] = 0x03; + + l2capoutbuf[44] = 0x08; + l2capoutbuf[45] = 0x02; // Two more bytes? + l2capoutbuf[46] = 0x00; // 19 more bytes to come + l2capoutbuf[47] = 0x19; + + SDP_Command(l2capoutbuf,48); +} +void RFCOMM::serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { + l2capoutbuf[0] = SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU; + l2capoutbuf[1] = transactionIDHigh; + l2capoutbuf[2] = transactionIDLow; + l2capoutbuf[3] = 0x00; // Parameter Length + l2capoutbuf[4] = 0x1C; // Parameter Length + l2capoutbuf[5] = 0x00; // AttributeListsByteCount + l2capoutbuf[6] = 0x19; // AttributeListsByteCount + + /* Attribute ID/Value Sequence: */ + l2capoutbuf[7] = 0x01; + l2capoutbuf[8] = 0x09; + l2capoutbuf[9] = 0x00; + l2capoutbuf[10] = 0x06; + l2capoutbuf[11] = 0x35; + + l2capoutbuf[12] = 0x09; + l2capoutbuf[13] = 0x09; + l2capoutbuf[14] = 0x65; + l2capoutbuf[15] = 0x6E; + l2capoutbuf[16] = 0x09; + l2capoutbuf[17] = 0x00; + l2capoutbuf[18] = 0x6A; + l2capoutbuf[19] = 0x09; + l2capoutbuf[20] = 0x01; + l2capoutbuf[21] = 0x00; + l2capoutbuf[22] = 0x09; + l2capoutbuf[23] = 0x01; + l2capoutbuf[24] = 0x00; + l2capoutbuf[25] = 0x25; + + l2capoutbuf[26] = 0x05; // Name length + l2capoutbuf[27] = 'T'; + l2capoutbuf[28] = 'K'; + l2capoutbuf[29] = 'J'; + l2capoutbuf[30] = 'S'; + l2capoutbuf[31] = 'P'; + l2capoutbuf[32] = 0x00; + + SDP_Command(l2capoutbuf,33); +} +void RFCOMM::l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow) { + serialPortResponse1(transactionIDHigh,transactionIDLow); // These has to send include all the supported functions, since it only support virtual serialport it just sends the message again +} +void RFCOMM::l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow) { + serialPortResponse2(transactionIDHigh,transactionIDLow); // Same data as serialPortResponse2 +} +/************************************************************/ +/* RFCOMM Commands */ +/************************************************************/ +void RFCOMM::RFCOMM_Command(uint8_t* data, uint16_t nbytes) { // See page 223 in the Bluetooth specs + L2CAP_Command(data,nbytes,rfcomm_scid[0],rfcomm_scid[1]); +} + +void RFCOMM::sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length) { + l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address + l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control + l2capoutbuf[2] = length << 1 | 0x01; // Length and format (allways 0x01 bytes format) + uint8_t i = 0; + for(; i < length; i++) + l2capoutbuf[i+3] = data[i]; + l2capoutbuf[i+3] = calcFcs(l2capoutbuf); +#ifdef EXTRADEBUG + Notify(PSTR(" - RFCOMM Data: ")); + for(i = 0; i < length+4; i++) { + Serial.print(l2capoutbuf[i],HEX); + Notify(PSTR(" ")); + } +#endif + RFCOMM_Command(l2capoutbuf,length+4); +} + +void RFCOMM::sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit) { + l2capoutbuf[0] = channel | direction | CR | extendAddress; // RFCOMM Address + l2capoutbuf[1] = channelType | pfBit; // RFCOMM Control + l2capoutbuf[2] = 0x01; // Length = 0 + l2capoutbuf[3] = credit; // Credit + l2capoutbuf[4] = calcFcs(l2capoutbuf); +#ifdef EXTRADEBUG + Notify(PSTR(" - RFCOMM Credit Data: ")); + for(uint8_t i = 0; i < 5; i++) { + Serial.print(l2capoutbuf[i],HEX); + Notify(PSTR(" ")); + } +#endif + RFCOMM_Command(l2capoutbuf,5); +} + +/* CRC on 2 bytes */ +uint8_t RFCOMM::__crc(uint8_t* data) { + return(pgm_read_byte(&rfcomm_crc_table[pgm_read_byte(&rfcomm_crc_table[0xff ^ data[0]]) ^ data[1]])); +} + +/* Calculate FCS - we never actually check if the host sends correct FCS to the Arduino */ +uint8_t RFCOMM::calcFcs(uint8_t *data) { + if((data[1] & 0xEF) == RFCOMM_UIH) + return (0xff - __crc(data)); // FCS on 2 bytes + else + return (0xff - pgm_read_byte(&rfcomm_crc_table[__crc(data) ^ data[2]])); // FCS on 3 bytes +} + +/* Serial commands */ +void RFCOMM::print(const char* data) { + rfcommbuf[0] = rfcommChannelPermanent | 0 | 0 | extendAddress;; // RFCOMM Address + rfcommbuf[1] = RFCOMM_UIH; // RFCOMM Control + rfcommbuf[2] = strlen(data) << 1 | 1; // Length + uint8_t i = 0; + for(; i < strlen(data); i++) + rfcommbuf[i+3] = data[i]; + rfcommbuf[i+3] = calcFcs(rfcommbuf); + + RFCOMM_Command(rfcommbuf,strlen(data)+4); +} +void RFCOMM::print(uint8_t data) { + rfcommbuf[0] = rfcommChannelPermanent | 0 | 0 | extendAddress;; // RFCOMM Address + rfcommbuf[1] = RFCOMM_UIH; // RFCOMM Control + rfcommbuf[2] = 1 << 1 | 1; // Length = 1 + rfcommbuf[3] = data; + rfcommbuf[4] = calcFcs(rfcommbuf); + + RFCOMM_Command(rfcommbuf,5); +} + +void RFCOMM::println(const char* data) { + char output[strlen(data)+2]; + strcpy(output,data); + strcat(output,"\r\n"); + print(output); +} +void RFCOMM::println(uint8_t data) { + print(data); + print("\r\n"); +} +uint8_t RFCOMM::read() { + uint8_t output = rfcommDataBuffer[0]; + for(uint8_t i = 1; i < rfcommAvailable; i++) + rfcommDataBuffer[i-1] = rfcommDataBuffer[i]; // Shift the buffer one left + rfcommAvailable--; + return output; +} \ No newline at end of file diff --git a/RFCOMM.h b/RFCOMM.h new file mode 100644 index 00000000..8729178a --- /dev/null +++ b/RFCOMM.h @@ -0,0 +1,332 @@ +/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. + + This software may be distributed and modified under the terms of the GNU + General Public License version 2 (GPL2) as published by the Free Software + Foundation and appearing in the file GPL2.TXT included in the packaging of + this file. Please note that GPL2 Section 2[b] requires that all works based + on this software must also be made publicly available under the terms of + the GPL2 ("Copyleft"). + + Contact information + ------------------- + + Kristian Lauszus, TKJ Electronics + Web : http://www.tkjelectronics.com + e-mail : kristianl@tkjelectronics.com + */ + +#ifndef _rfcomm_h_ +#define _rfcomm_h_ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include "Usb.h" +#include "confdescparser.h" + +/* CSR Bluetooth data taken from descriptors */ +#define BULK_MAXPKTSIZE 64 // max size for ACL data + +// used in control endpoint header for HCI Commands +#define bmREQ_HCI_OUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_DEVICE + +/* Bluetooth HCI states for hci_task() */ +#define HCI_INIT_STATE 0 +#define HCI_RESET_STATE 1 +#define HCI_BDADDR_STATE 2 +#define HCI_SET_NAME_STATE 3 +#define HCI_SCANNING_STATE 4 +#define HCI_CONNECT_IN_STATE 5 +#define HCI_REMOTE_NAME_STATE 6 +#define HCI_CONNECTED_STATE 7 +#define HCI_DISABLE_SCAN 8 +#define HCI_DONE_STATE 9 +#define HCI_DISCONNECT_STATE 10 + +/* HCI event flags*/ +#define HCI_FLAG_CMD_COMPLETE 0x01 +#define HCI_FLAG_CONN_COMPLETE 0x02 +#define HCI_FLAG_DISCONN_COMPLETE 0x04 +#define HCI_FLAG_REMOTE_NAME_COMPLETE 0x08 +#define HCI_FLAG_INCOMING_REQUEST 0x10 +#define HCI_FLAG_READ_BDADDR 0x20 + +/*Macros for HCI event flag tests */ +#define hci_cmd_complete (hci_event_flag & HCI_FLAG_CMD_COMPLETE) +#define hci_connect_complete (hci_event_flag & HCI_FLAG_CONN_COMPLETE) +#define hci_disconnect_complete (hci_event_flag & HCI_FLAG_DISCONN_COMPLETE) +#define hci_remote_name_complete (hci_event_flag & HCI_FLAG_REMOTE_NAME_COMPLETE) +#define hci_incoming_connect_request (hci_event_flag & HCI_FLAG_INCOMING_REQUEST) +#define hci_read_bdaddr_complete (hci_event_flag & HCI_FLAG_READ_BDADDR) + +/* HCI Events managed */ +#define EV_COMMAND_COMPLETE 0x0E +#define EV_COMMAND_STATUS 0x0F +#define EV_CONNECT_COMPLETE 0x03 +#define EV_DISCONNECT_COMPLETE 0x05 +#define EV_NUM_COMPLETE_PKT 0x13 +#define EV_INQUIRY_COMPLETE 0x01 +#define EV_INQUIRY_RESULT 0x02 +#define EV_REMOTE_NAME_COMPLETE 0x07 +#define EV_INCOMING_CONNECT 0x04 +#define EV_ROLE_CHANGED 0x12 +#define EV_PAGE_SCAN_REP_MODE 0x20 +#define EV_DATA_BUFFER_OVERFLOW 0x1A +#define EV_LOOPBACK_COMMAND 0x19 +#define EV_CHANGE_CONNECTION_LINK 0x09 +#define EV_AUTHENTICATION_COMPLETE 0x06 +#define EV_MAX_SLOTS_CHANGE 0x1B +#define EV_PIN_CODE_REQUEST 0x16 +#define EV_LINK_KEY_REQUEST 0x17 +#define EV_QOS_SETUP_COMPLETE 0x0D +#define EV_LINK_KEY_NOTIFICATION 0x18 +#define EV_ENCRYPTION_CHANGE 0x08 +#define EV_READ_REMOTE_VERSION_INFORMATION_COMPLETE 0x0C + +/* Bluetooth L2CAP states for SDP_task() and RFCOMM_task() */ +#define L2CAP_SDP_WAIT 0 +#define L2CAP_SDP_SETUP 1 +#define L2CAP_SDP_REQUEST 2 +#define L2CAP_SDP_SUCCESS 3 +#define L2CAP_SDP_DONE 4 +#define L2CAP_RFCOMM_WAIT 5 +#define L2CAP_RFCOMM_SETUP 6 +#define L2CAP_RFCOMM_REQUEST 7 +#define L2CAP_RFCOMM_SUCCESS 8 +#define L2CAP_RFCOMM_DONE 9 +#define L2CAP_DISCONNECT_RESPONSE 10 + +/* L2CAP event flags */ +#define L2CAP_FLAG_CONNECTION_SDP_REQUEST 0x001 +#define L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST 0x002 +#define L2CAP_FLAG_CONFIG_SDP_REQUEST 0x004 +#define L2CAP_FLAG_CONFIG_RFCOMM_REQUEST 0x008 +#define L2CAP_FLAG_CONFIG_SDP_SUCCESS 0x010 +#define L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS 0x020 +#define L2CAP_FLAG_DISCONNECT_SDP_REQUEST 0x040 +#define L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST 0x080 +#define L2CAP_FLAG_DISCONNECT_RESPONSE 0x100 + +/* Macros for L2CAP event flag tests */ +#define l2cap_connection_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_SDP_REQUEST) +#define l2cap_connection_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONNECTION_RFCOMM_REQUEST) +#define l2cap_config_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_SDP_REQUEST) +#define l2cap_config_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_RFCOMM_REQUEST) +#define l2cap_config_success_sdp_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_SDP_SUCCESS) +#define l2cap_config_success_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_RFCOMM_SUCCESS) +#define l2cap_disconnect_request_sdp_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_SDP_REQUEST) +#define l2cap_disconnect_request_rfcomm_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_RFCOMM_REQUEST) +#define l2cap_disconnect_response_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_RESPONSE) + +/* L2CAP signaling commands */ +#define L2CAP_CMD_COMMAND_REJECT 0x01 +#define L2CAP_CMD_CONNECTION_REQUEST 0x02 +#define L2CAP_CMD_CONNECTION_RESPONSE 0x03 +#define L2CAP_CMD_CONFIG_REQUEST 0x04 +#define L2CAP_CMD_CONFIG_RESPONSE 0x05 +#define L2CAP_CMD_DISCONNECT_REQUEST 0x06 +#define L2CAP_CMD_DISCONNECT_RESPONSE 0x07 +#define L2CAP_CMD_INFORMATION_REQUEST 0x0A +#define L2CAP_CMD_INFORMATION_RESPONSE 0x0B + +/* Bluetooth L2CAP PSM */ +#define SDP_PSM 0x01 // Service Discovery Protocol PSM Value +#define RFCOMM_PSM 0x03 // RFCOMM PSM Value + +// Used For Connection Response - Remember to Include High Byte +#define PENDING 0x01 +#define SUCCESSFUL 0x00 + +// Used to determine if it is a Bluetooth dongle +#define WI_SUBCLASS_RF 0x01 // RF Controller +#define WI_PROTOCOL_BT 0x01 // Bluetooth Programming Interface + +#define BTD_MAX_ENDPOINTS 4 + +/* Used for SDP */ +#define SDP_SERVICE_SEARCH_ATTRIBUTE_REQUEST_PDU 0x06 // See the RFCOMM specs +#define SDP_SERVICE_SEARCH_ATTRIBUTE_RESPONSE_PDU 0x07 // See the RFCOMM specs +#define SERIALPORT_UUID 0x1101 // See http://www.bluetooth.org/Technical/AssignedNumbers/service_discovery.htm +#define L2CAP_UUID 0x0100 + +/* Used for RFCOMM */ +#define RFCOMM_SABM 0x2F +#define RFCOMM_UA 0x63 +#define RFCOMM_UIH 0xEF +//#define RFCOMM_DM 0x0F +#define RFCOMM_DISC 0x43 + +#define extendAddress 0x01 // Allways 1 + +// Multiplexer message types +#define BT_RFCOMM_PN_CMD 0x83 +#define BT_RFCOMM_PN_RSP 0x81 +#define BT_RFCOMM_MSC_CMD 0xE3 +#define BT_RFCOMM_MSC_RSP 0xE1 +#define BT_RFCOMM_RPN_CMD 0x93 +#define BT_RFCOMM_RPN_RSP 0x91 +/* +#define BT_RFCOMM_TEST_CMD 0x23 +#define BT_RFCOMM_TEST_RSP 0x21 +#define BT_RFCOMM_FCON_CMD 0xA3 +#define BT_RFCOMM_FCON_RSP 0xA1 +#define BT_RFCOMM_FCOFF_CMD 0x63 +#define BT_RFCOMM_FCOFF_RSP 0x61 +#define BT_RFCOMM_RLS_CMD 0x53 +#define BT_RFCOMM_RLS_RSP 0x51 +#define BT_RFCOMM_NSC_RSP 0x11 +*/ + +class RFCOMM : public USBDeviceConfig, public UsbConfigXtracter { +public: + RFCOMM(USB *p, const char* name = "Arduino", const char* pin = "1234"); + + // USBDeviceConfig implementation + virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); + virtual uint8_t Release(); + virtual uint8_t Poll(); + virtual uint8_t GetAddress() { return bAddress; }; + virtual bool isReady() { return bPollEnable; }; + + // UsbConfigXtracter implementation + virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep); + + bool isWatingForConnection() { return watingForConnection; }; // Use this to indicate when it is ready for a incoming connection + + void disconnect(); // Used this void to disconnect the virtual serial port + bool connected;// Variable used to indicate if the connection is established + + /* Serial commands currently supported */ + void print(const char* data); // Used to send strings + void print(uint8_t data); // Used to send single bytes + void println(const char* data); // Include newline and carriage return + void println(uint8_t data); // Include newline and carriage return + uint8_t available() { return rfcommAvailable; }; // Get the bytes waiting to be read + uint8_t read(); // Used to read the buffer + +protected: + /* mandatory members */ + USB *pUsb; + uint8_t bAddress; // device address + EpInfo epInfo[BTD_MAX_ENDPOINTS]; //endpoint info structure + + uint8_t bConfNum; // configuration number + uint8_t bNumEP; // total number of endpoints in the configuration + uint32_t qNextPollTime; // next poll time + + #define BTD_CONTROL_PIPE 0 // Bluetooth dongles control endpoint + static const uint8_t BTD_EVENT_PIPE; // HCI event endpoint index + static const uint8_t BTD_DATAIN_PIPE; // ACL In endpoint index + static const uint8_t BTD_DATAOUT_PIPE; // ACL Out endpoint index + + void PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr); + +private: + const char* btdName; + const char* btdPin; + + bool bPollEnable; + uint8_t pollInterval; + bool watingForConnection; + + /* Set true when a channel is created */ + bool SDPConnected; + bool RFCOMMConnected; + + /*variables filled from HCI event management */ + uint16_t hci_handle; + uint8_t my_bdaddr[6]; // The bluetooth dongles Bluetooth address + uint8_t disc_bdaddr[6]; // the bluetooth address is always 6 bytes + uint8_t remote_name[30]; // first 30 chars of remote name + + /* variables used by high level HCI task */ + uint8_t hci_state; //current state of bluetooth hci connection + uint16_t hci_counter; // counter used for bluetooth hci reset loops + uint8_t hci_num_reset_loops; // this value indicate how many times it should read before trying to reset + uint16_t hci_event_flag;// hci flags of received bluetooth events + + /* variables used by high level L2CAP task */ + uint8_t l2cap_sdp_state; + uint8_t l2cap_rfcomm_state; + uint16_t l2cap_event_flag;// l2cap flags of received bluetooth events + + uint8_t hcibuf[BULK_MAXPKTSIZE];//General purpose buffer for hci data + uint8_t l2capinbuf[BULK_MAXPKTSIZE];//General purpose buffer for l2cap in data + uint8_t l2capoutbuf[BULK_MAXPKTSIZE];//General purpose buffer for l2cap out data + uint8_t rfcommbuf[BULK_MAXPKTSIZE]; // Buffer for RFCOMM Data + + /* L2CAP Channels */ + uint8_t sdp_scid[2]; // L2CAP source CID for HID_Control + uint8_t sdp_dcid[2]; // 0x0050 + uint8_t rfcomm_scid[2]; // L2CAP source CID for HID_Interrupt + uint8_t rfcomm_dcid[2]; // 0x0051 + uint8_t identifier; // Identifier for command + + /* RFCOMM Variables */ + uint8_t rfcommChannel; + uint8_t rfcommChannelPermanent; + uint8_t rfcommDirection; + uint8_t rfcommCommandResponse; + uint8_t rfcommChannelType; + uint8_t rfcommPfBit; + + unsigned long timer; + bool waitForLastCommand; + bool creditSent; + + uint8_t rfcommDataBuffer[256]; // Create a 256 sized buffer for incoming data + uint8_t rfcommAvailable; + + bool firstMessage; // Used to see if it's the first SDP request received + + /* State machines */ + void HCI_event_task(); //poll the HCI event pipe + void HCI_task(); // HCI state machine + void ACL_event_task(); // start polling the ACL input pipe too, though discard data until connected + void SDP_task(); // SDP state machine + void RFCOMM_task(); // RFCOMM state machine + + void readReport(); // read incoming data + void printReport(); // print incoming date - Uncomment "#define PRINTREPORT" to print incoming data debugging + + /* HCI Commands */ + void HCI_Command(uint8_t* data, uint16_t nbytes); + void hci_reset(); + void hci_write_scan_enable(); + void hci_write_scan_disable(); + void hci_read_bdaddr(); + void hci_accept_connection(); + void hci_remote_name(); + void hci_set_local_name(const char* name); + void hci_pin_code_request_reply(const char* key); + void hci_link_key_request_negative_reply(); + void hci_disconnect(); + + /* L2CAP Commands */ + void L2CAP_Command(uint8_t* data, uint16_t nbytes, uint8_t channelLow = 0x01, uint8_t channelHigh = 0x00); // Standard L2CAP header: Channel ID (0x01) for ACL-U + void l2cap_connection_response(uint8_t rxid, uint8_t* dcid, uint8_t* scid, uint8_t result); + void l2cap_config_request(uint8_t rxid, uint8_t* dcid); + void l2cap_config_response(uint8_t rxid, uint8_t* scid); + void l2cap_disconnection_request(uint8_t rxid, uint8_t* dcid, uint8_t* scid); + void l2cap_disconnection_response(uint8_t rxid, uint8_t* dcid, uint8_t* scid); + void l2cap_information_response(uint8_t rxid, uint8_t infoTypeLow, uint8_t infoTypeHigh); + + /* SDP Commands */ + void SDP_Command(uint8_t* data, uint16_t nbytes); + void serviceNotSupported(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void serialPortResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void serialPortResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void l2capResponse1(uint8_t transactionIDHigh, uint8_t transactionIDLow); + void l2capResponse2(uint8_t transactionIDHigh, uint8_t transactionIDLow); + + /* RFCOMM Commands */ + void RFCOMM_Command(uint8_t* data, uint16_t nbytes); + void sendRfcomm(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t* data, uint8_t length); + void sendRfcommCredit(uint8_t channel, uint8_t direction, uint8_t CR, uint8_t channelType, uint8_t pfBit, uint8_t credit); + uint8_t calcFcs(uint8_t *data); + uint8_t __crc(uint8_t* data); +}; +#endif \ No newline at end of file diff --git a/examples/RFCOMM/RFCOMM.ino b/examples/RFCOMM/RFCOMM.ino new file mode 100644 index 00000000..923d2e0e --- /dev/null +++ b/examples/RFCOMM/RFCOMM.ino @@ -0,0 +1,38 @@ +/* + Example sketch for the RFCOMM Bluetooth library - developed by Kristian Lauszus + For more information visit my blog: http://blog.tkjelectronics.dk/ or + send me an e-mail: kristianl@tkjelectronics.com + */ + +#include +USB Usb; +/* You can create the instance of the class in two ways */ +RFCOMM SerialBT(&Usb); // This will set the name to the defaults: "Arduino" and the pin to "1234" +//RFCOMM SerialBT(&Usb, "Lauszus' Arduino","0000"); // You can also set the name and pin like so + +boolean firstMessage = true; + +void setup() { + Serial.begin(115200); + if (Usb.Init() == -1) { + Serial.print(F("\r\nOSC did not start")); + while(1); //halt + } + Serial.print(F("\r\nRFCOMM Bluetooth Library Started")); +} +void loop() { + Usb.Task(); + if(SerialBT.connected) { + if(firstMessage) { + firstMessage = false; + SerialBT.println("Hello from Arduino"); // Send welcome message + } + if(Serial.available()) + SerialBT.print(Serial.read()); + if(SerialBT.available()) + Serial.write(SerialBT.read()); + } + else + firstMessage = true; + delay(5); +} diff --git a/keywords.txt b/keywords.txt deleted file mode 100644 index 378fc91a..00000000 --- a/keywords.txt +++ /dev/null @@ -1,152 +0,0 @@ -################################################ -# Syntax Coloring Map For PS3 Bluetooth Library -################################################ - -################################################ -# Datatypes (KEYWORD1) -################################################ - -PS3BT KEYWORD1 - -################################################ -# Methods and Functions (KEYWORD2) -################################################ -setBdaddr KEYWORD2 -setMoveBdaddr KEYWORD2 - -getButton KEYWORD2 -getAnalogButton KEYWORD2 -getAnalogHat KEYWORD2 -getSensor KEYWORD2 -getAngle KEYWORD2 -getStatus KEYWORD2 -getStatusString KEYWORD2 -disconnect KEYWORD2 - -setAllOff KEYWORD2 -setRumbleOff KEYWORD2 -setRumbleOn KEYWORD2 -setLedOff KEYWORD2 -setLedOn KEYWORD2 -moveSetBulb KEYWORD2 -moveSetRumble KEYWORD2 - -PS3BTConnected KEYWORD2 -PS3MoveBTConnected KEYWORD2 -PS3NavigationBTConnected KEYWORD2 -buttonChanged KEYWORD2 -buttonPressed KEYWORD2 -buttonReleased KEYWORD2 - -isWatingForConnection KEYWORD2 - -################################################ -# Constants and enums (LITERAL1) -################################################ -LED1 LITERAL1 -LED2 LITERAL1 -LED3 LITERAL1 -LED4 LITERAL1 -LED5 LITERAL1 -LED6 LITERAL1 -LED7 LITERAL1 -LED8 LITERAL1 -LED9 LITERAL1 -LED10 LITERAL1 - -Red LITERAL1 -Green LITERAL1 -Blue LITERAL1 -Yellow LITERAL1 -Lightblue LITERAL1 -Purble LITERAL1 -White LITERAL1 -Off LITERAL1 - -SELECT LITERAL1 -L3 LITERAL1 -R3 LITERAL1 -START LITERAL1 -UP LITERAL1 -RIGHT LITERAL1 -DOWN LITERAL1 -LEFT LITERAL1 -L2 LITERAL1 -R2 LITERAL1 -L1 LITERAL1 -R1 LITERAL1 -TRIANGLE LITERAL1 -CIRCLE LITERAL1 -CROSS LITERAL1 -SQUARE LITERAL1 -PS LITERAL1 -SELECT_MOVE LITERAL1 -START_MOVE LITERAL1 -TRIANGLE_MOVE LITERAL1 -CIRCLE_MOVE LITERAL1 -CROSS_MOVE LITERAL1 -SQUARE_MOVE LITERAL1 -PS_MOVE LITERAL1 -MOVE_MOVE LITERAL1 -T_MOVE LITERAL1 - -UP_ANALOG LITERAL1 -RIGHT_ANALOG LITERAL1 -DOWN_ANALOG LITERAL1 -LEFT_ANALOG LITERAL1 -L2_ANALOG LITERAL1 -R2_ANALOG LITERAL1 -L1_ANALOG LITERAL1 -R1_ANALOG LITERAL1 -TRIANGLE_ANALOG LITERAL1 -CIRCLE_ANALOG LITERAL1 -CROSS_ANALOG LITERAL1 -SQUARE_ANALOG LITERAL1 -T_MOVE_ANALOG LITERAL1 - -LeftHatX LITERAL1 -LeftHatY LITERAL1 -RightHatX LITERAL1 -RightHatY LITERAL1 - -aX LITERAL1 -aY LITERAL1 -aZ LITERAL1 -gZ LITERAL1 -aXmove LITERAL1 -aYmove LITERAL1 -aZmove LITERAL1 -gXmove LITERAL1 -gYmove LITERAL1 -gZmove LITERAL1 -tempMove LITERAL1 -mXmove LITERAL1 -mZmove LITERAL1 -mYmove LITERAL1 - -Pitch LITERAL1 -Roll LITERAL1 - -Plugged LITERAL1 -Unplugged LITERAL1 -Charging LITERAL1 -NotCharging LITERAL1 -Shutdown LITERAL1 -Dying LITERAL1 -Low LITERAL1 -High LITERAL1 -Full LITERAL1 -MoveCharging LITERAL1 -MoveNotCharging LITERAL1 -MoveShutdown LITERAL1 -MoveDying LITERAL1 -MoveLow LITERAL1 -MoveHigh LITERAL1 -MoveFull LITERAL1 -CableRumble LITERAL1 -Cable LITERAL1 -BluetoothRumble LITERAL1 -Bluetooth LITERAL1 - -RumbleHigh LITERAL1 -RumbleLow LITERAL1 \ No newline at end of file diff --git a/masstorage.cpp b/masstorage.cpp new file mode 100644 index 00000000..e95fd0d0 --- /dev/null +++ b/masstorage.cpp @@ -0,0 +1,646 @@ +#include "masstorage.h" + + +//bool BulkReadParser::IsValidCSW(uint8_t size, uint8_t *pcsw) +//{ +// if (size != 0x0d) +// { +// Notify(PSTR("CSW:Size error")); +// return false; +// } +// if (*((uint32_t*)pcsw) != MASS_CSW_SIGNATURE) +// { +// Notify(PSTR("CSW:Sig error")); +// return false; +// } +// //if (size != 0x0d || *((uint32_t*)pcsw) != MASS_CSW_SIGNATURE || +// // ((CommandStatusWrapper*)pcsw)->dCSWTag != dCBWTag) +// // return false; +// return true; +//} + +//bool BulkReadParser::IsMeaningfulCSW(uint8_t size, uint8_t *pcsw) +//{ +// if (((CommandStatusWrapper*)pcsw)->bCSWStatus < 2 && +// ((CommandStatusWrapper*)pcsw)->dCSWDataResidue <= dCBWDataTransferLength ) +// return true; +// if ( ((CommandStatusWrapper*)pcsw)->bCSWStatus == 2 ) +// return true; +// return false; +//} + +//void BulkReadParser::Parse(const uint16_t len, const uint8_t *pbuf, const uint16_t &offset) +//{ +// if (offset == 0 && len > sizeof(CommandStatusWrapper)) +// if (IsValidCSW(sizeof(CommandStatusWrapper), pbuf) && IsMeaningfulCSW(sizeof(CommandStatusWrapper), pbuf)) +// { +// CommandStatusWrapper *pCSW = (CommandStatusWrapper*)pbuf; +// +// Serial.println("Sig:"); +// PrintHex(pCSW->dCSWSignature); +// Serial.println("Tag:"); +// PrintHex(pCSW->dCSWTag); +// Serial.println("Res:"); +// PrintHex(pCSW->dCSWDataResidue); +// Serial.println("Ret:"); +// PrintHex(pCSW->bCSWStatus); +// } +//} + +const uint8_t BulkOnly::epDataInIndex = 1; +const uint8_t BulkOnly::epDataOutIndex = 2; +const uint8_t BulkOnly::epInterruptInIndex = 3; + +BulkOnly::BulkOnly(USB *p /*, CDCAsyncOper *pasync*/) : + pUsb(p), + //pAsync(pasync), + bAddress(0), + qNextPollTime(0), + bPollEnable(false), + bIface(0), + bNumEP(1) +{ + for(uint8_t i=0; iRegisterDeviceClass(this); +} + +uint8_t BulkOnly::Init(uint8_t parent, uint8_t port, bool lowspeed) +{ + const uint8_t constBufSize = sizeof(USB_DEVICE_DESCRIPTOR); + + uint8_t buf[constBufSize]; + uint8_t rcode; + UsbDevice *p = NULL; + EpInfo *oldep_ptr = NULL; + uint8_t num_of_conf; // number of configurations + + AddressPool &addrPool = pUsb->GetAddressPool(); + + USBTRACE("MS Init\r\n"); + + if (bAddress) + return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE; + + // Get pointer to pseudo device with address 0 assigned + p = addrPool.GetUsbDevicePtr(0); + + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + + if (!p->epinfo) + { + USBTRACE("epinfo\r\n"); + return USB_ERROR_EPINFO_IS_NULL; + } + + // Save old pointer to EP_RECORD of address 0 + oldep_ptr = p->epinfo; + + // Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence + p->epinfo = epInfo; + + p->lowspeed = lowspeed; + + // Get device descriptor + rcode = pUsb->getDevDescr( 0, 0, constBufSize, (uint8_t*)buf ); + + // Restore p->epinfo + p->epinfo = oldep_ptr; + + if( rcode ) + goto FailGetDevDescr; + + // Allocate new address according to device class + bAddress = addrPool.AllocAddress(parent, false, port); + + if (!bAddress) + return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL; + + // Extract Max Packet Size from the device descriptor + epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; + + // Assign new address to the device + rcode = pUsb->setAddr( 0, 0, bAddress ); + + if (rcode) + { + p->lowspeed = false; + addrPool.FreeAddress(bAddress); + bAddress = 0; + USBTRACE2("setAddr:",rcode); + return rcode; + } + + USBTRACE2("Addr:", bAddress); + + p->lowspeed = false; + + p = addrPool.GetUsbDevicePtr(bAddress); + + if (!p) + return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL; + + p->lowspeed = lowspeed; + + num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations; + + // Assign epInfo to epinfo pointer + rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo); + + if (rcode) + goto FailSetDevTblEntry; + + USBTRACE2("NC:", num_of_conf); + + for (uint8_t i=0; i HexDump; + ConfigDescParser< USB_CLASS_MASS_STORAGE, + MASS_SUBCLASS_SCSI, + MASS_PROTO_BBB, + CP_MASK_COMPARE_CLASS | + CP_MASK_COMPARE_SUBCLASS | + CP_MASK_COMPARE_PROTOCOL> BulkOnlyParser(this); + + rcode = pUsb->getConfDescr(bAddress, 0, i, &HexDump); + rcode = pUsb->getConfDescr(bAddress, 0, i, &BulkOnlyParser); + + + if (bNumEP > 1) + break; + } // for + + if (bNumEP < 3) + return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED; + + // Assign epInfo to epinfo pointer + rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo); + + USBTRACE2("Conf:", bConfNum); + + // Set Configuration Value + rcode = pUsb->setConf(bAddress, 0, bConfNum); + + if (rcode) + goto FailSetConf; + + delay(5000); + + //rcode = pAsync->OnInit(this); + + //if (rcode) + // goto FailOnInit; + + rcode = GetMaxLUN(&bMaxLUN); + + if (rcode) + goto FailGetMaxLUN; + + delay(10); + + { + InquiryResponse response; + rcode = Inquiry(bMaxLUN, sizeof(InquiryResponse), (uint8_t*)&response); + + if (rcode) + goto FailInquiry; + + //if (response.DeviceType != 0) + // goto FailInvalidDevice; + } + + delay(10); + + USBTRACE("MS configured\r\n"); + + bPollEnable = true; + + //USBTRACE("Poll enabled\r\n"); + return 0; + +FailGetDevDescr: + USBTRACE("getDevDescr:"); + goto Fail; + +FailSetDevTblEntry: + USBTRACE("setDevTblEn:"); + goto Fail; + +FailGetConfDescr: + USBTRACE("getConf:"); + goto Fail; + +FailSetConf: + USBTRACE("setConf:"); + goto Fail; + +FailOnInit: + USBTRACE("OnInit:"); + goto Fail; + +FailGetMaxLUN: + USBTRACE("GetMaxLUN:"); + goto Fail; + +FailInquiry: + USBTRACE("Inquiry:"); + goto Fail; + +Fail: + Serial.println(rcode, HEX); + Release(); + return rcode; +} + + +void BulkOnly::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) +{ + ErrorMessage(PSTR("Conf.Val"), conf); + ErrorMessage(PSTR("Iface Num"),iface); + ErrorMessage(PSTR("Alt.Set"), alt); + + bConfNum = conf; + + uint8_t index; + + if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80) + index = epInterruptInIndex; + else + if ((pep->bmAttributes & 0x02) == 2) + index = ((pep->bEndpointAddress & 0x80) == 0x80) ? epDataInIndex : epDataOutIndex; + else + return; + + // Fill in the endpoint info structure + epInfo[index].epAddr = (pep->bEndpointAddress & 0x0F); + epInfo[index].maxPktSize = (uint8_t)pep->wMaxPacketSize; + epInfo[index].epAttribs = 0; + + bNumEP ++; + + PrintEndpointDescriptor(pep); +} + +uint8_t BulkOnly::Release() +{ + pUsb->GetAddressPool().FreeAddress(bAddress); + + bIface = 0; + bNumEP = 1; + + bAddress = 0; + qNextPollTime = 0; + bPollEnable = false; + return 0; +} + +uint8_t BulkOnly::Poll() +{ + uint8_t rcode = 0; + + if (!bPollEnable) + return 0; + + uint32_t time_now = millis(); + + //if (qNextPollTime <= time_now) + //{ + // qNextPollTime = time_now + 100; + + // uint8_t rcode; + // const uint8_t constBufSize = 16; + // uint8_t buf[constBufSize]; + + // for (uint8_t i=0; i epInfo[epInterruptInIndex].maxPktSize) + // ? epInfo[epInterruptInIndex].maxPktSize : constBufSize; + // rcode = pUsb->inTransfer(bAddress, epInfo[epInterruptInIndex].epAddr, &read, buf); + + // if (rcode) + // return rcode; + + // for (uint8_t i=0; i(buf[i]); + // Serial.print(" "); + // } + // USBTRACE("\r\n"); + //} + return rcode; +} + +bool BulkOnly::IsValidCBW(uint8_t size, uint8_t *pcbw) +{ + if (size != 0x1f || *((uint32_t*)pcbw) != MASS_CBW_SIGNATURE) + return false; + return true; +} + +bool BulkOnly::IsMeaningfulCBW(uint8_t size, uint8_t *pcbw) +{ + if (((CommandBlockWrapper*)pcbw)->bmReserved1 != 0 || + ((CommandBlockWrapper*)pcbw)->bmReserved2 != 0 || + ((CommandBlockWrapper*)pcbw)->bmCBWLUN > bMaxLUN || + ((CommandBlockWrapper*)pcbw)->bmCBWCBLength > 0x10 ) + return false; + return true; +} + +uint8_t BulkOnly::Reset() +{ + return( pUsb->ctrlReq( bAddress, 0, bmREQ_MASSOUT, MASS_REQ_BOMSR, 0, 0, bIface, 0, 0, NULL, NULL )); +} + +uint8_t BulkOnly::GetMaxLUN(uint8_t *plun) +{ + uint8_t cnt = 3; + + bLastUsbError = pUsb->ctrlReq( bAddress, 0, bmREQ_MASSIN, MASS_REQ_GET_MAX_LUN, 0, 0, bIface, 1, 1, plun, NULL ); + + delay(10); + //Serial.println(F("bLastUsbError: ")); + //Serial.println(bLastUsbError); + + if (bLastUsbError == hrSTALL) + { + *plun = 0; + bLastUsbError = ClearEpHalt(epDataInIndex); + return MASS_ERR_SUCCESS; + } + if (bLastUsbError == hrJERR) + return MASS_ERR_DEVICE_DISCONNECTED; + else if (bLastUsbError) + return MASS_ERR_GENERAL_USB_ERROR; + return MASS_ERR_SUCCESS; +} + +uint8_t BulkOnly::HandleUsbError(uint8_t index) +{ + uint8_t count = 3; + + while (bLastUsbError && count) + { + switch (bLastUsbError) + { + case hrSUCCESS: + return MASS_ERR_SUCCESS; + case hrJERR: + bLastUsbError = hrSUCCESS; + return MASS_ERR_DEVICE_DISCONNECTED; + case hrSTALL: + bLastUsbError = ClearEpHalt(index); + break; + default: + return MASS_ERR_GENERAL_USB_ERROR; + } + count --; + } // while + + return MASS_ERR_SUCCESS; +} + +uint8_t BulkOnly::ClearEpHalt(uint8_t index) +{ + return (pUsb->ctrlReq( bAddress, 0, USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_STANDARD|USB_SETUP_RECIPIENT_ENDPOINT, + USB_REQUEST_CLEAR_FEATURE, USB_FEATURE_ENDPOINT_HALT, 0, epInfo[index].epAddr, 0, 0, NULL, NULL )); +} + +uint8_t BulkOnly::ResetRecovery() +{ + bLastUsbError = Reset(); + + if (bLastUsbError) + return bLastUsbError; + + delay(6); + + bLastUsbError = ClearEpHalt(epDataInIndex); + + if (bLastUsbError) + return bLastUsbError; + + delay(6); + + bLastUsbError = ClearEpHalt(epDataOutIndex); + + delay(6); + + return bLastUsbError; +} + +uint8_t BulkOnly::Inquiry(uint8_t lun, uint16_t bsize, uint8_t *buf) +{ + CommandBlockWrapper cbw; + + cbw.dCBWSignature = MASS_CBW_SIGNATURE; + cbw.dCBWTag = 0xdeadbeef; + cbw.dCBWDataTransferLength = bsize; + cbw.bmCBWFlags = MASS_CMD_DIR_IN, + cbw.bmCBWLUN = lun; + cbw.bmCBWCBLength = 6; + + for (uint8_t i=0; i<16; i++) + cbw.CBWCB[i] = 0; + + cbw.CBWCB[0] = SCSI_CMD_INQUIRY; + cbw.CBWCB[4] = bsize; + + return Transaction(&cbw, bsize, buf, 0); +} + +uint8_t BulkOnly::RequestSense(uint8_t lun, uint16_t size, uint8_t *buf) +{ + CommandBlockWrapper cbw; + + cbw.dCBWSignature = MASS_CBW_SIGNATURE; + cbw.dCBWTag = 0xdeadbeef; + cbw.dCBWDataTransferLength = size; + cbw.bmCBWFlags = MASS_CMD_DIR_IN, + cbw.bmCBWLUN = lun; + cbw.bmCBWCBLength = 6; + + for (uint8_t i=0; i<16; i++) + cbw.CBWCB[i] = 0; + + cbw.CBWCB[0] = SCSI_CMD_REQUEST_SENSE; + cbw.CBWCB[4] = size; + + return Transaction(&cbw, size, buf, 0); +} + +uint8_t BulkOnly::ReadCapacity(uint8_t lun, uint16_t bsize, uint8_t *buf) +{ + CommandBlockWrapper cbw; + + cbw.dCBWSignature = MASS_CBW_SIGNATURE; + cbw.dCBWTag = 0xdeadbeef; + cbw.dCBWDataTransferLength = bsize; + cbw.bmCBWFlags = MASS_CMD_DIR_IN, + cbw.bmCBWLUN = lun; + cbw.bmCBWCBLength = 10; + + for (uint8_t i=0; i<16; i++) + cbw.CBWCB[i] = 0; + + cbw.CBWCB[0] = SCSI_CMD_READ_CAPACITY_10; + cbw.CBWCB[4] = bsize; + + return Transaction(&cbw, bsize, buf, 0); +} + +uint8_t BulkOnly::TestUnitReady(uint8_t lun) +{ + CommandBlockWrapper cbw; + + cbw.dCBWSignature = MASS_CBW_SIGNATURE; + cbw.dCBWTag = 0xdeadbeef; + cbw.dCBWDataTransferLength = 0; + cbw.bmCBWFlags = MASS_CMD_DIR_OUT, + cbw.bmCBWLUN = lun; + cbw.bmCBWCBLength = 6; + + for (uint8_t i=0; i<16; i++) + cbw.CBWCB[i] = 0; + + cbw.CBWCB[0] = SCSI_CMD_TEST_UNIT_READY; + + return Transaction(&cbw, 0, NULL, 0); +} + +uint8_t BulkOnly::Read(uint8_t lun, uint32_t addr, uint16_t bsize, USBReadParser *prs) +{ + CommandBlockWrapper cbw; + + cbw.dCBWSignature = MASS_CBW_SIGNATURE; + cbw.dCBWTag = 0xdeadbeef; + cbw.dCBWDataTransferLength = bsize; + cbw.bmCBWFlags = MASS_CMD_DIR_IN, + cbw.bmCBWLUN = lun; + cbw.bmCBWCBLength = 10; + + for (uint8_t i=0; i<16; i++) + cbw.CBWCB[i] = 0; + + cbw.CBWCB[0] = SCSI_CMD_READ_10; + cbw.CBWCB[8] = 1; + cbw.CBWCB[5] = (addr & 0xff); + cbw.CBWCB[4] = ((addr >> 8) & 0xff); + cbw.CBWCB[3] = ((addr >> 16) & 0xff); + cbw.CBWCB[2] = ((addr >> 24) & 0xff); + + return Transaction(&cbw, bsize, prs, 1); +} + +uint8_t BulkOnly::Transaction(CommandBlockWrapper *cbw, uint16_t size, void *buf, uint8_t flags) +{ + uint16_t read; + { + bLastUsbError = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, sizeof(CommandBlockWrapper), (uint8_t*)cbw); + + uint8_t ret = HandleUsbError(epDataOutIndex); + + if (ret) + { + ErrorMessage(PSTR("CBW"), ret); + return ret; + } + } + + if (size && buf) + { + read = size; + + if (cbw->bmCBWFlags & MASS_CMD_DIR_IN) + { + if ((flags & MASS_TRANS_FLG_CALLBACK) == MASS_TRANS_FLG_CALLBACK) + { + const uint8_t bufSize = 64; + uint16_t total = size; + uint16_t count = 0; + uint8_t rbuf[bufSize]; + + read = bufSize; + + while(count < total && + ((bLastUsbError = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, (uint8_t*)rbuf)) == hrSUCCESS) + ) + { + ((USBReadParser*)buf)->Parse(read, rbuf, count); + + count += read; + read = bufSize; + } + + if (bLastUsbError == hrSTALL) + bLastUsbError = ClearEpHalt(epDataInIndex); + + if (bLastUsbError) + { + ErrorMessage(PSTR("RDR"), bLastUsbError); + return MASS_ERR_GENERAL_USB_ERROR; + } + } // if ((flags & 1) == 1) + else + bLastUsbError = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, (uint8_t*)buf); + } // if (cbw->bmCBWFlags & MASS_CMD_DIR_IN) + + else if (cbw->bmCBWFlags & MASS_CMD_DIR_OUT) + bLastUsbError = pUsb->outTransfer(bAddress, epInfo[epDataOutIndex].epAddr, read, (uint8_t*)buf); + } + + uint8_t ret = HandleUsbError((cbw->bmCBWFlags & MASS_CMD_DIR_IN) ? epDataInIndex : epDataOutIndex); + + if (ret) + { + ErrorMessage(PSTR("RSP"), ret); + return MASS_ERR_GENERAL_USB_ERROR; + } + { + CommandStatusWrapper csw; + read = sizeof(CommandStatusWrapper); + + bLastUsbError = pUsb->inTransfer(bAddress, epInfo[epDataInIndex].epAddr, &read, (uint8_t*)&csw); + + uint8_t ret = HandleUsbError(epDataInIndex); + + if (ret) + { + ErrorMessage(PSTR("CSW"), ret); + return ret; + } + //if (csw.bCSWStatus == MASS_ERR_PHASE_ERROR) + // bLastUsbError = ResetRecovery(); + + return csw.bCSWStatus; + } + //return MASS_ERR_SUCCESS; +} + +void BulkOnly::PrintEndpointDescriptor( const USB_ENDPOINT_DESCRIPTOR* ep_ptr ) +{ + Notify(PSTR("Endpoint descriptor:")); + Notify(PSTR("\r\nLength:\t\t")); + PrintHex(ep_ptr->bLength); + Notify(PSTR("\r\nType:\t\t")); + PrintHex(ep_ptr->bDescriptorType); + Notify(PSTR("\r\nAddress:\t")); + PrintHex(ep_ptr->bEndpointAddress); + Notify(PSTR("\r\nAttributes:\t")); + PrintHex(ep_ptr->bmAttributes); + Notify(PSTR("\r\nMaxPktSize:\t")); + PrintHex(ep_ptr->wMaxPacketSize); + Notify(PSTR("\r\nPoll Intrv:\t")); + PrintHex(ep_ptr->bInterval); + Notify(PSTR("\r\n")); +} diff --git a/masstorage.h b/masstorage.h new file mode 100644 index 00000000..e856cb70 --- /dev/null +++ b/masstorage.h @@ -0,0 +1,252 @@ +#if !defined(__MASSTORAGE_H__) +#define __MASSTORAGE_H__ + +#include +#include +#include "avrpins.h" +#include "max3421e.h" +#include "usbhost.h" +#include "usb_ch9.h" +#include "Usb.h" + +#if defined(ARDUINO) && ARDUINO >=100 +#include "Arduino.h" +#else +#include +#endif + +#include "printhex.h" +#include "hexdump.h" +#include "message.h" + +#include "confdescparser.h" + +#define SWAP(a, b) (((a) ^= (b)), ((b) ^= (a)), ((a) ^= (b))) + +#define bmREQ_MASSOUT USB_SETUP_HOST_TO_DEVICE|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE +#define bmREQ_MASSIN USB_SETUP_DEVICE_TO_HOST|USB_SETUP_TYPE_CLASS|USB_SETUP_RECIPIENT_INTERFACE + +// Mass Storage Subclass Constants +#define MASS_SUBCLASS_SCSI_NOT_REPORTED 0x00 // De facto use +#define MASS_SUBCLASS_RBC 0x01 +#define MASS_SUBCLASS_ATAPI 0x02 // MMC-5 (ATAPI) +#define MASS_SUBCLASS_OBSOLETE1 0x03 // Was QIC-157 +#define MASS_SUBCLASS_UFI 0x04 // Specifies how to interface Floppy Disk Drives to USB +#define MASS_SUBCLASS_OBSOLETE2 0x05 // Was SFF-8070i +#define MASS_SUBCLASS_SCSI 0x06 // SCSI Transparent Command Set +#define MASS_SUBCLASS_LSDFS 0x07 // Specifies how host has to negotiate access before trying SCSI +#define MASS_SUBCLASS_IEEE1667 0x08 + +// Mass Storage Class Protocols +#define MASS_PROTO_CBI 0x00 // CBI (with command completion interrupt) +#define MASS_PROTO_CBI_NO_INT 0x01 // CBI (without command completion interrupt) +#define MASS_PROTO_OBSOLETE 0x02 +#define MASS_PROTO_BBB 0x50 // Bulk Only Transport +#define MASS_PROTO_UAS 0x62 + +// Request Codes +#define MASS_REQ_ADSC 0x00 +#define MASS_REQ_GET 0xFC +#define MASS_REQ_PUT 0xFD +#define MASS_REQ_GET_MAX_LUN 0xFE +#define MASS_REQ_BOMSR 0xFF // Bulk-Only Mass Storage Reset + +#define MASS_CBW_SIGNATURE 0x43425355 +#define MASS_CSW_SIGNATURE 0x53425355 + +#define MASS_CMD_DIR_OUT (0 << 7) +#define MASS_CMD_DIR_IN (1 << 7) + +#define SCSI_CMD_INQUIRY 0x12 +#define SCSI_CMD_REPORT_LUNS 0xA0 +#define SCSI_CMD_REQUEST_SENSE 0x03 +#define SCSI_CMD_FORMAT_UNIT 0x04 +#define SCSI_CMD_READ_6 0x08 +#define SCSI_CMD_READ_10 0x28 +#define SCSI_CMD_READ_CAPACITY_10 0x25 +#define SCSI_CMD_TEST_UNIT_READY 0x00 +#define SCSI_CMD_WRITE_6 0x0A +#define SCSI_CMD_WRITE_10 0x2A +#define SCSI_CMD_MODE_SENSE_6 0x1A +#define SCSI_CMD_MODE_SENSE_10 0x5A + +#define MASS_ERR_SUCCESS 0x00 +#define MASS_ERR_PHASE_ERROR 0x01 +#define MASS_ERR_DEVICE_DISCONNECTED 0x11 +#define MASS_ERR_UNABLE_TO_RECOVER 0x12 // Reset recovery error +#define MASS_ERR_GENERAL_USB_ERROR 0xFF + +#define MASS_TRANS_FLG_CALLBACK 0x01 // Callback is involved +#define MASS_TRANS_FLG_NO_STALL_CHECK 0x02 // STALL condition is not checked +#define MASS_TRANS_FLG_NO_PHASE_CHECK 0x04 // PHASE_ERROR is not checked + + +struct Capacity +{ + uint8_t data[8]; + //uint32_t dwBlockAddress; + //uint32_t dwBlockLength; +}; + +struct InquiryResponse +{ + uint8_t DeviceType : 5; + uint8_t PeripheralQualifier : 3; + + unsigned Reserved : 7; + unsigned Removable : 1; + + uint8_t Version; + + unsigned ResponseDataFormat : 4; + unsigned Reserved2 : 1; + unsigned NormACA : 1; + unsigned TrmTsk : 1; + unsigned AERC : 1; + + uint8_t AdditionalLength; + uint8_t Reserved3[2]; + + unsigned SoftReset : 1; + unsigned CmdQue : 1; + unsigned Reserved4 : 1; + unsigned Linked : 1; + unsigned Sync : 1; + unsigned WideBus16Bit : 1; + unsigned WideBus32Bit : 1; + unsigned RelAddr : 1; + + uint8_t VendorID[8]; + uint8_t ProductID[16]; + uint8_t RevisionID[4]; +}; + +struct CommandBlockWrapper +{ + uint32_t dCBWSignature; + uint32_t dCBWTag; + uint32_t dCBWDataTransferLength; + uint8_t bmCBWFlags; + + struct + { + uint8_t bmCBWLUN : 4; + uint8_t bmReserved1 : 4; + }; + struct + { + uint8_t bmCBWCBLength : 4; + uint8_t bmReserved2 : 4; + }; + + uint8_t CBWCB[16]; +} ; + +struct CommandStatusWrapper +{ + uint32_t dCSWSignature; + uint32_t dCSWTag; + uint32_t dCSWDataResidue; + uint8_t bCSWStatus; +}; + +struct RequestSenseResponce +{ + uint8_t bResponseCode; + uint8_t bSegmentNumber; + + uint8_t bmSenseKey : 4; + uint8_t bmReserved : 1; + uint8_t bmILI : 1; + uint8_t bmEOM : 1; + uint8_t bmFileMark : 1; + + uint8_t Information[4]; + uint8_t bAdditionalLength; + uint8_t CmdSpecificInformation[4]; + uint8_t bAdditionalSenseCode; + uint8_t bAdditionalSenseQualifier; + uint8_t bFieldReplaceableUnitCode; + uint8_t SenseKeySpecific[3]; +}; + +//class BulkReadParser : public USBReadParser +//{ +//protected: +// bool IsValidCSW(uint8_t size, uint8_t *pcsw); +// bool IsMeaningfulCSW(uint8_t size, uint8_t *pcsw); +// +//public: +// virtual void Parse(const uint16_t len, const uint8_t *pbuf, const uint16_t &offset) = 0; +//}; + +#define MASS_MAX_ENDPOINTS 3 + +class BulkOnly : public USBDeviceConfig, public UsbConfigXtracter +{ +protected: + static const uint8_t epDataInIndex; // DataIn endpoint index + static const uint8_t epDataOutIndex; // DataOUT endpoint index + static const uint8_t epInterruptInIndex; // InterruptIN endpoint index + + USB *pUsb; + uint8_t bAddress; + uint8_t bConfNum; // configuration number + uint8_t bIface; // interface value + uint8_t bNumEP; // total number of EP in the configuration + uint32_t qNextPollTime; // next poll time + bool bPollEnable; // poll enable flag + + EpInfo epInfo[MASS_MAX_ENDPOINTS]; + + uint32_t dCBWTag; // Tag + uint32_t dCBWDataTransferLength; // Data Transfer Length + uint8_t bMaxLUN; // Max LUN + uint8_t bLastUsbError; // Last USB error + +protected: + //union TransFlags + //{ + // uint8_t nValue; + + // struct { + // uint8_t bmCallback : 1; + // uint8_t bmCheckPhaseErr : 1; + // uint8_t bmDummy : 6; + // }; + //}; + void PrintEndpointDescriptor(const USB_ENDPOINT_DESCRIPTOR* ep_ptr); + + bool IsValidCBW(uint8_t size, uint8_t *pcbw); + bool IsMeaningfulCBW(uint8_t size, uint8_t *pcbw); + + uint8_t ClearEpHalt(uint8_t index); + uint8_t Transaction(CommandBlockWrapper *cbw, uint16_t bsize, void *buf, uint8_t flags); + uint8_t HandleUsbError(uint8_t index); + +public: + BulkOnly(USB *p); + uint8_t GetLastUsbError() { return bLastUsbError; }; + + uint8_t Reset(); + uint8_t GetMaxLUN(uint8_t *max_lun); + + uint8_t ResetRecovery(); + uint8_t Inquiry(uint8_t lun, uint16_t size, uint8_t *buf); + uint8_t TestUnitReady(uint8_t lun); + uint8_t ReadCapacity(uint8_t lun, uint16_t size, uint8_t *buf); + uint8_t RequestSense(uint8_t lun, uint16_t size, uint8_t *buf); + //uint8_t Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t *buf); + uint8_t Read(uint8_t lun, uint32_t addr, uint16_t bsize, USBReadParser *prs); + + // USBDeviceConfig implementation + virtual uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed); + virtual uint8_t Release(); + virtual uint8_t Poll(); + virtual uint8_t GetAddress() { return bAddress; }; + + // UsbConfigXtracter implementation + virtual void EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *ep); +}; + +#endif // __MASSTORAGE_H__ \ No newline at end of file