bnep_test.c

linux下蓝牙

	if (test_packet.packet_type == TEST_PAYLOAD_DATA) {

		__u16 *h802_1p = NULL;
		__u16 value16;

		//Copy payload
		if (test_packet.payload) {
			memcpy(skb_put(skb, test_packet.payload->len), test_packet.payload->data,
			       test_packet.payload->len);
		}
		//Create eth header
		memcpy(&(eth.h_dest), &bnep_test_ioctl_send->destination_address, sizeof(bdaddr_t));
		memcpy(&(eth.h_source), net_dev->dev_addr, ETH_ALEN);
		value16 = cpu_to_bnep16(bnep_test_ioctl_send->protocol);
		memcpy(&(eth.h_proto), &value16, 2);

		if (bnep_test_ioctl_send->h802_1p == H802_1P_ON) {
			h802_1p = (__u16 *) kmalloc(2, GFP_ATOMIC);
			memset(h802_1p, 0, 2);
		}
		//Send packet
		bnep_etherdata_multiplexor(net_dev, NULL, skb, &eth, h802_1p, ExtensionLength,
					   bnep_test_ioctl_send->packet_type);

	} else {
		struct bnep_connection *bnep_conn =
		    bnep_get_conn_by_bdaddr(net_dev, &bnep_test_ioctl_send->destination_address);
		if (bnep_conn) {
			switch (test_packet.packet_type) {
			case TEST_PAYLOAD_UNKNOWN_CONTROL:
				bnep_tx_controldata(bnep_conn, test_packet.control_value,
						    ExtensionLength, skb);
				break;
			case TEST_PAYLOAD_CONTROL_NETFILTER:
				bnep_tx_controldata(bnep_conn, BNEP_FILTER_NET_TYPE_SET_MSG,
						    ExtensionLength, skb);
				break;
			case TEST_PAYLOAD_CONTROL_MULTICASTFILTER:
				bnep_tx_controldata(bnep_conn, BNEP_FILTER_MULT_ADDR_SET_MSG,
						    ExtensionLength, skb);
				break;
			case TEST_PAYLOAD_CONNECT_CONTROL:
				bnep_tx_controldata(bnep_conn, BNEP_SETUP_CONNECTION_REQUEST_MSG,
						    ExtensionLength, skb);
				break;
			}
		} else {
			BT_DBG("can not send packet to %s - No connection found",
					batostr(&bnep_test_ioctl_send->destination_address));
		}
	}

	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_packet_add_payload_data(struct bnep_test_ioctl_data_opt_struct
					*bnep_test_ioctl_data_opt)
{

	__u8 value;
	__u16 counter;
	__u16 length = bnep_test_ioctl_data_opt->length + bnep_test_ioctl_data_opt->header_length;

	test_packet.packet_type = TEST_PAYLOAD_DATA;

	//Remove old payload entry
	//if (test_packet . payload) kfree_skb(test_packet . payload);

	if (length == 0)
		test_packet.payload = NULL;
	else {
		if (!(test_packet.payload = alloc_skb(length, GFP_ATOMIC)))
			return -ENOMEM;

		//Add skipped header
		value = 0xff;
		for (counter = 0; counter < bnep_test_ioctl_data_opt->header_length; counter++) {
			memcpy(skb_put(test_packet.payload, 1), &value, 1);
		}

		//Add data pattern
		value = 0;
		for (counter = 0; counter < bnep_test_ioctl_data_opt->length; counter++) {
			memcpy(skb_put(test_packet.payload, 1), &value, 1);
			value = (value == 0xff) ? 0 : value + 1;
		}
	}
	return 0;
}

/************************************************************************/
/************************************************************************/
static int test_packet_add(struct bnep_test_ioctl_add_field_struct *bnep_test_ioctl_add_field)
{

	int status = 0;

	switch (bnep_test_ioctl_add_field->content) {
	case TESTCASE_CONTENT_DATA:{
			struct bnep_test_ioctl_data_opt_struct bnep_test_ioctl_data_opt;
			copy_from_user(&bnep_test_ioctl_data_opt,
				       bnep_test_ioctl_add_field->add_opts,
				       sizeof(struct bnep_test_ioctl_data_opt_struct));
			BT_DBG("register payload: data : [[0x%x][0x%x]]",
					bnep_test_ioctl_data_opt.header_length,
					bnep_test_ioctl_data_opt.length);
			status = test_packet_add_payload_data(&bnep_test_ioctl_data_opt);
			break;
		}
	case TESTCASE_CONTENT_PROTOCOL_FILTER:{
			struct filter_request_struct filter_request;
			copy_from_user(&filter_request, bnep_test_ioctl_add_field->add_opts,
				       sizeof(struct filter_request_struct));
			if (bnep_test_ioctl_add_field->field == TESTCASE_ADD_PAYLOAD) {
				BT_DBG("register payload: netfilter");
				test_packet.packet_type = TEST_PAYLOAD_CONTROL_NETFILTER;
				test_bnep_packet_add_netfilter(&test_packet.payload, &filter_request);
			} else {
				if (test_packet.ExtensionCounter < MAX_EXTENSION_HEADER) {
					BT_DBG("register extension: netfilter");
					test_packet.ExtensionType[test_packet.ExtensionCounter] =
							BNEP_FILTER_NET_TYPE_SET_MSG;
					test_bnep_packet_add_netfilter(
						&test_packet.Extension[test_packet.ExtensionCounter],
						&filter_request);
					test_packet.ExtensionCounter++;
				} else {
					BT_DBG("can't register extension netfilter: Too much header defined");
				}
			}
			break;
		}
	case TESTCASE_CONTENT_MULTICAST_FILTER:{
			struct filter_request_struct filter_request;
			copy_from_user(&filter_request, bnep_test_ioctl_add_field->add_opts,
				       sizeof(struct filter_request_struct));
			if (bnep_test_ioctl_add_field->field == TESTCASE_ADD_PAYLOAD) {
				BT_DBG("register payload: multicast filter");
				test_packet.packet_type = TEST_PAYLOAD_CONTROL_MULTICASTFILTER;
				test_bnep_packet_add_multicastfilter(&test_packet.payload,
								     &filter_request);
			} else {
				if (test_packet.ExtensionCounter < MAX_EXTENSION_HEADER) {
					BT_DBG("register extension: multicastfilter");
					test_packet.ExtensionType[test_packet.ExtensionCounter] =
					    BNEP_FILTER_MULT_ADDR_SET_MSG;
					test_bnep_packet_add_multicastfilter(&test_packet.
									     Extension[test_packet.
										       ExtensionCounter],
									     &filter_request);
					test_packet.ExtensionCounter++;
				} else {
					BT_DBG("can't register extension multicastfilter: Too much header defined");
				}
			}
			break;
		}
	case TESTCASE_CONTENT_CONNECT:{
			struct bnep_test_ioctl_connect_opt_struct bnep_test_ioctl_connect_opt;
			copy_from_user(&bnep_test_ioctl_connect_opt,
				       bnep_test_ioctl_add_field->add_opts,
				       sizeof(struct bnep_test_ioctl_connect_opt_struct));
			BT_DBG("register payload: connect message");
			test_packet.packet_type = TEST_PAYLOAD_UNKNOWN_CONTROL;
			status =
			    test_bnep_packet_add_payload_connect(&test_packet.payload,
								 &bnep_test_ioctl_connect_opt);
			break;
		}
	case TESTCASE_CONTENT_CONTROL:{
			struct bnep_test_ioctl_control_opt_struct bnep_test_ioctl_control_opt;
			copy_from_user(&bnep_test_ioctl_control_opt,
				       bnep_test_ioctl_add_field->add_opts,
				       sizeof(struct bnep_test_ioctl_control_opt_struct));

			if (bnep_test_ioctl_add_field->field == TESTCASE_ADD_PAYLOAD) {
				BT_DBG("register payload. type: control 0x%x",
						bnep_test_ioctl_control_opt.control_value);
				test_packet.packet_type = TEST_PAYLOAD_UNKNOWN_CONTROL;
				test_packet.control_value =
				    bnep_test_ioctl_control_opt.control_value;
				test_bnep_packet_add_control(&test_packet.payload,
							     &bnep_test_ioctl_control_opt);
			} else {
				if (test_packet.ExtensionCounter < MAX_EXTENSION_HEADER) {
					BT_DBG("register extension. type: control extension 0x%x",
							bnep_test_ioctl_control_opt.control_value);
					test_packet.ExtensionType[test_packet.ExtensionCounter] =
							BNEP_UNKNOWN_COMMAND;
					test_packet.ext_control_value[test_packet.ExtensionCounter] =
							bnep_test_ioctl_control_opt.control_value;
					test_bnep_packet_add_control(
							&test_packet.Extension[test_packet.ExtensionCounter],
							&bnep_test_ioctl_control_opt);
					test_packet.ExtensionCounter++;
				} else {
					BT_DBG("can't register extension control: Too much header defined");
				}
			}

			break;
		}
	case TESTCASE_CONTENT_UNKNOWN_HEADER:{
			if (test_packet.ExtensionCounter < MAX_EXTENSION_HEADER) {
				struct bnep_test_ioctl_unknown_header_opt_struct
						bnep_test_ioctl_unknown_header_opt;
				copy_from_user(&bnep_test_ioctl_unknown_header_opt,
						bnep_test_ioctl_add_field->add_opts,
						sizeof(struct bnep_test_ioctl_unknown_header_opt_struct));
				BT_DBG("register extension. type: unknown extension 0x%x\n",
						bnep_test_ioctl_unknown_header_opt.control_value);
				test_packet.ExtensionType[test_packet.ExtensionCounter] =
						bnep_test_ioctl_unknown_header_opt.control_value;
				test_bnep_packet_add_unknown_header(
						&test_packet.Extension[test_packet.ExtensionCounter],
						&bnep_test_ioctl_unknown_header_opt);
				test_packet.ExtensionCounter++;
			} else {
				BT_DBG("can't register extension control: Too much header defined");
			}
			break;
		}
	default:{
			BT_DBG("unknown test packet content: %d",
					bnep_test_ioctl_add_field->content);
			status = 1;
			break;
		}
	}
	return status;
}

/************************************************************************/
/************************************************************************/
static int test_set_ignore_var(struct bnep_test_ioctl_ignore_msg_struct *bnep_test_ioctl_ignore_msg)
{
	switch (bnep_test_ioctl_ignore_msg->msg_type) {
	case TESTCASE_IGNORE_CONNECT:
		test_ignore_connect = bnep_test_ioctl_ignore_msg->switch_value;
		BT_DBG("switch ignore connect to %s",
				(test_ignore_connect == TESTCASE_IGNORE_MESSAGE_ON) ? "On" : "Off");
		break;
	case TESTCASE_IGNORE_NETFILTER:
		test_ignore_netfilter = bnep_test_ioctl_ignore_msg->switch_value;
		BT_DBG("switch ignore protocol filter to %s",
				(test_ignore_netfilter == TESTCASE_IGNORE_MESSAGE_ON) ? "On" : "Off");
		break;
	case TESTCASE_IGNORE_MULTICASTFILTER:
		test_ignore_multicastfilter = bnep_test_ioctl_ignore_msg->switch_value;
		BT_DBG("switch ignore multicast filter to %s",
				(test_ignore_multicastfilter == TESTCASE_IGNORE_MESSAGE_ON) ? "On" : "Off");
		break;
	default:
		return 1;
	}
	return 0;
}

/************************************************************************/
/************************************************************************/
int test_execute_test(struct net_device *net_dev, struct bnep_test_ctrl_struct *bnep_test_ctrl)
{

	int status = 0;

	switch (bnep_test_ctrl->test_command) {
	case TESTCASE_ADD:{
			struct bnep_test_ioctl_add_field_struct bnep_test_ioctl_add_field;
			copy_from_user(&bnep_test_ioctl_add_field, bnep_test_ctrl->opt_args,
				       sizeof(struct bnep_test_ioctl_add_field_struct));
			status = test_packet_add(&bnep_test_ioctl_add_field);
			break;
		}
	case TESTCASE_RESET:{
			return test_bnep_packet_free();
		}
	case TESTCASE_SEND:{
			struct bnep_test_ioctl_send_struct bnep_test_ioctl_send;
			copy_from_user(&bnep_test_ioctl_send, bnep_test_ctrl->opt_args,
				       sizeof(struct bnep_test_ioctl_send_struct));
			status = test_bnep_packet_send(net_dev, &bnep_test_ioctl_send);
			break;
		}
	case TESTCASE_IGNORE:{
			struct bnep_test_ioctl_ignore_msg_struct bnep_test_ioctl_ignore_msg;
			copy_from_user(&bnep_test_ioctl_ignore_msg, bnep_test_ctrl->opt_args,
				       sizeof(struct bnep_test_ioctl_ignore_msg_struct));
			status = test_set_ignore_var(&bnep_test_ioctl_ignore_msg);
			break;
		}
	default:{
			BT_DBG("unknown test command: %d", bnep_test_ctrl->test_command);
			status = 1;
		}
	}
	return status;
}
#endif