/*

 * Service Discover Protocol server for QEMU L2CAP devices

 *

 * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>

 *

 * This program is free software; you can redistribute it and/or

 * modify it under the terms of the GNU General Public License as

 * published by the Free Software Foundation; either version 2 of

 * the License, or (at your option) any later version.

 *

 * This program is distributed in the hope that it will be useful,

 * but WITHOUT ANY WARRANTY; without even the implied warranty of

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * GNU General Public License for more details.

 *

 * You should have received a copy of the GNU General Public License

 * along with this program; if not, write to the Free Software

 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,

 * MA 02111-1307 USA

 */

#include "qemu-common.h"

#include "bt.h"

struct bt_l2cap_sdp_state_s {

    struct bt_l2cap_conn_params_s *channel;

    struct sdp_service_record_s {

        int match;

        int *uuid;

        int uuids;

        struct sdp_service_attribute_s {

            int match;

            int attribute_id;

            int len;

            void *pair;

        } *attribute_list;

        int attributes;

    } *service_list;

    int services;

};

static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)

{

    size_t len = *(*element) ++ & SDP_DSIZE_MASK;

    if (!*left)

        return -1;

    (*left) --;

    if (len < SDP_DSIZE_NEXT1)

        return 1 << len;

    else if (len == SDP_DSIZE_NEXT1) {

        if (*left < 1)

            return -1;

        (*left) --;

        return *(*element) ++;

    } else if (len == SDP_DSIZE_NEXT2) {

        if (*left < 2)

            return -1;

        (*left) -= 2;

        len = (*(*element) ++) << 8;

        return len | (*(*element) ++);

    } else {

        if (*left < 4)

            return -1;

        (*left) -= 4;

        len = (*(*element) ++) << 24;

        len |= (*(*element) ++) << 16;

        len |= (*(*element) ++) << 8;

        return len | (*(*element) ++);

    }

}

static const uint8_t bt_base_uuid[12] = {

    0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,

};

static int sdp_uuid_match(struct sdp_service_record_s *record,

                const uint8_t *uuid, ssize_t datalen)

{

    int *lo, hi, val;

    if (datalen == 16 || datalen == 4) {

        if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))

            return 0;

        if (uuid[0] | uuid[1])

            return 0;

        uuid += 2;

    }

    val = (uuid[0] << 8) | uuid[1];

    lo = record->uuid;

    hi = record->uuids;

    while (hi >>= 1)

        if (lo[hi] <= val)

            lo += hi;

    return *lo == val;

}

#define CONTINUATION_PARAM_SIZE	(1 + sizeof(int))

#define MAX_PDU_OUT_SIZE	96	/* Arbitrary */

#define PDU_HEADER_SIZE		5

#define MAX_RSP_PARAM_SIZE	(MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \

                CONTINUATION_PARAM_SIZE)

static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,

                const uint8_t **req, ssize_t *len)

{

    size_t datalen;

    int i;

    if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)

        return 1;

    datalen = sdp_datalen(req, len);

    if (datalen != 2 && datalen != 4 && datalen != 16)

        return 1;

    for (i = 0; i < sdp->services; i ++)

        if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))

            sdp->service_list[i].match = 1;

    (*req) += datalen;

    (*len) -= datalen;

    return 0;

}

static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,

                uint8_t *rsp, const uint8_t *req, ssize_t len)

{

    ssize_t seqlen;

    int i, count, start, end, max;

    int32_t handle;

    /* Perform the search */

    for (i = 0; i < sdp->services; i ++)

        sdp->service_list[i].match = 0;

    if (len < 1)

        return -SDP_INVALID_SYNTAX;

    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {

        seqlen = sdp_datalen(&req, &len);

        if (seqlen < 3 || len < seqlen)

            return -SDP_INVALID_SYNTAX;

        len -= seqlen;

        while (seqlen)

            if (sdp_svc_match(sdp, &req, &seqlen))

                return -SDP_INVALID_SYNTAX;

    } else if (sdp_svc_match(sdp, &req, &seqlen))

        return -SDP_INVALID_SYNTAX;

    if (len < 3)

        return -SDP_INVALID_SYNTAX;

    end = (req[0] << 8) | req[1];

    req += 2;

    len -= 2;

    if (*req) {

        if (len <= sizeof(int))

            return -SDP_INVALID_SYNTAX;

        len -= sizeof(int);

        memcpy(&start, req + 1, sizeof(int));

    } else

        start = 0;

    if (len > 1);

        return -SDP_INVALID_SYNTAX;

    /* Output the results */

    len = 4;

    count = 0;

    end = start;

    for (i = 0; i < sdp->services; i ++)

        if (sdp->service_list[i].match) {

            if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {

                handle = i;

                memcpy(rsp + len, &handle, 4);

                len += 4;

                end = count + 1;

            }

            count ++;

        }

    rsp[0] = count >> 8;

    rsp[1] = count & 0xff;

    rsp[2] = (end - start) >> 8;

    rsp[3] = (end - start) & 0xff;

    if (end < count) {

        rsp[len ++] = sizeof(int);

        memcpy(rsp + len, &end, sizeof(int));

        len += 4;

    } else

        rsp[len ++] = 0;

    return len;

}

static int sdp_attr_match(struct sdp_service_record_s *record,

                const uint8_t **req, ssize_t *len)

{

    int i, start, end;

    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {

        (*req) ++;

        if (*len < 3)

            return 1;

        start = (*(*req) ++) << 8;

        start |= *(*req) ++;

        end = start;

        *len -= 3;

    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {

        (*req) ++;

        if (*len < 5)

            return 1;

        start = (*(*req) ++) << 8;

        start |= *(*req) ++;

        end = (*(*req) ++) << 8;

        end |= *(*req) ++;

        *len -= 5;

    } else

        return 1;

    for (i = 0; i < record->attributes; i ++)

        if (record->attribute_list[i].attribute_id >= start &&

                        record->attribute_list[i].attribute_id <= end)

            record->attribute_list[i].match = 1;

    return 0;

}

static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,

                uint8_t *rsp, const uint8_t *req, ssize_t len)

{

    ssize_t seqlen;

    int i, start, end, max;

    int32_t handle;

    struct sdp_service_record_s *record;

    uint8_t *lst;

    /* Perform the search */

    if (len < 7)

        return -SDP_INVALID_SYNTAX;

    memcpy(&handle, req, 4);

    req += 4;

    len -= 4;

    if (handle < 0 || handle > sdp->services)

        return -SDP_INVALID_RECORD_HANDLE;

    record = &sdp->service_list[handle];

    for (i = 0; i < record->attributes; i ++)

        record->attribute_list[i].match = 0;

    max = (req[0] << 8) | req[1];

    req += 2;

    len -= 2;

    if (max < 0x0007)

        return -SDP_INVALID_SYNTAX;

    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {

        seqlen = sdp_datalen(&req, &len);

        if (seqlen < 3 || len < seqlen)

            return -SDP_INVALID_SYNTAX;

        len -= seqlen;

        while (seqlen)

            if (sdp_attr_match(record, &req, &seqlen))

                return -SDP_INVALID_SYNTAX;

    } else if (sdp_attr_match(record, &req, &seqlen))

        return -SDP_INVALID_SYNTAX;

    if (len < 1)

        return -SDP_INVALID_SYNTAX;

    if (*req) {

        if (len <= sizeof(int))

            return -SDP_INVALID_SYNTAX;

        len -= sizeof(int);

        memcpy(&start, req + 1, sizeof(int));

    } else

        start = 0;

    if (len > 1)

        return -SDP_INVALID_SYNTAX;

    /* Output the results */

    lst = rsp + 2;

    max = MIN(max, MAX_RSP_PARAM_SIZE);

    len = 3 - start;

    end = 0;

    for (i = 0; i < record->attributes; i ++)

        if (record->attribute_list[i].match) {

            if (len >= 0 && len + record->attribute_list[i].len < max) {

                memcpy(lst + len, record->attribute_list[i].pair,

                                record->attribute_list[i].len);

                end = len + record->attribute_list[i].len;

            }

            len += record->attribute_list[i].len;

        }

    if (0 >= start) {

       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;

       lst[1] = (len + start - 3) >> 8;

       lst[2] = (len + start - 3) & 0xff;

    }

    rsp[0] = end >> 8;

    rsp[1] = end & 0xff;

    if (end < len) {

        len = end + start;

        lst[end ++] = sizeof(int);

        memcpy(lst + end, &len, sizeof(int));

        end += sizeof(int);

    } else

        lst[end ++] = 0;

    return end + 2;

}

static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,

                const uint8_t **req, ssize_t *len)

{

    int i, j, start, end;

    struct sdp_service_record_s *record;

    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {

        (*req) ++;

        if (*len < 3)

            return 1;

        start = (*(*req) ++) << 8;

        start |= *(*req) ++;

        end = start;

        *len -= 3;

    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {

        (*req) ++;

        if (*len < 5)

            return 1;

        start = (*(*req) ++) << 8;

        start |= *(*req) ++;

        end = (*(*req) ++) << 8;

        end |= *(*req) ++;

        *len -= 5;

    } else

        return 1;

    for (i = 0; i < sdp->services; i ++)

        if ((record = &sdp->service_list[i])->match)

            for (j = 0; j < record->attributes; j ++)

                if (record->attribute_list[j].attribute_id >= start &&

                                record->attribute_list[j].attribute_id <= end)

                    record->attribute_list[j].match = 1;

    return 0;

}

static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,

                uint8_t *rsp, const uint8_t *req, ssize_t len)

{

    ssize_t seqlen;

    int i, j, start, end, max;

    struct sdp_service_record_s *record;

    uint8_t *lst;

    /* Perform the search */

    for (i = 0; i < sdp->services; i ++) {

        sdp->service_list[i].match = 0;

            for (j = 0; j < sdp->service_list[i].attributes; j ++)

                sdp->service_list[i].attribute_list[j].match = 0;

    }

    if (len < 1)

        return -SDP_INVALID_SYNTAX;

    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {

        seqlen = sdp_datalen(&req, &len);

        if (seqlen < 3 || len < seqlen)

            return -SDP_INVALID_SYNTAX;

        len -= seqlen;

        while (seqlen)

            if (sdp_svc_match(sdp, &req, &seqlen))

                return -SDP_INVALID_SYNTAX;

    } else if (sdp_svc_match(sdp, &req, &seqlen))

        return -SDP_INVALID_SYNTAX;

    if (len < 3)

        return -SDP_INVALID_SYNTAX;

    max = (req[0] << 8) | req[1];

    req += 2;

    len -= 2;

    if (max < 0x0007)

        return -SDP_INVALID_SYNTAX;

    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {

        seqlen = sdp_datalen(&req, &len);

        if (seqlen < 3 || len < seqlen)

            return -SDP_INVALID_SYNTAX;

        len -= seqlen;

        while (seqlen)

            if (sdp_svc_attr_match(sdp, &req, &seqlen))

                return -SDP_INVALID_SYNTAX;

    } else if (sdp_svc_attr_match(sdp, &req, &seqlen))

        return -SDP_INVALID_SYNTAX;

    if (len < 1)

        return -SDP_INVALID_SYNTAX;

    if (*req) {

        if (len <= sizeof(int))

            return -SDP_INVALID_SYNTAX;

        len -= sizeof(int);

        memcpy(&start, req + 1, sizeof(int));

    } else

        start = 0;

    if (len > 1)

        return -SDP_INVALID_SYNTAX;

    /* Output the results */

    /* This assumes empty attribute lists are never to be returned even

     * for matching Service Records.  In practice this shouldn't happen

     * as the requestor will usually include the always present

     * ServiceRecordHandle AttributeID in AttributeIDList.  */

    lst = rsp + 2;

    max = MIN(max, MAX_RSP_PARAM_SIZE);

    len = 3 - start;

    end = 0;

    for (i = 0; i < sdp->services; i ++)

        if ((record = &sdp->service_list[i])->match) {

            len += 3;

            seqlen = len;

            for (j = 0; j < record->attributes; j ++)

                if (record->attribute_list[j].match) {

                    if (len >= 0)

                        if (len + record->attribute_list[j].len < max) {

                            memcpy(lst + len, record->attribute_list[j].pair,

                                            record->attribute_list[j].len);

                            end = len + record->attribute_list[j].len;

                        }

                    len += record->attribute_list[j].len;

                }

            if (seqlen == len)

                len -= 3;

            else if (seqlen >= 3 && seqlen < max) {

                lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;

                lst[seqlen - 2] = (len - seqlen) >> 8;

                lst[seqlen - 1] = (len - seqlen) & 0xff;

            }

        }

    if (len == 3 - start)

        len -= 3;

    else if (0 >= start) {

       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;

       lst[1] = (len + start - 3) >> 8;

       lst[2] = (len + start - 3) & 0xff;

    }

    rsp[0] = end >> 8;

    rsp[1] = end & 0xff;

    if (end < len) {

        len = end + start;

        lst[end ++] = sizeof(int);

        memcpy(lst + end, &len, sizeof(int));

        end += sizeof(int);

    } else

        lst[end ++] = 0;

    return end + 2;

}

static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)

{

    struct bt_l2cap_sdp_state_s *sdp = opaque;

    enum bt_sdp_cmd pdu_id;

    uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;

    int transaction_id, plen;

    int err = 0;

    int rsp_len = 0;

    if (len < 5) {

        fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);

        return;

    }

    pdu_id = *data ++;

    transaction_id = (data[0] << 8) | data[1];

    plen = (data[2] << 8) | data[3];

    data += 4;

    len -= 5;

    if (len != plen) {

        fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",

                        __FUNCTION__, plen, len);

        err = SDP_INVALID_PDU_SIZE;

        goto respond;

    }

    switch (pdu_id) {

    case SDP_SVC_SEARCH_REQ:

        rsp_len = sdp_svc_search(sdp, rsp, data, len);

        pdu_id = SDP_SVC_SEARCH_RSP;

        break;

    case SDP_SVC_ATTR_REQ:

        rsp_len = sdp_attr_get(sdp, rsp, data, len);

        pdu_id = SDP_SVC_ATTR_RSP;

        break;

    case SDP_SVC_SEARCH_ATTR_REQ:

        rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);

        pdu_id = SDP_SVC_SEARCH_ATTR_RSP;