Adding upstream version 4.2.2.upstream/4.2.2

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

1 files changed, 1150 insertions, 0 deletions

diff --git a/epan/dissectors/packet-bthsp.c b/epan/dissectors/packet-bthsp.c
new file mode 100644
index 00000000..c62f7017
--- /dev/null
+++ b/epan/dissectors/packet-bthsp.c
@@ -0,0 +1,1150 @@
+/* packet-bthsp.c
+ * Routines for Bluetooth Headset Profile (HSP)
+ *
+ * Copyright 2013, Michal Labedzki for Tieto Corporation
+ *
+ * Wireshark - Network traffic analyzer
+ * By Gerald Combs <gerald@wireshark.org>
+ * Copyright 1998 Gerald Combs
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later
+ */
+
+#include "config.h"
+
+#include <epan/packet.h>
+#include <epan/prefs.h>
+#include <epan/expert.h>
+#include <epan/strutil.h>
+#include "packet-btrfcomm.h"
+#include "packet-btsdp.h"
+
+static int proto_bthsp = -1;
+
+static int hf_command                                                      = -1;
+static int hf_parameters                                                   = -1;
+static int hf_command_in                                                   = -1;
+static int hf_unsolicited                                                  = -1;
+static int hf_role                                                         = -1;
+static int hf_at_cmd                                                       = -1;
+static int hf_at_cmd_type                                                  = -1;
+static int hf_at_command_line_prefix                                       = -1;
+static int hf_at_ignored                                                   = -1;
+static int hf_parameter                                                    = -1;
+static int hf_unknown_parameter                                            = -1;
+static int hf_data                                                         = -1;
+static int hf_fragment                                                     = -1;
+static int hf_fragmented                                                   = -1;
+static int hf_vgs                                                          = -1;
+static int hf_vgm                                                          = -1;
+static int hf_ckpd                                                         = -1;
+
+static expert_field ei_non_mandatory_command                          = EI_INIT;
+static expert_field ei_invalid_usage                                  = EI_INIT;
+static expert_field ei_unknown_parameter                              = EI_INIT;
+static expert_field ei_vgm_gain                                       = EI_INIT;
+static expert_field ei_vgs_gain                                       = EI_INIT;
+static expert_field ei_ckpd                                           = EI_INIT;
+
+static gint ett_bthsp            = -1;
+static gint ett_bthsp_command    = -1;
+static gint ett_bthsp_parameters = -1;
+
+static dissector_handle_t bthsp_handle;
+
+static wmem_tree_t *fragments = NULL;
+
+#define ROLE_UNKNOWN  0
+#define ROLE_AG       1
+#define ROLE_HS       2
+
+#define TYPE_UNKNOWN       0x0000
+#define TYPE_RESPONSE_ACK  0x0d0a
+#define TYPE_RESPONSE      0x003a
+#define TYPE_ACTION        0x003d
+#define TYPE_ACTION_SIMPLY 0x000d
+#define TYPE_READ          0x003f
+#define TYPE_TEST          0x3d3f
+
+static gint hsp_role = ROLE_UNKNOWN;
+
+enum reassemble_state_t {
+    REASSEMBLE_FRAGMENT,
+    REASSEMBLE_PARTIALLY,
+    REASSEMBLE_DONE
+};
+
+typedef struct _fragment_t {
+    guint32                  interface_id;
+    guint32                  adapter_id;
+    guint32                  chandle;
+    guint32                  dlci;
+    guint32                  role;
+
+    guint                    idx;
+    guint                    length;
+    guint8                  *data;
+    struct _fragment_t      *previous_fragment;
+
+    guint                    reassemble_start_offset;
+    guint                    reassemble_end_offset;
+    enum reassemble_state_t  reassemble_state;
+} fragment_t;
+
+typedef struct _at_cmd_t {
+    const char *name;
+    const char *long_name;
+
+    gboolean (*check_command)(gint role, guint16 type);
+    gboolean (*dissect_parameter)(tvbuff_t *tvb, packet_info *pinfo,
+            proto_tree *tree, gint offset, gint role, guint16 type,
+            guint8 *parameter_stream, guint parameter_number,
+            gint parameter_length, void **data);
+} at_cmd_t;
+
+static const value_string role_vals[] = {
+    { ROLE_UNKNOWN,   "Unknown" },
+    { ROLE_AG,        "AG - Audio Gate" },
+    { ROLE_HS,        "HS - Headset" },
+    { 0, NULL }
+};
+
+static const value_string at_cmd_type_vals[] = {
+    { 0x0d,   "Action Command" },
+    { 0x3a,   "Response" },
+    { 0x3d,   "Action Command" },
+    { 0x3f,   "Read Command" },
+    { 0x0d0a, "Response" },
+    { 0x3d3f, "Test Command" },
+    { 0, NULL }
+};
+
+static const enum_val_t pref_hsp_role[] = {
+    { "off",     "Off",                    ROLE_UNKNOWN },
+    { "ag",      "Sent is AG, Rcvd is HS", ROLE_AG },
+    { "hs",      "Sent is HS, Rcvd is AG", ROLE_HS },
+    { NULL, NULL, 0 }
+};
+
+static const unit_name_string units_slash15 = { "/15", NULL };
+
+void proto_register_bthsp(void);
+void proto_reg_handoff_bthsp(void);
+
+static guint32 get_uint_parameter(guint8 *parameter_stream, gint parameter_length)
+{
+    guint32      value;
+    gchar *val;
+
+    val = (guint8 *) wmem_alloc(wmem_packet_scope(), parameter_length + 1);
+    memcpy(val, parameter_stream, parameter_length);
+    val[parameter_length] = '\0';
+    value = (guint32) g_ascii_strtoull(val, NULL, 10);
+
+    return value;
+}
+
+static gboolean check_vgs(gint role, guint16 type) {
+    if (role == ROLE_HS && type == TYPE_ACTION) return TRUE;
+    if (role == ROLE_AG && type == TYPE_RESPONSE) return TRUE;
+
+    return FALSE;
+}
+
+static gboolean check_vgm(gint role, guint16 type) {
+    if (role == ROLE_HS && type == TYPE_ACTION) return TRUE;
+    if (role == ROLE_AG && type == TYPE_RESPONSE) return TRUE;
+
+    return FALSE;
+}
+
+static gboolean check_ckpd(gint role, guint16 type) {
+    if (role == ROLE_HS && type == TYPE_ACTION) return TRUE;
+
+    return FALSE;
+}
+
+static gboolean check_only_ag_role(gint role, guint16 type) {
+    if (role == ROLE_AG && type == TYPE_RESPONSE_ACK) return TRUE;
+
+    return FALSE;
+}
+
+static gint
+dissect_vgs_parameter(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
+        gint offset, gint role, guint16 type, guint8 *parameter_stream,
+        guint parameter_number, gint parameter_length, void **data _U_)
+{
+    proto_item  *pitem;
+    guint32      value;
+
+    if (!check_vgs(role, type)) return FALSE;
+
+    if (parameter_number > 0) return FALSE;
+
+    value = get_uint_parameter(parameter_stream, parameter_length);
+
+    pitem = proto_tree_add_uint(tree, hf_vgs, tvb, offset, parameter_length, value);
+
+    if (value > 15) {
+        expert_add_info(pinfo, pitem, &ei_vgs_gain);
+    }
+
+    return TRUE;
+}
+
+static gint
+dissect_vgm_parameter(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
+        gint offset, gint role, guint16 type, guint8 *parameter_stream,
+        guint parameter_number, gint parameter_length, void **data _U_)
+{
+    proto_item  *pitem;
+    guint32      value;
+
+    if (!check_vgm(role, type)) return FALSE;
+
+    if (parameter_number > 0) return FALSE;
+
+    value = get_uint_parameter(parameter_stream, parameter_length);
+
+    pitem = proto_tree_add_uint(tree, hf_vgm, tvb, offset, parameter_length, value);
+
+    if (value > 15) {
+        expert_add_info(pinfo, pitem, &ei_vgm_gain);
+    }
+
+    return TRUE;
+}
+
+static gint
+dissect_ckpd_parameter(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
+        gint offset, gint role, guint16 type, guint8 *parameter_stream,
+        guint parameter_number, gint parameter_length, void **data _U_)
+{
+    proto_item  *pitem;
+    guint32      value;
+
+    if (!check_ckpd(role, type)) return FALSE;
+
+
+    if (parameter_number > 0) return FALSE;
+
+    value = get_uint_parameter(parameter_stream, parameter_length);
+
+    pitem = proto_tree_add_uint(tree, hf_ckpd, tvb, offset, parameter_length, value);
+
+    if (value != 200) {
+        expert_add_info(pinfo, pitem, &ei_ckpd);
+    }
+
+    return TRUE;
+}
+
+static gint
+dissect_no_parameter(tvbuff_t *tvb _U_, packet_info *pinfo _U_, proto_tree *tree _U_,
+        gint offset _U_, gint role _U_, guint16 type _U_, guint8 *parameter_stream _U_,
+        guint parameter_number _U_, gint parameter_length _U_, void **data _U_)
+{
+    return FALSE;
+}
+
+static const at_cmd_t at_cmds[] = {
+    { "+VGS",       "Gain of Speaker",                          check_vgs,  dissect_vgs_parameter  },
+    { "+VGM",       "Gain of Microphone",                       check_vgm,  dissect_vgm_parameter  },
+    { "+CKPD",      "Control Keypad",                           check_ckpd, dissect_ckpd_parameter },
+    { "ERROR",      "ERROR",                                    check_only_ag_role, dissect_no_parameter },
+    { "RING",       "Incoming Call Indication",                 check_only_ag_role, dissect_no_parameter },
+    { "OK",         "OK",                                       check_only_ag_role, dissect_no_parameter },
+    { NULL, NULL, NULL, NULL }
+};
+
+
+static gint
+dissect_at_command(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree,
+        gint offset, guint32 role, gint command_number)
+{
+    proto_item      *pitem;
+    proto_tree      *command_item;
+    proto_item      *command_tree;
+    proto_tree      *parameters_item = NULL;
+    proto_item      *parameters_tree = NULL;
+    char            *col_str = NULL;
+    char            *at_stream;
+    char            *at_command = NULL;
+    gint             i_char = 0;
+    guint            i_char_fix = 0;
+    gint             length;
+    const at_cmd_t  *i_at_cmd;
+    gint             parameter_length;
+    guint            parameter_number = 0;
+    gint             first_parameter_offset = offset;
+    gint             last_parameter_offset  = offset;
+    guint16          type = TYPE_UNKNOWN;
+    guint32          brackets;
+    gboolean         quotation;
+    gboolean         next;
+    void            *data;
+
+    length = tvb_reported_length_remaining(tvb, offset);
+    if (length <= 0)
+        return tvb_reported_length(tvb);
+
+    if (!command_number) {
+        proto_tree_add_item(tree, hf_data, tvb, offset, length, ENC_NA | ENC_ASCII);
+        col_str = (char *) wmem_alloc(wmem_packet_scope(), length + 1);
+        tvb_memcpy(tvb, col_str, offset, length);
+        col_str[length] = '\0';
+    }
+
+    at_stream = (char *) wmem_alloc(wmem_packet_scope(), length + 1);
+    tvb_memcpy(tvb, at_stream, offset, length);
+    at_stream[length] = '\0';
+    while (at_stream[i_char]) {
+        at_stream[i_char] = g_ascii_toupper(at_stream[i_char]);
+        if (!command_number) {
+            col_str[i_char] = g_ascii_toupper(col_str[i_char]);
+            if (!g_ascii_isgraph(col_str[i_char])) col_str[i_char] = ' ';
+        }
+        i_char += 1;
+    }
+
+    command_item = proto_tree_add_none_format(tree, hf_command, tvb,
+            offset, 0, "Command %u", command_number);
+    command_tree = proto_item_add_subtree(command_item, ett_bthsp_command);
+
+    if (!command_number) col_append_str(pinfo->cinfo, COL_INFO, col_str);
+
+    if (role == ROLE_HS) {
+        if (command_number) {
+            at_command = at_stream;
+            i_char = 0;
+        } else {
+            at_command = g_strstr_len(at_stream, length, "AT");
+
+            if (at_command) {
+                i_char = (gint) (at_command - at_stream);
+
+                if (i_char) {
+                    proto_tree_add_item(command_tree, hf_at_ignored, tvb, offset,
+                        i_char, ENC_NA | ENC_ASCII);
+                    offset += i_char;
+                }
+
+                proto_tree_add_item(command_tree, hf_at_command_line_prefix,
+                        tvb, offset, 2, ENC_NA | ENC_ASCII);
+                offset += 2;
+                i_char += 2;
+                at_command = at_stream;
+
+                at_command += i_char;
+                length -= i_char;
+                i_char_fix += i_char;
+                i_char = 0;
+            }
+        }
+    } else {
+        at_command = at_stream;
+        i_char = 0;
+        while (i_char <= length &&
+                (at_command[i_char] == '\r' || at_command[i_char] == '\n' ||
+                at_command[i_char] == ' ' || at_command[i_char] == '\t')) {
+            /* ignore white characters */
+            i_char += 1;
+        }
+
+        offset += i_char;
+        at_command += i_char;
+        length -= i_char;
+        i_char_fix += i_char;
+        i_char = 0;
+    }
+
+    if (at_command) {
+
+        while (i_char < length &&
+                        (at_command[i_char] != '\r' && at_command[i_char] != '=' &&
+                        at_command[i_char] != ';' && at_command[i_char] != '?' &&
+                        at_command[i_char] != ':')) {
+            i_char += 1;
+        }
+
+        i_at_cmd = at_cmds;
+        if (at_command[0] == '\r') {
+            pitem = proto_tree_add_item(command_tree, hf_at_cmd, tvb, offset - 2,
+                    2, ENC_NA | ENC_ASCII);
+            i_at_cmd = NULL;
+        } else {
+            pitem = NULL;
+            while (i_at_cmd->name) {
+                if (g_str_has_prefix(&at_command[0], i_at_cmd->name)) {
+                    pitem = proto_tree_add_item(command_tree, hf_at_cmd, tvb, offset,
+                            (gint) strlen(i_at_cmd->name), ENC_NA | ENC_ASCII);
+                    proto_item_append_text(pitem, " (%s)", i_at_cmd->long_name);
+                    break;
+                }
+                i_at_cmd += 1;
+            }
+
+            if (!pitem) {
+                pitem = proto_tree_add_item(command_tree, hf_at_cmd, tvb, offset,
+                        i_char, ENC_NA | ENC_ASCII);
+            }
+        }
+
+
+        if (i_at_cmd && i_at_cmd->name == NULL) {
+            char *name;
+
+            name = format_text(wmem_packet_scope(), at_command, i_char + 1);
+            proto_item_append_text(command_item, ": %s (Unknown)", name);
+            proto_item_append_text(pitem, " (Unknown - Non-Standard HSP Command)");
+            expert_add_info(pinfo, pitem, &ei_non_mandatory_command);
+        } else if (i_at_cmd == NULL) {
+            proto_item_append_text(command_item, ": AT");
+        } else {
+            proto_item_append_text(command_item, ": %s", i_at_cmd->name);
+        }
+
+        offset += i_char;
+
+        if (i_at_cmd && g_strcmp0(i_at_cmd->name, "D")) {
+            if (length >= 2 && at_command[i_char] == '=' && at_command[i_char + 1] == '?') {
+                type = at_command[i_char] << 8 | at_command[i_char + 1];
+                proto_tree_add_uint(command_tree, hf_at_cmd_type, tvb, offset, 2, type);
+                offset += 2;
+                i_char += 2;
+            } else if (role == ROLE_AG && length >= 2 && at_command[i_char] == '\r' && at_command[i_char + 1] == '\n') {
+                type = at_command[i_char] << 8 | at_command[i_char + 1];
+                proto_tree_add_uint(command_tree, hf_at_cmd_type, tvb, offset, 2, type);
+                offset += 2;
+                i_char += 2;
+            } else if (length >= 1 && (at_command[i_char] == '=' ||
+                        at_command[i_char] == '\r' ||
+                        at_command[i_char] == ':' ||
+                        at_command[i_char] == '?')) {
+                type = at_command[i_char];
+                proto_tree_add_uint(command_tree, hf_at_cmd_type, tvb, offset, 1, type);
+                offset += 1;
+                i_char += 1;
+            }
+        }
+
+        if (i_at_cmd && i_at_cmd->check_command && !i_at_cmd->check_command(role, type)) {
+            expert_add_info(pinfo, command_item, &ei_invalid_usage);
+        }
+
+        parameters_item = proto_tree_add_none_format(command_tree, hf_parameters, tvb,
+                offset, 0, "Parameters");
+        parameters_tree = proto_item_add_subtree(parameters_item, ett_bthsp_parameters);
+
+        data = NULL;
+
+        while (i_char < length) {
+
+            while (at_command[i_char] == ' ' || at_command[i_char]  == '\t') {
+                offset += 1;
+                i_char += 1;
+            }
+
+            parameter_length = 0;
+            brackets = 0;
+            quotation = FALSE;
+            next = FALSE;
+
+            if (at_command[i_char + parameter_length] != '\r') {
+                while (i_char + parameter_length < length &&
+                        at_command[i_char + parameter_length] != '\r') {
+
+                    if (at_command[i_char + parameter_length] == ';') {
+                        next = TRUE;
+                        break;
+                    }
+
+                    if (at_command[i_char + parameter_length] == '"') {
+                        quotation = quotation ? FALSE : TRUE;
+                    }
+
+                    if (quotation == TRUE) {
+                        parameter_length += 1;
+                        continue;
+                    }
+
+                    if (at_command[i_char + parameter_length] == '(') {
+                        brackets += 1;
+                    }
+                    if (at_command[i_char + parameter_length] == ')') {
+                        brackets -= 1;
+                    }
+
+                    if (brackets == 0 && at_command[i_char + parameter_length] == ',') {
+                        break;
+                    }
+
+                    parameter_length += 1;
+                }
+
+/* TODO: Save bthsp.at_cmd, bthsp.at_cmd.type, frame_time  and frame_num here in
+
+                if (role == ROLE_HS && pinfo->fd->visited == 0) {
+
+    at_cmd_db = wmem_tree_new_autoreset(wmem_epan_scope(), wmem_file_scope());
+
+    interface_id
+    adapter_id
+    chandle
+    dlci
+
+    frame_number
+-------------------
+    at_command
+    at_type
+    frame_num
+    frame_time
+    status
+    first_response_in (if 0 - no response)
+
+
+            interface_id = interface_id;
+            adapter_id   = adapter_id;
+            chandle      = chandle;
+            dlci         = dlci;
+            frame_number = pinfo->num;
+
+
+            key[0].length = 1;
+            key[0].key = &interface_id;
+            key[1].length = 1;
+            key[1].key = &adapter_id;
+            key[2].length = 1;
+            key[2].key = &chandle;
+            key[3].length = 1;
+            key[3].key = &dlci;
+            key[4].length = 1;
+            key[4].key = &frame_number;
+            key[5].length = 0;
+            key[5].key = NULL;
+
+            cmd = wmem_new(wmem_file_scope(), at_cmd_entry_t);
+            cmd->interface_id = interface_id;
+            cmd->adapter_id   = adapter_id;
+            cmd->chandle      = chandle;
+            cmd->dlci         = dlci;
+
+            cmd->frame_number = pinfo->num;
+            cmd->status = STATUS_NO_RESPONSE;
+            cmd->time = pinfo->abs_ts;
+            cmd->at_command
+            cmd->at_type
+            cmd->first_response_in = 0;
+
+            wmem_tree_insert32_array(at_cmd_db, key, cmd);
+    }
+
+*/
+
+                first_parameter_offset = offset;
+                if (type == TYPE_ACTION || type == TYPE_RESPONSE) {
+                    if (i_at_cmd && (i_at_cmd->dissect_parameter != NULL &&
+                            !i_at_cmd->dissect_parameter(tvb, pinfo, parameters_tree, offset, role,
+                            type, &at_command[i_char], parameter_number, parameter_length, &data) )) {
+                        pitem = proto_tree_add_item(parameters_tree,
+                                hf_unknown_parameter, tvb, offset,
+                                parameter_length, ENC_NA | ENC_ASCII);
+                        expert_add_info(pinfo, pitem, &ei_unknown_parameter);
+                    } else if (i_at_cmd && i_at_cmd->dissect_parameter == NULL) {
+                        proto_tree_add_item(parameters_tree, hf_parameter, tvb, offset,
+                                parameter_length, ENC_NA | ENC_ASCII);
+                    }
+                }
+            }
+
+            if (type != TYPE_ACTION_SIMPLY && type != TYPE_RESPONSE_ACK && type != TYPE_TEST && type != TYPE_READ)
+                parameter_number += 1;
+            i_char += parameter_length;
+            offset += parameter_length;
+            last_parameter_offset = offset;
+
+            if (role == ROLE_AG &&
+                    i_char + 1 <= length &&
+                    at_command[i_char] == '\r' &&
+                    at_command[i_char + 1] == '\n') {
+                offset += 2;
+                i_char += 2;
+                break;
+            } else if (at_command[i_char] == ',' ||
+                        at_command[i_char] == '\r' ||
+                        at_command[i_char] == ';') {
+                    i_char += 1;
+                    offset += 1;
+            }
+
+            if (next) break;
+        }
+
+        i_char += i_char_fix;
+        proto_item_set_len(command_item, i_char);
+    } else {
+        length = tvb_reported_length_remaining(tvb, offset);
+        if (length < 0)
+            length = 0;
+        proto_item_set_len(command_item, length);
+        offset += length;
+    }
+
+    if (parameter_number > 0 && last_parameter_offset - first_parameter_offset > 0)
+        proto_item_set_len(parameters_item, last_parameter_offset - first_parameter_offset);
+    else
+        proto_item_append_text(parameters_item, ": No");
+
+    if (role == ROLE_AG) {
+        guint command_frame_number = 0;
+
+        if (command_frame_number) {
+            pitem = proto_tree_add_uint(command_tree, hf_command_in, tvb, offset,
+                    0, command_frame_number);
+            proto_item_set_generated(pitem);
+        } else {
+            pitem = proto_tree_add_item(command_tree, hf_unsolicited, tvb, offset, 0, ENC_NA);
+            proto_item_set_generated(pitem);
+        }
+    }
+
+    return offset;
+}
+
+static gint
+dissect_bthsp(tvbuff_t *tvb, packet_info *pinfo, proto_tree *tree, void *data)
+{
+    proto_item       *main_item;
+    proto_tree       *main_tree;
+    proto_item       *pitem;
+    gint              offset = 0;
+    guint32           role = ROLE_UNKNOWN;
+    wmem_tree_key_t   key[10];
+    guint32           interface_id;
+    guint32           adapter_id;
+    guint32           chandle;
+    guint32           dlci;
+    guint32           frame_number;
+    guint32           direction;
+    guint32           bd_addr_oui;
+    guint32           bd_addr_id;
+    fragment_t       *fragment;
+    fragment_t       *previous_fragment;
+    fragment_t       *i_fragment;
+    guint8           *at_stream;
+    gint              length;
+    gint              command_number;
+    gint              i_length;
+    tvbuff_t         *reassembled_tvb = NULL;
+    guint             reassemble_start_offset = 0;
+    guint             reassemble_end_offset   = 0;
+    gint              previous_proto;
+
+    previous_proto = (GPOINTER_TO_INT(wmem_list_frame_data(wmem_list_frame_prev(wmem_list_tail(pinfo->layers)))));
+    if (data && previous_proto == proto_btrfcomm) {
+        btrfcomm_data_t  *rfcomm_data;
+
+        rfcomm_data = (btrfcomm_data_t *) data;
+
+        interface_id = rfcomm_data->interface_id;
+        adapter_id   = rfcomm_data->adapter_id;
+        chandle      = rfcomm_data->chandle;
+        dlci         = rfcomm_data->dlci;
+        direction    = (rfcomm_data->is_local_psm) ? P2P_DIR_SENT : P2P_DIR_RECV;
+
+        if (direction == P2P_DIR_RECV) {
+            bd_addr_oui     = rfcomm_data->remote_bd_addr_oui;
+            bd_addr_id      = rfcomm_data->remote_bd_addr_id;
+        } else {
+            bd_addr_oui     = 0;
+            bd_addr_id      = 0;
+        }
+    } else {
+        interface_id = HCI_INTERFACE_DEFAULT;
+        adapter_id   = HCI_ADAPTER_DEFAULT;
+        chandle      = 0;
+        dlci         = 0;
+        direction    = P2P_DIR_UNKNOWN;
+
+        bd_addr_oui     = 0;
+        bd_addr_id      = 0;
+    }
+
+    main_item = proto_tree_add_item(tree, proto_bthsp, tvb, 0, tvb_captured_length(tvb), ENC_NA);
+    main_tree = proto_item_add_subtree(main_item, ett_bthsp);
+
+    col_set_str(pinfo->cinfo, COL_PROTOCOL, "HSP");
+
+    switch (pinfo->p2p_dir) {
+        case P2P_DIR_SENT:
+            col_set_str(pinfo->cinfo, COL_INFO, "Sent ");
+            break;
+        case P2P_DIR_RECV:
+            col_set_str(pinfo->cinfo, COL_INFO, "Rcvd ");
+            break;
+        default:
+            col_set_str(pinfo->cinfo, COL_INFO, "UnknownDirection ");
+            break;
+    }
+
+    if ((hsp_role == ROLE_AG && pinfo->p2p_dir == P2P_DIR_SENT) ||
+            (hsp_role == ROLE_HS && pinfo->p2p_dir == P2P_DIR_RECV)) {
+        role = ROLE_AG;
+    } else if (hsp_role != ROLE_UNKNOWN) {
+        role = ROLE_HS;
+    }
+
+    if (role == ROLE_UNKNOWN) {
+        guint32          sdp_psm;
+        guint32          service_type;
+        guint32          service_channel;
+        service_info_t  *service_info;
+
+        sdp_psm         = SDP_PSM_DEFAULT;
+
+        service_type    = BTSDP_RFCOMM_PROTOCOL_UUID;
+        service_channel = dlci >> 1;
+        frame_number    = pinfo->num;
+
+        key[0].length = 1;
+        key[0].key = &interface_id;
+        key[1].length = 1;
+        key[1].key = &adapter_id;
+        key[2].length = 1;
+        key[2].key = &sdp_psm;
+        key[3].length = 1;
+        key[3].key = &direction;
+        key[4].length = 1;
+        key[4].key = &bd_addr_oui;
+        key[5].length = 1;
+        key[5].key = &bd_addr_id;
+        key[6].length = 1;
+        key[6].key = &service_type;
+        key[7].length = 1;
+        key[7].key = &service_channel;
+        key[8].length = 1;
+        key[8].key = &frame_number;
+        key[9].length = 0;
+        key[9].key = NULL;
+
+        service_info = btsdp_get_service_info(key);
+        if (service_info && service_info->interface_id == interface_id &&
+                service_info->adapter_id == adapter_id &&
+                service_info->sdp_psm == SDP_PSM_DEFAULT &&
+                ((service_info->direction == P2P_DIR_RECV &&
+                service_info->bd_addr_oui == bd_addr_oui &&
+                service_info->bd_addr_id == bd_addr_id) ||
+                (service_info->direction != P2P_DIR_RECV &&
+                service_info->bd_addr_oui == 0 &&
+                service_info->bd_addr_id == 0)) &&
+                service_info->type == BTSDP_RFCOMM_PROTOCOL_UUID &&
+                service_info->channel == (dlci >> 1)) {
+            if ((service_info->uuid.bt_uuid == BTSDP_HSP_GW_SERVICE_UUID && service_info->direction == P2P_DIR_RECV && pinfo->p2p_dir == P2P_DIR_SENT) ||
+                (service_info->uuid.bt_uuid == BTSDP_HSP_GW_SERVICE_UUID && service_info->direction == P2P_DIR_SENT && pinfo->p2p_dir == P2P_DIR_RECV) ||
+                ((service_info->uuid.bt_uuid == BTSDP_HSP_SERVICE_UUID || service_info->uuid.bt_uuid == BTSDP_HSP_HS_SERVICE_UUID) && service_info->direction == P2P_DIR_RECV && pinfo->p2p_dir == P2P_DIR_RECV) ||
+                ((service_info->uuid.bt_uuid == BTSDP_HSP_SERVICE_UUID || service_info->uuid.bt_uuid == BTSDP_HSP_HS_SERVICE_UUID) && service_info->direction == P2P_DIR_SENT && pinfo->p2p_dir == P2P_DIR_SENT)) {
+                role = ROLE_HS;
+            } else {
+                role = ROLE_AG;
+            }
+        }
+    }
+
+    pitem = proto_tree_add_uint(main_tree, hf_role, tvb, 0, 0, role);
+    proto_item_set_generated(pitem);
+
+    if (role == ROLE_UNKNOWN) {
+        col_append_fstr(pinfo->cinfo, COL_INFO, "Data: %s",
+                tvb_format_text(pinfo->pool, tvb, 0, tvb_reported_length(tvb)));
+        proto_tree_add_item(main_tree, hf_data, tvb, 0, tvb_captured_length(tvb), ENC_NA | ENC_ASCII);
+        return tvb_reported_length(tvb);
+    }
+
+    /* save fragments */
+    if (!pinfo->fd->visited) {
+        frame_number = pinfo->num - 1;
+
+        key[0].length = 1;
+        key[0].key = &interface_id;
+        key[1].length = 1;
+        key[1].key = &adapter_id;
+        key[2].length = 1;
+        key[2].key = &chandle;
+        key[3].length = 1;
+        key[3].key = &dlci;
+        key[4].length = 1;
+        key[4].key = &role;
+        key[5].length = 1;
+        key[5].key = &frame_number;
+        key[6].length = 0;
+        key[6].key = NULL;
+
+        previous_fragment = (fragment_t *) wmem_tree_lookup32_array_le(fragments, key);
+        if (!(previous_fragment && previous_fragment->interface_id == interface_id &&
+                previous_fragment->adapter_id == adapter_id &&
+                previous_fragment->chandle == chandle &&
+                previous_fragment->dlci == dlci &&
+                previous_fragment->role == role &&
+                previous_fragment->reassemble_state != REASSEMBLE_DONE)) {
+            previous_fragment = NULL;
+        }
+
+        frame_number = pinfo->num;
+
+        key[0].length = 1;
+        key[0].key = &interface_id;
+        key[1].length = 1;
+        key[1].key = &adapter_id;
+        key[2].length = 1;
+        key[2].key = &chandle;
+        key[3].length = 1;
+        key[3].key = &dlci;
+        key[4].length = 1;
+        key[4].key = &role;
+        key[5].length = 1;
+        key[5].key = &frame_number;
+        key[6].length = 0;
+        key[6].key = NULL;
+
+        fragment = wmem_new(wmem_file_scope(), fragment_t);
+        fragment->interface_id      = interface_id;
+        fragment->adapter_id        = adapter_id;
+        fragment->chandle           = chandle;
+        fragment->dlci              = dlci;
+        fragment->role              = role;
+        fragment->idx               = previous_fragment ? previous_fragment->idx + previous_fragment->length : 0;
+        fragment->reassemble_state  = REASSEMBLE_FRAGMENT;
+        fragment->length            = tvb_reported_length(tvb);
+        fragment->data              = (guint8 *) wmem_alloc(wmem_file_scope(), fragment->length);
+        fragment->previous_fragment = previous_fragment;
+        tvb_memcpy(tvb, fragment->data, offset, fragment->length);
+
+        wmem_tree_insert32_array(fragments, key, fragment);
+
+        /* Detect reassemble end character: \r for HS or \n for AG */
+        length = tvb_reported_length(tvb);
+        at_stream = tvb_get_string_enc(wmem_packet_scope(), tvb, 0, length, ENC_ASCII);
+
+        reassemble_start_offset = 0;
+
+        for (i_length = 0; i_length < length; i_length += 1) {
+            if (!((role == ROLE_HS && at_stream[i_length] == '\r') ||
+                    (role == ROLE_AG && at_stream[i_length] == '\n'))) {
+                continue;
+            }
+
+            if (role == ROLE_HS && at_stream[i_length] == '\r') {
+                reassemble_start_offset = i_length + 1;
+                if (reassemble_end_offset == 0) reassemble_end_offset = i_length + 1;
+            }
+
+            if (role == ROLE_AG && at_stream[i_length] == '\n') {
+                reassemble_start_offset = i_length + 1;
+            }
+
+            frame_number = pinfo->num;
+
+            key[0].length = 1;
+            key[0].key = &interface_id;
+            key[1].length = 1;
+            key[1].key = &adapter_id;
+            key[2].length = 1;
+            key[2].key = &chandle;
+            key[3].length = 1;
+            key[3].key = &dlci;
+            key[4].length = 1;
+            key[4].key = &role;
+            key[5].length = 1;
+            key[5].key = &frame_number;
+            key[6].length = 0;
+            key[6].key = NULL;
+
+            fragment = (fragment_t *) wmem_tree_lookup32_array_le(fragments, key);
+            if (fragment && fragment->interface_id == interface_id &&
+                    fragment->adapter_id == adapter_id &&
+                    fragment->chandle == chandle &&
+                    fragment->dlci == dlci &&
+                    fragment->role == role) {
+                i_fragment = fragment;
+                while (i_fragment && i_fragment->idx > 0) {
+                    i_fragment = i_fragment->previous_fragment;
+                }
+
+                if (i_length + 1 == length &&
+                        role == ROLE_HS &&
+                        at_stream[i_length] == '\r') {
+                    fragment->reassemble_state = REASSEMBLE_DONE;
+                } else if (i_length + 1 == length &&
+                        role == ROLE_AG &&
+                        i_length >= 4 &&
+                        at_stream[i_length] == '\n' &&
+                        at_stream[i_length - 1] == '\r' &&
+                        at_stream[0] == '\r' &&
+                        at_stream[1] == '\n') {
+                    fragment->reassemble_state = REASSEMBLE_DONE;
+                } else if (i_length + 1 == length &&
+                        role == ROLE_AG &&
+                        i_length >= 2 &&
+                        at_stream[i_length] == '\n' &&
+                        at_stream[i_length - 1] == '\r' &&
+                        i_fragment &&
+                        i_fragment->reassemble_state == REASSEMBLE_FRAGMENT &&
+                        i_fragment->length >= 2 &&
+                        i_fragment->data[0] == '\r' &&
+                        i_fragment->data[1] == '\n') {
+                    fragment->reassemble_state = REASSEMBLE_DONE;
+                } else if (role == ROLE_HS) {
+/* XXX: Temporary disable reassembling of partial message, it seems to be broken */
+/*                    fragment->reassemble_state = REASSEMBLE_PARTIALLY;*/
+                }
+                fragment->reassemble_start_offset = reassemble_start_offset;
+                fragment->reassemble_end_offset = reassemble_end_offset;
+            }
+        }
+    }
+
+    /* recover reassembled payload */
+    frame_number = pinfo->num;
+
+    key[0].length = 1;
+    key[0].key = &interface_id;
+    key[1].length = 1;
+    key[1].key = &adapter_id;
+    key[2].length = 1;
+    key[2].key = &chandle;
+    key[3].length = 1;
+    key[3].key = &dlci;
+    key[4].length = 1;
+    key[4].key = &role;
+    key[5].length = 1;
+    key[5].key = &frame_number;
+    key[6].length = 0;
+    key[6].key = NULL;
+
+    fragment = (fragment_t *) wmem_tree_lookup32_array_le(fragments, key);
+    if (fragment && fragment->interface_id == interface_id &&
+            fragment->adapter_id == adapter_id &&
+            fragment->chandle == chandle &&
+            fragment->dlci == dlci &&
+            fragment->role == role &&
+            fragment->reassemble_state != REASSEMBLE_FRAGMENT) {
+        guint8    *at_data;
+        guint      i_data_offset;
+
+        i_data_offset = fragment->idx + fragment->length;
+        at_data = (guint8 *) wmem_alloc(pinfo->pool, fragment->idx + fragment->length);
+
+        i_fragment = fragment;
+
+        if (i_fragment && i_fragment->reassemble_state == REASSEMBLE_PARTIALLY) {
+            i_data_offset -= i_fragment->reassemble_end_offset;
+            memcpy(at_data + i_data_offset, i_fragment->data, i_fragment->reassemble_end_offset);
+            i_fragment = i_fragment->previous_fragment;
+        }
+
+        if (i_fragment) {
+            while (i_fragment && i_fragment->idx > 0) {
+                i_data_offset -= i_fragment->length;
+                memcpy(at_data + i_data_offset, i_fragment->data, i_fragment->length);
+                i_fragment = i_fragment->previous_fragment;
+            }
+
+            if (i_fragment && i_fragment->reassemble_state == REASSEMBLE_PARTIALLY) {
+                i_data_offset -= (i_fragment->length - i_fragment->reassemble_start_offset);
+                memcpy(at_data + i_data_offset, i_fragment->data + i_fragment->reassemble_start_offset,
+                        i_fragment->length - i_fragment->reassemble_start_offset);
+            } else if (i_fragment) {
+                i_data_offset -= i_fragment->length;
+                memcpy(at_data + i_data_offset, i_fragment->data, i_fragment->length);
+            }
+        }
+
+        if (fragment->idx > 0 && fragment->length > 0) {
+            proto_tree_add_item(main_tree, hf_fragment, tvb, offset,
+                    tvb_captured_length_remaining(tvb, offset), ENC_ASCII | ENC_NA);
+            reassembled_tvb = tvb_new_child_real_data(tvb, at_data,
+                    fragment->idx + fragment->length, fragment->idx + fragment->length);
+            add_new_data_source(pinfo, reassembled_tvb, "Reassembled HSP");
+        }
+
+        command_number = 0;
+        if (reassembled_tvb) {
+            guint reassembled_offset = 0;
+
+            while (tvb_reported_length(reassembled_tvb) > reassembled_offset) {
+                reassembled_offset = dissect_at_command(reassembled_tvb,
+                        pinfo, main_tree, reassembled_offset, role, command_number);
+                command_number += 1;
+            }
+            offset = tvb_captured_length(tvb);
+        } else {
+            while (tvb_reported_length(tvb) > (guint) offset) {
+                offset = dissect_at_command(tvb, pinfo, main_tree, offset, role, command_number);
+                command_number += 1;
+            }
+        }
+    } else {
+        pitem = proto_tree_add_item(main_tree, hf_fragmented, tvb, 0, 0, ENC_NA);
+        proto_item_set_generated(pitem);
+        char *display_str;
+        proto_tree_add_item_ret_display_string(main_tree, hf_fragment, tvb, offset, -1, ENC_ASCII, pinfo->pool, &display_str);
+        col_append_fstr(pinfo->cinfo, COL_INFO, "Fragment: %s", display_str);
+        offset = tvb_captured_length(tvb);
+    }
+
+    return offset;
+}
+
+void
+proto_register_bthsp(void)
+{
+    module_t         *module;
+    expert_module_t  *expert_bthsp;
+
+    static hf_register_info hf[] = {
+        { &hf_command,
+           { "Command",                          "bthsp.command",
+           FT_NONE, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_parameters,
+           { "Parameters",                       "bthsp.parameters",
+           FT_NONE, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_command_in,
+           { "Command frame number in",          "bthsp.command_in",
+           FT_FRAMENUM, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_unsolicited,
+           { "Unsolicited",                      "bthsp.unsolicited",
+           FT_NONE, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_data,
+           { "AT Stream",                        "bthsp.data",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_fragment,
+           { "Fragment",                         "bthsp.fragment",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_fragmented,
+           { "Fragmented",                       "bthsp.fragmented",
+           FT_NONE, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_at_ignored,
+           { "Ignored",                          "bthsp.ignored",
+           FT_BYTES, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_at_cmd,
+           { "Command",                          "bthsp.at_cmd",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_at_cmd_type,
+           { "Type",                             "bthsp.at_cmd.type",
+           FT_UINT16, BASE_HEX, VALS(at_cmd_type_vals), 0,
+           NULL, HFILL}
+        },
+        { &hf_at_command_line_prefix,
+           { "Command Line Prefix",              "bthsp.command_line_prefix",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_parameter,
+           { "Parameter",                        "bthsp.parameter",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_unknown_parameter,
+           { "Unknown Parameter",                "bthsp.unknown_parameter",
+           FT_STRING, BASE_NONE, NULL, 0,
+           NULL, HFILL}
+        },
+        { &hf_role,
+           { "Role",                             "bthsp.role",
+           FT_UINT8, BASE_DEC, VALS(role_vals), 0,
+           NULL, HFILL}
+        },
+        { &hf_vgs,
+           { "Gain",                             "bthsp.vgs",
+           FT_UINT8, BASE_DEC|BASE_UNIT_STRING, &units_slash15, 0,
+           NULL, HFILL}
+        },
+        { &hf_vgm,
+           { "Gain",                             "bthsp.vgm",
+           FT_UINT8, BASE_DEC|BASE_UNIT_STRING, &units_slash15, 0,
+           NULL, HFILL}
+        },
+        { &hf_ckpd,
+           { "Key",                             "bthsp.ckpd",
+           FT_UINT8, BASE_DEC, NULL, 0,
+           NULL, HFILL}
+        }
+    };
+
+    static ei_register_info ei[] = {
+        { &ei_non_mandatory_command, { "bthsp.expert.non_mandatory_command", PI_PROTOCOL, PI_NOTE, "Non-mandatory command in HSP", EXPFILL }},
+        { &ei_invalid_usage,         { "bthsp.expert.invalid_usage", PI_PROTOCOL, PI_WARN, "Non mandatory type or command in this role", EXPFILL }},
+        { &ei_unknown_parameter,     { "bthsp.expert.unknown_parameter", PI_PROTOCOL, PI_WARN, "Unknown parameter", EXPFILL }},
+        { &ei_vgm_gain,              { "bthsp.expert.vgm", PI_PROTOCOL, PI_WARN, "Gain of microphone exceeds range 0-15", EXPFILL }},
+        { &ei_vgs_gain,              { "bthsp.expert.vgs", PI_PROTOCOL, PI_WARN, "Gain of speaker exceeds range 0-15", EXPFILL }},
+        { &ei_ckpd,              { "bthsp.expert.ckpd", PI_PROTOCOL, PI_WARN, "Only key 200 is covered in HSP", EXPFILL }}    };
+
+    static gint *ett[] = {
+        &ett_bthsp,
+        &ett_bthsp_command,
+        &ett_bthsp_parameters
+    };
+
+    fragments = wmem_tree_new_autoreset(wmem_epan_scope(), wmem_file_scope());
+
+    proto_bthsp = proto_register_protocol("Bluetooth HSP Profile", "BT HSP", "bthsp");
+    bthsp_handle = register_dissector("bthsp", dissect_bthsp, proto_bthsp);
+
+    proto_register_field_array(proto_bthsp, hf, array_length(hf));
+    proto_register_subtree_array(ett, array_length(ett));
+
+    module = prefs_register_protocol_subtree("Bluetooth", proto_bthsp, NULL);
+    prefs_register_static_text_preference(module, "hsp.version",
+            "Bluetooth Profile HSP version: 1.2",
+            "Version of profile supported by this dissector.");
+
+    prefs_register_enum_preference(module, "hsp.hsp_role",
+            "Force treat packets as AG or HS role",
+            "Force treat packets as AG or HS role",
+            &hsp_role, pref_hsp_role, TRUE);
+
+    expert_bthsp = expert_register_protocol(proto_bthsp);
+    expert_register_field_array(expert_bthsp, ei, array_length(ei));
+}
+
+void
+proto_reg_handoff_bthsp(void)
+{
+    dissector_add_string("bluetooth.uuid",  "1108", bthsp_handle);
+    dissector_add_string("bluetooth.uuid",  "1112", bthsp_handle);
+    dissector_add_string("bluetooth.uuid",  "1131", bthsp_handle);
+
+    dissector_add_for_decode_as("btrfcomm.dlci", bthsp_handle);
+}
+
+/*
+ * Editor modelines  -  https://www.wireshark.org/tools/modelines.html
+ *
+ * Local variables:
+ * c-basic-offset: 4
+ * tab-width: 8
+ * indent-tabs-mode: nil
+ * End:
+ *
+ * vi: set shiftwidth=4 tabstop=8 expandtab:
+ * :indentSize=4:tabSize=8:noTabs=true:
+ */