bcsp_integrity.c

bluetooth 驱动

/*
 * bcsp_integrity.c -- Implementation of the Integrity layer in the BCSP
 *                     protocol stack
 *
 * Copyright (C) 2001  Axis Communications AB
 *
 * Author: Mats Friden <[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.
 *
 * Exceptionally, Axis Communications AB grants discretionary and
 * conditional permissions for additional use of the text contained
 * in the company's release of the AXIS OpenBT Stack under the
 * provisions set forth hereunder.
 *
 * Provided that, if you use the AXIS OpenBT Stack with other files,
 * that do not implement functionality as specified in the Bluetooth
 * System specification, to produce an executable, this does not by
 * itself cause the resulting executable to be covered by the GNU
 * General Public License. Your use of that executable is in no way
 * restricted on account of using the AXIS OpenBT Stack code with it.
 *
 * This exception does not however invalidate any other reasons why
 * the executable file might be covered by the provisions of the GNU
 * General Public License.
 *
 * $Id: bcsp_integrity.c,v 1.8 2001/09/18 13:04:27 pkj Exp $
 *
 */

/****************** INCLUDE FILES SECTION ***********************************/

#define __NO_VERSION__ /* don't define kernel_version in module.h */

#ifdef __KERNEL__
#include <asm/byteorder.h>
#include <asm/unaligned.h>

#include <linux/bluetooth/sysdep-2.1.h>
#include <linux/bluetooth/btcommon.h>
#include <linux/bluetooth/bcsp.h>
#include <linux/bluetooth/bcsp_debug.h>
#else
#include <asm/unaligned.h>

#include "include/btcommon.h"
#include "include/bcsp.h"
#include "include/bcsp_debug.h"
#endif

/****************** CONSTANT AND MACRO SECTION ******************************/

#if INTERGRITY_DEBUG
#define D(fmt...) printk("INTEGRITY: " fmt)
#define PRINTPKT(data, len) print_data(NULL, data, len)
#else
#define D(fmt...)
#define PRINTPKT(data, len)
#endif

/****************** TYPE DEFINITION SECTION *********************************/

/****************** LOCAL FUNCTION DECLARATION SECTION **********************/

static s32 verify_checksum(const u8 *hdr, u8 checksum);
static u8 calc_checksum(u8 flags, u8 identifier, u16 payload_length);
static s32 verify_length(struct bcsp* bcsp);
static s32 verify_crc(struct bcsp* bcsp);
static u16 calc_crc(struct bcsp* bcsp);

static void init_crc(u16 *crc);
static void update_crc(u16 *crc, u16 databyte);
static u16 rev16(u16 src);
static u16 get_big_endian_crc(u16 crc);

/****************** GLOBAL VARIABLE DECLARATION SECTION *********************/

/****************** LOCAL VARIABLE DECLARATION SECTION **********************/

static const u16 crc_table[] = {
	0x0000, 0x1081, 0x2102, 0x3183,
	0x4204, 0x5285, 0x6306, 0x7387,
	0x8408, 0x9489, 0xa50a, 0xb58b,
	0xc60c, 0xd68d,	0xe70e, 0xf78f
};

/****************** FUNCTION DEFINITION SECTION *****************************/

s32
bcsp_integrity_receive(struct bcsp* bcsp)
{
	bcsp->flags = BCSP_GET_FLAGS(bcsp);
	bcsp->identifier = BCSP_GET_IDENTIFIER(bcsp);
	bcsp->payload_length = BCSP_GET_PAYLOAD_LENGTH(bcsp);
	bcsp->checksum = BCSP_GET_CHECKSUM(bcsp);

	if (!verify_checksum(bcsp->packet, bcsp->checksum)) {
		BCSP_SYS(__FUNCTION__ ": Incorrect checksum, discarding data\n");
		return -ERR_CHECKSUM;
	}

	if (!verify_length(bcsp)) {
		BCSP_SYS(__FUNCTION__ ": Incorrect length, discarding data\n");
		return -ERR_PAYLOAD_LENGTH;
	}
	
	if (BCSP_GET_CRC_PRESENT(bcsp)) {
		bcsp->crc = le16_to_cpu(get_unaligned((u16 *)&bcsp->packet[bcsp->packet_length - 2]));
		
		if (!verify_crc(bcsp)) {
			BCSP_SYS(__FUNCTION__ ": Incorrect CRC, discarding data\n");
		}
	}

	D("Found the payload:\n");
	bcsp->payload = bcsp->packet + BCSP_HDR_SIZE;
	PRINTPKT(bcsp->payload, bcsp->payload_length);

	return bcsp_mux_receive(bcsp);
}

s32
bcsp_integrity_send(struct bcsp* bcsp)
{
	bcsp->checksum = calc_checksum(bcsp->flags, bcsp->identifier, bcsp->payload_length);

	if (BCSP_GET_CRC_PRESENT(bcsp)) {
		bcsp->crc = calc_crc(bcsp);
	}

	return bcsp_slip_send(bcsp);
}

s32
verify_checksum(const u8 *hdr, u8 checksum)
{
	u8 hdr_sum;

	hdr_sum = (hdr[0] + hdr[1] + hdr[2]) % 256;

	if ((u8)(~hdr_sum) == checksum) {
		return TRUE;
	} else {
		D(__FUNCTION__ ": hdr_sum: 0x%x\n", ~hdr_sum);
		D(__FUNCTION__ ": checksum: 0x%x\n", checksum);
		return FALSE;
	}
}

u8
calc_checksum(u8 flags, u8 identifier, u16 payload_length)
{
	u8 hdr[3];

	hdr[0] = flags;
	hdr[1] = ((payload_length & 0xf) << 4) | (identifier & 0x0f);
	hdr[2] = payload_length >> 4;
	
	return (u8) (~((hdr[0] + hdr[1] + hdr[2]) % 256));
}

s32
verify_length(struct bcsp* bcsp)
{

	if (BCSP_GET_CRC_PRESENT(bcsp)) {
		D(__FUNCTION__ ": CRC is present\n");
		D(__FUNCTION__ ": Payload length field: %d bytes, actual length: %d bytes\n", bcsp->payload_length, bcsp->packet_length - BCSP_HDR_SIZE - BCSP_CRC_SIZE); 
		return (bcsp->payload_length == (bcsp->packet_length - BCSP_HDR_SIZE - BCSP_CRC_SIZE));
	} else {
		D(__FUNCTION__ ": CRC is not present\n");
		D(__FUNCTION__ ": Payload length field says %d bytes, actual length is %d bytes\n", bcsp->payload_length, bcsp->packet_length - BCSP_HDR_SIZE);
		return (bcsp->payload_length == (bcsp->packet_length - BCSP_HDR_SIZE));
	}
}

s32
verify_crc(struct bcsp* bcsp)
{
	return (get_big_endian_crc(calc_crc(bcsp)) == bcsp->crc);
}

u16
calc_crc(struct bcsp* bcsp)
{
	s32 i;
	u16 crc;

	init_crc(&crc);
	
	for (i = 0; i < (bcsp->packet_length - BCSP_CRC_SIZE); i++) {
		update_crc(&crc, bcsp->packet[i]);
	}

	D(__FUNCTION__ ": CRC value is 0x%04x\n", crc);

	return crc;
}

/*********************** Code for the CRC calculation ***********************/

void
init_crc(u16 *crc)
{
	*crc =  0xffff; 
}

void
update_crc(u16 *crc, u16 databyte)
{
	u16 reg = *crc;
	reg = (reg >> 4) ^ crc_table[(reg ^ databyte) & 0xf];
	reg = (reg >> 4) ^ crc_table[(reg ^ (databyte >> 4)) & 0xf];
	*crc = reg;
}

u16
rev16(u16 src)
{
	u16 dest = 0;
	s32 b;
	
	for (b = 0; b < 16; b ++)
	{
		dest = dest << 1;
		dest |= (src & 1);
		src = src >> 1;
	}
	return dest;
}

u16
get_big_endian_crc(u16 crc)
{
	return cpu_to_be16(rev16(crc));
}

/****************** END OF FILE integrity.c *********************************/