l2cap.c

Bluezan implementation of the Bluetooth&#8482 wireless standards specifications for Linux. The code

/*
 *
 *  Bluetooth packet analyzer - L2CAP parser
 *
 *  Copyright (C) 2000-2002  Maxim Krasnyansky <[e-mail]>
 *  Copyright (C) 2003-2005  Marcel Holtmann <[e-mail]>
 *
 *
 *  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
 *
 *
 *  $Id: l2cap.c,v 1.45 2005/05/15 20:28:18 holtmann Exp $
 */

#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/l2cap.h>

#include "parser.h"
#include "sdp.h"

typedef struct {
	uint16_t handle;
	struct frame frm;
} handle_info;
#define HANDLE_TABLE_SIZE 10

static handle_info handle_table[HANDLE_TABLE_SIZE];

typedef struct {
	uint16_t handle;
	uint16_t cid;
	uint16_t psm;
	uint16_t num;
	uint8_t mode;
} cid_info;
#define CID_TABLE_SIZE 20

static cid_info cid_table[2][CID_TABLE_SIZE];

#define SCID cid_table[0]
#define DCID cid_table[1]

static struct frame *add_handle(uint16_t handle)
{
	register handle_info *t = handle_table;
	register int i;

	for (i = 0; i < HANDLE_TABLE_SIZE; i++)
		if (!t[i].handle) {
			t[i].handle = handle;
			return &t[i].frm;
		}
	return NULL;
}

static struct frame *get_frame(uint16_t handle)
{
	register handle_info *t = handle_table;
	register int i;

	for (i = 0; i < HANDLE_TABLE_SIZE; i++)
		if (t[i].handle == handle)
			return &t[i].frm;

	return add_handle(handle);
}

static void add_cid(int in, uint16_t handle, uint16_t cid, uint16_t psm)
{
	register cid_info *table = cid_table[in];
	register int i, pos = -1;
	uint16_t num = 1;

	for (i = 0; i < CID_TABLE_SIZE; i++) {
		if ((pos < 0 && !table[i].cid) || table[i].cid == cid)
			pos = i;
		if (table[i].psm == psm)
			num++;
	}

	if (pos >= 0) {
		table[pos].handle = handle;
		table[pos].cid    = cid;
		table[pos].psm    = psm;
		table[pos].num    = num;
		table[pos].mode   = 0;
	}
}

static void del_cid(int in, uint16_t dcid, uint16_t scid)
{
	register int t, i;
	uint16_t cid[2];

	if (!in) {
		cid[0] = dcid;
		cid[1] = scid;
	} else {
		cid[0] = scid;
		cid[1] = dcid;
	}

	for (t = 0; t < 2; t++) {
		for (i = 0; i < CID_TABLE_SIZE; i++)
			if (cid_table[t][i].cid == cid[t]) {
				cid_table[t][i].handle = 0;
				cid_table[t][i].cid    = 0;
				cid_table[t][i].psm    = 0;
				cid_table[t][i].num    = 0;
				cid_table[t][i].mode   = 0;
				break;
			}
	}
}

static void del_handle(uint16_t handle)
{
	register int t, i;

	for (t = 0; t < 2; t++) {
		for (i = 0; i < CID_TABLE_SIZE; i++)
			if (cid_table[t][i].handle == handle) {
				cid_table[t][i].handle = 0;
				cid_table[t][i].cid    = 0;
				cid_table[t][i].psm    = 0;
				cid_table[t][i].num    = 0;
				cid_table[t][i].mode   = 0;
				break;
			}
	}
}
static uint16_t get_psm(int in, uint16_t cid)
{
	register cid_info *table = cid_table[in];
	register int i;

	for (i = 0; i < CID_TABLE_SIZE; i++)
		if (table[i].cid == cid)
			return table[i].psm;
	return parser.defpsm;
}

static uint16_t get_num(int in, uint16_t cid)
{
	register cid_info *table = cid_table[in];
	register int i;

	for (i = 0; i < CID_TABLE_SIZE; i++)
		if (table[i].cid == cid)
			return table[i].num;
	return 0;
}

static void set_mode(int in, uint16_t cid, uint8_t mode)
{
	register cid_info *table = cid_table[in];
	register int i;

	for (i = 0; i < CID_TABLE_SIZE; i++)
		if (table[i].cid == cid)
			table[i].mode = mode;
}

static uint8_t get_mode(int in, uint16_t cid)
{
	register cid_info *table = cid_table[in];
	register int i;

	for (i = 0; i < CID_TABLE_SIZE; i++)
		if (table[i].cid == cid)
			return table[i].mode;
	return 0;
}

static uint32_t get_val(uint8_t *ptr, uint8_t len)
{
	switch (len) {
	case 1:
		return *ptr;
	case 2:
		return btohs(bt_get_unaligned((uint16_t *) ptr));
	case 4:
		return btohl(bt_get_unaligned((uint32_t *) ptr));
	}
	return 0;
}

static char *reason2str(uint16_t reason)
{
	switch (reason) {
	case 0x0000:
		return "Command not understood";
	case 0x0001:
		return "Signalling MTU exceeded";
	case 0x0002:
		return "Invalid CID in request";
	default:
		return "Reserved";
	}
}

static char *connresult2str(uint16_t result)
{
	switch (result) {
	case 0x0000:
		return "Connection successful";
	case 0x0001:
		return "Connection pending";
	case 0x0002:
		return "Connection refused - PSM not supported";
	case 0x0003:
		return "Connection refused - security block";
	case 0x0004:
		return "Connection refused - no resources available";
	default:
		return "Reserved";
	}
}

static char *status2str(uint16_t status)
{
	switch (status) {
	case 0x0000:
		return "No futher information available";
	case 0x0001:
		return "Authentication pending";
	case 0x0002:
		return "Authorization pending";
	default:
		return "Reserved";
	}
}

static char *confresult2str(uint16_t result)
{
	switch (result) {
	case 0x0000:
		return "Success";
	case 0x0001:
		return "Failure - unacceptable parameters";
	case 0x0002:
		return "Failure - rejected (no reason provided)";
	case 0x0003:
		return "Failure - unknown options";
	default:
		return "Reserved";
	}
}
static char *inforesult2str(uint16_t result)
{
	switch (result) {
	case 0x0000:
		return "Success";
	case 0x0001:
		return "Not supported";
	default:
		return "Reserved";
	}
}

static char *type2str(uint8_t type)
{
	switch (type) {
	case 0x00:
		return "No traffic";
	case 0x01:
		return "Best effort";
	case 0x02:
		return "Guaranteed";
	default:
		return "Reserved";
	}
}

static char *mode2str(uint8_t mode)
{
	switch (mode) {
	case 0x00:
		return "Basic";
	case 0x01:
		return "Retransmission";
	case 0x02:
		return "Flow control";
	default:
		return "Reserved";
	}
}

static char *sar2str(uint8_t sar)
{
	switch (sar) {
	case 0x00:
		return "Unsegmented";
	case 0x01:
		return "Start";
	case 0x02:
		return "End";
	case 0x03:
		return "Continuation";
	default:
		return "Bad SAR";

	}
}

static char *supervisory2str(uint8_t supervisory)
{
	switch (supervisory) {
	case 0x00:
		return "Receiver Ready (RR)";
	case 0x01:
		return "Reject (REJ)";
	case 0x02:
	case 0x03:
		return "Reserved Supervisory";
	default:
		return "Bad Supervisory";
	}
}

static inline void command_rej(int level, struct frame *frm)
{
	l2cap_cmd_rej *h = frm->ptr;
	uint16_t reason = btohs(h->reason);
	uint32_t cid;

	printf("Command rej: reason %d", reason);

	switch (reason) {
	case 0x0001:
		printf(" mtu %d\n", get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 2));
		break;
	case 0x0002:
		cid = get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 4);
		printf(" dcid 0x%4.4x scid 0x%4.4x\n", cid & 0xffff, cid >> 16);
		break;
	default:
		printf("\n");
		break;
	}

	p_indent(level + 1, frm);
	printf("%s\n", reason2str(reason));
}

static inline void conn_req(int level, struct frame *frm)
{
	l2cap_conn_req *h = frm->ptr;
	uint16_t psm = btohs(h->psm);
	uint16_t scid = btohs(h->scid);

	add_cid(frm->in, frm->handle, scid, psm);

	if (p_filter(FILT_L2CAP))
		return;

	printf("Connect req: psm %d scid 0x%4.4x\n", psm, scid);
}

static inline void conn_rsp(int level, struct frame *frm)
{
	l2cap_conn_rsp *h = frm->ptr;
	uint16_t scid = btohs(h->scid);
	uint16_t dcid = btohs(h->dcid);
	uint16_t result = btohs(h->result);
	uint16_t status = btohs(h->status);
	uint16_t psm;

	switch (h->result) {
	case L2CAP_CR_SUCCESS:
		if ((psm = get_psm(!frm->in, scid)))
			add_cid(frm->in, frm->handle, dcid, psm);
		break;

	case L2CAP_CR_PEND:
		break;

	default:
		del_cid(frm->in, dcid, scid);
		break;
	}

	if (p_filter(FILT_L2CAP))
		return;

	printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n",
		dcid, scid, result, status);

	p_indent(level + 1, frm);
	printf("%s", connresult2str(result));

	if (result == 0x0001)
		printf(" - %s\n", status2str(status));
	else
		printf("\n");
}

static void conf_opt(int level, void *ptr, int len, int in, uint16_t cid)
{
	p_indent(level, 0);
	while (len > 0) {
		l2cap_conf_opt *h = ptr;
		uint8_t mode;

		ptr += L2CAP_CONF_OPT_SIZE + h->len;
		len -= L2CAP_CONF_OPT_SIZE + h->len;

		if (h->type & 0x80)
			printf("[");

		switch (h->type & 0x7f) {
		case L2CAP_CONF_MTU:
			printf("MTU");
			if (h->len > 0)
				printf(" %d", get_val(h->val, h->len));
			break;

		case L2CAP_CONF_FLUSH_TO:
			printf("FlushTO");
			if (h->len > 0)
				printf(" %d", get_val(h->val, h->len));
			break;

		case L2CAP_CONF_QOS:
			printf("QoS");
			if (h->len > 0)
				printf(" 0x%02x (%s)", *(h->val + 1), type2str(*(h->val + 1)));
			break;

		case L2CAP_CONF_RFC:
			printf("Mode");
			if (h->len > 0) {