From b8e280747401534acab90404f38f299e6f161a1f Mon Sep 17 00:00:00 2001
From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
Date: Fri, 22 Aug 2014 13:23:03 +0300
Subject: [PATCH] monitor: Add helper functions to get data from L2CAP frames

---
 monitor/l2cap.h | 118 ++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 109 insertions(+), 9 deletions(-)

diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index a0f844bbe..645e3ef6a 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -40,15 +40,115 @@ struct l2cap_frame {
 static inline void l2cap_frame_pull(struct l2cap_frame *frame,
 				const struct l2cap_frame *source, uint16_t len)
 {
-	frame->index   = source->index;
-	frame->in      = source->in;
-	frame->handle  = source->handle;
-	frame->cid     = source->cid;
-	frame->psm     = source->psm;
-	frame->chan    = source->chan;
-	frame->mode    = source->mode;
-	frame->data    = source->data + len;
-	frame->size    = source->size - len;
+	if (frame != source) {
+		frame->index   = source->index;
+		frame->in      = source->in;
+		frame->handle  = source->handle;
+		frame->cid     = source->cid;
+		frame->psm     = source->psm;
+		frame->chan    = source->chan;
+		frame->mode    = source->mode;
+	}
+
+	frame->data = source->data + len;
+	frame->size = source->size - len;
+}
+
+static inline int l2cap_frame_get_u8(struct l2cap_frame *frame, uint8_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = *((uint8_t *) frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_be16(struct l2cap_frame *frame,
+								uint16_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_be16(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_le16(struct l2cap_frame *frame,
+								uint16_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_le16(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_be32(struct l2cap_frame *frame,
+								uint32_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_be32(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_le32(struct l2cap_frame *frame,
+								uint32_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_le32(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_be64(struct l2cap_frame *frame,
+								uint64_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_be64(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
+}
+
+static inline int l2cap_frame_get_le64(struct l2cap_frame *frame,
+								uint64_t *value)
+{
+	if (frame->size < sizeof(*value))
+		return -1;
+
+	if (value)
+		*value = get_le64(frame->data);
+
+	l2cap_frame_pull(frame, frame, sizeof(*value));
+
+	return 0;
 }
 
 void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
-- 
2.47.3