mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-03-30 16:20:24 +00:00
Code Issues Packages Projects Releases Wiki Activity

respond to PTS ICMP Ping

Browse Source
This commit is contained in:
Matthias Ringwald 2015-07-15 15:51:21 +02:00
parent 33cd41fb71
commit e02d2a7fba
1 changed files with 93 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
test/pts/bnep_test.c
View File

@ -66,6 +66,11 @@
#include "pan.h"
#include "stdin_support.h"
#define NETWORK_TYPE_IPv4 0x800
#define NETWORK_TYPE_IPv6 0x86DD
#define ICMP_TYPE_PING_REQUEST 0x08
#define ICMP_TYPE_PING_RESPONSE 0x00
// prototypes
static void show_usage();
@ -134,7 +139,7 @@ static void send_buffer(uint16_t pos){
}
static void send_ethernet_packet(int src_compressed, int dst_compressed){
int pos = setup_ethernet_header(src_compressed, dst_compressed, 0, 0x0800); // IPv4
int pos = setup_ethernet_header(src_compressed, dst_compressed, 0, NETWORK_TYPE_IPv4); // IPv4
// dummy data Ethernet packet
int i;
for (i = 60; i >= 0 ; i--){
@ -178,7 +183,7 @@ static void send_arp_probe_ipv4(void){
net_store_16(network_buffer, pos, 0x0001); // Hardware type (HTYPE), 1 = Ethernet
pos += 2;
net_store_16(network_buffer, pos, 0x0800); // Protocol type (PTYPE), 0x800 = IPv4
net_store_16(network_buffer, pos, NETWORK_TYPE_IPv4); // Protocol type (PTYPE), 0x800 = IPv4
pos += 2;
network_buffer[pos++] = 6; // Hardware length (HLEN) - 6 MAC Address
network_buffer[pos++] = 4; // Protocol length (PLEN) - 4 IPv4 Address
@ -220,7 +225,7 @@ static void send_some_ipv6_packet(void){
bd_addr_t an_addr = { 0x33, 0x33, 0x00, 0x00, 0x00, 0x16};
memcpy(other_addr, an_addr, 6);
int pos = setup_ethernet_header(1, 0, 0, 0x86DD); // IPv6
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6); // IPv6
uint8_t ipv6_packet[] = {
0x60, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x02,
@ -239,7 +244,7 @@ static void send_some_ipv6_packet_2(void){
bd_addr_t an_addr = { 0x33, 0x33, 0xFF, 0x60, 0x7B, 0x87};
memcpy(other_addr, an_addr, 6);
int pos = setup_ethernet_header(1, 0, 0, 0x86DD); // IPv6
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6); // IPv6
uint8_t ipv6_packet[] = {
0x60, 0x00, 0x00, 0x00, 0x00, 0x18, 0x3a, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x02,
@ -252,6 +257,60 @@ static void send_some_ipv6_packet_2(void){
send_buffer(pos);
}
static uint16_t calc_internet_checksum(uint8_t * data, int size){
uint32_t checksum = 0;
while (size){
// add 16-bit value
checksum += *(uint16_t*)data;
data += 2;
size -= 2;
// handle end-aroud-carry
while (checksum > 0xffff){
checksum = (checksum & 0xffff) + 1;
}
}
return checksum;
}
static void send_ping_response(bd_addr_t src_addr, bd_addr_t dst_addr){
uint8_t ipv4_packet[] = {
// ip
0x45, 0x00, 0x00, 0x00, // version + ihl, dscp } ecn, total len
0x00, 0x00, 0x00, 0x00, // identification (16), flags + fragment offset
0x01, 0x01, 0x00, 0x00, // time to live, procotol: icmp, checksum (16),
0x00, 0x00, 0x00, 0x00, // source IP address
0x00, 0x00, 0x00, 0x00, // destination IP address
};
uint8_t icmp_packet[] = {
// icmp
0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00
};
// ethernet header
int pos = setup_ethernet_header(1, 0, 0, NETWORK_TYPE_IPv6); // IPv6
// ipv4
int total_length = sizeof(ipv4_packet) + sizeof(icmp_packet);
net_store_16(ipv4_packet, 2, total_length);
uint16_t ipv4_checksum = calc_internet_checksum(ipv4_packet, sizeof(ipv4_packet));
net_store_16(ipv4_packet, 10, ipv4_checksum);
// TODO: also set src/dest ip address
memcpy(&network_buffer[pos], ipv4_packet, sizeof(ipv4_packet));
pos += sizeof(ipv4_packet);
// icmp
uint16_t icmp_checksum = calc_internet_checksum(icmp_packet, sizeof(icmp_packet));
net_store_16(icmp_packet, 2, icmp_checksum);
memcpy(&network_buffer[pos], icmp_packet, sizeof(icmp_packet));
pos += sizeof(icmp_packet);
// send
send_buffer(pos);
}
static void show_usage(void){
printf("\n--- Bluetooth BNEP Test Console ---\n");
@ -336,15 +395,20 @@ static int stdin_process(struct data_source *ds){
}
/*************** PANU client routines *********************/
static void packet_handler (void * connection, uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size)
{
uint8_t event;
bd_addr_t event_addr;
bd_addr_t src_addr;
bd_addr_t dst_addr;
uint16_t uuid_source;
uint16_t uuid_dest;
uint16_t mtu;
uint16_t network_type;
uint8_t icmp_type;
int ihl;
int payload_offset;
switch (packet_type) {
case HCI_EVENT_PACKET:
event = packet[0];
@ -408,12 +472,34 @@ static void packet_handler (void * connection, uint8_t packet_type, uint16_t cha
break;
case BNEP_DATA_PACKET:
// show received packet on console
// bt_flip_addr(dst_addr, &packet[0]);
// bt_flip_addr(src_addr, &packet[6]);
memcpy(dst_addr, &packet[0], 6);
memcpy(src_addr, &packet[6], 6);
network_type = READ_NET_16(packet, 12);
printf("BNEP packet received\n");
printf("Dst Addr: %s\n", bd_addr_to_str(&packet[0]));
printf("Src Addr: %s\n", bd_addr_to_str(&packet[6]));
printf("Net Type: %04x\n", READ_NET_16(packet, 12));
printf("Net Type: %04x\n", network_type);
// ignore the next 60 bytes
hexdumpf(&packet[74], size - 74);
switch (network_type){
case NETWORK_TYPE_IPv4:
ihl = packet[14] & 0x0f;
payload_offset = 14 + (ihl << 2);
//
icmp_type = packet[payload_offset];
hexdumpf(&packet[payload_offset], size - payload_offset);
printf("ICMP packet of type %x\n", icmp_type);
switch (icmp_type){
case ICMP_TYPE_PING_REQUEST:
printf("IPv4 Ping Request received, sending pong\n");
send_ping_response(src_addr, dst_addr);
break;
break;
}
}
break;
default: