File content as of revision 1:70ee392bcfd4:

#include "mbed.h"
#include "Utils.h"
#include "hci.h"
#include "sdp_data.h"
#include "sdp.h"

SDPManager SDP; //instance
const unsigned char base_uuid[16] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0, 0x07, 0x70, 0, 0x10, 0, 0};

void attribHandler(serv_rec *r) {
    printf("Service 0x%08X\n", (*r)[0x0000]->asUnsigned());
    map<unsigned short, sdp_data*>::iterator it = r->begin();
    for (;it != r->end();it++) {
        printf("   0x%04X: %s\n", (*it).first, (*it).second->asString());
    }
}

#define BROWSEROOT  0x1002
#define SERIALSERV  0x1101

//this function is called when the L2CAP layer receives SDP packets (see SDPManager::Open), userdata is the sdpmanager instance
//void SDPManager::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
void SDPManager::OnSdpRsp(const u8* data, int len) {
    static sdp_data list(sdp_data::SEQUENCE); 
    static sdp_data all(0x0000ffffU,4);
    static sdp_data root(sdp_data::UUID, BROWSEROOT);
    static sdp_data req(sdp_data::SEQUENCE);
    static bool once = true;
    printf("_state=%d first=%d   ", _state, once);
    if (once) {
       list.add_element(&all);
       req.add_element(&root);
       once = false;
    }
    if (data){
        parseRsp(data, len);
    }    
    switch (_state) {
        case 0: //closed
            if (len==0) { //socket just opened
                //'Open' cleared the services list
                ServiceSearchRequest(&req, 1);
                _state = 1; //wait for service handles
            }
            break;
        case 1: //service handles arriving
            if (contState[0]) {//continuation, repeat request
                ServiceSearchRequest(&req, 5);
            } else {
                if (data[0]==3) {
                    index = services.begin();
                    if (index != services.end()) {
                        unsigned handle = (*index).first;
                        printf("req.: handle %#X\n", handle);
                        ServiceAttributeRequest(handle, 100, &list);//0x1001D
                    } else 
                        printf(" - empty list - \n");//should not happen
                    _state = 2; //wait for attribute response
                } else 
                    printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]);
            }
            break;
        case 2:
            if (contState[0])//repeat request
                ServiceAttributeRequest((*index).first, 100, &list);
            else {
                if (data[0]==5) {
                    index++; //move to next service
                    if (index != services.end()) {
                        printf("req.: handle %#X\n", (*index).first);
                        ServiceAttributeRequest((*index).first, 100, &list);
                    } else {
                        printf(" - end of list - \n");
                        Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!!
                        _state = 0;
                    }
                } else 
                    printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]);
            }
            break;
    }
}

//this function is called when the SDP sockets receives data (see HCICallback in TestShell),
//currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
void SDPManager::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) {
    printf("OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len);
    printfBytes("Got SDP Response from socket: ", data, len);
}

void SDPManager::errorhandler(unsigned err) {//default error handler
    switch (err) {
        case 1:
            printf("Unsupported version of SDP\n");
            break;
        case 2:
            printf("Invalid SDP ServiceRecordHandle\n");
            break;
        case 3:
            printf("SDP syntax error\n");
            break;
        case 4:
            printf("PDU size was invalid\n");
            break;
        case 5:
            printf("Continuation state was invalid\n");
            break;
        case 6:
            printf("SDP server has insufficient resources\n");
            break;
        default:
            printf("Unknown SDP error code\n");
            break;
    }
}

int SDPManager::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) {
    int parlen = sp->Size() + contState[0] + 3;
    buf[0] = 2; //pdu
    buf[1] = txid>>8;
    buf[2] = txid++;
    buf[4] = parlen;
    buf[3] = parlen>>8;
    int p = sp->build(buf+5, 100-10);
    buf[p+6] = count;
    buf[p+5] = count>>8;
    buf[p+7] = contState[0];
    for (int j = 1; j <= contState[0]; j++)
        buf[p+j+7] = contState[j];
    //printfBytes("SDP Send: ", buf, parlen+5);
    return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
}

int SDPManager::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) {
    int parlen = al->Size() + contState[0] + 7;
    buf[0] = 4; //pdu
    buf[1] = txid>>8;
    buf[2] = txid++;
    buf[4] = parlen;
    buf[3] = parlen>>8;
    for (int i = 0; i < 4; i++)
        buf[i+5] = ((char*)&handle)[3-i];
    buf[9] = count>>8;
    buf[10] = count;
    int p = al->build(buf+11, 100-26);
    buf[p+11] = contState[0];
    for (int j = 1; j <= contState[0]; j++)
        buf[p+j+11] = contState[j];
    //printfBytes("SDP Send: ", buf, parlen+5);
    return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
}

int SDPManager::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) {
    int parlen = sp->Size() + al->Size() + contState[0] + 3; // count (2 bytes) + at least 1 cont
    buf[0] = 6; //pdu
    buf[1] = txid>>8;
    buf[2] = txid++;
    buf[4] = parlen;
    buf[3] = parlen>>8;
    int p = sp->build(buf+5, 30);
    buf[p+6] = count;
    buf[p+5] = count>>8;
    p += al->build(buf+11, 100-38);
    buf[p+7] = contState[0];
    for (int j = 1; j <= contState[0]; j++)
        buf[p+j+7] = contState[j];
    //printfBytes("SDP Send: ", buf, parlen+5);
    return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
}

unsigned SDPManager::getval(const unsigned char *p, int n) {
    unsigned ret = 0;
    for (int i = 0; i < n; i++)
        ret = (ret<<8) + (unsigned)p[i];
    return ret;
}

unsigned SDPManager::length(const unsigned char *el, unsigned &p) {
    unsigned len = 0;
    switch (el[p++] & 7) {//length
        case 0:
            len = 1;
            break;
        case 1:
            len = 2;
            break;
        case 2:
            len = 4;
            break;
        case 3:
            len = 8;
            break;
        case 4:
            len = 16;
            break;
        case 7://4bytes
            len= el[p++]<<24;
            len += el[p++]<<16;
        case 6://2bytes
            len += el[p++]<<8;
        case 5://1byte
            len += el[p++];
            break;
    }
    return len;
}

extern "C" void HardFault_Handler() {
    printf("Hard Fault! %d bytes left\n", AvailableMemory(1));
    while (1);
}

unsigned SDPManager::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
    unsigned p = 0;
    unsigned len = length(el, p);
    int end = p+len;//end is the index of the item just after the sequence
    sdp_data *item = 0;
    switch (el[0]>>3) {//type
        case sdp_data::NULL_:
            printf("NULL ");
            break;
        case sdp_data::UNSIGNED:
            printf("UINT%d=%u ", len, (unsigned)getval(el+p, len));
            break;
        case sdp_data::SIGNED:
            printf("INT%d=%d ", len, (unsigned)getval(el+p, len));
            break;
        case sdp_data::UUID:
            if (len==16) {
                char rev[16];
                printf("UUID16= ");
                for (int i = 0; i < 16; i++)
                    printf("%02x ", el[p+i]);
            } else
                printf("UUID%d=%u ", len, (unsigned)getval(el+p, len));
            break;
        case sdp_data::STRING:
            printf("STR%d='%s' ", len, (char*)el+p);
            break;
        case sdp_data::BOOL:
            printf("BOOL%d=%d ", len, (unsigned)getval(el+p, len));
            break;
        case sdp_data::SEQUENCE:
            goto skip;
        case sdp_data::ALTERNATIVE:
skip: {//p points just after the length indicator, hence at the first item IN the sequence
               printf("SEQ%d{%p ", len, item);
                int n = 0;
                unsigned short key;
                serv_rec *dummy = 0;
                while (p < end) {
                    sdp_data *elem = 0;
                    p += parseLight(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
                    if (record) {
                        if (n & 1) { //value
                            record->insert(pair<unsigned short, sdp_data*>(key, elem));
                        } else //key
                            key = n;
                        n++;
                    }
                }
            }
            printf("}\n");
            break;
        case 8:
            printf("URL%d='%s' ", len, (char*)el+p);
            break;
        default:
            printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
    }
    result = item;
    return end;
}

unsigned SDPManager::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
    unsigned p = 0;
    unsigned len = length(el, p);
    int end = p+len;//end is the index of the item just after the sequence
    sdp_data *item = 0;
    switch (el[0]>>3) {//type
        case sdp_data::NULL_:
            item = new sdp_data();
            break;
        case sdp_data::UNSIGNED:
            item = new sdp_data((unsigned)getval(el+p, len), len);
            break;
        case sdp_data::SIGNED:
            item = new sdp_data((int)getval(el+p, len), len);
            break;
        case sdp_data::UUID:
            if (len==16) {
                char rev[16];
                for (int i = 0; i < 16; i++)
                    rev[i] = el[p+15-i];
                item = new sdp_data(sdp_data::UUID, rev, len);
            } else
                item = new sdp_data(sdp_data::UUID, getval(el+p, len), len);
            break;
        case sdp_data::STRING:
            item = new sdp_data((char*)el+p, len);
            break;
        case sdp_data::BOOL:
            item = new sdp_data((bool)getval(el+p, len), len);
            break;
        case sdp_data::SEQUENCE:
            item = new sdp_data(sdp_data::SEQUENCE);
            goto skip;
        case sdp_data::ALTERNATIVE:
            item = new sdp_data(sdp_data::ALTERNATIVE);
skip: {//p points just after the length indicator, hence at the first item IN the sequence
               //printf("SEQ%d{%p ", len, item);
                int n = 0;
                unsigned short key;
                serv_rec *dummy = 0;
                while (p < end) {
                    sdp_data *elem = 0;
                    p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
                    item->add_element(elem);
                    if (record) {
                        if (n & 1) { //value
                            record->insert(pair<unsigned short, sdp_data*>(key, elem));
                        } else //key
                            key = elem->asUnsigned();
                        n++;
                    }
                }
            }
            //printf("}\n");
            break;
        case 8:
            item = new sdp_data(sdp_data::URL, (char*)el+p, len);
            break;
        default:
            printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
    }
    result = item;
    return end;
}

void SDPManager::append(const unsigned char *payload, int len) {
    unsigned char *tmp = new unsigned char[byteCount + len];//append the payload to the previous continuation buffer
    if (contBuf && byteCount) {
      memcpy(tmp, contBuf, byteCount); //copy the existing part
      delete[] contBuf;//delete the old buffer
    }
    memcpy(tmp+byteCount, payload, len); //append the new part
    contBuf = tmp;
    byteCount += len;
}

void SDPManager::freeBuf() {
    if (contBuf) {
       delete[] contBuf;
       contBuf = 0;
    }
    byteCount = 0;
}

int SDPManager::parseRsp(const unsigned char*rsp, int len) {
    unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
    unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8);
    printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
    unsigned cont = 0;
    switch (rsp[0]) {
        case 1: {//errorRsp
            unsigned errorcode = rsp[6] + ((unsigned)rsp[5]<<8);
            if (parlen > 2) {
                printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
            }
            if (ErrorResponse)
                ErrorResponse(errorcode);
            return errorcode;
        }
        //break;
        case 3: { //servicesearchRsp
            unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
            unsigned current = rsp[8] + ((unsigned)rsp[7]<<8);
            cont = rsp[9+4*current];
            memcpy(contState, &rsp[9+4*current], cont+1);//copy the continuation state
            printf("total=%d, current=%d, cont=%d\n", total, current, cont);
            if (cont) {
                //no special handling here, just append the servicerecordhandles
            }
            //linear list of 32bit service-handles
            for (int i = 0; i < current; i++) {
                unsigned result = 0;
                for (int j = 0; j< 4; j++)
                    result = (result<<8) + rsp[9 + 4*i + j];
                printf("SDP Search handle %08X\n", result);
                services.insert(pair<unsigned, serv_rec*>(result, 0));
            }
            if (ServiceSearchResponse)
                ServiceSearchResponse();
        }
        break;
        case 5: { //serviceattributeRsp
            if (tree) delete tree;
            unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list
            append(rsp+7, count);
            cont = rsp[7+count];
            memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state
            if (cont) break;
            printf("count=%d parsing...\n", byteCount);
            serv_rec *serv = new serv_rec;
            parse(contBuf, byteCount, tree, serv);
            printf("...parsing done,  ");
            unsigned key = (*serv)[0]->asUnsigned();//'0' is always the serviceID
            printf("Key=%#X\n", key);
            services[key] = serv;
            freeBuf();
            if (ServiceAttributeResponse)
                ServiceAttributeResponse(serv);
        }
        break;
        //below is UNTESTED
        case 7: { //servicesearchattributeRsp
            if (tree) delete tree;
            unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);
            append(rsp+7, count);
            cont = rsp[7+count];
            memcpy(contState, &rsp[7+count], cont+1);
            if (cont)
                break;
            unsigned pos = 0;
            if (contBuf[pos]>>3 != sdp_data::SEQUENCE) {
                printf("Expected a sequence of attribute lists\n");
                break;
            }
            unsigned len = length(contBuf, pos);//get the length of the list of lists and advance pos to the first list
            while (pos<len) {
                printf("pos=%d, count=%d, parsing...\n", pos, len);
                serv_rec *serv = new serv_rec;
                pos = parse(contBuf+pos, len, tree, serv);
                unsigned key = (*serv)[0]->asUnsigned();
                services[key] = serv;
            }
            freeBuf();
            printf("...parsing done, pos=%d\n", pos);
            if (ServiceSearchAttributeResponse)
                ServiceSearchAttributeResponse();
        }
        break;
        default:
            printf("Unknown SDP response type %02X\n", rsp[0]);
        break;
    }
    return 0;
}