path: root/net/bluetooth/a2mp.c

/*
   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
   Copyright (c) 2011,2012 Intel Corp.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 and
   only version 2 as published by the Free Software Foundation.

   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.
*/

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/l2cap.h>
#include <net/bluetooth/a2mp.h>
#include <net/bluetooth/amp.h>

/* Global AMP Manager list */
LIST_HEAD(amp_mgr_list);
DEFINE_MUTEX(amp_mgr_list_lock);

/* A2MP build & send command helper functions */
static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
{
	struct a2mp_cmd *cmd;
	int plen;

	plen = sizeof(*cmd) + len;
	cmd = kzalloc(plen, GFP_KERNEL);
	if (!cmd)
		return NULL;

	cmd->code = code;
	cmd->ident = ident;
	cmd->len = cpu_to_le16(len);

	memcpy(cmd->data, data, len);

	return cmd;
}

void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
{
	struct l2cap_chan *chan = mgr->a2mp_chan;
	struct a2mp_cmd *cmd;
	u16 total_len = len + sizeof(*cmd);
	struct kvec iv;
	struct msghdr msg;

	cmd = __a2mp_build(code, ident, len, data);
	if (!cmd)
		return;

	iv.iov_base = cmd;
	iv.iov_len = total_len;

	memset(&msg, 0, sizeof(msg));

	msg.msg_iov = (struct iovec *) &iv;
	msg.msg_iovlen = 1;

	l2cap_chan_send(chan, &msg, total_len, 0);

	kfree(cmd);
}

u8 __next_ident(struct amp_mgr *mgr)
{
	if (++mgr->ident == 0)
		mgr->ident = 1;

	return mgr->ident;
}

static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
{
	cl->id = 0;
	cl->type = 0;
	cl->status = 1;
}

/* hci_dev_list shall be locked */
static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
{
	int i = 0;
	struct hci_dev *hdev;

	__a2mp_cl_bredr(cl);

	list_for_each_entry(hdev, &hci_dev_list, list) {
		/* Iterate through AMP controllers */
		if (hdev->id == HCI_BREDR_ID)
			continue;

		/* Starting from second entry */
		if (++i >= num_ctrl)
			return;

		cl[i].id = hdev->id;
		cl[i].type = hdev->amp_type;
		cl[i].status = hdev->amp_status;
	}
}

/* Processing A2MP messages */
static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
			    struct a2mp_cmd *hdr)
{
	struct a2mp_cmd_rej *rej = (void *) skb->data;

	if (le16_to_cpu(hdr->len) < sizeof(*rej))
		return -EINVAL;

	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));

	skb_pull(skb, sizeof(*rej));

	return 0;
}

static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
			     struct a2mp_cmd *hdr)
{
	struct a2mp_discov_req *req = (void *) skb->data;
	u16 len = le16_to_cpu(hdr->len);
	struct a2mp_discov_rsp *rsp;
	u16 ext_feat;
	u8 num_ctrl;

	if (len < sizeof(*req))
		return -EINVAL;

	skb_pull(skb, sizeof(*req));

	ext_feat = le16_to_cpu(req->ext_feat);

	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);

	/* check that packet is not broken for now */
	while (ext_feat & A2MP_FEAT_EXT) {
		if (len < sizeof(ext_feat))
			return