From 841d8f5b37d920355a71ad8fb5b2450f56f1a67a Mon Sep 17 00:00:00 2001 From: chengkai Date: Tue, 19 Jul 2022 22:10:07 +0800 Subject: [PATCH] net:add btsnoop and snoop Signed-off-by: chengkai --- include/nuttx/net/snoop.h | 150 ++++++++++++++ net/utils/Make.defs | 2 +- net/utils/net_snoop.c | 398 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 549 insertions(+), 1 deletion(-) create mode 100644 include/nuttx/net/snoop.h create mode 100644 net/utils/net_snoop.c diff --git a/include/nuttx/net/snoop.h b/include/nuttx/net/snoop.h new file mode 100644 index 0000000000..b234032ffb --- /dev/null +++ b/include/nuttx/net/snoop.h @@ -0,0 +1,150 @@ +/**************************************************************************** + * include/nuttx/net/snoop.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_NET_SNOOP_H +#define __INCLUDE_NUTTX_NET_SNOOP_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Datalink Type: + * + * A 32-bit (4 octet) field identifying the type of + * datalink header used in the packet records that follow. + * The datalink type codes are listed in the table below: + * + * Datalink Type Code + * ------------- ---- + * IEEE 802.3 0 + * IEEE 802.4 Token Bus 1 + * IEEE 802.5 Token Ring 2 + * IEEE 802.6 Metro Net 3 + * Ethernet 4 + * HDLC 5 + * Character Synchronous 6 + * IBM Channel-to-Channel 7 + * FDDI 8 + * Other 9 + * Unassigned 10 - 4294967295 + * + * Un-encapsulated HCI (H1) 1001 + * HCI UART (H4) 1002 + * HCI BSCP 1003 + * HCI Serial (H5) 1004 + * Unassigned 1005 - 4294967295 + * + * More info about snoop datalink type, please refer to + * https://www.rfc-editor.org/rfc/rfc1761.txt and + * https://fte.com/webhelpii/hsu/Content/Technical_Information/ + * BT_Snoop_File_Format.htm + */ + +#define SNOOP_DATALINK_TYPE_TOKENBUS 1 +#define SNOOP_DATALINK_TYPE_TOKERING 2 +#define SNOOP_DATALINK_TYPE_METRONET 3 +#define SNOOP_DATALINK_TYPE_ETHERNET 4 +#define SNOOP_DATALINK_TYPE_HDLC 5 +#define SNOOP_DATALINK_TYPE_CHARSYNC 6 +#define SNOOP_DATALINK_TYPE_IBMC2C 7 +#define SNOOP_DATALINK_TYPE_FDDI 8 +#define SNOOP_DATALINK_TYPE_OTHER 9 + +#define SNOOP_DATALINK_HCI_UNENCAP 1001 +#define SNOOP_DATALINK_HCI_UART 1002 +#define SNOOP_DATALINK_HCI_BSCP 1003 +#define SNOOP_DATALINK_HCI_SERIAL 1004 + +#define SNOOP_DIRECTION_FLAG_SENT 0 /* Direction flag 0 = Sent */ +#define SNOOP_DIRECTION_FLAG_RECV 1 /* Direction flag 1 = Received */ + +struct snoop_s +{ + bool autosync; + uint32_t datalink; + struct file filep; +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: snoop_open + * + * Description: + * This function open snoop file by datalink. + * + ****************************************************************************/ + +int snoop_open(FAR struct snoop_s *snoop, FAR const char *filename, + uint32_t datalink, bool autosync); + +/**************************************************************************** + * Name: snoop_dump + * + * Description: + * This function dump nbytes buf data into snoop file. + * + ****************************************************************************/ + +int snoop_dump(FAR struct snoop_s *snoop, FAR const void *buf, + uint32_t nbytes, uint32_t drops, uint32_t flags); + +/**************************************************************************** + * Name: snoop_sync + * + * Description: + * This function sync snoop buffer. + * + ****************************************************************************/ + +int snoop_sync(FAR struct snoop_s *snoop); + +/**************************************************************************** + * Name: snoop_close + * + * Description: + * This function close snoop file. + * + ****************************************************************************/ + +int snoop_close(FAR struct snoop_s *snoop); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_NUTTX_NET_SNOOP_H */ diff --git a/net/utils/Make.defs b/net/utils/Make.defs index bc38dd8c91..235650ed0e 100644 --- a/net/utils/Make.defs +++ b/net/utils/Make.defs @@ -21,7 +21,7 @@ # Common utilities NET_CSRCS += net_dsec2tick.c net_dsec2timeval.c net_timeval2dsec.c -NET_CSRCS += net_chksum.c net_ipchksum.c net_incr32.c net_lock.c +NET_CSRCS += net_chksum.c net_ipchksum.c net_incr32.c net_lock.c net_snoop.c # IPv6 utilities diff --git a/net/utils/net_snoop.c b/net/utils/net_snoop.c new file mode 100644 index 0000000000..75caa9c1d0 --- /dev/null +++ b/net/utils/net_snoop.c @@ -0,0 +1,398 @@ +/**************************************************************************** + * net/utils/net_snoop.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifndef ARRAY_SIZE +# define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0])) +#endif + +#define SNOOP_VERSION_1 1 +#define SNOOP_VERSION_2 2 + +/* microseconds since midnight, January 1st, 0 AD nominal Gregorian. */ + +#define SNOOP_EPOCH_USEC(tv) (((tv).tv_sec - 0x386d4380ll) * 1000000ll \ + + (tv).tv_usec + 0x00e03ab44a676000ll) + +/**************************************************************************** + * Private Type Definitions + ****************************************************************************/ + +/* The availability of tools to capture, display and interpret packets + * traversing a network has proven extremely useful in debugging + * networking problems. The ability to capture packets and store them + * for later analysis allows one to de-couple the tasks of collecting + * information about a network problem and analysing that information. + * + * More info about snoop datalink type, please refer to + * https://www.rfc-editor.org/rfc/rfc1761.txt and + * https://fte.com/webhelpii/hsu/Content/Technical_Information/ + * BT_Snoop_File_Format.htm + */ + +/* The snoop packet capture file is an array of octets structured as + * follows: + * + * +------------------------+ + * | | + * | File Header | + * | | + * +------------------------+ + * | | + * | Packet Record | + * ~ Number 1 ~ + * | | + * +------------------------+ + * . . + * . . + * . . + * +------------------------+ + * | | + * | Packet Record | + * ~ Number N ~ + * | | + * +------------------------+ + */ + +/* snoop_file_header_s + * + * The structure of the File Header is as follows: + * + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * + Identification Pattern + + * | | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Version Number | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Datalink Type | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +begin_packed_struct struct snoop_file_header_s +{ + uint8_t magic[8]; /* Identification Pattern */ + uint32_t version; /* Version Number */ + uint32_t datalink; /* Datalink Type */ +} end_packed_struct; + +/* snoop_packet_header_s + * + * The structure of the packet record is as follows: + * + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Original Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Included Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Packet Record Length | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Cumulative Drops | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Timestamp Seconds | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | Timestamp Microseconds | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + * | | + * . . + * . Packet Data . + * . . + * + +- - - - - - - -+ + * | | Pad | + * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ + */ + +begin_packed_struct struct snoop_packet_header_s +{ + uint32_t orig_len; /* actual length of packet */ + uint32_t incl_len; /* number of octets captured in file */ + union + { + uint32_t flags; /* Packet Flags: 1 hci cmd , eg: btsnoop */ + uint32_t rec_len; /* length of record */ + }; + uint32_t cum_drops; /* cumulative number of dropped packets */ + union + { + uint64_t ts_usec; /* timestamp microseconds, eg: btsnoop */ + struct + { + uint32_t ts_sec; /* timestamp seconds */ + uint32_t ts_usec; /* timestamp microseconds */ + } ts; + }; +} end_packed_struct; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: snoop_dump_packet_header + * + * Description: + * This function fill snoop headr info. + * + ****************************************************************************/ + +static int snoop_dump_packet_header(FAR struct snoop_s *snoop, + uint32_t bytes, uint32_t drops, + uint32_t flags) +{ + struct snoop_packet_header_s header; + int ret; + + if (!snoop) + { + return -EINVAL; + } + + switch (snoop->datalink) + { + case SNOOP_DATALINK_HCI_UNENCAP: + case SNOOP_DATALINK_HCI_UART: + case SNOOP_DATALINK_HCI_BSCP: + case SNOOP_DATALINK_HCI_SERIAL: + { + struct timeval tv; + + gettimeofday(&tv, NULL); + header.ts_usec = htobe64(SNOOP_EPOCH_USEC(tv)); + header.flags = htobe32(flags); + break; + } + + case SNOOP_DATALINK_TYPE_TOKENBUS: + case SNOOP_DATALINK_TYPE_TOKERING: + case SNOOP_DATALINK_TYPE_METRONET: + case SNOOP_DATALINK_TYPE_ETHERNET: + case SNOOP_DATALINK_TYPE_HDLC: + case SNOOP_DATALINK_TYPE_CHARSYNC: + case SNOOP_DATALINK_TYPE_IBMC2C: + case SNOOP_DATALINK_TYPE_FDDI: + case SNOOP_DATALINK_TYPE_OTHER: + { + struct timeval tv; + + gettimeofday(&tv, NULL); + header.ts.ts_sec = htobe32(tv.tv_sec); + header.ts.ts_usec = htobe32(tv.tv_usec); + header.rec_len = htobe32(flags); + break; + } + + default: + { + return -EINVAL; + } + } + + header.orig_len = htobe32(bytes); + header.incl_len = htobe32(bytes); + header.cum_drops = htobe32(drops); + + ret = file_write(&snoop->filep, &header, sizeof(header)); + if (ret != sizeof(header)) + { + return ret < 0 ? ret : -EINVAL; + } + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: snoop_open + * + * Description: + * This function open snoop file by datalink. + * + ****************************************************************************/ + +int snoop_open(FAR struct snoop_s *snoop, FAR const char *filename, + uint32_t datalink, bool autosync) +{ + struct snoop_file_header_s header; + int ret; + + if (!snoop) + { + return -EINVAL; + } + + switch (datalink) + { + case SNOOP_DATALINK_TYPE_TOKENBUS: + case SNOOP_DATALINK_TYPE_TOKERING: + case SNOOP_DATALINK_TYPE_METRONET: + case SNOOP_DATALINK_TYPE_ETHERNET: + case SNOOP_DATALINK_TYPE_HDLC: + case SNOOP_DATALINK_TYPE_CHARSYNC: + case SNOOP_DATALINK_TYPE_IBMC2C: + case SNOOP_DATALINK_TYPE_FDDI: + case SNOOP_DATALINK_TYPE_OTHER: + { + uint8_t snoop_magic[] = + { + 's', 'n', 'o', 'o', 'p', '\0', '\0', '\0' + }; + + memcpy(header.magic, snoop_magic, ARRAY_SIZE(snoop_magic)); + header.version = htobe32(SNOOP_VERSION_2); + break; + }; + + case SNOOP_DATALINK_HCI_UNENCAP: + case SNOOP_DATALINK_HCI_UART: + case SNOOP_DATALINK_HCI_BSCP: + case SNOOP_DATALINK_HCI_SERIAL: + { + uint8_t btsnoop_magic[] = + { + 'b', 't', 's', 'n', 'o', 'o', 'p', '\0' + }; + + memcpy(header.magic, btsnoop_magic, ARRAY_SIZE(btsnoop_magic)); + header.version = htobe32(SNOOP_VERSION_1); + break; + } + + default: + { + return -EINVAL; + } + } + + ret = file_open(&snoop->filep, filename, O_RDWR | O_CREAT); + if (ret < 0) + { + return ret; + } + + snoop->datalink = datalink; + snoop->autosync = autosync; + header.datalink = htobe32(datalink); + + ret = file_write(&snoop->filep, &header, sizeof(header)); + if (ret != sizeof(header)) + { + ret = ret < 0 ? ret : -EINVAL; + goto error; + } + + return OK; + +error: + file_close(&snoop->filep); + return ret; +} + +/**************************************************************************** + * Name: snoop_dump + * + * Description: + * This function dump nbytes buf data into snoop file. + * + ****************************************************************************/ + +int snoop_dump(FAR struct snoop_s *snoop, FAR const void *buf, + uint32_t nbytes, uint32_t drops, uint32_t flags) +{ + int ret; + + if (!snoop) + { + return -EINVAL; + } + + ret = snoop_dump_packet_header(snoop, nbytes, drops, flags); + if (ret != OK) + { + return ret; + } + + ret = file_write(&snoop->filep, buf, nbytes); + if (ret != nbytes) + { + return ret < 0 ? ret : -EINVAL; + } + + return snoop->autosync ? snoop_sync(snoop) : OK; +} + +/**************************************************************************** + * Name: snoop_sync + * + * Description: + * This function sync snoop buffer. + * + ****************************************************************************/ + +int snoop_sync(FAR struct snoop_s *snoop) +{ + int ret = OK; + + if (!snoop) + { + return -EINVAL; + } + +#ifndef CONFIG_DISABLE_MOUNTPOINT + ret = file_fsync(&snoop->filep); +#endif + return ret; +} + +/**************************************************************************** + * Name: snoop_close + * + * Description: + * This function close snoop file. + * + ****************************************************************************/ + +int snoop_close(FAR struct snoop_s *snoop) +{ + if (!snoop) + { + return -EINVAL; + } + + return file_close(&snoop->filep); +}