summaryrefslogtreecommitdiff
path: root/library/ADK2/btRFCOMM.c
diff options
context:
space:
mode:
Diffstat (limited to 'library/ADK2/btRFCOMM.c')
-rw-r--r--library/ADK2/btRFCOMM.c507
1 files changed, 507 insertions, 0 deletions
diff --git a/library/ADK2/btRFCOMM.c b/library/ADK2/btRFCOMM.c
new file mode 100644
index 0000000..de152db
--- /dev/null
+++ b/library/ADK2/btRFCOMM.c
@@ -0,0 +1,507 @@
+/*
+ * Copyright (C) 2012 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#define ADK_INTERNAL
+#include "fwk.h"
+#include "btRFCOMM.h"
+#include "BT.h"
+#include "dbg.h"
+#include "btL2CAP.h"
+#include "coop.h"
+
+//NOTE: to be good citizens, we really should verify incoming CRC. We have better things to do...sorry...
+//Plus, they do not provide safety for the data since data is not checksummed.
+//We are, however, forced to produce good CRCs.
+
+#define UGLY_SCARY_DEBUGGING_CODE 0
+
+#define BT_RFCOMM_DLCI_MSK_EA 0x01
+#define BT_RFCOMM_DLCI_MSK_CR 0x02
+#define BT_RFCOMM_DLCI_MSK_D 0x04
+#define BT_RFCOMM_DLCI_SHIFT 2
+
+#define BT_SZ_MASK_OVER 0x01
+
+#define BT_RFCOMM_CMD_DM 0x0F
+#define BT_RFCOMM_CMD_SABM 0x3F
+#define BT_RFCOMM_CMD_DISC 0x53
+#define BT_RFCOMM_CMD_UA 0x73
+#define BT_RFCOMM_CMD_UIH 0xEF
+#define BT_RFCOMM_CMD_UIH_CRED 0xFF
+
+//_C = command _R = resp
+#define BT_RFCOMM_NSC_R 0x11
+#define BT_RFCOMM_TEST_R 0x21
+#define BT_RFCOMM_TEST_C 0x23
+#define BT_RFCOMM_RLS_R 0x51
+#define BT_RFCOMM_RLS_C 0x53
+#define BT_RFCOMM_FCOFF_R 0x61
+#define BT_RFCOMM_FCOFF_C 0x63
+#define BT_RFCOMM_PN_R 0x81
+#define BT_RFCOMM_PN_C 0x83
+#define BT_RFCOMM_RPN_R 0x91
+#define BT_RFCOMM_RPN_C 0x93
+#define BT_RFCOMM_FCON_R 0xA1
+#define BT_RFCOMM_FCON_C 0xA3
+#define BT_RFCOMM_CLD_C 0xC3
+#define BT_RFCOMM_MSC_R 0xE1
+#define BT_RFCOMM_MSC_C 0xE3
+
+
+#define BT_RFCOMM_REMOTE_CREDS 4
+#define BT_RFCOMM_MAX_CRED_DEBT (BT_RFCOMM_REMOTE_CREDS - 1)
+
+typedef struct{
+
+ uint8_t addr;
+ uint8_t control;
+ uint8_t length; //we do not support 2-byte length
+ uint8_t data[]; //and one byte CRC (FCS)
+}RfcommPacket;
+
+typedef struct{
+
+ BtRfcommPortOpenF oF;
+ BtRfcommPortCloseF cF;
+ BtRfcommPortRxF rF;
+
+}RfcommPort;
+
+static RfcommPort gPortHandlers[NUM_DLCIs] = {{0,}, };
+
+static uint64_t reserved = 0xC000000000000003; //these are always reserved
+
+typedef struct{
+
+ uint16_t aclConn;
+ uint16_t remChan;
+ struct{
+
+ sg_buf* queued;
+ uint16_t MTU;
+ uint8_t credits;
+ uint8_t creditsOwed;
+ char on;
+
+ }ports[NUM_DLCIs];
+
+}RfcommInstanceState;
+
+extern uint8_t btRfcommFcs(uint8_t *data, uint32_t len);
+
+static void btRfcommSend(uint16_t conn, uint16_t remChan, const uint8_t* data, uint16_t sz){
+
+ int i;
+
+ #if UGLY_SCARY_DEBUGGING_CODE
+ dbgPrintf("Sending RFCOMM packet:");
+ for(i = 0; i < sz; i++) dbgPrintf(" %02X", data[i]);
+ dbgPrintf("\n\n");
+ #endif
+
+ sg_buf* buf = sg_alloc();
+ if(!buf) return;
+ if(sg_add_front(buf, data, sz, SG_FLAG_MAKE_A_COPY)){
+
+ l2capServiceTx(conn, remChan, buf);
+ return;
+ }
+ sg_free(buf);
+ free(buf);
+}
+
+static void btRfcommTxData(RfcommInstanceState* state, uint8_t dlci, sg_buf* buf){
+
+ uint8_t data[5] = {(dlci << BT_RFCOMM_DLCI_SHIFT) | BT_RFCOMM_DLCI_MSK_EA, BT_RFCOMM_CMD_UIH};
+ uint32_t dataLen = sg_length(buf);
+ uint8_t hdrLen = 3;
+
+ if(dataLen >= 128){ //need 2 length bytes
+
+ data[2] = dataLen << 1;
+ data[3] = dataLen >> 7;
+ hdrLen++;
+ } else data[2] = (dataLen << 1) | 1;
+
+ if(state->ports[dlci].creditsOwed){ //use the oportunity to send the credits
+
+ data[1] = BT_RFCOMM_CMD_UIH_CRED;
+ data[hdrLen++] = state->ports[dlci].creditsOwed;
+ state->ports[dlci].creditsOwed = 0;
+ }
+
+ if(sg_add_front(buf, data, hdrLen, SG_FLAG_MAKE_A_COPY)){
+
+ data[0] = btRfcommFcs(data, 2);
+
+ if(sg_add_back(buf, data, 1, SG_FLAG_MAKE_A_COPY)){
+
+ l2capServiceTx(state->aclConn, state->remChan, buf);
+ return;
+ }
+ }
+ sg_free(buf);
+ free(buf);
+}
+
+static void rfcommServiceDataRx(void* service, const uint8_t* req, uint16_t reqSz){
+
+ RfcommInstanceState* state = service;
+ const uint16_t conn = state->aclConn;
+ const uint16_t remChan = state->remChan;
+ const RfcommPacket* p = (RfcommPacket*)req; //note that we can do this only because packet has no non-8-bit-members
+ const uint8_t* data = p->data;
+ int i;
+ uint8_t DLCI = p->addr >> BT_RFCOMM_DLCI_SHIFT;
+ uint8_t ctrl = p->control;
+ uint16_t len;
+ uint8_t buf[32];
+
+
+ if(p->length & 1){
+
+ len = p->length >> 1;
+ }
+ else{
+
+ len = *data++;
+ len <<= 7;
+ len |= p->length >> 1;
+ }
+
+ if(ctrl == BT_RFCOMM_CMD_SABM || ctrl == BT_RFCOMM_CMD_DISC){
+
+ buf[ 0] = p->addr;
+ buf[ 1] = BT_RFCOMM_CMD_UA;
+ buf[ 2] = BT_SZ_MASK_OVER;
+ buf[ 3] = btRfcommFcs(buf, 3);
+ btRfcommSend(conn, remChan, buf, 4);
+
+ if(state->ports[DLCI].queued){
+
+ sg_free(state->ports[DLCI].queued);
+ free(state->ports[DLCI].queued);
+ }
+
+ state->ports[DLCI].creditsOwed = 0;
+ state->ports[DLCI].credits = 0;
+ state->ports[DLCI].queued = NULL;
+
+ if(ctrl == BT_RFCOMM_CMD_SABM){
+
+ if(gPortHandlers[DLCI].oF) gPortHandlers[DLCI].oF(state, DLCI);
+ else if(UGLY_SCARY_DEBUGGING_CODE) dbgPrintf("DLCI %2d opened\n", DLCI);
+ state->ports[DLCI].on = 0;
+ }
+ else{
+
+ if(gPortHandlers[DLCI].cF) gPortHandlers[DLCI].cF(state, DLCI);
+ else if(UGLY_SCARY_DEBUGGING_CODE) dbgPrintf("DLCI %2d closed\n", DLCI);
+ state->ports[DLCI].on = 1;
+ }
+ return;
+ }
+
+ if(ctrl == BT_RFCOMM_CMD_UIH && DLCI){ //we got a msg - send back credits if we already owe the limit, else buffer it
+
+ if(++state->ports[DLCI].creditsOwed == BT_RFCOMM_MAX_CRED_DEBT){
+
+ buf[ 0] = p->addr;
+ buf[ 1] = BT_RFCOMM_CMD_UIH_CRED;
+ buf[ 2] = BT_SZ_MASK_OVER;
+ buf[ 3] = state->ports[DLCI].creditsOwed + 1;
+ buf[ 4] = btRfcommFcs(buf, 2);
+ btRfcommSend(conn, remChan, buf, 5);
+ state->ports[DLCI].creditsOwed = 0;
+ }
+ }
+ if(ctrl == BT_RFCOMM_CMD_UIH_CRED){
+
+ state->ports[DLCI].credits += *data++;
+ ctrl = BT_RFCOMM_CMD_UIH;
+
+ if(state->ports[DLCI].queued && state->ports[DLCI].credits){
+
+ state->ports[DLCI].credits--;
+ btRfcommTxData(state, DLCI, state->ports[DLCI].queued);
+ state->ports[DLCI].queued = NULL;
+ }
+ }
+
+ if(DLCI == 0){ //control channel transaction
+ if(ctrl == BT_RFCOMM_CMD_UIH){ //data recieved
+
+ switch(data[0]){
+
+ case BT_RFCOMM_PN_C:{ //negotiations are for wusses!
+
+ uint8_t targetDLCI = data[2] & 0x3F;
+
+ state->ports[targetDLCI].credits = data[9];
+ state->ports[targetDLCI].MTU = (((uint16_t)data[7]) << 8) | data[6];
+
+ buf[ 0] = BT_RFCOMM_DLCI_MSK_EA;
+ buf[ 1] = BT_RFCOMM_CMD_UIH;
+ buf[ 2] = BT_SZ_MASK_OVER + (10 << 1);
+ buf[ 3] = BT_RFCOMM_PN_R;
+ buf[ 4] = BT_SZ_MASK_OVER + (8 << 1);
+ buf[ 5] = targetDLCI;
+ buf[ 6] = (data[3] == 0xF0) ? 0xE0 : 0x00; //as per spec
+ buf[ 7] = data[4]; //copy as per spec
+ buf[ 8] = 0; //0 as per spec
+ buf[ 9] = RFCOMM_MTU & 0xFF; //max_packet_size.lo
+ buf[ 10] = RFCOMM_MTU >> 8; //max_packet_size.hi
+ buf[ 11] = 0; //0 retransmissions
+ buf[ 12] = BT_RFCOMM_REMOTE_CREDS; //initial credits
+ buf[ 13] = btRfcommFcs(buf, 2);
+ btRfcommSend(conn, remChan, buf, 14);
+ break;
+ }
+
+ case BT_RFCOMM_RPN_C: //we need to echo back the negotiation
+ case BT_RFCOMM_MSC_C:{ //we need to echo back the control signals
+
+ if(len > 127) dbgPrintf("Control packet way too big\n");
+ else{
+
+ buf[ 0] = BT_RFCOMM_DLCI_MSK_EA;
+ buf[ 1] = BT_RFCOMM_CMD_UIH;
+ buf[ 2] = p->length;
+ buf[ 3] = (data[0] == BT_RFCOMM_MSC_C) ? BT_RFCOMM_MSC_R : BT_RFCOMM_RPN_C;
+ buf[ 4] = data[1];
+ for(i = 0; i < data[1] >> 1; i++) buf[ 5 + i] = data[2 + i];
+ buf[ 5 + i] = btRfcommFcs(buf, 2);
+ btRfcommSend(conn, remChan, buf, 6 + i);
+
+ if(data[0] == BT_RFCOMM_MSC_C){
+ //now we send our own signals (coincidentally they are identical ;-)
+ buf[ 3] = BT_RFCOMM_MSC_C;
+ btRfcommSend(conn, remChan, buf, 6 + i);
+ }
+ }
+ break;
+ }
+
+ case BT_RFCOMM_RPN_R:
+ case BT_RFCOMM_MSC_R:{
+
+ //nothing to do about this :)
+ break;
+ }
+
+ default:
+
+ dbgPrintf("Recieved unhandled RFCOMM packet:");
+ for(i = 0; i < reqSz; i++) dbgPrintf(" %02X", req[i]);
+ dbgPrintf("\n\n");
+ break;
+ }
+ }
+ }
+ else if(ctrl == BT_RFCOMM_CMD_UIH){
+
+ if(len){
+
+ if(gPortHandlers[DLCI].rF) gPortHandlers[DLCI].rF(state, DLCI, data, len);
+ else if(UGLY_SCARY_DEBUGGING_CODE){
+
+ dbgPrintf("RFCOMM DLCI %d data:", p->addr >> 2);
+ for(i = 0; i < len; i++) dbgPrintf(" %02X", data[i]);
+ dbgPrintf("\n\n\n");
+ }
+ }
+ }
+ else{
+
+ dbgPrintf("Recieved unhandled RFCOMM cmd (dlci=%d):", DLCI);
+ for(i = 0; i < reqSz; i++) dbgPrintf(" %02X", req[i]);
+ dbgPrintf("\n\n");
+ }
+}
+
+static void* rfcommServiceAlloc(uint16_t conn, uint16_t chan, uint16_t remChan){
+
+ RfcommInstanceState* state = malloc(sizeof(RfcommInstanceState));
+ int i;
+
+ if(!state) return NULL;
+
+ state->aclConn = conn;
+ state->remChan = remChan;
+
+ for(i = 0; i < NUM_DLCIs; i++){
+
+ state->ports[i].creditsOwed = 0;
+ state->ports[i].credits = 0;
+ state->ports[i].on = 0;
+ state->ports[i].queued = 0;
+ }
+
+ return state;
+}
+
+static void rfcommServiceFree(void* service){
+
+ free(service);
+}
+
+void btRfcommRegisterL2capService(){
+
+ const L2capService rfcom = {L2CAP_FLAG_SUPPORT_CONNECTIONS, rfcommServiceAlloc, rfcommServiceFree, rfcommServiceDataRx};
+ if(!l2capServiceRegister(L2CAP_PSM_RFCOMM, &rfcom)) dbgPrintf("SDP L2CAP registration failed\n");
+}
+
+void btRfcommRegisterPort(uint8_t dlci, BtRfcommPortOpenF oF, BtRfcommPortCloseF cF, BtRfcommPortRxF rF){
+
+ if(dlci >= NUM_DLCIs) return; //no such DLCI;
+
+ gPortHandlers[dlci].oF = oF;
+ gPortHandlers[dlci].cF = cF;
+ gPortHandlers[dlci].rF = rF;
+}
+
+void btRfcommPortTx(void* port, uint8_t dlci, const uint8_t* data, uint16_t size){
+
+ RfcommInstanceState* state = port;
+
+ if(dlci >= NUM_DLCIs || dlci == 0) return; //cannot send there, buddy...
+
+ sg_buf* buf = sg_alloc();
+ if(!buf) return;
+
+ if(!sg_add_front(buf, data, size, SG_FLAG_MAKE_A_COPY)){
+
+ sg_free(buf);
+ free(buf);
+ }
+ else{
+
+ if(state->ports[dlci].credits){ //send immediately
+
+ state->ports[dlci].credits--;
+ btRfcommTxData(state, dlci, buf);
+ }
+ else{ //enqueue it
+
+ if(state->ports[dlci].queued){
+
+ while(state->ports[dlci].queued && state->ports[dlci].on) coopYield();
+ if(!state->ports[dlci].on) return; //port closed - packet dropped
+ }
+ state->ports[dlci].queued = buf;
+ }
+ }
+}
+
+#define RFCOMM_DLCI_PREFERENCE_NONE 0x80 //use this param to the below
+#define RFCOMM_DLCI_NEED_EVEN 0x81
+#define RFCOMM_DLCI_NEED_ODD 0x82
+
+uint8_t btRfcommReserveDlci(uint8_t preference){
+
+ uint8_t start = 0, end = 64, step = 2;
+
+ if(preference == RFCOMM_DLCI_PREFERENCE_NONE) step = 1;
+ else if(preference == RFCOMM_DLCI_NEED_EVEN);
+ else if(preference == RFCOMM_DLCI_NEED_ODD) start++;
+ else{
+
+ start = preference;
+ end = preference + 1;
+ step = 1;
+ }
+
+ while(start < end && (reserved & (1ULL << ((uint64_t)start)))) start += step;
+
+ if(start >= end) return 0; //we failed
+
+ reserved |= (1ULL << ((uint64_t)start));
+
+ return start;
+}
+
+void btRfcommReleaseDlci(uint8_t dlci){
+
+ reserved &=~ (1ULL << ((uint64_t)dlci));
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+static const uint8_t crctab[256] = {
+ 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
+};
+
+uint8_t btRfcommFcs(uint8_t *data, uint32_t len){ //calculate fcs value for RFCOMM
+
+ uint32_t i;
+ uint8_t crcval = 0xFF;
+
+ for (i = 0; i < len; i++) crcval = crctab[crcval ^ (*data++)];
+
+ return 0xFF - crcval;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-25 07:24:35 +0000