monitor/l2cap.h

   1 /* SPDX-License-Identifier: LGPL-2.1-or-later */
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2014  Intel Corporation
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #include <stdint.h>
  13 #include <stdbool.h>
  14
  15 struct l2cap_frame {
  16         uint16_t index;
  17         bool in;
  18         uint16_t handle;
  19         uint8_t ident;
  20         uint16_t cid;
  21         uint16_t psm;
  22         uint16_t chan;
  23         uint8_t mode;
  24         uint8_t seq_num;
  25         const void *data;
  26         uint16_t size;
  27 };
  28
  29 void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
  30                                 uint16_t handle, uint8_t ident,
  31                                 uint16_t cid, uint16_t psm,
  32                                 const void *data, uint16_t size);
  33
  34 static inline void l2cap_frame_clone_size(struct l2cap_frame *frame,
  35                                 const struct l2cap_frame *source,
  36                                 uint16_t size)
  37 {
  38         if (frame != source) {
  39                 frame->index   = source->index;
  40                 frame->in      = source->in;
  41                 frame->handle  = source->handle;
  42                 frame->ident   = source->ident;
  43                 frame->cid     = source->cid;
  44                 frame->psm     = source->psm;
  45                 frame->chan    = source->chan;
  46                 frame->mode    = source->mode;
  47                 frame->data    = source->data;
  48                 frame->size    = size;
  49         }
  50 }
  51
  52 static inline void l2cap_frame_clone(struct l2cap_frame *frame,
  53                                 const struct l2cap_frame *source)
  54 {
  55         l2cap_frame_clone_size(frame, source, source->size);
  56 }
  57
  58 static inline void *l2cap_frame_pull(struct l2cap_frame *frame,
  59                                 const struct l2cap_frame *source, uint16_t len)
  60 {
  61         void *data;
  62
  63         l2cap_frame_clone(frame, source);
  64
  65         if (source->size < len)
  66                 return NULL;
  67
  68         data = (void *)frame->data;
  69         frame->data = source->data + len;
  70         frame->size = source->size - len;
  71
  72         return data;
  73 }
  74
  75 static inline bool l2cap_frame_get_u8(struct l2cap_frame *frame, uint8_t *value)
  76 {
  77         if (frame->size < sizeof(*value))
  78                 return false;
  79
  80         if (value)
  81                 *value = *((uint8_t *) frame->data);
  82
  83         l2cap_frame_pull(frame, frame, sizeof(*value));
  84
  85         return true;
  86 }
  87
  88 static inline bool l2cap_frame_print_u8(struct l2cap_frame *frame,
  89                                         const char *label)
  90 {
  91         uint8_t u8;
  92
  93         if (!l2cap_frame_get_u8(frame, &u8)) {
  94                 print_text(COLOR_ERROR, "%s: invalid size", label);
  95                 return false;
  96         }
  97
  98         print_field("%s: 0x%2.2x", label, u8);
  99
 100         return true;
 101 }
 102
 103 static inline bool l2cap_frame_get_be16(struct l2cap_frame *frame,
 104                                                                 uint16_t *value)
 105 {
 106         if (frame->size < sizeof(*value))
 107                 return false;
 108
 109         if (value)
 110                 *value = get_be16(frame->data);
 111
 112         l2cap_frame_pull(frame, frame, sizeof(*value));
 113
 114         return true;
 115 }
 116
 117 static inline bool l2cap_frame_print_be16(struct l2cap_frame *frame,
 118                                                 const char *label)
 119 {
 120         uint16_t u16;
 121
 122         if (!l2cap_frame_get_be16(frame, &u16)) {
 123                 print_text(COLOR_ERROR, "%s: invalid size", label);
 124                 return false;
 125         }
 126
 127         print_field("%s: 0x%4.4x", label, u16);
 128
 129         return true;
 130 }
 131
 132 static inline bool l2cap_frame_get_le16(struct l2cap_frame *frame,
 133                                                                 uint16_t *value)
 134 {
 135         if (frame->size < sizeof(*value))
 136                 return false;
 137
 138         if (value)
 139                 *value = get_le16(frame->data);
 140
 141         l2cap_frame_pull(frame, frame, sizeof(*value));
 142
 143         return true;
 144 }
 145
 146 static inline bool l2cap_frame_print_le16(struct l2cap_frame *frame,
 147                                                 const char *label)
 148 {
 149         uint16_t u16;
 150
 151         if (!l2cap_frame_get_le16(frame, &u16)) {
 152                 print_text(COLOR_ERROR, "%s: invalid size", label);
 153                 return false;
 154         }
 155
 156         print_field("%s: 0x%4.4x", label, u16);
 157
 158         return true;
 159 }
 160
 161 static inline bool l2cap_frame_get_be24(struct l2cap_frame *frame,
 162                                                                 uint32_t *value)
 163 {
 164         if (frame->size < sizeof(uint24_t))
 165                 return false;
 166
 167         if (value)
 168                 *value = get_be24(frame->data);
 169
 170         l2cap_frame_pull(frame, frame, sizeof(uint24_t));
 171
 172         return true;
 173 }
 174
 175 static inline bool l2cap_frame_print_be24(struct l2cap_frame *frame,
 176                                                 const char *label)
 177 {
 178         uint32_t u24;
 179
 180         if (!l2cap_frame_get_be24(frame, &u24)) {
 181                 print_text(COLOR_ERROR, "%s: invalid size", label);
 182                 return false;
 183         }
 184
 185         print_field("%s: 0x%6.6x", label, u24);
 186
 187         return true;
 188 }
 189
 190 static inline bool l2cap_frame_get_le24(struct l2cap_frame *frame,
 191                                                                 uint32_t *value)
 192 {
 193         if (frame->size < sizeof(uint24_t))
 194                 return false;
 195
 196         if (value)
 197                 *value = get_le24(frame->data);
 198
 199         l2cap_frame_pull(frame, frame, sizeof(uint24_t));
 200
 201         return true;
 202 }
 203
 204 static inline bool l2cap_frame_print_le24(struct l2cap_frame *frame,
 205                                                 const char *label)
 206 {
 207         uint32_t u24;
 208
 209         if (!l2cap_frame_get_le24(frame, &u24)) {
 210                 print_text(COLOR_ERROR, "%s: invalid size", label);
 211                 return false;
 212         }
 213
 214         print_field("%s: 0x%6.6x", label, u24);
 215
 216         return true;
 217 }
 218
 219 static inline bool l2cap_frame_get_be32(struct l2cap_frame *frame,
 220                                                                 uint32_t *value)
 221 {
 222         if (frame->size < sizeof(*value))
 223                 return false;
 224
 225         if (value)
 226                 *value = get_be32(frame->data);
 227
 228         l2cap_frame_pull(frame, frame, sizeof(*value));
 229
 230         return true;
 231 }
 232
 233 static inline bool l2cap_frame_print_be32(struct l2cap_frame *frame,
 234                                                 const char *label)
 235 {
 236         uint32_t u32;
 237
 238         if (!l2cap_frame_get_be32(frame, &u32)) {
 239                 print_text(COLOR_ERROR, "%s: invalid size", label);
 240                 return false;
 241         }
 242
 243         print_field("%s: 0x%8.8x", label, u32);
 244
 245         return true;
 246 }
 247
 248 static inline bool l2cap_frame_get_le32(struct l2cap_frame *frame,
 249                                                                 uint32_t *value)
 250 {
 251         if (frame->size < sizeof(*value))
 252                 return false;
 253
 254         if (value)
 255                 *value = get_le32(frame->data);
 256
 257         l2cap_frame_pull(frame, frame, sizeof(*value));
 258
 259         return true;
 260 }
 261
 262 static inline bool l2cap_frame_print_le32(struct l2cap_frame *frame,
 263                                                 const char *label)
 264 {
 265         uint32_t u32;
 266
 267         if (!l2cap_frame_get_le32(frame, &u32)) {
 268                 print_text(COLOR_ERROR, "%s: invalid size", label);
 269                 return false;
 270         }
 271
 272         print_field("%s: 0x%8.8x", label, u32);
 273
 274         return true;
 275 }
 276
 277 static inline bool l2cap_frame_get_be64(struct l2cap_frame *frame,
 278                                                                 uint64_t *value)
 279 {
 280         if (frame->size < sizeof(*value))
 281                 return false;
 282
 283         if (value)
 284                 *value = get_be64(frame->data);
 285
 286         l2cap_frame_pull(frame, frame, sizeof(*value));
 287
 288         return true;
 289 }
 290
 291 static inline bool l2cap_frame_print_be64(struct l2cap_frame *frame,
 292                                                 const char *label)
 293 {
 294         uint64_t u64;
 295
 296         if (!l2cap_frame_get_be64(frame, &u64)) {
 297                 print_text(COLOR_ERROR, "%s: invalid size", label);
 298                 return false;
 299         }
 300
 301         print_field("%s: 0x%" PRIx64, label, u64);
 302
 303         return true;
 304 }
 305
 306 static inline bool l2cap_frame_get_le64(struct l2cap_frame *frame,
 307                                                                 uint64_t *value)
 308 {
 309         if (frame->size < sizeof(*value))
 310                 return false;
 311
 312         if (value)
 313                 *value = get_le64(frame->data);
 314
 315         l2cap_frame_pull(frame, frame, sizeof(*value));
 316
 317         return true;
 318 }
 319
 320 static inline bool l2cap_frame_print_le64(struct l2cap_frame *frame,
 321                                                 const char *label)
 322 {
 323         uint64_t u64;
 324
 325         if (!l2cap_frame_get_le64(frame, &u64)) {
 326                 print_text(COLOR_ERROR, "%s: invalid size", label);
 327                 return false;
 328         }
 329
 330         print_field("%s: 0x%" PRIx64, label, u64);
 331
 332         return true;
 333 }
 334
 335 static inline bool l2cap_frame_get_be128(struct l2cap_frame *frame,
 336                                         uint64_t *lvalue, uint64_t *rvalue)
 337 {
 338         if (frame->size < (sizeof(*lvalue) + sizeof(*rvalue)))
 339                 return false;
 340
 341         if (lvalue && rvalue) {
 342                 *lvalue = get_be64(frame->data);
 343                 *rvalue = get_be64(frame->data);
 344         }
 345
 346         l2cap_frame_pull(frame, frame, (sizeof(*lvalue) + sizeof(*rvalue)));
 347
 348         return true;
 349 }
 350
 351 void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
 352                 uint16_t psm, const void *data, uint16_t size);
 353
 354 void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
 355                                         const void *data, uint16_t size);
 356
 357 void rfcomm_packet(const struct l2cap_frame *frame);
 358
 359 void l2cap_dequeue_frame(struct timeval *delta, struct packet_conn_data *conn);