/* * *  BlueZ - Bluetooth protocol stack for Linux * *  Copyright (C) 2000-2002  Maxim Krasnyansky <maxk@qualcomm.com> *  Copyright (C) 2003-2005  Marcel Holtmann <marcel@holtmann.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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA * */#ifdef HAVE_CONFIG_H#include <config.h>#endif#include <stdio.h>#include <errno.h>#include <unistd.h>#include <stdlib.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 10static 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 20static 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) {