emulator/bthost: Add support for ECRED Connection request/response

This adds support for ECRED Connection request/response which will be
used by l2cap-runner to test L2CAP_MODE_EXT_FLOWCTL mode.

Signed-off-by: Anuj Jain <anuj01.jain@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>

diff --git a/emulator/bthost.c b/emulator/bthost.c
index d37957f..ef5614f 100755 (executable)
--- a/emulator/bthost.c
+++ b/emulator/bthost.c
@@ -1772,6 +1772,69 @@ static bool l2cap_le_conn_rsp(struct bthost *bthost, struct btconn *conn,
        return true;
 }
 
+static bool l2cap_ecred_conn_req(struct bthost *bthost, struct btconn *conn,
+                               uint8_t ident, const void *data, uint16_t len)
+{
+       const struct bt_l2cap_pdu_ecred_conn_req *req = data;
+       struct {
+               struct bt_l2cap_pdu_ecred_conn_rsp pdu;
+               uint16_t dcid[5];
+       } __attribute__ ((packed)) rsp;
+       uint16_t psm;
+       int num_scid, i = 0;
+
+       if (len < sizeof(*req))
+               return false;
+
+       psm = le16_to_cpu(req->psm);
+
+       memset(&rsp, 0, sizeof(rsp));
+
+       rsp.pdu.mtu = 64;
+       rsp.pdu.mps = 64;
+       rsp.pdu.credits = 1;
+
+       if (!bthost_find_l2cap_cb_by_psm(bthost, psm)) {
+               rsp.pdu.result = cpu_to_le16(0x0002); /* PSM Not Supported */
+               goto respond;
+       }
+
+       len -= sizeof(rsp.pdu);
+       num_scid = len / sizeof(*req->scid);
+
+       for (; i < num_scid; i++)
+               rsp.dcid[i] = cpu_to_le16(conn->next_cid++);
+
+respond:
+       l2cap_sig_send(bthost, conn, BT_L2CAP_PDU_ECRED_CONN_RSP, ident, &rsp,
+                       sizeof(rsp.pdu) + i * sizeof(*rsp.dcid));
+
+       return true;
+}
+
+static bool l2cap_ecred_conn_rsp(struct bthost *bthost, struct btconn *conn,
+                               uint8_t ident, const void *data, uint16_t len)
+{
+       const struct  {
+               const struct bt_l2cap_pdu_ecred_conn_rsp *pdu;
+               uint16_t scid[5];
+       } __attribute__ ((packed)) *rsp = data;
+       int num_scid, i;
+
+       if (len < sizeof(*rsp))
+               return false;
+
+       num_scid = len / sizeof(*rsp->scid);
+
+       for (i = 0; i < num_scid; i++)
+               /* TODO add L2CAP connection before with proper PSM */
+               bthost_add_l2cap_conn(bthost, conn, 0,
+                                     le16_to_cpu(rsp->scid[i]), 0);
+
+
+       return true;
+}
+
 static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
                                                const void *data, uint16_t len)
 {
@@ -1818,6 +1881,15 @@ static void l2cap_le_sig(struct bthost *bthost, struct btconn *conn,
                ret = l2cap_le_conn_rsp(bthost, conn, hdr->ident,
                                                data + sizeof(*hdr), hdr_len);
                break;
+       case BT_L2CAP_PDU_ECRED_CONN_REQ:
+               ret = l2cap_ecred_conn_req(bthost, conn, hdr->ident,
+                                               data + sizeof(*hdr), hdr_len);
+               break;
+
+       case BT_L2CAP_PDU_ECRED_CONN_RSP:
+               ret = l2cap_ecred_conn_rsp(bthost, conn, hdr->ident,
+                                               data + sizeof(*hdr), hdr_len);
+               break;
 
        default:
                printf("Unknown L2CAP code 0x%02x\n", hdr->code);