示例#1
0
文件: btL2CAP.c 项目: Bhuva28/xda-adk
static void l2capHandleControlChannel(uint16_t conn, const uint8_t* data, uint16_t size){

    char rejectCommand = 0;
    uint16_t rejectReason = L2CAP_REJECT_REASON_WTF;
    Service* s = services;
    Connection* c;
    uint8_t cmd, id;
    uint8_t buf[16];
    uint16_t chan, remChan, len;

    if(!gL2capID) gL2capID++;

    if(size < 2){

        rejectCommand = 1;
        dbgPrintf("L2CAP: control packet too small\n");
    }
    else{

        cmd = *data++;
        id = *data++;
        len = getLE16(data);
        data += 2;
        size -= 4;

        if(len != size){

            dbgPrintf("L2CAP (control packet internal sizes mismatch (%u != %u)\n", len, size);
            rejectCommand = 1;
        }
        else switch(cmd){

            case L2CAP_CMD_CONN_REQ:{

                uint16_t PSM;

                //get some request data
                if(size != 4){

                    dbgPrintf("L2CAP: ConnectionRequest packet size is wrong(%u)\n", size);
                    rejectCommand = 1;
                    break;
                }

                PSM = getLE16(data + 0);
                remChan = getLE16(data + 2);

                //init the reply
                buf[0] = L2CAP_CMD_CONN_RESP;
                buf[1] = id;
                putLE16(buf + 2, 8);		//length
                putLE16(buf + 4, 0);		//DCID
                putLE16(buf + 6, remChan);	//SCID
                putLE16(buf + 10, 0);		//no further information

                //find the service
                while(s && s->PSM != PSM) s = s->next;
                if(!s || !(s->descr.flags & L2CAP_FLAG_SUPPORT_CONNECTIONS)){

                    dbgPrintf("L2CAP: rejecting conection to unknown PSM 0x%04X\n", PSM);
                    putLE16(buf + 8, L2CAP_CONN_FAIL_NO_SUCH_PSM);
                }
                else{

                    void* instance = NULL;

                    chan = 0;
                    c = malloc(sizeof(Connection));
                    if(c) chan = l2capFindFreeLocalChannel(conn);
                    if(chan) instance = s->descr.serviceInstanceAllocate(conn, chan, remChan);

                    if(instance){

                        putLE16(buf + 4, chan);
                        putLE16(buf + 8, L2CAP_CONN_SUCCESS);

                        c->service = s;
                        c->serviceInstance = instance;
                        c->conn = conn;
                        c->chan = chan;
                        c->remChan = remChan;
                        c->next = connections;
                        connections = c;
                    }
                    else{

                        putLE16(buf + 8, L2CAP_CONN_FAIL_RESOURCES);

                        if(c) free(c);
                    }
                }
                size = 12;
                break;
            }

            case L2CAP_CMD_CONFIG_REQ:{

                uint16_t flags;

                //get some request data
                if(size < 4){
                    dbgPrintf("L2CAP: ConfigurationRequest packet size is wrong(%u)\n", size);
                    rejectCommand = 1;
                    break;
                }

                chan = getLE16(data + 0);
                flags = getLE16(data + 2);
                if(flags & 1){ //flags continue - we do not support that

                    size = 0;
                    break;
                }
                size -= 4;
                data += 4;

                //locate the connection at hand
                c = l2capFindConnection(conn, chan, NULL);
                if(!c){
                    dbgPrintf("L2CAP: ConfigurationRequest for an unknown channel %u.%u\n", conn, chan);
                    rejectCommand = 1;
                    break;
                }
                chan = c->remChan;

                //reply with just our rx-MTU
                buf[0] = L2CAP_CMD_CONFIG_RESP;	
                buf[1] = id;
                putLE16(buf + 2, 10);			//length
                putLE16(buf + 4, c->remChan);		//SCID
                putLE16(buf + 6, 0);			//flags
                putLE16(buf + 8, 0);			//success
                buf[10] = L2CAP_OPTION_MTU;		//mtu_property.type
                buf[11] = 2;				//mtu_property.len
                putLE16(buf + 12, BT_RX_BUF_SZ - 8);	//mtu value
                l2capSendControlRawBuf(conn, buf, 14);	//send it
                
                //we need to send such a packet there too
                buf[0] = L2CAP_CMD_CONFIG_REQ;		//configuration request
                buf[1] = gL2capID++;
                putLE16(buf + 2, 4);			//length
                putLE16(buf + 4, c->remChan);		//SCID
                putLE16(buf + 6, 0);			//flags
                size = 8;
                break;
            }

            case L2CAP_CMD_CONFIG_RESP:{

                //we do nothing here - perhaps we should?
                size = 0;
                break;
            }

            case L2CAP_CMD_DISC_REQ:{

                Connection* p;

                //get some request data
                if(size != 4){

                    dbgPrintf("L2CAP: DisconnectionRequest packet size is wrong(%u)\n", size);
                    rejectCommand = 1;
                    break;
                }
                chan = getLE16(data + 0);
                remChan = getLE16(data + 2);
                c = l2capFindConnection(conn, chan, &p);

                //handle some likely error cases
                if(!c){
                    dbgPrintf("L2CAP: DisconnectionRequest for an unknown channel %u.%u\n", conn, chan);
                    rejectReason = L2CAP_REJECT_REASON_INVALID_CID;
                    rejectCommand = 1;	//reject as per spec
                    break;
                }
                if(c->remChan != remChan){
                    dbgPrintf("L2CAP: DisconnectionRequest for an unmatched channel on %u.%u (%u != %u)\n", conn, chan, c->remChan, remChan);
                    size = 0;		//drop as per spec
                    break;
                }

                //perform needed cleanup
                l2capConnectionCloseEx(c, p, 0);

                //send the reply
                buf[0] = L2CAP_CMD_DISC_RESP;	//disconnection response
                buf[1] = id;			//copied as required
                putLE16(buf + 2, 4);		//length
                putLE16(buf + 4, chan);		//DCID
                putLE16(buf + 6, remChan);	//SCID
                size = 8;
                break;
            }

            case L2CAP_CMD_DISC_RESP:{

                //nothing to do - we did cleanup when we requested the connection closure...
                size = 0;
                break;
            }

            case L2CAP_CMD_ECHO_REQ:{

                buf[0] = L2CAP_CMD_ECHO_RESP;	//ping response
                buf[1] = id;			//copied as required
                putLE16(buf + 2, 0);		//0-length replies are ok
                size = 4;
                break;
            }

            case L2CAP_CMD_INFO_REQ:{

                uint16_t info;

                //get some request data
                if(size != 2){

                    dbgPrintf("L2CAP: InformationRequest packet size is wrong(%u)\n", size);
                    rejectCommand = 1;
                    break;
                }
                info = getLE16(data + 0);

                buf[0] = L2CAP_CMD_INFO_RESP;	//information response
                buf[1] = id;			//copied as required
                putLE16(buf + 4, info);		//info type
                if(info == 1){	//connectionless mtu

                    putLE16(buf + 6, 0);	//success
                    putLE16(buf + 8, BT_RX_BUF_SZ - 8);
                    putLE16(buf + 2, 6);	//length
                    size = 10;
                }
                else if(info == 2){ //extended features

                    putLE16(buf + 6, 0);	//success
                    putLE16(buf + 8, 0); 
                    putLE16(buf + 10, 0); 
                    putLE16(buf + 2, 8);	//length
                    size = 12;
                }
                else{		//whatever this request is, we do not support it

                    putLE16(buf + 6, 1);	//info type not supported
                    putLE16(buf + 2, 4);	//length
                    size = 8;
                }
                break;
            }

            default:{

                dbgPrintf("L2CAP: unexpected command 0x%02X recieved\n", cmd);
                size = 0;
            }
        }
    }

    if(rejectCommand){

        buf[0] = L2CAP_CMD_REJECT;	//disconnection response
        buf[1] = id;			//copied as required
        putLE16(buf + 2, 4);		//length
        putLE16(buf + 4, rejectReason);	//rejection reason
        size = 6;
    }

    if(size) l2capSendControlRawBuf(conn, buf, size);
}
示例#2
0
文件: btL2CAP.c 项目: Bhuva28/xda-adk
void l2capAclLinkDataRx(uint16_t conn, char first, const uint8_t* data, uint16_t size){

    uint16_t chan, len;
    unsigned i;
    char freeIt = 0;

    #if UGLY_SCARY_DEBUGGING_CODE
        dbgPrintf("L2CAP data:");
        for(chan = 0; chan < size; chan++) dbgPrintf(" %02X", data[chan]);
        dbgPrintf("\n\n");
    #endif

    if(first){

        len = getLE16(data + 0);
        chan = getLE16(data + 2);
        data += 4;
        size -= 4;

        if(size >= len){
            if(size > len) dbgPrintf("L2CAP: ACL provided likely invalid L2CAP packet (ACL.len=%u L2CAP.len=%u)\n", size, len);
        }
        else{
            for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES; i++){

                if(gIncomingPieces[i].conn == conn){

                    dbgPrintf("L2CAP: conn %d: 'first' frame while another incomplete already buffered. Dropping the old one.\n", conn);
                    free(gIncomingPieces[i].buf);
                    gIncomingPieces[i].conn = 0;
                    break;
                }
            }
            if(i == L2CAP_MAX_PIECED_MESSAGES) for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES && gIncomingPieces[i].conn; i++);
            if(i == L2CAP_MAX_PIECED_MESSAGES){

                dbgPrintf("L2CAP: not enough buffer slots to buffer incomplete frame. Dropping\n");
            }
            else{

                uint8_t* ptr = malloc(size);
                if(!ptr){

                    dbgPrintf("L2CAP: cannot allocate partial frame buffer. Dropping\n");
                    return;
                }
                memcpy(ptr, data, size);
                gIncomingPieces[i].buf = ptr;
                gIncomingPieces[i].lenGot = size;
                gIncomingPieces[i].lenNeed = len - size;
                gIncomingPieces[i].chan = chan;
                gIncomingPieces[i].conn = conn;
            }
            return;
        }
    }
    else{

        uint8_t* ptr;

        for(i = 0; i < L2CAP_MAX_PIECED_MESSAGES && gIncomingPieces[i].conn != conn; i++);
        if(i == L2CAP_MAX_PIECED_MESSAGES){
            dbgPrintf("L2CAP: unexpected 'non-first' frame for conn %u. Dropping.\n", conn);
            return;
        }

        if(size > gIncomingPieces[i].lenNeed){

            dbgPrintf("L2CAP: 'non-first' frame too large. Need %u bytes, got %u. Dropping.\n", gIncomingPieces[i].lenNeed, size);
            return;
        }

        ptr = realloc(gIncomingPieces[i].buf, gIncomingPieces[i].lenGot + size);
        if(!ptr){

            dbgPrintf("L2CAP: failed to resize buffer for partial frame receive. Droping\n");
            free(gIncomingPieces[i].buf);
            gIncomingPieces[i].conn = 0;
            return;
        }

        memcpy(ptr + gIncomingPieces[i].lenGot, data, size);
        gIncomingPieces[i].buf = ptr;
        gIncomingPieces[i].lenGot += size;
        gIncomingPieces[i].lenNeed -= size;

        if(gIncomingPieces[i].lenNeed) return; //data still not complete

        gIncomingPieces[i].conn = 0;
        chan = gIncomingPieces[i].chan;
        len = gIncomingPieces[i].lenGot;
        data = ptr;
        freeIt = 1;
    }

    if(chan == 0) dbgPrintf("L2CAP: data on connection %u.0\n", conn);
    else if(chan == 1) l2capHandleControlChannel(conn, data, len);
    else if(chan == 2){	//connectionless

        uint16_t PSM = getLE16(data + 0);
        data += 2;
        len -= 2;

        Service* s = services;

        while(s && s->PSM != PSM) s = s->next;
        if(!s || !(s->descr.flags & L2CAP_FLAG_SUPPORT_CONNECTIONLESS)) dbgPrintf("L2CAP: connectionless data on %u.2 for unknown PSM 0x%04X\n", conn, PSM);
        else s->descr.serviceRx(NULL, data, len);
    }
    else{			//conection-oriented

        Connection* c = l2capFindConnection(conn, chan, NULL);
        if(!c) dbgPrintf("L2CAP: data for nonexistent connection %u.%u\n", conn, chan);
        else c->service->descr.serviceRx(c->serviceInstance, data, len);
    }

    if(freeIt) free(data);
}
示例#3
0
int StreamBase::get16(void)
{
    return bigendian() ? getBE16() : getLE16();
}