RFCOMM.cpp

00001 #include "mbed.h"
00002 #include "Utils.h"
00003 #include "RFCOMM.h"
00004 
00005 // Control field values      bit no.       1 2 3 4 5   6 7 8
00006 #define BT_RFCOMM_SABM       0x3F       // 1 1 1 1 P/F 1 0 0
00007 #define BT_RFCOMM_UA         0x73       // 1 1 0 0 P/F 1 1 0
00008 #define BT_RFCOMM_DM         0x0F       // 1 1 1 1 P/F 0 0 0
00009 #define BT_RFCOMM_DM_PF      0x1F
00010 #define BT_RFCOMM_DISC       0x53       // 1 1 0 0 P/F 0 1 0
00011 #define BT_RFCOMM_UIH        0xEF       // 1 1 1 1 P/F 1 1 1
00012 #define BT_RFCOMM_UIH_PF     0xFF
00013 
00014 // Multiplexer message types
00015 #define BT_RFCOMM_PN_CMD     0x83
00016 #define BT_RFCOMM_PN_RSP     0x81
00017 #define BT_RFCOMM_TEST_CMD   0x23
00018 #define BT_RFCOMM_TEST_RSP   0x21
00019 #define BT_RFCOMM_FCON_CMD   0xA3
00020 #define BT_RFCOMM_FCON_RSP   0xA1
00021 #define BT_RFCOMM_FCOFF_CMD  0x63
00022 #define BT_RFCOMM_FCOFF_RSP  0x61
00023 #define BT_RFCOMM_MSC_CMD    0xE3
00024 #define BT_RFCOMM_MSC_RSP    0xE1
00025 #define BT_RFCOMM_RPN_CMD    0x93
00026 #define BT_RFCOMM_RPN_RSP    0x91
00027 #define BT_RFCOMM_RLS_CMD    0x53
00028 #define BT_RFCOMM_RLS_RSP    0x51
00029 #define BT_RFCOMM_NSC_RSP    0x11
00030 
00031 // FCS calc
00032 #define BT_RFCOMM_CODE_WORD         0xE0 // pol = x8+x2+x1+1
00033 #define BT_RFCOMM_CRC_CHECK_LEN     3
00034 #define BT_RFCOMM_UIHCRC_CHECK_LEN  2
00035 
00036 #define NR_CREDITS  1
00037 #define INITIAL_CREDITS 1 //0...7
00038 
00039 #define DCD (1<<7) //DV data carrier detect
00040 #define RI  (1<<6) //IC ring indicator
00041 #define RTS (1<<3) //RTR request to send
00042 #define DSR (1<<2) //RTC data set ready
00043 #define FC  (1<<1) //Flow Control
00044 #define EA  (1<<0) //extended address (always 1)
00045 #define SIGNALS (DCD | RTS | DSR | EA)
00046 
00047 #define DEBUG   1
00048 #define TAKE_INITIATIVE
00049 
00050 RFCOMMManager rfcomm_manager;
00051 
00052 //uint8_t rfcomm_out_buffer[1000];//seems a bit big as default max framesize is 127
00053 //unsigned rfcomm::maxframesize = MAX_FRAME_SIZE; //only initial value
00054 
00055 //these functions are obtained from rfcomm.c on google code
00056 void _bt_rfcomm_send_sabm(unsigned short source_cid, unsigned char initiator, unsigned char channel);
00057 void _bt_rfcomm_send_uih_pn_command(unsigned short source_cid, unsigned char initiator, unsigned char channel, unsigned short max_frame_size);
00058 void _bt_rfcomm_send_uih_msc_cmd(unsigned short source_cid, unsigned char initiator, unsigned char channel, unsigned char signals);
00059 void _bt_rfcomm_send_uih_rpn_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, port_settings *val);
00060 int rfcomm_send_packet(unsigned short source_cid, unsigned char address, unsigned char control, unsigned char credits, const unsigned char *data, unsigned short len);
00061 uint8_t crc8_calc(uint8_t *data, uint16_t len);
00062 
00063 
00064 //find a free socket slot for channel ch
00065 int rfcomm::find_slot(unsigned ch) {
00066     for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
00067         if (sckts[i] != 0) { //socket is in use
00068             RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
00069             if (s==0) {
00070                 printf("find_slot: socket  %d not found\n", sckts[i]);
00071                 continue;
00072             }
00073             if (s->dlci >> 1 == ch) {
00074                 printf("Channel %d is already in use on socket %d\n", ch, sckts[i]);
00075                 return -1;
00076             }
00077         } else //slot is free
00078             return i;
00079     }
00080     return -2; //no free slots
00081 }
00082 
00083 //find the rfcomm socket for dlci
00084 RFCOMMSocket* rfcomm::find_socket(unsigned dlci) {
00085     for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
00086         if (sckts[i] != 0) { //socket is in use
00087             RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
00088             if (s==0) {
00089                 printf("find_socket: socket  %d not found\n", sckts[i]);
00090                 continue;
00091             }
00092             if (s->dlci == dlci) {
00093                 return s;
00094             }
00095         }
00096     }
00097     printf("RFCOMMSocket for dlci %d was not found!\n", dlci);
00098     return 0; //socket not found
00099 }
00100 
00101 //send a PN command to all sockets waiting to be opened
00102 void rfcomm::initChannels(int socket) {
00103     for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
00104         if (sckts[i] != 0) { //socket is in use
00105             RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
00106             if (s==0) {
00107                 printf("initChannels: socket  %d not found\n", sckts[i]);
00108                 continue;
00109             }
00110             if (s->State == SocketState_Opening) {
00111                 printf("Sending PN for DLCI %d on socket %d\n", s->dlci, sckts[i]);
00112                 _bt_rfcomm_send_uih_pn_command(socket, 1, s->dlci, maxframesize);
00113                 s->State = SocketState_L2CAP_Config_wait;
00114             }
00115         }
00116     }
00117 }
00118 
00119 unsigned rfcomm::release_channel(unsigned dlci) {
00120     int n = 0;
00121     for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
00122         if (sckts[i] != 0) { //socket is in use
00123             RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
00124             if (s==0) {
00125                 printf("Release: socket for dlci %d not found\n", dlci);
00126                 continue;
00127             }
00128             if (s->dlci == dlci)
00129                 sckts[i] = 0;
00130             else
00131                 n++;
00132         }
00133     }
00134     return n;
00135 }
00136 
00137 int rfcomm::Send(SocketInternal *sock, const u8* data, int len) {//also see if credits need to be send
00138     RFCOMMSocket *s = (RFCOMMSocket*)sock;
00139     char credits = 0;
00140     char control = BT_RFCOMM_UIH;
00141     if (len + 14 > maxframesize) //hci/l2cap header =8, rfcomm header ~ 6
00142         printf("Error! packetsize = %d, maxframesize = %d\n", len, maxframesize);
00143     if (s->peer_credits == 0) {//peer is low on credits
00144         credits = NR_CREDITS;
00145         control = BT_RFCOMM_UIH_PF;
00146         s->peer_credits += NR_CREDITS;//so provide some more
00147     }
00148     unsigned char address = (1 << 0) | (initiator << 1) | (s->dlci << 2);
00149     if (s->my_credits) {
00150         s->my_credits--;
00151         return rfcomm_send_packet(_l2cap, address, control, credits, data, len);
00152     } else
00153         return rfcomm_send_packet(_l2cap, address, control, credits, data, 0);//send an empty packet when credits run out
00154 }
00155 
00156 int rfcomm::Close(SocketInternal* sock) {
00157     RFCOMMSocket *s = (RFCOMMSocket*)sock;
00158     int id = s->dlci;
00159     printf("Closing rfcomm dlci %d state=%d\n", id, s->State);
00160     Disconnect(s);
00161     int n = release_channel(id);
00162     printf("%d channels are still open\n", n);
00163     if (n == 0) {//all rfcomm channels are closed
00164         printf("....L2CAP must be closed as well\n");
00165         rfcomm_send_packet(_l2cap, (1 << 0) | (initiator << 1), BT_RFCOMM_DISC, 0, 0, 0); //close dlci0
00166         Socket_Close(_l2cap);
00167         _l2cap = 0; //return rfcomm to the pool
00168     }
00169     return 0;
00170 }
00171 
00172 int rfcomm::SetOpt(SocketInternal *sock, int so, int* data, int len) {
00173   switch (so) {
00174     case SO_RECBUF:
00175     case SO_SNDBUF:
00176        maxframesize = *data; //pointless because setting takes effect only before socket is opened (before PN)
00177        break;
00178     default: return NOPROTOOPT;
00179   }
00180   return 0;
00181 }
00182 
00183 int rfcomm::GetOpt(SocketInternal *sock, int so, int* data, int len) {
00184   switch (so) {
00185     case SO_RECBUF:
00186     case SO_SNDBUF:
00187          if (len >= sizeof(int))
00188            *data = maxframesize;
00189          break;
00190     default: return NOPROTOOPT;
00191   }
00192   return 0;
00193 }
00194 
00195 int rfcomm::Disconnect(RFCOMMSocket *s) {
00196     unsigned char address = (1 << 0) | (initiator << 1) | (s->dlci << 2);
00197     return rfcomm_send_packet(_l2cap, address, BT_RFCOMM_DISC, 0, 0, 0);
00198 }
00199 
00200 //expect this to be called with socket type=SOCKET_RFCOM and addr->psm = channel and addr->bdaddr is the BT addr
00201 //of the device to connect to.
00202 //eg. Socket_Open(SOCKET_RFCOM, rfcommaddr(bdaddr, chan), receiver_func, appl_obj);
00203 int rfcomm::Open(SocketInternal* sock, SocketAddrHdr* addr) {
00204     int ch = ((L2CAPAddr*)addr)->psm;//abused psm for channel ID
00205     RFCOMMSocket *s = (RFCOMMSocket*)sock;
00206     int slot = find_slot(ch);
00207     if (slot < 0) return 0;
00208     sckts[slot] = s->ID;
00209     s->serdevice = this;
00210     s->State = SocketState_Opening;
00211 
00212     if (_l2cap == 0) { //no rfcomm -> l2cap connection yet
00213         printf("Need to open L2CAP channel first before opening RFCOMM channel %d\n", s->dlci);
00214         ((L2CAPAddr*)addr)->psm = L2CAP_PSM_RFCOMM;//open the l2cap channel and the rfcomm_ch channel
00215         initiator = 1;
00216         s->dlci = (ch<<1)|!initiator; 
00217         _l2cap = Socket_Open(SOCKET_L2CAP, addr, OnRfCommControl, this);//this is the socket between the RFCOMM and the L2CAP layer
00218         if (_l2cap)
00219             printf("Successfully opened L2CAP channel on socket %d\n", _l2cap);
00220         else {
00221             printf("Opening L2CAP channel failed\n");
00222             sckts[slot] = 0;
00223             s->State = SocketState_Closed;
00224             return 0;
00225         }
00226     } else {//bypass the l2cap channel creation
00227         s->dlci = (ch<<1)|!initiator; 
00228         _bt_rfcomm_send_uih_pn_command(_l2cap, initiator, s->dlci, maxframesize);
00229         s->State = SocketState_L2CAP_Config_wait;
00230     }
00231     return s->ID; //return the application unique socket nr.
00232 }
00233 
00234 int rfcomm::set_remote_port_parameters(unsigned char dlci, port_settings *p) {
00235     _bt_rfcomm_send_uih_rpn_cmd(_l2cap, initiator, dlci, p);
00236     return 0;
00237 }
00238 
00239 //socket is an L2CAP socket and state is the state of the L2CAP socket, not the RFCOMM socket
00240 void rfcomm::OnRfCommControl(int socket, SocketState state, const u8* data, int len, void* userData) {
00241     int packet_processed = 0;
00242     rfcomm* self = (rfcomm*)userData;
00243     const u8 initiator = self->initiator;
00244     printf("\x1B[%dm", 32); //debug: set a different colour
00245     printf("OnRfCommControl sock = %d, state = %d, length = %d\n", socket, state, len);
00246 
00247     if (len == 0) {//client only
00248         if (state==SocketState_Open) {//callback after change to 'open', the rfcomm->l2cap channel is now open
00249             _bt_rfcomm_send_sabm(socket, initiator, 0); //setup the rfcomm control channel dlci==0
00250             return;
00251         }
00252         return; //or other states to handle, e.g. Closing or Closed
00253     }
00254     //we have data, so parse the header
00255     const u8 &addr = data[0];
00256     u8 dlci = addr>>2;
00257     const u8 &control = data[1];
00258     u16 length = data[2]>>1;
00259     const u8 *payload = data+3;
00260     const u8 *pFCS = data+len-1; //expected position of the CRC
00261     if (!(data[2]&1)) { //two byte length
00262         length += ((unsigned)data[3])<<7;
00263         payload++;
00264     }
00265     u8 credits = 0;
00266     if (control == BT_RFCOMM_UIH_PF)//this packet carries credits
00267         credits = *(payload++);
00268     //sanity check
00269     if (payload+length != pFCS)
00270         printf("RFCOMM size mismatch, expected %d payload bytes, got %d\n", length, pFCS-payload);
00271 
00272     if (DEBUG) {
00273         printf("RFCOMM: EA=%d, C/R=%d, D=%d, ch=%d; control=%02X (P/F=%d); length=%d\n", addr&1, (addr>>1)&1, (addr>>2)&1, (addr>>3), control, (control>>4)&1, length);
00274         printfBytes("payload:", payload, length);
00275     }
00276     if (dlci == 0) { //dlci==0 control channel
00277         L2CAPSocket *s = (L2CAPSocket*)GetSocketInternal(socket);
00278         switch (control) {
00279             case BT_RFCOMM_UA://     received 1. message BT_RF_COMM_UA
00280                 packet_processed++;
00281                 if (s->si.State == SocketState_Closing || s->si.State==SocketState_L2CAP_WaitDisconnect) { //Confirmation of disconnect
00282                     printf("Remote side confirmed disconnect for socket %d\n", s->si.ID);
00283                     s->si.SetState(SocketState_Closed);
00284                     break;
00285                 }
00286                 printf("Received RFCOMM unnumbered acknowledgement for channel 0 - multiplexer working\n");
00287                 printf("Sending UIH Parameter Negotiation Command from OnRfCommControl\n");
00288                 self->initChannels(socket);
00289                 break;
00290             case BT_RFCOMM_UIH://  received UIH Parameter Negotiation Response
00291                 switch (payload[0]) {
00292                     case BT_RFCOMM_PN_RSP: {//client
00293                         packet_processed++;
00294                         printf("UIH Parameter Negotiation Response\n");
00295                         printf("Sending SABM #%u\n", payload[2]);
00296                         _bt_rfcomm_send_sabm(socket, initiator, payload[2]);//was rfcomm_ch
00297                         RFCOMMSocket *r = self->find_socket(payload[2]);
00298                         if (r==0) break;
00299                         r->my_credits = payload[9]; //initial amount of credits
00300                         self->maxframesize = min(self->maxframesize, payload[6] + (payload[7]<<8));
00301                         printf("Max Frame Size = %d, got %d initial credits\n", self->maxframesize, payload[9]);
00302                     }
00303                     break;
00304                     case BT_RFCOMM_PN_CMD: { //remote side sent PN command, mtu and initial credits
00305                         packet_processed++;
00306                         printf("UIH Parameter Negotiation Indication\n");
00307                         self->maxframesize = min(self->maxframesize, payload[6] + (payload[7]<<8));
00308                         unsigned char cred = payload[9] & 7;
00309                         unsigned char _dlci = payload[2];
00310 
00311                         int skt = rfcomm_manager.find_socket(_dlci>>1);
00312                         if (skt == 0) { //No-one is listening
00313                             printf("No-one is Listening on channel %d\n", _dlci>>1);
00314                             rfcomm_send_packet(socket, (_dlci<<2)|1, BT_RFCOMM_DM, 0, 0, 0);
00315                             break;
00316                         }
00317                         RFCOMMSocket *r = (RFCOMMSocket*)GetSocketInternal(skt);
00318                         r->my_credits = cred;
00319                         r->peer_credits = INITIAL_CREDITS;
00320                         unsigned char reply[10];
00321                         memcpy(reply, payload, sizeof(reply));
00322                         reply[0] = BT_RFCOMM_PN_RSP;//[1]=len, [2]=dlci, [4]=priority, [5]=timer(must be 0), [8] retransmissions (must be 0)
00323                         reply[3] = payload[3]==0xF0 ? 0xE0 : 0; //support credit based flow control
00324                         reply[6] = self->maxframesize;
00325                         reply[7] = self->maxframesize>>8;
00326                         reply[9] = payload[3]==0xF0 ? r->peer_credits : 0;
00327                         printf("Max Frame Size = %d, give %d initial credits\n", self->maxframesize, reply[9]);
00328                         rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, sizeof(reply));
00329                     }
00330                     break;
00331                     case BT_RFCOMM_MSC_CMD:
00332                         packet_processed++;
00333                         {
00334                             printf("Received BT_RFCOMM_MSC_IND\n");
00335                             // fine with this, return the same status and ignore the value, there is no room in the socket to store the value, rfcomm could generate an event
00336                             RFCOMMSocket *r = self->find_socket(payload[2]>>2);
00337                             if (r==0) break;
00338                             unsigned char reply[5];
00339                             memcpy(reply, payload, 5); //keep length, dlci and value(s)
00340                             reply[0]  = BT_RFCOMM_MSC_RSP;  // change command into response
00341                             printf("Sending MSC_RSP (%d bytes)\n", length);
00342                             rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, (payload[1]>>1)+2); // reply is 4 or 5 bytes
00343                             switch (r->State) {
00344                                 case SocketState_L2CAP_Config_wait:
00345                                     r->State = SocketState_L2CAP_Config_wait_send;
00346                                     printf("Sending MSC_CMD\n");
00347                                     _bt_rfcomm_send_uih_msc_cmd(socket, initiator, payload[2]>>2, SIGNALS);  // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
00348                                     r->State = SocketState_L2CAP_Config_wait_rsp;
00349                                     break;
00350                                 case SocketState_L2CAP_Config_wait_reqrsp:
00351                                     r->State = SocketState_L2CAP_Config_wait_rsp;
00352                                     break;
00353                                 case SocketState_L2CAP_Config_wait_req:
00354                                     r->SetState(SocketState_Open);
00355                                     break;
00356                                 case SocketState_Open:
00357                                     //inform port adaptation layer
00358                                     printf("Received MSC IND in state Open for dlci 0x%02x\n", payload[2]>>2);
00359                                     break;
00360                                 default:
00361                                     printf("Received MSC IND in state %d for dlci 0x%02x\n", r->State, payload[2]>>2);
00362                             }
00363                         }
00364                         break;
00365                     case BT_RFCOMM_MSC_RSP:
00366                         packet_processed++;
00367                         {
00368                             RFCOMMSocket *r = self->find_socket(payload[2]>>2);
00369                             if (r==0) break;
00370                             if (r->State == SocketState_L2CAP_Config_wait_reqrsp)
00371                                 r->State =  SocketState_L2CAP_Config_wait_req;
00372                             else if (r->State == SocketState_L2CAP_Config_wait_rsp)
00373                                 r->SetState(SocketState_Open);
00374                             else
00375                                 printf("Received MSC confirmation in state %d for dlci 0x%02x\n", r->State, payload[2]>>2);
00376                         }
00377                         break;
00378                     case BT_RFCOMM_RPN_CMD:
00379                         packet_processed++;
00380                         //accept and ignore all settings
00381                         unsigned char reply[10];
00382                         memcpy(reply, payload, length); //keep length, dlci and value(s)
00383                         reply[0]  = BT_RFCOMM_RPN_RSP;  // change command into response
00384                         printf("Responding to RPN indication (%d bytes)\n", length);
00385                         rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, length);
00386                         break;
00387                     case BT_RFCOMM_RPN_RSP:
00388                         packet_processed++;
00389                         //ignore a response
00390                         printf("Received RPN confirmation\n");
00391                         break;
00392                     default:
00393                         printf("Unsupported multiplexer frame, type=%02XH\n", data[3]);
00394                 }
00395                 break;
00396             case BT_RFCOMM_DISC:
00397                 printf("Remote site actively disconnected from DLCI0\n");
00398                 rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm disconnection
00399                 //intentional fall through
00400             case BT_RFCOMM_DM:
00401                 packet_processed++;
00402                 printf("Remote side refused connection on DLCI0\n");
00403                 self->_l2cap = Socket_Close(socket);
00404                 break;
00405             case BT_RFCOMM_SABM:
00406                 packet_processed++;
00407                 printf("Remote site is seeking connection on DLCI0\n"); //respond with UA
00408                 rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm connection
00409                 break;
00410             default:
00411                 printf("Unexpected RFCOMM cmd %02XH for address %02XH, length=%d\n", control, addr, length);
00412         }
00413     } else { //data is for one of the serial sockets
00414         RFCOMMSocket *s = 0;
00415         if (control == BT_RFCOMM_SABM) { //req. for conn on this dlci
00416             //cannot call self->rfcomm::find_socket because it has no socket yet
00417             int skt = rfcomm_manager.find_socket(dlci>>1); //find the server socket
00418             s = (RFCOMMSocket*)GetSocketInternal(skt);//the listening socket
00419             if (s) {//move the listening socket to the appropriate rfcomm
00420                 int slot = self->find_slot(dlci>>1);
00421                 if (slot < 0) {
00422                     printf("RFCOMM Channel %d is not free on rfcomm with l2cap socket %d\n", dlci>>1, self->_l2cap);
00423                     return;
00424                 }
00425                 s->serdevice = self; //bind the socket to this refcomm entity
00426                 self->sckts[slot] = skt;
00427                 rfcomm_manager.remove_socket(skt);
00428             } else {
00429                 printf("Couln't find a listening socket for dlci %d\n", dlci);
00430                 return;
00431             }
00432         } else
00433             s = self->find_socket(dlci);
00434         if (s==0){
00435           printf("DLCI %d not found\n", dlci);
00436           return;
00437         }
00438         switch (control) {
00439             case BT_RFCOMM_SABM:
00440                 packet_processed++;
00441                 rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm connection
00442                 s->State = SocketState_L2CAP_Config_wait; //wait for msc cmd
00443 #ifdef TAKE_INITIATIVE
00444                   printf("Sending MSC_CMD\n");
00445                   _bt_rfcomm_send_uih_msc_cmd(socket, initiator, dlci, 0x8d);  // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
00446                   s->State = SocketState_L2CAP_Config_wait_reqrsp;
00447 #endif
00448                 break;
00449             case BT_RFCOMM_UA://     received 2. message BT_RF_COMM_UA
00450                 packet_processed++;
00451                 if (s->State == SocketState_Closing) { //Confirmation of disconnect
00452                     printf("Remote side confirmed disconnect for socket %d\n", s->ID);
00453                     s->SetState(SocketState_Closed);
00454                     break;
00455                 }
00456                 printf("Received RFCOMM unnumbered acknowledgement for dlci %u - channel opened\n", dlci);
00457                 if (s->State == SocketState_L2CAP_Config_wait) {
00458                     printf("Sending MSC  CMD\n");
00459                     _bt_rfcomm_send_uih_msc_cmd(socket, initiator, dlci, 0x8d);  // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
00460                     s->State = SocketState_L2CAP_Config_wait_reqrsp;
00461                 }
00462                 break;
00463             case BT_RFCOMM_UIH_PF: //user data with credits
00464                 printf("Got %u credits\n", credits);
00465                 s->my_credits += credits;
00466                 //intentional fall-through
00467             case BT_RFCOMM_UIH: //user data
00468                 packet_processed++;
00469                 if (DEBUG) {
00470                     printf("RX: address %02x, control %02x: ", addr, control);
00471                     printHex( payload, length);
00472                 }
00473                 if (length) {
00474                     s->peer_credits--;
00475                     s->Recv(payload, length);
00476                 } else
00477                     printf("Received empty packet\n");
00478                 if (length == 0 || s->peer_credits == 0) {//send credits when peer runs out
00479                     //char ini = !(dlci & 1);
00480                     unsigned char address = (1 << 0) | (initiator << 1) | (dlci << 2);
00481                     printf("send %d credits to dlci %d\n", NR_CREDITS, addr>>2);
00482                     rfcomm_send_packet(socket, address, BT_RFCOMM_UIH_PF, NR_CREDITS, NULL, 0);
00483                     s->peer_credits += NR_CREDITS;
00484                 }
00485                 break;
00486             case BT_RFCOMM_DISC:
00487                 packet_processed++;
00488                 printf("Received DISC IND for dlci %d\n", dlci);
00489                 rfcomm_send_packet(socket, addr, BT_RFCOMM_UA, 0, 0, 0);//confirm disconnection
00490                 s->SetState(SocketState_Closed);                
00491                 break;
00492             case BT_RFCOMM_DM:
00493             case BT_RFCOMM_DM_PF:
00494                 packet_processed++;
00495                 printf("Received DM IND (%02X) for dlci %d\n", control, dlci);
00496                 s->SetState(SocketState_Closed);
00497                 break;
00498             default:
00499                 printf("Unexpected RFCOMM cmd %02XH for address %02XH, length=%d\n", control, addr, length);
00500         }
00501     }
00502 
00503     if (!packet_processed) {
00504         // just dump data for now
00505         printf("??: address %02x, control %02x: ", data[0], data[1]);
00506         printHex( data, len );
00507     }
00508     printf("\x1B[%dm", 0);//reset terminal colour
00509 }
00510 
00511 //should make the functions below member functions
00512 
00513 /**
00514  * @param credits - only used for RFCOMM flow control in UIH wiht P/F = 1
00515  */
00516 /* Questionable optimisation. When OFFSET==8 a buffer is (dynamically) allocated that provides space for the lower
00517    layer headers, this reduces copying to, and allocation of, a lower layer buffer. However, all other layers using
00518    HCI/L2CAP must do the same.
00519 */
00520 #define OFFSET  8
00521 
00522 int rfcomm_send_packet(uint16_t source_cid, uint8_t address, uint8_t control, uint8_t credits, const uint8_t *data, uint16_t len) {
00523 
00524     uint16_t pos = OFFSET;
00525     uint8_t crc_fields = 3;
00526 
00527 #if OFFSET == 8
00528     uint8_t* rfcomm_out_buffer = new uint8_t[OFFSET+len+6];
00529 #else
00530     static uint8_t rfcomm_out_buffer[MAXFRAMESIZE+6];//seems a bit big as default max framesize is 127
00531 #endif
00532     rfcomm_out_buffer[pos++] = address;
00533     rfcomm_out_buffer[pos++] = control;
00534 
00535     // length field can be 1 or 2 octets
00536     if (len < 128) {
00537         rfcomm_out_buffer[pos++] = (len << 1)| 1;     // bits 0-6
00538     } else {
00539         rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6
00540         rfcomm_out_buffer[pos++] = len >> 7;          // bits 7-14
00541         crc_fields++;
00542     }
00543 
00544     // add credits for UIH frames when PF bit is set
00545     if (control == BT_RFCOMM_UIH_PF) {
00546         rfcomm_out_buffer[pos++] = credits;
00547     }
00548 
00549     // copy actual data
00550     memcpy(&rfcomm_out_buffer[pos], data, len);
00551     pos += len;
00552 
00553     // UIH frames only calc FCS over address + control (5.1.1)
00554     if ((control & 0xef) == BT_RFCOMM_UIH) {
00555         crc_fields = 2;
00556     }
00557     rfcomm_out_buffer[pos++] =  crc8_calc(rfcomm_out_buffer+OFFSET, crc_fields); // calc fcs
00558     int retval = Socket_Send( source_cid, rfcomm_out_buffer, pos);
00559 #if OFFSET == 8
00560     delete[] rfcomm_out_buffer;
00561 #endif
00562     if (retval <= 0)
00563         return retval;
00564     return len;//return the size of the payload
00565 }
00566 
00567 void _bt_rfcomm_send_sabm(uint16_t source_cid, uint8_t initiator, uint8_t dlci) {
00568     uint8_t address = (1 << 0) | (initiator << 1) |  (dlci << 2);
00569     rfcomm_send_packet(source_cid, address, BT_RFCOMM_SABM, 0, NULL, 0);
00570 }
00571 
00572 void _bt_rfcomm_send_uih_pn_command(uint16_t source_cid, uint8_t initiator, uint8_t dlci, uint16_t max_frame_size) {
00573     uint8_t payload[10];
00574     uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
00575     uint8_t pos = 0;
00576     payload[pos++] = BT_RFCOMM_PN_CMD;
00577     payload[pos++] = 8 << 1 | 1;  // len
00578     payload[pos++] = dlci;
00579     payload[pos++] = 0xf0; // pre defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
00580     payload[pos++] = 0; // priority
00581     payload[pos++] = 0; // max 60 seconds ack
00582     payload[pos++] = max_frame_size & 0xff; // max framesize low
00583     payload[pos++] = max_frame_size >> 8;   // max framesize high
00584     payload[pos++] = 0x00; // number of retransmissions
00585     payload[pos++] = INITIAL_CREDITS; // unused error recovery window
00586     rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
00587 }
00588 
00589 void _bt_rfcomm_send_uih_data(uint16_t source_cid, uint8_t initiator, uint8_t channel, uint8_t *data, uint16_t len) {
00590     uint8_t address = (1 << 0) | (initiator << 1) |  (!initiator << 2) | (channel << 3);
00591     rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, data, len);
00592 }
00593 
00594 void _bt_rfcomm_send_uih_msc_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, uint8_t signals) {
00595     uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
00596     uint8_t payload[5];
00597     uint8_t pos = 0;
00598     payload[pos++] = BT_RFCOMM_MSC_CMD;
00599     payload[pos++] = 2 << 1 | 1;  // len, should be adapted when adding break byte
00600     payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); //C/R is always 1
00601     payload[pos++] = signals;
00602     // payload[pos++]   = brk;
00603     rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
00604 }
00605 
00606 void _bt_rfcomm_send_uih_rpn_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, port_settings *val) {
00607     uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
00608     uint8_t payload[sizeof(port_settings)+3];
00609     uint8_t pos = 0;
00610     payload[pos++] = BT_RFCOMM_RPN_CMD;//type
00611     if (val) {
00612         payload[pos++] = ((1+sizeof(port_settings)) << 1) | 1;  // len
00613         payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2);
00614         memcpy(payload+pos, (char*)val, sizeof(port_settings));
00615         pos += sizeof(port_settings);
00616     } else {
00617         payload[pos++] = (1 << 1) | 1;  // len
00618         payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2);
00619     }
00620     rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
00621 }
00622 
00623 int set_remote_port_parameters(int socket, port_settings *p) {
00624     RFCOMMSocket* si = (RFCOMMSocket*)GetSocketInternal(socket);//gets the RFCOMM socket
00625     if (!si || si->ID != socket)
00626         return ERR_SOCKET_NOT_FOUND;
00627     return si->serdevice->set_remote_port_parameters(si->dlci, p);
00628 }
00629 
00630 
00631 /*
00632  * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0.
00633  */
00634 static const uint8_t crc8table[256] = {    /* reversed, 8-bit, poly=0x07 */
00635     0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
00636     0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
00637     0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
00638     0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
00639     0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
00640     0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
00641     0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
00642     0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
00643     0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
00644     0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
00645     0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
00646     0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
00647     0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
00648     0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
00649     0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
00650     0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
00651 };
00652 
00653 #define CRC8_INIT  0xFF          // Initial FCS value 
00654 #define CRC8_OK    0xCF          // Good final FCS value 
00655 /*-----------------------------------------------------------------------------------*/
00656 uint8_t crc8(uint8_t *data, uint16_t len) {
00657     uint16_t count;
00658     uint8_t crc = CRC8_INIT;
00659     for (count = 0; count < len; count++)
00660         crc = crc8table[crc ^ data[count]];
00661     return crc;
00662 }
00663 
00664 /*-----------------------------------------------------------------------------------*/
00665 uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) {
00666     uint8_t crc;
00667 
00668     crc = crc8(data, len);
00669 
00670     crc = crc8table[crc ^ check_sum];
00671     if (crc == CRC8_OK)
00672         return 0;               /* Valid */
00673     else
00674         return 1;               /* Failed */
00675 
00676 }
00677 
00678 /*-----------------------------------------------------------------------------------*/
00679 uint8_t crc8_calc(uint8_t *data, uint16_t len) {
00680     /* Ones complement */
00681     return 0xFF - crc8(data, len);
00682 }